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

added test for this issue

release/4.3a0
Varun Agrawal 2021-06-07 10:23:15 -04:00
parent 1209857a33
commit f1bed302e4
1 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -363,6 +363,34 @@ TEST(TestLinearContainerFactor, Rekey) {
CHECK(assert_equal(linearization_pt_rekeyed.keys()[0], lcf_factor_rekey_ptr->keys()[1]));
}
/* ************************************************************************* */
TEST(TestLinearContainerFactor, Rekey2) {
// Make an example factor
auto nonlinear_factor =
boost::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0),
gtsam::noiseModel::Isotropic::Sigma(3, 1));
// Linearize and create an LCF
gtsam::Values linearization_pt;
linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0));
linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0));
LinearContainerFactor lcf_factor(
nonlinear_factor->linearize(linearization_pt), linearization_pt);
// Define a key mapping with only a single key remapped.
// This should throw an exception if there is a bug.
std::map<gtsam::Key, gtsam::Key> key_map;
key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
// Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>(
lcf_factor.rekey(key_map));
CHECK(lcf_factor_rekey_ptr);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */