remove unitest of issue1452
							parent
							
								
									a9da253d00
								
							
						
					
					
						commit
						49abc02b4e
					
				|  | @ -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); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue