12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update testIncrementalFixedLagSmoother.cpp to reproduce the bug in marginalization

release/4.3a0
Jeffrey 2024-10-28 20:33:06 +08:00
parent d48b1fc840
commit 1dd3b180b1
2 changed files with 100 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/inference/VariableIndex.h
View File

@ -87,9 +87,11 @@ class GTSAM_EXPORT VariableIndex {
const FactorIndices& operator[](Key variable) const {
KeyMap::const_iterator item = index_.find(variable);
if(item == index_.end())
throw std::invalid_argument("Requested non-existent variable from VariableIndex");
throw std::invalid_argument("Requested non-existent variable '" +
DefaultKeyFormatter(variable) +
"' from VariableIndex");
else
return item->second;
return item->second;
}
/// Return true if no factors associated with a variable

97
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -49,6 +49,41 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini
return assert_equal(expected, actual);
}
/* ************************************************************************* */
void PrintSymbolicTreeHelper(
const ISAM2Clique::shared_ptr& clique, const std::string indent = "") {
// Print the current clique
std::cout << indent << "P( ";
for(Key key: clique->conditional()->frontals()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
if (clique->conditional()->nrParents() > 0)
std::cout << "| ";
for(Key key: clique->conditional()->parents()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
// Recursively print all of the children
for(const ISAM2Clique::shared_ptr& child: clique->children) {
PrintSymbolicTreeHelper(child, indent + " ");
}
}
/* ************************************************************************* */
void PrintSymbolicTree(const ISAM2& isam,
const std::string& label) {
std::cout << label << std::endl;
if (!isam.roots().empty()) {
for(const ISAM2::sharedClique& root: isam.roots()) {
PrintSymbolicTreeHelper(root);
}
} else
std::cout << "{Empty Tree}" << std::endl;
}
/* ************************************************************************* */
TEST( IncrementalFixedLagSmoother, Example )
{
@ -64,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example )
// Create a Fixed-Lag Smoother
typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
IncrementalFixedLagSmoother smoother(7.0, ISAM2Params());
IncrementalFixedLagSmoother smoother(9.0, ISAM2Params());
// Create containers to keep the full graph
Values fullinit;
@ -158,6 +193,9 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues;
Timestamps newTimestamps;
// Add the odometry factor twice to ensure the removeFactor test below works,
// where we need to keep the connectivity of the graph.
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i);
@ -210,6 +248,10 @@ TEST( IncrementalFixedLagSmoother, Example )
const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl;
std::cout << "smootherFactorsBeforeRemove.size() = "
<< smootherFactorsBeforeRemove.size() << std::endl;
// remove factor
smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
@ -231,6 +273,59 @@ TEST( IncrementalFixedLagSmoother, Example )
}
}
}
{
PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:");
i = 17;
while(i <= 200) {
Key key_0 = MakeKey(i);
Key key_1 = MakeKey(i-1);
Key key_2 = MakeKey(i-2);
Key key_3 = MakeKey(i-3);
Key key_4 = MakeKey(i-4);
Key key_5 = MakeKey(i-5);
Key key_6 = MakeKey(i-6);
Key key_7 = MakeKey(i-7);
Key key_8 = MakeKey(i-8);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
// To make a complex graph
newFactors.push_back(BetweenFactor<Point2>(key_1, key_0, Point2(1.0, 0.0), odometerNoise));
if (i % 2 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_2, key_1, Point2(1.0, 0.0), odometerNoise));
if (i % 3 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_3, key_2, Point2(1.0, 0.0), odometerNoise));
if (i % 4 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_4, key_3, Point2(1.0, 0.0), odometerNoise));
if (i % 5 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_5, key_4, Point2(1.0, 0.0), odometerNoise));
if (i % 6 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_6, key_5, Point2(1.0, 0.0), odometerNoise));
if (i % 7 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_7, key_6, Point2(1.0, 0.0), odometerNoise));
if (i % 8 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_8, key_7, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key_0, Point2(double(i)+0.1, -0.1));
newTimestamps[key_0] = double(i);
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key_0));
PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i));
++i;
}
}
}
/* ************************************************************************* */