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

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<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) {

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<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(); }
 
   /**

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<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
 

gtsam/nonlinear/tests/testLinearContainerFactor.cpp

@@ -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); }
 /* ************************************************************************* */