12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Constrain SFM scale with range prior

release/4.3a0
Richard Roberts 2012-03-24 19:53:46 +00:00
parent 5e6a1836e9
commit df1b9c32c1
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -20,6 +20,7 @@ using namespace boost;
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -345,7 +346,12 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
values.insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
// Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, X[0]); 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 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(X,L);

6
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -20,6 +20,7 @@ using namespace boost;
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -343,7 +344,12 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
values.insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
// Constrain position of system with the first camera constrained to the origin
graph.addCameraConstraint(0, X[0]); 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 ; const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L); Ordering ordering = *getOrdering(X,L);