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

fix bug in LinearContainerFactor and warnings about Point3

release/4.3a0
Varun Agrawal 2021-06-05 20:08:31 -04:00
parent 2e29d45e1a
commit 1209857a33
2 changed files with 5 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;

6
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);