diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 7e2a1a468..497f2205c 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,8 +18,8 @@ * @date Sept 2013 */ +// TODO #include #include - #include #include #include @@ -32,8 +32,6 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = false; - // make a realistic calibration matrix static double fov = 60; // degrees static size_t w = 640, h = 480; @@ -83,9 +81,10 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, return measurements_cam; } +LevenbergMarquardtParams params; + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr, "Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } @@ -119,8 +118,6 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { /* *************************************************************************/ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -160,8 +157,6 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -217,10 +212,6 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { - cout - << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" - << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); @@ -277,8 +268,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); EXPECT( assert_equal( Pose3( @@ -288,14 +277,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - Values result; - gttic_ (SmartStereoProjectionPoseFactor); + gttic_(SmartStereoProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); @@ -306,14 +289,10 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); VectorValues delta = GFG->optimize(); VectorValues expected = VectorValues::Zero(delta); - EXPECT(assert_equal(expected, delta,1e-6)); + EXPECT(assert_equal(expected, delta, 1e-6)); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3))); - if (isDebugTest) - tictoc_print_(); } /* *************************************************************************/ @@ -376,7 +355,6 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { values.insert(x2, pose2); values.insert(x3, pose3 * noise_pose); - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -449,7 +427,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -533,7 +510,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { values.insert(x3, pose3); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -595,8 +571,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // values.insert(x2, pose2); // values.insert(x3, pose3*noise_pose); // -// LevenbergMarquardtParams params; -// Values result; +//// Values result; // LevenbergMarquardtOptimizer optimizer(graph, values, params); // result = optimizer.optimize(); // EXPECT(assert_equal(pose3,result.at(x3))); @@ -604,7 +579,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ -// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // // vector views; // views.push_back(x1); @@ -656,15 +630,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // values.insert(L(1), landmark1); // values.insert(L(2), landmark2); // values.insert(L(3), landmark3); -// if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); // -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // LevenbergMarquardtOptimizer optimizer(graph, values, params); // Values result = optimizer.optimize(); // -// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); // EXPECT(assert_equal(pose3,result.at(x3))); //} // @@ -726,8 +695,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); // TODO: next line throws Cheirality exception on Mac boost::shared_ptr hessianFactor1 = smartFactor1->linearize( @@ -752,7 +719,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { + hessianFactor3->augmentedInformation(); // Check Information vector - // cout << AugInformationMatrix.size() << endl; Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian @@ -761,7 +727,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // // vector views; // views.push_back(x1); @@ -814,11 +779,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x2, pose2*noise_pose); // // initialize third pose with some noise, we expect it to move back to original pose3 // values.insert(x3, pose3*noise_pose*noise_pose); -// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); @@ -828,15 +788,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // tictoc_finishedIteration_(); // // // result.print("results of 3 camera, 3 landmark optimization \n"); -// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); -// if(isDebugTest) tictoc_print_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // // vector views; // views.push_back(x1); @@ -897,11 +853,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x2, pose2); // // initialize third pose with some noise, we expect it to move back to original pose3 // values.insert(x3, pose3*noise_pose); -// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); @@ -911,15 +862,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // tictoc_finishedIteration_(); // // // result.print("results of 3 camera, 3 landmark optimization \n"); -// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); -// if(isDebugTest) tictoc_print_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // // vector views; // views.push_back(x1); @@ -952,7 +899,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x2, pose2); // // boost::shared_ptr hessianFactor = smartFactor1->linearize(values); -// if(isDebugTest) hessianFactor->print("Hessian factor \n"); // // // compute triangulation from linearization point // // compute reprojection errors (sum squared) @@ -963,8 +909,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - vector views; views.push_back(x1); views.push_back(x2); @@ -1034,8 +978,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - vector views; views.push_back(x1); views.push_back(x2); @@ -1066,8 +1008,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); - if (isDebugTest) - hessianFactor->print("Hessian factor \n"); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); @@ -1078,8 +1018,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); - if (isDebugTest) - hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case EXPECT(