Cleaned up test

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -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(