From 8149bffcb731b99beb52c16e19cd5a75f0f8c881 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <dta-huynh@irobot.com>
Date: Mon, 7 Mar 2016 15:20:42 -0500
Subject: [PATCH] Fix a bug in ExpressionFactor::initialize that changes the
 key ordering of factors derived from ExpressionFactor2. This impacts
 serialization and user expectation.

Example code:
Key key1 = 1, key2 = 2;
RangeFactorWithTransform factor(key2, key1,...); // keys_ should be {key2, key1}
After intialization, the bug will rearrange keys_ to an increasing order: keys_={key1, key2}.
---
 gtsam/nonlinear/ExpressionFactor.h  | 8 +++++++-
 gtsam/sam/tests/testRangeFactor.cpp | 4 ++--
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 04836a1cb..0bf20f303 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -158,7 +158,13 @@ protected:
 
    // Get keys and dimensions for Jacobian matrices
    // An Expression is assumed unmutable, so we do this now
-   boost::tie(keys_, dims_) = expression_.keysAndDims();
+   if (keys_.empty())
+     boost::tie(keys_, dims_) = expression_.keysAndDims();
+   else {
+      std::map<Key, int> map;
+      expression_.dims(map);
+      for (Key key : keys_) dims_.push_back(map[key]);   
+   }
  }
 
  /// Recreate expression from keys_ and measured_, used in load below.
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index 73ff34d2a..2b23e595d 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -38,8 +38,8 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
 typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
 typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
 
-Key poseKey(1);
-Key pointKey(2);
+Key poseKey(2);
+Key pointKey(1);
 double measurement(10.0);
 
 /* ************************************************************************* */