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

Merge pull request #785 from borglab/fix/linear-container-factor

release/4.3a0
Varun Agrawal 2021-06-07 20:53:11 -04:00 committed by GitHub
parent b1e91671fd f1bed302e4
commit 4270399083
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 33 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -175,9 +175,10 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
Values newLinearizationPoint;
for (size_t i = 0; i < factor_->size(); ++i) {
auto mapping = rekey_mapping.find(factor_->keys()[i]);
if (mapping != rekey_mapping.end())
if (mapping != rekey_mapping.end()) {
new_factor->factor_->keys()[i] = mapping->second;
newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
}
}
new_factor->linearizationPoint_ = newLinearizationPoint;

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

@ -326,13 +326,13 @@ TEST(TestLinearContainerFactor, Rekey) {
// Make an example factor
auto nonlinear_factor =
boost::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), 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());
linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3());
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);
@ -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); }
/* ************************************************************************* */