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

Fixing compilation errors

release/4.3a0
Richard Roberts 2012-03-01 16:07:23 +00:00
parent ff7a78b854
commit 3a1175323c
9 changed files with 219 additions and 312 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -146,11 +146,11 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
vector<GeneralCamera> X = genCameraDefaultCalibration(); vector<GeneralCamera> X = genCameraDefaultCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
@ -158,23 +158,22 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
// Create an ordering of the variables // Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements); EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
} }
@ -183,11 +182,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> X = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
@ -195,9 +194,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -205,19 +204,18 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
else { else {
values->insert(Symbol('l',i), L[i]) ; values.insert(Symbol('l',i), L[i]) ;
} }
} }
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
const double reproj_error = 1e-5; const double reproj_error = 1e-5;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
@ -229,36 +227,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
// add measurement with noise // add measurement with noise
const double noise = baseline*0.1; const double noise = baseline*0.1;
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z()); //Point3 pt(L[i].x(), L[i].y(), L[i].z());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
graph->addCameraConstraint(i, X[i]); graph.addCameraConstraint(i, X[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
@ -269,17 +266,17 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> X = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) { for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double const double
rot_noise = 1e-5, rot_noise = 1e-5,
@ -287,7 +284,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
focal_noise = 1, focal_noise = 1,
skew_noise = 1e-5; skew_noise = 1e-5;
if ( i == 0 ) { if ( i == 0 ) {
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
} }
else { else {
@ -298,24 +295,23 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
skew_noise, // s skew_noise, // s
trans_noise, trans_noise // ux, uy trans_noise, trans_noise // ux, uy
) ; ) ;
values->insert(Symbol('x',i), X[i].retract(delta)) ; values.insert(Symbol('x',i), X[i].retract(delta)) ;
} }
} }
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(Symbol('l',i), L[i]) ; values.insert(Symbol('l',i), L[i]) ;
} }
// fix X0 and all landmarks, allow only the X[1] to move // fix X0 and all landmarks, allow only the X[1] to move
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
for ( size_t i = 0 ; i < L.size() ; ++i ) for ( size_t i = 0 ; i < L.size() ; ++i )
graph->addPoint3Constraint(i, L[i]); graph.addPoint3Constraint(i, L[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
@ -325,11 +321,11 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> X = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
@ -337,24 +333,23 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }

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

@ -148,11 +148,11 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
vector<GeneralCamera> X = genCameraDefaultCalibration(); vector<GeneralCamera> X = genCameraDefaultCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
@ -160,23 +160,22 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
// Create an ordering of the variables // Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements); EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
} }
@ -185,11 +184,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> X = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
@ -197,9 +196,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -207,19 +206,18 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
else { else {
values->insert(Symbol('l',i), L[i]) ; values.insert(Symbol('l',i), L[i]) ;
} }
} }
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
const double reproj_error = 1e-5; const double reproj_error = 1e-5;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
@ -231,36 +229,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
// add measurement with noise // add measurement with noise
const double noise = baseline*0.1; const double noise = baseline*0.1;
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z()); //Point3 pt(L[i].x(), L[i].y(), L[i].z());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
graph->addCameraConstraint(i, X[i]); graph.addCameraConstraint(i, X[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
@ -271,23 +268,23 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> X = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) { for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double const double
rot_noise = 1e-5, trans_noise = 1e-3, rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3; focal_noise = 1, distort_noise = 1e-3;
if ( i == 0 ) { if ( i == 0 ) {
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
} }
else { else {
@ -296,24 +293,23 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2 focal_noise, distort_noise, distort_noise // f, k1, k2
) ; ) ;
values->insert(Symbol('x',i), X[i].retract(delta)) ; values.insert(Symbol('x',i), X[i].retract(delta)) ;
} }
} }
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(Symbol('l',i), L[i]) ; values.insert(Symbol('l',i), L[i]) ;
} }
// fix X0 and all landmarks, allow only the X[1] to move // fix X0 and all landmarks, allow only the X[1] to move
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
for ( size_t i = 0 ; i < L.size() ; ++i ) for ( size_t i = 0 ; i < L.size() ; ++i )
graph->addPoint3Constraint(i, L[i]); graph.addPoint3Constraint(i, L[i]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
@ -323,11 +319,11 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
vector<GeneralCamera> X = genCameraVariableCalibration(); vector<GeneralCamera> X = genCameraVariableCalibration();
// add measurement with noise // add measurement with noise
shared_ptr<Graph> graph(new Graph()); Graph graph;
for ( size_t j = 0 ; j < X.size() ; ++j) { for ( size_t j = 0 ; j < X.size() ; ++j) {
for ( size_t i = 0 ; i < L.size() ; ++i) { for ( size_t i = 0 ; i < L.size() ; ++i) {
Point2 pt = X[j].project(L[i]) ; Point2 pt = X[j].project(L[i]) ;
graph->addMeasurement(j, i, pt, sigma1); graph.addMeasurement(j, i, pt, sigma1);
} }
} }
@ -335,24 +331,23 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); Values values;
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(Symbol('x',i), X[i]) ; values.insert(Symbol('x',i), X[i]) ;
// add noise only to the first landmark // add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(Symbol('l',i), pt) ; values.insert(Symbol('l',i), pt) ;
} }
graph->addCameraConstraint(0, X[0]); graph.addCameraConstraint(0, X[0]);
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L); Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }

37
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -53,33 +53,32 @@ TEST(Pose3Graph, optimizeCircle) {
Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1)); Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1));
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph); Pose3Graph fg;
fg->addHardConstraint(0, gT0); fg.addHardConstraint(0, gT0);
Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1 Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1
double theta = M_PI/3.0; double theta = M_PI/3.0;
CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1)); CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1));
fg->addConstraint(0,1, _0T1, covariance); fg.addConstraint(0,1, _0T1, covariance);
fg->addConstraint(1,2, _0T1, covariance); fg.addConstraint(1,2, _0T1, covariance);
fg->addConstraint(2,3, _0T1, covariance); fg.addConstraint(2,3, _0T1, covariance);
fg->addConstraint(3,4, _0T1, covariance); fg.addConstraint(3,4, _0T1, covariance);
fg->addConstraint(4,5, _0T1, covariance); fg.addConstraint(4,5, _0T1, covariance);
fg->addConstraint(5,0, _0T1, covariance); fg.addConstraint(5,0, _0T1, covariance);
// Create initial config // Create initial config
boost::shared_ptr<Values> initial(new Values()); Values initial;
initial->insert(PoseKey(0), gT0); initial.insert(PoseKey(0), gT0);
initial->insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial.insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial.insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial.insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial.insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial.insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize // Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering); Ordering ordering;
*ordering += kx0,kx1,kx2,kx3,kx4,kx5; ordering += kx0,kx1,kx2,kx3,kx4,kx5;
Values actual = *LevenbergMarquardtOptimizer( Values actual = *LevenbergMarquardtOptimizer(fg, initial, ordering).optimize()->values();
fg, initial, LevenbergMarquardtOptimizer::SharedLMParams(), ordering).optimize()->values();
// Check with ground truth // Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4)); CHECK(assert_equal(hexagon, actual,1e-4));

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -59,13 +59,13 @@ TEST( StereoFactor, singlePoint)
graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
Values values(new Values()); Values values;
values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0); Point3 l1(0, 0, 0);
values.insert(PointKey(1), l1); // add point at origin; values.insert(PointKey(1), l1); // add point at origin;
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, values))); NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, values));
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);

58
gtsam/slam/tests/testVSLAM.cpp
View File

@ -81,28 +81,28 @@ visualSLAM::Graph testGraph() {
TEST( Graph, optimizeLM) TEST( Graph, optimizeLM)
{ {
// build a graph // build a graph
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph())); visualSLAM::Graph graph(testGraph());
// add 3 landmark constraints // add 3 landmark constraints
graph->addPointConstraint(1, landmark1); graph.addPointConstraint(1, landmark1);
graph->addPointConstraint(2, landmark2); graph.addPointConstraint(2, landmark2);
graph->addPointConstraint(3, landmark3); graph.addPointConstraint(3, landmark3);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values); Values initialEstimate;
initialEstimate->insert(PoseKey(1), camera1); initialEstimate.insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2); initialEstimate.insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1); initialEstimate.insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2); initialEstimate.insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3); initialEstimate.insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4); initialEstimate.insert(PointKey(4), landmark4);
// Create an ordering of the variables // Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering); Ordering ordering;
*ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2); ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering)); NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
@ -111,7 +111,7 @@ TEST( Graph, optimizeLM)
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// check if correct // check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values()))); CHECK(assert_equal(initialEstimate,*(afterOneIteration->values())));
} }
@ -119,27 +119,27 @@ TEST( Graph, optimizeLM)
TEST( Graph, optimizeLM2) TEST( Graph, optimizeLM2)
{ {
// build a graph // build a graph
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph())); visualSLAM::Graph graph(testGraph());
// add 2 camera constraints // add 2 camera constraints
graph->addPoseConstraint(1, camera1); graph.addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2); graph.addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values); Values initialEstimate;
initialEstimate->insert(PoseKey(1), camera1); initialEstimate.insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2); initialEstimate.insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1); initialEstimate.insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2); initialEstimate.insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3); initialEstimate.insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4); initialEstimate.insert(PointKey(4), landmark4);
// Create an ordering of the variables // Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering); Ordering ordering;
*ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2); ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering)); NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
@ -148,7 +148,7 @@ TEST( Graph, optimizeLM2)
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// check if correct // check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values()))); CHECK(assert_equal(initialEstimate,*(afterOneIteration->values())));
} }

6
tests/testBoundingConstraint.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/slam/simulated2DConstraints.h> #include <gtsam/slam/simulated2DConstraints.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
namespace iq2D = simulated2D::inequality_constraints; namespace iq2D = simulated2D::inequality_constraints;
using namespace std; using namespace std;
@ -228,7 +228,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Symbol x1('x',1), x2('x',2); Symbol x1('x',1), x2('x',2);
Graph graph; NonlinearFactorGraph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2)); graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2));
graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0)); graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0));
@ -254,7 +254,7 @@ TEST( testBoundingConstraint, avoid_demo) {
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0); Point2 odo(2.0, 0.0);
Graph graph; NonlinearFactorGraph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1)); graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1));
graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2)); graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2));
graph.add(iq2D::LandmarkAvoid(x2, l1, radius)); graph.add(iq2D::LandmarkAvoid(x2, l1, radius));

1
tests/testInference.cpp
View File

@ -18,6 +18,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/slam/smallExample.h> #include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h> #include <gtsam/slam/planarSLAM.h>

160
tests/testNonlinearEquality.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/slam/visualSLAM.h> #include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -35,9 +35,6 @@ typedef PriorFactor<Pose2> PosePrior;
typedef NonlinearEquality<Pose2> PoseNLE; typedef NonlinearEquality<Pose2> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE; typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
typedef NonlinearFactorGraph PoseGraph;
typedef NonlinearOptimizer<PoseGraph> PoseOptimizer;
Symbol key('x',1); Symbol key('x',1);
/* ************************************************************************* */ /* ************************************************************************* */
@ -188,25 +185,23 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
PoseNLE nle(key1, feasible1, error_gain); PoseNLE nle(key1, feasible1, error_gain);
// add to a graph // add to a graph
boost::shared_ptr<PoseGraph> graph(new PoseGraph()); NonlinearFactorGraph graph;
graph->add(nle); graph.add(nle);
// initialize away from the ideal // initialize away from the ideal
Pose2 initPose(0.0, 2.0, 3.0); Pose2 initPose(0.0, 2.0, 3.0);
boost::shared_ptr<Values> init(new Values()); Values init;
init->insert(key1, initPose); init.insert(key1, initPose);
// optimize // optimize
boost::shared_ptr<Ordering> ord(new Ordering()); Ordering ordering;
ord->push_back(key1); ord.push_back(key1);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); NonlinearOptimizer::auto_ptr result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
PoseOptimizer optimizer(graph, init, ord, params);
PoseOptimizer result = optimizer.levenbergMarquardt();
// verify // verify
Values expected; Values expected;
expected.insert(key1, feasible1); expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, *result.values())); EXPECT(assert_equal(expected, *result->values()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -217,9 +212,9 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
Pose2 feasible1(1.0, 2.0, 3.0); Pose2 feasible1(1.0, 2.0, 3.0);
// initialize away from the ideal // initialize away from the ideal
boost::shared_ptr<Values> init(new Values()); Values init;
Pose2 initPose(0.0, 2.0, 3.0); Pose2 initPose(0.0, 2.0, 3.0);
init->insert(key1, initPose); init.insert(key1, initPose);
double error_gain = 500.0; double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain); PoseNLE nle(key1, feasible1, error_gain);
@ -228,32 +223,25 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1)); PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));
// add to a graph // add to a graph
boost::shared_ptr<PoseGraph> graph(new PoseGraph()); NonlinearFactorGraph graph;
graph->add(nle); graph.add(nle);
graph->add(prior); graph.add(prior);
// optimize // optimize
boost::shared_ptr<Ordering> ord(new Ordering()); Ordering ordering;
ord->push_back(key1); ord.push_back(key1);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); Values actual = *LevenbergMarquardtOptimizer(graph, values, ordering).optimize()->values();
PoseOptimizer optimizer(graph, init, ord, params);
PoseOptimizer result = optimizer.levenbergMarquardt();
// verify // verify
Values expected; Values expected;
expected.insert(key1, feasible1); expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, *result.values())); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
SharedDiagonal hard_model = noiseModel::Constrained::All(2); SharedDiagonal hard_model = noiseModel::Constrained::All(2);
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
typedef NonlinearFactorGraph Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<Values> shared_values;
typedef NonlinearOptimizer<Graph> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_basics ) { TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 pt(1.0, 2.0); Point2 pt(1.0, 2.0);
@ -314,23 +302,23 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
simulated2D::Prior::shared_ptr factor( simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key)); new simulated2D::Prior(badPt, soft_model, key));
shared_graph graph(new Graph()); NonlinearFactorGraph graph;
graph->push_back(constraint); graph.push_back(constraint);
graph->push_back(factor); graph.push_back(factor);
shared_values initValues(new Values()); Values initValues;
initValues->insert(key, badPt); initValues.insert(key, badPt);
// verify error values // verify error values
EXPECT(constraint->active(*initValues)); EXPECT(constraint->active(initValues));
Values expected; Values expected;
expected.insert(key, truth_pt); expected.insert(key, truth_pt);
EXPECT(constraint->active(expected)); EXPECT(constraint->active(expected));
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
EXPECT(assert_equal(expected, *actual, tol)); EXPECT(assert_equal(expected, actual, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -411,20 +399,20 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
new eq2D::OdoEqualityConstraint( new eq2D::OdoEqualityConstraint(
truth_pt1.between(truth_pt2), key1, key2)); truth_pt1.between(truth_pt2), key1, key2));
shared_graph graph(new Graph()); Graph graph;
graph->push_back(constraint1); graph.push_back(constraint1);
graph->push_back(constraint2); graph.push_back(constraint2);
graph->push_back(factor); graph.push_back(factor);
shared_values initValues(new Values()); Values initValues;
initValues->insert(key1, Point2()); initValues.insert(key1, Point2());
initValues->insert(key2, badPt); initValues.insert(key2, badPt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
Values expected; Values expected;
expected.insert(key1, truth_pt1); expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2); expected.insert(key2, truth_pt2);
CHECK(assert_equal(expected, *actual, tol)); CHECK(assert_equal(expected, actual, tol));
} }
/* ********************************************************************* */ /* ********************************************************************* */
@ -435,44 +423,44 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
* constrained to a particular value * constrained to a particular value
*/ */
shared_graph graph(new Graph()); Graph graph;
Symbol x1('x',1), x2('x',2); Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2); Symbol l1('l',1), l2('l',2);
Point2 pt_x1(1.0, 1.0), Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0); pt_x2(5.0, 6.0);
graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); graph.add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); graph.add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
Point2 z1(0.0, 5.0); Point2 z1(0.0, 5.0);
SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph->add(simulated2D::Measurement(z1, sigma, x1,l1)); graph.add(simulated2D::Measurement(z1, sigma, x1,l1));
Point2 z2(-4.0, 0.0); Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2,l2)); graph.add(simulated2D::Measurement(z2, sigma, x2,l2));
graph->add(eq2D::PointEqualityConstraint(l1, l2)); graph.add(eq2D::PointEqualityConstraint(l1, l2));
shared_values initialEstimate(new Values()); Values initialEstimate;
initialEstimate->insert(x1, pt_x1); initialEstimate.insert(x1, pt_x1);
initialEstimate->insert(x2, Point2()); initialEstimate.insert(x2, Point2());
initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimize()->values();
Values expected; Values expected;
expected.insert(x1, pt_x1); expected.insert(x1, pt_x1);
expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0)); expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, *actual, 1e-5)); CHECK(assert_equal(expected, actual, 1e-5));
} }
/* ********************************************************************* */ /* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, map_warp ) { TEST (testNonlinearEqualityConstraint, map_warp ) {
// get a graph // get a graph
shared_graph graph(new Graph()); Graph graph;
// keys // keys
Symbol x1('x',1), x2('x',2); Symbol x1('x',1), x2('x',2);
@ -480,37 +468,37 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
// constant constraint on x1 // constant constraint on x1
Point2 pose1(1.0, 1.0); Point2 pose1(1.0, 1.0);
graph->add(eq2D::UnaryEqualityConstraint(pose1, x1)); graph.add(eq2D::UnaryEqualityConstraint(pose1, x1));
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);
// measurement from x1 to l1 // measurement from x1 to l1
Point2 z1(0.0, 5.0); Point2 z1(0.0, 5.0);
graph->add(simulated2D::Measurement(z1, sigma, x1, l1)); graph.add(simulated2D::Measurement(z1, sigma, x1, l1));
// measurement from x2 to l2 // measurement from x2 to l2
Point2 z2(-4.0, 0.0); Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2, l2)); graph.add(simulated2D::Measurement(z2, sigma, x2, l2));
// equality constraint between l1 and l2 // equality constraint between l1 and l2
graph->add(eq2D::PointEqualityConstraint(l1, l2)); graph.add(eq2D::PointEqualityConstraint(l1, l2));
// create an initial estimate // create an initial estimate
shared_values initialEstimate(new Values()); Values initialEstimate;
initialEstimate->insert(x1, Point2( 1.0, 1.0)); initialEstimate.insert(x1, Point2( 1.0, 1.0));
initialEstimate->insert(l1, Point2( 1.0, 6.0)); initialEstimate.insert(l1, Point2( 1.0, 6.0));
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
// optimize // optimize
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimize()->values();
Values expected; Values expected;
expected.insert(x1, Point2(1.0, 1.0)); expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0)); expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, *actual, tol)); CHECK(assert_equal(expected, actual, tol));
} }
// make a realistic calibration matrix // make a realistic calibration matrix
@ -520,9 +508,7 @@ Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K)); boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example // typedefs for visual SLAM example
typedef boost::shared_ptr<Values> shared_vconfig;
typedef visualSLAM::Graph VGraph; typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph> VOptimizer;
// factors for visual slam // factors for visual slam
typedef NonlinearEquality2<Point3> Point3Equality; typedef NonlinearEquality2<Point3> Point3Equality;
@ -546,32 +532,32 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
Symbol l1('l',1), l2('l',2); Symbol l1('l',1), l2('l',2);
// create graph // create graph
VGraph::shared_graph graph(new VGraph()); VGraph graph;
// create equality constraints for poses // create equality constraints for poses
graph->addPoseConstraint(1, camera1.pose()); graph.addPoseConstraint(1, camera1.pose());
graph->addPoseConstraint(2, camera2.pose()); graph.addPoseConstraint(2, camera2.pose());
// create factors // create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(3); SharedDiagonal vmodel = noiseModel::Unit::Create(3);
graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK); graph.addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK);
graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK); graph.addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK);
// add equality constraint // add equality constraint
graph->add(Point3Equality(l1, l2)); graph.add(Point3Equality(l1, l2));
// create initial data // create initial data
Point3 landmark1(0.5, 5.0, 0.0); Point3 landmark1(0.5, 5.0, 0.0);
Point3 landmark2(1.5, 5.0, 0.0); Point3 landmark2(1.5, 5.0, 0.0);
shared_vconfig initValues(new Values()); Values initValues;
initValues->insert(x1, pose1); initValues.insert(x1, pose1);
initValues->insert(x2, pose2); initValues.insert(x2, pose2);
initValues->insert(l1, landmark1); initValues.insert(l1, landmark1);
initValues->insert(l2, landmark2); initValues.insert(l2, landmark2);
// optimize // optimize
VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
// create config // create config
Values truthValues; Values truthValues;
@ -581,7 +567,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
truthValues.insert(l2, landmark); truthValues.insert(l2, landmark);
// check if correct // check if correct
CHECK(assert_equal(truthValues, *actual, 1e-5)); CHECK(assert_equal(truthValues, actual, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */

83
tests/testNonlinearOptimizer.cpp
View File

@ -43,66 +43,6 @@ const double tol = 1e-5;
Key kx(size_t i) { return Symbol('x',i); } Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); } Key kl(size_t i) { return Symbol('l',i); }
typedef NonlinearOptimizer<example::Graph> Optimizer;
/* ************************************************************************* */
TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createNonlinearFactorGraph()));
Optimizer::shared_values initial = example::sharedNoisyValues();
// Expected values structure is the difference between the noisy config
// and the ground-truth config. One step only because it's linear !
Ordering ord1; ord1 += kx(2),kl(1),kx(1);
VectorValues expected(initial->dims(ord1));
Vector dl1(2);
dl1(0) = -0.1;
dl1(1) = 0.1;
expected[ord1[kl(1)]] = dl1;
Vector dx1(2);
dx1(0) = -0.1;
dx1(1) = -0.1;
expected[ord1[kx(1)]] = dx1;
Vector dx2(2);
dx2(0) = 0.1;
dx2(1) = -0.2;
expected[ord1[kx(2)]] = dx2;
// Check one ordering
Optimizer optimizer1(fg, initial, Optimizer::shared_ordering(new Ordering(ord1)));
VectorValues actual1 = optimizer1.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual1,expected));
// SL-FIX // Check another
// shared_ptr<Ordering> ord2(new Ordering());
// *ord2 += kx(1),kx(2),kl(1);
// solver = Optimizer::shared_solver(new Optimizer::solver(ord2));
// Optimizer optimizer2(fg, initial, solver);
//
// VectorValues actual2 = optimizer2.linearizeAndOptimizeForDelta();
// CHECK(assert_equal(actual2,expected));
//
// // And yet another...
// shared_ptr<Ordering> ord3(new Ordering());
// *ord3 += kl(1),kx(1),kx(2);
// solver = Optimizer::shared_solver(new Optimizer::solver(ord3));
// Optimizer optimizer3(fg, initial, solver);
//
// VectorValues actual3 = optimizer3.linearizeAndOptimizeForDelta();
// CHECK(assert_equal(actual3,expected));
//
// // More...
// shared_ptr<Ordering> ord4(new Ordering());
// *ord4 += kx(1),kx(2), kl(1);
// solver = Optimizer::shared_solver(new Optimizer::solver(ord4));
// Optimizer optimizer4(fg, initial, solver);
//
// VectorValues actual4 = optimizer4.linearizeAndOptimizeForDelta();
// CHECK(assert_equal(actual4,expected));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, iterateLM ) TEST( NonlinearOptimizer, iterateLM )
{ {
@ -120,22 +60,13 @@ TEST( NonlinearOptimizer, iterateLM )
ord->push_back(kx(1)); ord->push_back(kx(1));
// create initial optimization state, with lambda=0 // create initial optimization state, with lambda=0
Optimizer optimizer(fg, config, ord, NonlinearOptimizationParameters::newLambda(0.)); NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0);
// normal iterate // normal iterate
Optimizer iterated1 = optimizer.iterate(); NonlinearOptimizer::auto_ptr iterated1 = GaussNewtonOptimizer(fg, config, GaussNewtonParams(), ord).iterate();
// LM iterate with lambda 0 should be the same // LM iterate with lambda 0 should be the same
Optimizer iterated2 = optimizer.iterateLM(); NonlinearOptimizer::auto_ptr iterated2 = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0)->iterate();
// Try successive iterates. TODO: ugly pointers, better way ?
Optimizer *pointer = new Optimizer(iterated2);
for (int i=0;i<10;i++) {
Optimizer* newOptimizer = new Optimizer(pointer->iterateLM());
delete pointer;
pointer = newOptimizer;
}
delete(pointer);
CHECK(assert_equal(*iterated1.values(), *iterated2.values(), 1e-9)); CHECK(assert_equal(*iterated1.values(), *iterated2.values(), 1e-9));
} }
@ -169,12 +100,12 @@ TEST( NonlinearOptimizer, optimize )
Optimizer optimizer(fg, c0, ord, params); Optimizer optimizer(fg, c0, ord, params);
// Gauss-Newton // Gauss-Newton
Optimizer actual1 = optimizer.gaussNewton(); NonlinearOptimizer::auto_ptr actual1 = *GaussNewtonOptimizer(fg, c0, GaussNewtonParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual1.values())),tol); DOUBLES_EQUAL(0,fg->error(*(actual1->values())),tol);
// Levenberg-Marquardt // Levenberg-Marquardt
Optimizer actual2 = optimizer.levenbergMarquardt(); Optimizer actual2 = *LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimizer();
DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol); DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */