Cleaned up test
							parent
							
								
									b40c0f7f15
								
							
						
					
					
						commit
						758aab6e80
					
				|  | @ -18,8 +18,8 @@ | |||
|  *  @date   Sept 2013 | ||||
|  */ | ||||
| 
 | ||||
| // TODO #include <gtsam/slam/tests/smartFactorScenarios.h>
 | ||||
| #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h> | ||||
| 
 | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/slam/PoseTranslationPrior.h> | ||||
| #include <gtsam/slam/ProjectionFactor.h> | ||||
|  | @ -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<StereoPoint2> 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<Pose3>(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<Pose3>(x3).print("Smart: Pose3 after optimization: "); | ||||
|   EXPECT(assert_equal(pose3, result.at<Pose3>(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<Pose3>(x3)));
 | ||||
|  | @ -604,7 +579,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { | |||
| //
 | ||||
| ///* *************************************************************************/
 | ||||
| //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
 | ||||
| //  //  cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 | ||||
| //
 | ||||
| //  vector<Key> 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<Pose3>(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<Pose3>(x3).print("Pose3 after optimization: ");
 | ||||
| //  EXPECT(assert_equal(pose3,result.at<Pose3>(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<Pose3>(x3).print("Smart: Pose3 before optimization: "); | ||||
| 
 | ||||
|   // TODO: next line throws Cheirality exception on Mac
 | ||||
|   boost::shared_ptr<GaussianFactor> 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<Key> 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<Pose3>(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<Pose3>(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<Pose3>(x3)));
 | ||||
| //  if(isDebugTest) tictoc_print_();
 | ||||
| //}
 | ||||
| //
 | ||||
| ///* *************************************************************************/
 | ||||
| //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
 | ||||
| //  // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
 | ||||
| //
 | ||||
| //  vector<Key> 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<Pose3>(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<Pose3>(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<Pose3>(x3)));
 | ||||
| //  if(isDebugTest) tictoc_print_();
 | ||||
| //}
 | ||||
| //
 | ||||
| ///* *************************************************************************/
 | ||||
| //TEST( SmartStereoProjectionPoseFactor, Hessian ){
 | ||||
| //  // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
 | ||||
| //
 | ||||
| //  vector<Key> views;
 | ||||
| //  views.push_back(x1);
 | ||||
|  | @ -952,7 +899,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { | |||
| //  values.insert(x2, pose2);
 | ||||
| //
 | ||||
| //  boost::shared_ptr<GaussianFactor> 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<Key> 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<Key> views; | ||||
|   views.push_back(x1); | ||||
|   views.push_back(x2); | ||||
|  | @ -1066,8 +1008,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { | |||
| 
 | ||||
|   boost::shared_ptr<GaussianFactor> 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<GaussianFactor> hessianFactorRot = smartFactor->linearize( | ||||
|       rotValues); | ||||
|   if (isDebugTest) | ||||
|     hessianFactorRot->print("Hessian factor \n"); | ||||
| 
 | ||||
|   // Hessian is invariant to rotations in the nondegenerate case
 | ||||
|   EXPECT( | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue