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 #include #include +#include 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(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 #include #include +#include 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(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 10.0))); + const double reproj_error = 1e-5 ; Ordering ordering = *getOrdering(X,L);