Added clone, minimization test; converges, but not to grund truth (which has high error)

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -32,6 +32,12 @@ public:
       Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) {
   }
 
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast < gtsam::NonlinearFactor
+        > (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
   /// print
   virtual void print(const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
@@ -114,6 +120,8 @@ public:
 using namespace std;
 using namespace gtsam;
 
+typedef noiseModel::Isotropic::shared_ptr Model;
+
 const string filename = findExampleDataFile("5pointExample1.txt");
 SfM_data data;
 bool readOK = readBAL(filename, data);
@@ -133,6 +141,8 @@ Vector vB(size_t i) {
   return EssentialMatrix::Homogeneous(pB(i));
 }
 
+Cal3_S2 K;
+
 //*************************************************************************
 TEST (EssentialMatrixFactor, testData) {
   CHECK(readOK);
@@ -198,8 +208,7 @@ TEST (EssentialMatrixFactor, fromConstraints) {
   // We start with a factor graph and add constraints to it
   // Noise sigma is 1cm, assuming metric measurements
   NonlinearFactorGraph graph;
-  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1,
-      0.01);
+  Model model = noiseModel::Isotropic::Sigma(1, 0.01);
   for (size_t i = 0; i < 5; i++)
     graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model));
 
@@ -240,7 +249,6 @@ TEST (EssentialMatrixFactor2, factor) {
   noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
 
   for (size_t i = 0; i < 5; i++) {
-    Cal3_S2 K;
     EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model);
 
     // Check evaluation
@@ -280,6 +288,43 @@ TEST (EssentialMatrixFactor2, factor) {
   }
 }
 
+//*************************************************************************
+TEST (EssentialMatrixFactor2, minimization) {
+  // Here we want to optimize for E and inverse depths at the same time
+
+  // We start with a factor graph and add constraints to it
+  // Noise sigma is 1cm, assuming metric measurements
+  NonlinearFactorGraph graph;
+  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
+  for (size_t i = 0; i < 5; i++)
+    graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
+
+  // Check error at ground truth
+  Values truth;
+  EssentialMatrix trueE(aRb, aTb);
+  truth.insert(100, trueE);
+  for (size_t i = 0; i < 5; i++) {
+    Point3 P1 = data.tracks[i].p;
+    truth.insert(i, LieScalar(1.0 / P1.z()));
+  }
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Optimize
+  LevenbergMarquardtParams parameters;
+  // parameters.setVerbosity("ERROR");
+  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actual = result.at<EssentialMatrix>(100);
+  EXPECT(assert_equal(trueE, actual,1e-1));
+  for (size_t i = 0; i < 5; i++)
+    EXPECT(assert_equal(result.at<LieScalar>(i), truth.at<LieScalar>(i),1e-1));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;