Working version of rotation only smart factor

gtsam/geometry/PinholeCamera.h

@@ -329,10 +329,13 @@ namespace gtsam {
           }
 
           // world to camera coordinate
-          Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
+          Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ;
-          const Point3 pc = pose_.rotation().unrotate(pw, Hc1, Hc2) ;
+          const Point3 pc = pose_.rotation().unrotate(pw, Hc1_rot, Hc2) ;
           if( pc.z() <= 0 ) throw CheiralityException();
 
+          Matrix Hc1 = Matrix::Zero(3,6);
+          Hc1.block(0,0,3,3) = Hc1_rot;
+
           // camera to normalized image coordinate
           Matrix Hn; // 2*3
           const Point2 pn = projectPointAtInfinityToCamera(pc, Hn) ;

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -206,9 +206,13 @@ namespace gtsam {
           //std::cout << e.what() << std::end;
           BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
           BOOST_FOREACH(Vector& v, gs) v = zero(6);
-          return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));         
+          //return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
+          // TODO: this is a debug condition, should be removed the comment
       }
 
+      degenerate = true; // TODO: this is a debug condition, should be removed
+      dim_landmark = 2; // TODO: this is a debug condition, should be removed the comment
+
       if (blockwise){
         // ==========================================================================================================
         std::vector<Matrix> Hx(numKeys);
@@ -265,7 +269,6 @@ namespace gtsam {
 
       if (blockwise == false){ // version with full matrix multiplication
         // ==========================================================================================================
-
         Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
         Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
         Vector b2 = zero(2 * numKeys);
@@ -276,10 +279,12 @@ namespace gtsam {
             PinholeCamera<CALIBRATION> camera(pose, *K_);
             if(i==0){ // first pose
               point_ = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
-              std::cout << "point_ " << point_<< std::endl;
+              // std::cout << "point_ " << point_<< std::endl;
             }
             Matrix Hxi, Hli;
             Vector bi = -( camera.projectPointAtInfinity(point_,Hxi,Hli) - measured_.at(i) ).vector();
+            // std::cout << "Hxi \n" << Hxi<< std::endl;
+            // std::cout << "Hli \n" << Hli<< std::endl;
 
             noise_-> WhitenSystem(Hxi, Hli, bi);
             f += bi.squaredNorm();
@@ -289,6 +294,8 @@ namespace gtsam {
 
             subInsert(b2,bi,2*i);
           }
+          // std::cout << "Hx2 \n" << Hx2<< std::endl;
+          // std::cout << "Hl2 \n" << Hl2<< std::endl;
         }
         else{
           std::cout << "non degenerate " << point_<< std::endl;
@@ -308,14 +315,16 @@ namespace gtsam {
           }
         }
 
+        std::cout << "dim_landmark " << dim_landmark << std::endl;
         // Shur complement trick
         Matrix H(6 * numKeys, 6 * numKeys);
-        Matrix3 C2 = (Hl2.transpose() * Hl2).inverse();
+        std::cout << "Hl2.transpose() * Hl2 \n" << Hl2.transpose() * Hl2 << std::endl;
+        Matrix C2 = (Hl2.transpose() * Hl2).inverse();
+        std::cout << "C2 \n" << C2.size() << std::endl;
         H = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2))));
 
         Vector gs_vector = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2))));
 
-
         // Populate Gs and gs
         int GsCount2 = 0;
         for(size_t i1 = 0; i1 < numKeys; i1++) {
@@ -362,17 +371,17 @@ namespace gtsam {
         try {
             point_ = triangulatePoint3(cameraPoses, measured_, *K_);
         } catch( TriangulationCheiralityException& e) {
-             std::cout << "TriangulationCheiralityException "  << std::endl;
+             // std::cout << "TriangulationCheiralityException "  << std::endl;
             // point is behind one of the cameras, turn factor off by setting everything to 0
             //std::cout << e.what() << std::end;
-            return 0.0;
+            // return 0.0; // TODO: this is a debug condition, should be removed the comment
         } catch( TriangulationUnderconstrainedException& e) {
           // point is triangulated at infinity
           //std::cout << e.what() << std::endl;
           degenerate = true;
         }
 
-        std::cout << "degenerate " << degenerate << std::endl;
+        degenerate = true; // TODO: this is a debug condition, should be removed
 
         if(degenerate){
           for(size_t i = 0; i < measured_.size(); i++) {
@@ -380,7 +389,6 @@ namespace gtsam {
             PinholeCamera<CALIBRATION> camera(pose, *K_);
             if(i==0){ // first pose
               point_ = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
-              std::cout << "point_ " << point_<< std::endl;
             }
             Point2 reprojectionError(camera.projectPointAtInfinity(point_) - measured_.at(i));
             overallError += noise_->distance( reprojectionError.vector() );

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp

@@ -120,7 +120,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
 }
 
 
-/* ************************************************************************* */
+/* *************************************************************************
 TEST( SmartProjectionFactor, noisy ){
   cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl;
 
@@ -311,7 +311,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
 
 }
 
-/* ************************************************************************* */
+/* *************************************************************************
 TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
   cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 
@@ -405,7 +405,7 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
 
 }
 
-/* ************************************************************************* */
+/* *************************************************************************
 TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
   cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 
@@ -510,7 +510,7 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
 
 }
 
-/* ************************************************************************* */
+/* *************************************************************************
 TEST( SmartProjectionFactor, 3poses_projection_factor ){
 //  cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 
@@ -583,7 +583,94 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
 
 }
 
-/* *************************************************************************
+/* ************************************************************************* */
+TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor ){
+  cout << " ************************ SmartProjectionFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
+
+  Symbol x1('X',  1);
+  Symbol x2('X',  2);
+  Symbol x3('X',  3);
+
+  const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
+
+  std::vector<Key> views;
+  views += x1, x2, x3;
+
+  Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
+
+  // 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), gtsam::Point3(0,0,1));
+  SimpleCamera cam1(pose1, *K);
+
+  // create second camera 1 meter to the right of first camera
+  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
+  SimpleCamera cam2(pose2, *K);
+
+  // create third camera 1 meter above the first camera
+  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
+  SimpleCamera cam3(pose3, *K);
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  Point2 cam1_uv1 = cam1.project(landmark1);
+  Point2 cam2_uv1 = cam2.project(landmark1);
+  Point2 cam3_uv1 = cam3.project(landmark1);
+  measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1;
+
+  //
+  Point2 cam1_uv2 = cam1.project(landmark2);
+  Point2 cam2_uv2 = cam2.project(landmark2);
+  Point2 cam3_uv2 = cam3.project(landmark2);
+  measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2;
+
+  typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K));
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
+  Point3 positionPrior = gtsam::Point3(0,0,1);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
+  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Values values;
+  values.insert(x1, pose1);
+  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);
+  values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
+
+  LevenbergMarquardtParams params;
+  params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
+  params.verbosity = NonlinearOptimizerParams::ERROR;
+
+  Values result;
+  gttic_(SmartProjectionFactor);
+  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  result = optimizer.optimize();
+  gttoc_(SmartProjectionFactor);
+  tictoc_finishedIteration_();
+
+  // result.print("results of 3 camera, 3 landmark optimization \n");
+  result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+  tictoc_print_();
+}
+
+/* ************************************************************************* */
 TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
   cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
 
@@ -603,11 +690,11 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
   SimpleCamera cam1(pose1, *K);
 
   // create second camera 1 meter to the right of first camera
-  Pose3 pose2 = Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)) * pose1;
+  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
   SimpleCamera cam2(pose2, *K);
 
   // create third camera 1 meter above the first camera
-  Pose3 pose3 = Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)) * pose2;
+  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
   SimpleCamera cam3(pose3, *K);
 
   // three landmarks ~5 meters infront of camera
@@ -652,7 +739,7 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
   graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
-  graph.print();
+
 //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
   Values values;
@@ -677,11 +764,10 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
   result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
   EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
   tictoc_print_();
-
 }
 
 
-/* ************************************************************************* */
+/* *************************************************************************
 TEST( SmartProjectionFactor, Hessian ){
   cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl;