From df1b9c32c14492c2107906bf2efb07cf1bdbb6b7 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 24 Mar 2012 19:53:46 +0000
Subject: [PATCH] Constrain SFM scale with range prior

---
 gtsam/slam/tests/testGeneralSFMFactor.cpp             | 6 ++++++
 gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 402b6e17e..d24624d21 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -20,6 +20,7 @@ using namespace boost;
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/RangeFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -345,7 +346,12 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
     values.insert(Symbol('l',i), pt) ;
   }
 
+  // Constrain position of system with the first camera constrained to the origin
   graph.addCameraConstraint(0, X[0]);
+
+  // Constrain the scale of the problem with a soft range factor of 1m between the cameras
+  graph.add(RangeFactor<GeneralCamera,GeneralCamera>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0)));
+
   const double reproj_error = 1e-5 ;
 
   Ordering ordering = *getOrdering(X,L);
diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
index ca698365a..044908e56 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
@@ -20,6 +20,7 @@ using namespace boost;
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/RangeFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -343,7 +344,12 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
     values.insert(Symbol('l',i), pt) ;
   }
 
+  // Constrain position of system with the first camera constrained to the origin
   graph.addCameraConstraint(0, X[0]);
+
+  // Constrain the scale of the problem with a soft range factor of 1m between the cameras
+  graph.add(RangeFactor<GeneralCamera,GeneralCamera>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0)));
+
   const double reproj_error = 1e-5 ;
 
   Ordering ordering = *getOrdering(X,L);