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

Test now succeeds after adding 3 landmark constraints 

Also cleaned up a bit
release/4.3a0
Frank Dellaert 2009-11-13 06:19:08 +00:00
parent d26abf3ccf
commit d5ada2a80c
1 changed files with 36 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
cpp/testVSLAMGraph.cpp
View File

@ -18,56 +18,52 @@ using namespace gtsam;
typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> Optimizer; typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
Point3 landmark1_local(-1.0,-1.0, 0.0); Point3 landmark1(-1.0,-1.0, 0.0);
Point3 landmark2_local(-1.0, 1.0, 0.0); Point3 landmark2(-1.0, 1.0, 0.0);
Point3 landmark3_local( 1.0, 1.0, 0.0); Point3 landmark3( 1.0, 1.0, 0.0);
Point3 landmark4_local( 1.0,-1.0, 0.0); Point3 landmark4( 1.0,-1.0, 0.0);
Pose3 camera1_local(Matrix_(3,3, Pose3 camera1(Matrix_(3,3,
1., 0., 0., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,
0., 0.,-1. 0., 0.,-1.
), ),
Point3(0,0,6.25)); Point3(0,0,6.25));
Pose3 camera2_local(Matrix_(3,3, Pose3 camera2(Matrix_(3,3,
1., 0., 0., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,
0., 0.,-1. 0., 0.,-1.
), ),
Point3(0,0,5.00)); Point3(0,0,5.00));
Point3 landmark1() { return landmark1_local;}
Point3 landmark2() { return landmark2_local;}
Point3 landmark3() { return landmark3_local;}
Point3 landmark4() { return landmark4_local;}
Pose3 camera1() { return camera1_local;}
Pose3 camera2() { return camera2_local;}
/* ************************************************************************* */ /* ************************************************************************* */
VSLAMGraph testGraph() { VSLAMGraph testGraph() {
Point2 z11(-100, 100); Point2 z11(-100, 100);
Point2 z12(-100, -100); Point2 z12(-100,-100);
Point2 z13(100, -100); Point2 z13( 100,-100);
Point2 z14(100, 100); Point2 z14( 100, 100);
Point2 z21(-125, 125); Point2 z21(-125, 125);
Point2 z22(-125, -125); Point2 z22(-125,-125);
Point2 z23(125, -125); Point2 z23( 125,-125);
Point2 z24(125, 125); Point2 z24( 125, 125);
double sigma = 1; double sigma = 1;
Cal3_S2 K(625, 625, 0, 0, 0); Cal3_S2 K(625, 625, 0, 0, 0);
VSLAMGraph g; VSLAMGraph g;
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11.vector(), sigma, 1, 1, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11, sigma, 1, 1, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12.vector(), sigma, 1, 2, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12, sigma, 1, 2, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13.vector(), sigma, 1, 3, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13, sigma, 1, 3, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14.vector(), sigma, 1, 4, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14, sigma, 1, 4, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21.vector(), sigma, 2, 1, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21, sigma, 2, 1, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22.vector(), sigma, 2, 2, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22, sigma, 2, 2, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23.vector(), sigma, 2, 3, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23, sigma, 2, 3, K)));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24.vector(), sigma, 2, 4, K))); g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24, sigma, 2, 4, K)));
// add 3 landmark constraints
g.addLandmarkConstraint(1, landmark1);
g.addLandmarkConstraint(2, landmark2);
g.addLandmarkConstraint(3, landmark3);
return g; return g;
} }
@ -78,14 +74,14 @@ TEST( VSLAMGraph, optimizeLM)
// build a graph // build a graph
VSLAMGraph graph = testGraph(); VSLAMGraph graph = testGraph();
// Create a configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
VSLAMConfig groundTruth; boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
groundTruth.addCameraPose(1, camera1()); initialEstimate->addCameraPose(1, camera1);
groundTruth.addCameraPose(2, camera2()); initialEstimate->addCameraPose(2, camera2);
groundTruth.addLandmarkPoint(1, landmark1()); initialEstimate->addLandmarkPoint(1, landmark1);
groundTruth.addLandmarkPoint(2, landmark2()); initialEstimate->addLandmarkPoint(2, landmark2);
groundTruth.addLandmarkPoint(3, landmark3()); initialEstimate->addLandmarkPoint(3, landmark3);
groundTruth.addLandmarkPoint(4, landmark4()); initialEstimate->addLandmarkPoint(4, landmark4);
// Create an ordering of the variables // Create an ordering of the variables
list<string> keys; list<string> keys;
@ -97,14 +93,9 @@ TEST( VSLAMGraph, optimizeLM)
keys.push_back("x2"); keys.push_back("x2");
Ordering ordering(keys); Ordering ordering(keys);
// optimize it // Create an optimizer and check its error
double relativeErrorTreshold=1e-6, absoluteErrorTreshold=1.0;
int verbosity=0;
Optimizer::shared_config initialConfig(new VSLAMConfig(groundTruth));
Optimizer optimizer(graph, ordering, initialConfig, 1e-5);
// 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
Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
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
@ -112,10 +103,8 @@ TEST( VSLAMGraph, optimizeLM)
Optimizer afterOneIteration = optimizer.iterate(); Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
//afterOneIteration.config()->print();
// check if correct // check if correct
CHECK(groundTruth.equals(*(afterOneIteration.config())) ); CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
} }
/* ************************************************************************* */ /* ************************************************************************* */