remove unitest of issue1452

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<Pose3>(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<Pose3>(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<Pose3>(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);