degenerate case in SmartProjectionFactor

gtsam/geometry/PinholeCamera.h

@@ -250,6 +250,20 @@ namespace gtsam {
       return Point2(P.x() / P.z(), P.y() / P.z());
     }
 
+    /**
+     * projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the
+     * camera and returns a 2-dimensional point, no calibration applied
+     * With optional 2by3 derivative
+     */
+    inline static Point2 project_point_at_infinity_to_camera(const Point3& P,
+                             boost::optional<Matrix&> H1 = boost::none){
+      if (H1) {
+        double d = 1.0 / P.z(), d2 = d * d;
+        *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
+      }
+      return Point2(P.x() / P.z(), P.y() / P.z());
+    }
+
     /// Project a point into the image and check depth
     inline std::pair<Point2,bool> projectSafe(const Point3& pw) const {
       const Point3 pc = pose_.transform_to(pw) ;
@@ -296,6 +310,48 @@ namespace gtsam {
       return pi;
     }
 
+    /** project a point at infinity from world coordinate to the image
+         *  @param pw is a point in the world coordinate (it is pw = lambda*[pw_x  pw_y  pw_z] with lambda->inf)
+         *  @param H1 is the jacobian w.r.t. pose3
+         *  @param H2 is the jacobian w.r.t. point3
+         *  @param H3 is the jacobian w.r.t. calibration
+         */
+        inline Point2 project_point_at_infinity(const Point3& pw,
+            boost::optional<Matrix&> H1 = boost::none,
+            boost::optional<Matrix&> H2 = boost::none,
+            boost::optional<Matrix&> H3 = boost::none) const {
+
+          if (!H1 && !H2 && !H3) {
+            const Point3 pc = pose_.rotation().unrotate(pw) ;
+            if ( pc.z() <= 0 ) throw CheiralityException();
+            const Point2 pn = project_point_at_infinity_to_camera(pc) ;
+            return K_.uncalibrate(pn);
+          }
+
+          // world to camera coordinate
+          Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
+          const Point3 pc = pose_.rotation().unrotate(pw, Hc1, Hc2) ;
+          if( pc.z() <= 0 ) throw CheiralityException();
+
+          // camera to normalized image coordinate
+          Matrix Hn; // 2*3
+          const Point2 pn = project_point_at_infinity_to_camera(pc, Hn) ;
+
+          // uncalibration
+          Matrix Hk, Hi; // 2*2
+          Matrix H23 = Matrix::Zero(3,2);
+          H23.block(0,0,2,2) = Matrix::Identity(2,2);
+          const Point2 pi = K_.uncalibrate(pn, Hk, Hi);
+
+          // chain the jacobian matrices
+          const Matrix tmp = Hi*Hn;
+          if (H1) *H1 = tmp * Hc1;
+          if (H2) *H2 = tmp * Hc2 * H23;
+          if (H3) *H3 = Hk;
+          return pi;
+        }
+
+
     /** project a point from world coordinate to the image
      *  @param pw is a point in the world coordinate
      *  @param H1 is the jacobian w.r.t. [pose3 calibration]

gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp

@@ -285,7 +285,7 @@ int main(int argc, char** argv) {
 
       // This is a new landmark, create a new factor and add to mapping
       boost::shared_ptr<SmartProjectionFactorState> smartFactorState(new SmartProjectionFactorState());
-      SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K, boost::none, boost::none, smartFactorState));
+      SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
       smartFactorStates.insert( make_pair(L(l), smartFactorState) );
       smartFactors.insert( make_pair(L(l), smartFactor) );
       graph.push_back(smartFactor);

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -45,9 +45,9 @@ namespace gtsam {
     const SharedNoiseModel noise_;   ///< noise model used
     ///< (important that the order is the same as the keys that we use to create the factor)
     boost::shared_ptr<CALIBRATION> K_;  ///< shared pointer to calibration object
-    boost::optional<Point3> point_;
     boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
     boost::shared_ptr<SmartProjectionFactorState> state_;
+    boost::optional<Point3> point_;
 
     // verbosity handling for Cheirality Exceptions
     bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@@ -78,10 +78,9 @@ namespace gtsam {
      */
     SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
         std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
-        boost::optional<LANDMARK> point = boost::none,
         boost::optional<POSE> body_P_sensor = boost::none,
         boost::shared_ptr<SmartProjectionFactorState> state = boost::shared_ptr<SmartProjectionFactorState>()) :
-          measured_(measured), noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor),
+          measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor),
           state_(state), throwCheirality_(false), verboseCheirality_(false) {
       keys_.assign(poseKeys.begin(), poseKeys.end());
     }
@@ -100,10 +99,9 @@ namespace gtsam {
     SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
         std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
         bool throwCheirality, bool verboseCheirality,
-        boost::optional<LANDMARK> point = boost::none,
         boost::optional<POSE> body_P_sensor = boost::none,
         boost::shared_ptr<SmartProjectionFactorState> state = boost::shared_ptr<SmartProjectionFactorState>()) :
-          measured_(measured), noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor),
+          measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor),
           state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
 
     /**
@@ -112,10 +110,9 @@ namespace gtsam {
      * @param K shared pointer to the constant calibration
      */
     SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr<CALIBRATION>& K, 
-        boost::optional<LANDMARK> point = boost::none,
         boost::optional<POSE> body_P_sensor = boost::none,
         boost::shared_ptr<SmartProjectionFactorState> state = boost::shared_ptr<SmartProjectionFactorState>()) :
-        noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor), state_(state) {
+        noise_(model), K_(K), body_P_sensor_(body_P_sensor), state_(state) {
     }
 
     /** Virtual destructor */
@@ -195,13 +192,8 @@ namespace gtsam {
       }
 
       // We triangulate the 3D position of the landmark
-      Point3 point;
       try {
-          if (point_) {
+          point_ = triangulatePoint3(cameraPoses, measured_, *K_);
-            point = *point_;
-          } else {
-            point = triangulatePoint3(cameraPoses, measured_, *K_);
-          }
       } catch( TriangulationCheiralityException& e) {
           // point is behind one of the cameras, turn factor off by setting everything to 0
           //std::cout << e.what() << std::end;
@@ -210,6 +202,12 @@ namespace gtsam {
           return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));         
       }
 
+      bool degenerate = true;
+
+      int dim_landmark = 2;
+
+
+
       if (blockwise){
         // ==========================================================================================================
         std::vector<Matrix> Hx(numKeys);
@@ -219,7 +217,7 @@ namespace gtsam {
         for(size_t i = 0; i < measured_.size(); i++) {
           Pose3 pose = cameraPoses.at(i);
           PinholeCamera<CALIBRATION> camera(pose, *K_);
-          b.at(i) = - ( camera.project(point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
+          b.at(i) = - ( camera.project(point_,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
           noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
           f += b.at(i).squaredNorm();
         }
@@ -266,24 +264,42 @@ namespace gtsam {
 
       if (blockwise == false){ // version with full matrix multiplication
         // ==========================================================================================================
+
         Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
-        Matrix Hl2 = zeros(2 * numKeys, 3);
+        Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
         Vector b2 = zero(2 * numKeys);
 
-        for(size_t i = 0; i < measured_.size(); i++) {
+        if(degenerate){
-          Pose3 pose = cameraPoses.at(i);
+          for(size_t i = 0; i < measured_.size(); i++) {
-          PinholeCamera<CALIBRATION> camera(pose, *K_);
+            Pose3 pose = cameraPoses.at(i);
-          Matrix Hxi, Hli;
+            PinholeCamera<CALIBRATION> camera(pose, *K_);
-           Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector();
+            Matrix Hxi, Hli;
+            Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector();
 
-           noise_-> WhitenSystem(Hxi, Hli, bi);
+            noise_-> WhitenSystem(Hxi, Hli, bi);
-           f += bi.squaredNorm();
+            f += bi.squaredNorm();
 
-           Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
+            Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
-           Hl2.block( 2*i, 0, 2, 3  ) = Hli;
+            Hl2.block( 2*i, 0, 2, 2  ) = Hli;
 
-           subInsert(b2,bi,2*i);
+            subInsert(b2,bi,2*i);
+          }
+        }
+        else{
+          for(size_t i = 0; i < measured_.size(); i++) {
+            Pose3 pose = cameraPoses.at(i);
+            PinholeCamera<CALIBRATION> camera(pose, *K_);
+            Matrix Hxi, Hli;
+            Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector();
 
+            noise_-> WhitenSystem(Hxi, Hli, bi);
+            f += bi.squaredNorm();
+
+            Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
+            Hl2.block( 2*i, 0, 2, 3  ) = Hli;
+
+            subInsert(b2,bi,2*i);
+          }
         }
 
         // Shur complement trick
@@ -334,13 +350,8 @@ namespace gtsam {
         }
 
         // We triangulate the 3D position of the landmark
-        Point3 point;
         try {
-          if (point_) {
+          point_ = triangulatePoint3(cameraPoses, measured_, *K_);
-            point = *point_;
-          } else {
-            point = triangulatePoint3(cameraPoses, measured_, *K_);
-          }
         } catch( TriangulationCheiralityException& e) {
             // point is behind one of the cameras, turn factor off by setting everything to 0
             //std::cout << e.what() << std::end;
@@ -351,7 +362,7 @@ namespace gtsam {
           Pose3 pose = cameraPoses.at(i);
           PinholeCamera<CALIBRATION> camera(pose, *K_);
 
-          Point2 reprojectionError(camera.project(point) - measured_.at(i));
+          Point2 reprojectionError(camera.project(point_) - measured_.at(i));
           overallError += noise_->distance( reprojectionError.vector() );
         }
         return overallError;
@@ -370,7 +381,7 @@ namespace gtsam {
       return noise_; 
     }
 
-    /** return the noise landmark */
+    /** return the landmark */
     boost::optional<Point3> point() const {
       return point_;
     }

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp

@@ -167,7 +167,7 @@ TEST( SmartProjectionFactor, noisy ){
   // DOUBLES_EQUAL(expectedError, actualError, 1e-7);
 }
 
-/* ************************************************************************* */
+/* *************************************************************************
 TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
   cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl;