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