diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index f025c4470a..269177b5db 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -356,156 +356,6 @@ TEST(IncrementalFixedLagSmoother, Example) { } } -/* ************************************************************************* */ -namespace issue1452 { - -// Factor types definition -enum FactorType { PRIOR = 0, BETWEEN = 1 }; - -// Helper function to read covariance matrix from input stream -Matrix6 readCovarianceMatrix(istringstream& iss) { - Matrix6 cov; - for (int r = 0; r < 6; ++r) { - for (int c = 0; c < 6; ++c) { - iss >> cov(r, c); - } - } - return cov; -} - -// Helper function to create pose from parameters -Pose3 createPose(double x, double y, double z, double roll, double pitch, - double yaw) { - return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); -} - -/** - * Data Format - * PRIOR factor: factor_type timestamp key pose(x y z roll pitch yaw) cov(6*6) - * BETWEEN factor: factor_type timestamp key1 key2 pose(x y z r p y) cov(6*6) - * */ - -TEST(IncrementalFixedLagSmoother, Issue1452) { - // Open factor graph file - auto path = findExampleDataFile("issue1452.txt"); - cout << "path = " << path << endl; - ifstream infile(path); - CHECK(infile.is_open()); - - // Setup ISAM2 parameters for smoother - ISAM2Params isam_parameters; - isam_parameters.relinearizeThreshold = 0.1; - isam_parameters.relinearizeSkip = 1; - // isam_parameters.cacheLinearizedFactors = true; - isam_parameters.findUnusedFactorSlots = true; - // isam_parameters.evaluateNonlinearError = false; - // isam_parameters.enableDetailedResults = true; - - // Initialize fixed-lag smoother with 1-second window - IncrementalFixedLagSmoother smoother(1, isam_parameters); - NonlinearFactorGraph newFactors; - Values newValues, currentEstimate; - FixedLagSmoother::KeyTimestampMap newTimestamps; - Pose3 lastPose; - - // check the isam parameters - isam_parameters.print(); - - string line; - int lineCount = 0; - while (getline(infile, line)) { - if (line.empty()) continue; - istringstream iss(line); - - // if we only want to read less data - // if (lineCount > 100) break; - - cout << "\n========================Processing line " << ++lineCount - << " =========================" << endl; - int factorType; - iss >> factorType; - if (factorType == PRIOR) { - // Read prior factor data, only the first line to fix the coordinate - // system - double timestamp; - int key; - double x, y, z, roll, pitch, yaw; - iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw; - // Create pose and add prior factor - Pose3 pose = createPose(x, y, z, roll, pitch, yaw); - Matrix6 cov = readCovarianceMatrix(iss); - auto noise = noiseModel::Gaussian::Covariance(cov); - newFactors.add(PriorFactor(X(key), pose, noise)); - if (!newValues.exists(X(key))) { - newValues.insert(X(key), pose); - newTimestamps[X(key)] = timestamp; - } - cout << "Add prior factor " << key << endl; - } else if (factorType == BETWEEN) { - // Read between factor data - double timestamp; - int key1, key2; - // Read timestamps and keys - iss >> timestamp >> key1 >> key2; - // Read relative pose between key1 and key2 - double x1, y1, z1, roll1, pitch1, yaw1; - iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1; - Pose3 relative_pose = createPose(x1, y1, z1, roll1, pitch1, yaw1); - // Read covariance of relative_pose - Matrix6 cov = readCovarianceMatrix(iss); - auto noise = noiseModel::Gaussian::Covariance(cov); - // Add between factor of key1 and key2 - newFactors.add( - BetweenFactor(X(key1), X(key2), relative_pose, noise)); - if (!newValues.exists(X(key2))) { - // Use last optimized pose composed with relative pose for key2 - newValues.insert(X(key2), lastPose.compose(relative_pose)); - newTimestamps[X(key2)] = timestamp; - } - cout << "Add between factor " << key1 << " -> " << key2 << endl; - } - // Print statistics before update - cout << "Before update - Factors: " << smoother.getFactors().size() - << ", NR Factors: " << smoother.getFactors().nrFactors() << endl; - cout << "New factors: " << newFactors.size() - << ", New values: " << newValues.size() << endl; - // Update smoother - try { - smoother.update(newFactors, newValues, newTimestamps); - int max_extra_iterations = 3; - for (size_t n_iter = 1; n_iter < max_extra_iterations; ++n_iter) { - smoother.update(); - } - - cout << "After update - Factors: " << smoother.getFactors().size() - << ", NR Factors: " << smoother.getFactors().nrFactors() << endl; - - // Update current estimate and last pose - currentEstimate = smoother.calculateEstimate(); - if (!currentEstimate.empty()) { - lastPose = currentEstimate.at(currentEstimate.keys().back()); - // Optional: Print the latest pose for debugging - // cout << "Latest pose: " << - // lastPose.translation().transpose() << endl; - } - - // Clear containers for next iteration - newFactors.resize(0); - newValues.clear(); - newTimestamps.clear(); - } catch (const exception& e) { - cerr << "Update failed: " << e.what() << endl; - } - } - - // Check that the number of factors is correct - CHECK_EQUAL(12, smoother.getFactors().size()); - CHECK_EQUAL(11, smoother.getFactors().nrFactors()); - infile.close(); -} - -} // namespace issue1452 -/* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);