diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 4f19f36f8..d715eb5c7 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -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& 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(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(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(rekeyed_base_factor); + new_factor->factor_->keys() = new_factor->keys(); + // Create a new Values to assign later + Values newLinearizationPoint; + for(size_t i=0; ikeys()[i]; + newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key)); + } + new_factor->linearizationPoint_ = newLinearizationPoint; + + // upcast back and return + return boost::static_pointer_cast(new_factor); +} + /* ************************************************************************* */ NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph( const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) { diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 8587e6b91..8c5b34f01 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -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& 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(); } /** diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 21c05dc2c..6deaaf7fe 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -126,13 +126,13 @@ public: * factor with different keys using * a map from old->new keys */ - shared_ptr rekey(const std::map& rekey_mapping) const; + virtual shared_ptr rekey(const std::map& 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 diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 9e5e08e92..2afa2b5fc 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -6,13 +6,15 @@ */ #include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include + #include 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::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 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 + 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(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); } /* ************************************************************************* */