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

Added more detail to VSLAM test that diverges.

release/4.3a0
Alex Cunningham 2009-11-24 16:03:57 +00:00
parent d788e9833d
commit 43e4c66407
1 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cpp/testSQP.cpp
View File

@ -539,6 +539,15 @@ TEST (SQP, stereo_truth_noisy ) {
bool verbose = false; bool verbose = false;
int maxIt = 5; int maxIt = 5;
// setting to determine how far away the noisy landmark is,
// given that the ground truth is 5m in front of the cameras
double noisyDist = 7.4;
/** RESULTS of optimization based on size of error:
* With a ground truth distance away of 5m, results based on initial input:
* >= 7.5m: the system diverges
* < 7.5m: system finds a solution with near zero error
*/
// create initial estimates // create initial estimates
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
@ -549,7 +558,7 @@ TEST (SQP, stereo_truth_noisy ) {
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
SimpleCamera camera2(K, pose2); SimpleCamera camera2(K, pose2);
Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
Point3 landmarkNoisy(1.0, 15.0, 0.0); // initial point is too far out Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out
// create truth config // create truth config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig); boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
@ -592,19 +601,30 @@ TEST (SQP, stereo_truth_noisy ) {
// create optimizer // create optimizer
VOptimizer optimizer(graph, ord, noisyConfig, 1e-5); VOptimizer optimizer(graph, ord, noisyConfig, 1e-5);
if (verbose)
cout << "Initial Error: " << optimizer.error() << endl;
// use Gauss-Newton optimization
double relThresh = 1e-5, absThresh = 1e-5;
optimizer = optimizer.gaussNewton(relThresh, absThresh);
// // optimize // // optimize
// for (int i = 0; i<maxIt; ++i) { // for (int i = 0; i<maxIt; ++i) {
// cout << "Iteration: " << i << endl; // cout << "Iteration: " << i << endl;
// optimizer = optimizer.iterate(); // optimizer = optimizer.iterate();
// optimizer.config()->print("Updated Config"); // optimizer.config()->print("Updated Config");
// cout << "Error after iteration: " << optimizer.error() << endl;
// } // }
// // verify // verify
// DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
//
// // check if correct // check if correct
// if (verbose) optimizer.config()->print("After iteration"); if (verbose) {
// CHECK(assert_equal(*truthConfig,*(optimizer.config()))); optimizer.config()->print("After iteration");
cout << "Final error: " << optimizer.error() << endl;
}
CHECK(assert_equal(*truthConfig,*(optimizer.config())));
} }
// Utility function to strip out a landmark number from a string key // Utility function to strip out a landmark number from a string key