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

Merge pull request #777 from borglab/fix/linear-rekey

release/4.3a0
Varun Agrawal 2021-05-30 22:09:46 -07:00 committed by GitHub
parent 1011055007 457d548015
commit d5dbaf7ce7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 114 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -164,6 +164,47 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const {
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_));
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const {
auto rekeyed_base_factor = Base::rekey(rekey_mapping);
// Update the keys to the properties as well
// Downncast so we have access to members
auto new_factor = boost::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
// Create a new Values to assign later
Values newLinearizationPoint;
for (size_t i = 0; i < factor_->size(); ++i) {
auto mapping = rekey_mapping.find(factor_->keys()[i]);
if (mapping != rekey_mapping.end())
new_factor->factor_->keys()[i] = mapping->second;
newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
}
new_factor->linearizationPoint_ = newLinearizationPoint;
// upcast back and return
return boost::static_pointer_cast<NonlinearFactor>(new_factor);
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
const KeyVector& new_keys) const {
auto rekeyed_base_factor = Base::rekey(new_keys);
// Update the keys to the properties as well
// Downncast so we have access to members
auto new_factor = boost::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
new_factor->factor_->keys() = new_factor->keys();
// Create a new Values to assign later
Values newLinearizationPoint;
for(size_t i=0; i<new_keys.size(); ++i) {
Key cur_key = linearizationPoint_->keys()[i];
newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key));
}
new_factor->linearizationPoint_ = newLinearizationPoint;
// upcast back and return
return boost::static_pointer_cast<NonlinearFactor>(new_factor);
}
/* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph(
const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) {

15
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -120,8 +120,21 @@ public:
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
}
// casting syntactic sugar
/**
* Creates a shared_ptr clone of the
* factor with different keys using
* a map from old->new keys
*/
NonlinearFactor::shared_ptr rekey(
const std::map<Key, Key>& rekey_mapping) const override;
/**
* Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys
*/
NonlinearFactor::shared_ptr rekey(const KeyVector& new_keys) const override;
/// Casting syntactic sugar
inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }
/**

4
gtsam/nonlinear/NonlinearFactor.h
View File

@ -126,13 +126,13 @@ public:
* factor with different keys using
* a map from old->new keys
*/
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
/**
* Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys
*/
shared_ptr rekey(const KeyVector& new_keys) const;
virtual shared_ptr rekey(const KeyVector& new_keys) const;
}; // \class NonlinearFactor

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

@ -6,13 +6,15 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
@ -28,7 +30,7 @@ Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
TEST(TestLinearContainerFactor, generic_jacobian_factor) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
@ -61,7 +63,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
TEST(TestLinearContainerFactor, jacobian_factor_withlinpoints) {
Matrix A1 = (Matrix(2, 2) <<
2.74222, -0.0067457,
@ -115,7 +117,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_hessian_factor ) {
TEST(TestLinearContainerFactor, generic_hessian_factor) {
Matrix G11 = (Matrix(1, 1) << 1.0).finished();
Matrix G12 = (Matrix(1, 2) << 2.0, 4.0).finished();
Matrix G13 = (Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
@ -153,7 +155,7 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
TEST(TestLinearContainerFactor, hessian_factor_withlinpoints) {
// 2 variable example, one pose, one landmark (planar)
// Initial ordering: x1, l1
@ -226,7 +228,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, creation ) {
TEST(TestLinearContainerFactor, Creation) {
// Create a set of local keys (No robot label)
Key l1 = 11, l3 = 13, l5 = 15;
@ -252,7 +254,7 @@ TEST( testLinearContainerFactor, creation ) {
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_relinearize )
TEST(TestLinearContainerFactor, jacobian_relinearize)
{
// Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1);
@ -286,7 +288,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, hessian_relinearize )
TEST(TestLinearContainerFactor, hessian_relinearize)
{
// Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1);
@ -319,6 +321,48 @@ TEST( testLinearContainerFactor, hessian_relinearize )
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
}
/* ************************************************************************* */
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::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());
LinearContainerFactor lcf_factor(
nonlinear_factor->linearize(linearization_pt), linearization_pt);
// Define a key mapping
std::map<gtsam::Key, gtsam::Key> key_map;
key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4);
key_map[gtsam::Symbol('l', 0)] = gtsam::Symbol('l', 4);
// Rekey (Calls NonlinearFactor::rekey() which should probably be overriden)
// This of type boost_ptr<NonlinearFactor>
auto lcf_factor_rekeyed = lcf_factor.rekey(key_map);
// Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>(lcf_factor_rekeyed);
CHECK(lcf_factor_rekey_ptr);
// For extra fun lets try linearizing this LCF
gtsam::Values linearization_pt_rekeyed;
for (auto key_val : linearization_pt) {
linearization_pt_rekeyed.insert(key_map.at(key_val.key), key_val.value);
}
// Check independent values since we don't want to unnecessarily sort
// The keys are just in the reverse order wrt the other container
CHECK(assert_equal(linearization_pt_rekeyed.keys()[1], lcf_factor_rekey_ptr->keys()[0]));
CHECK(assert_equal(linearization_pt_rekeyed.keys()[0], lcf_factor_rekey_ptr->keys()[1]));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */