moved jacobians to point and frame class

svo/include/svo/frame.h

@@ -110,6 +110,32 @@ public:
 
   /// Return the pose of the frame in the (w)orld coordinate frame.
   inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
+
+  /// Frame jacobian for projection of 3D point in (f)rame coordinate to
+  /// unit plane coordinates uv (focal length = 1).
+  inline static void jacobian_xyz2uv(
+      const Vector3d& xyz_in_f,
+      Matrix<double,2,6>& J)
+  {
+    const double x = xyz_in_f[0];
+    const double y = xyz_in_f[1];
+    const double z_inv = 1./xyz_in_f[2];
+    const double z_inv_2 = z_inv*z_inv;
+
+    J(0,0) = -z_inv;              // -1/z
+    J(0,1) = 0.0;                 // 0
+    J(0,2) = x*z_inv_2;           // x/z^2
+    J(0,3) = y*J(0,2);            // x*y/z^2
+    J(0,4) = -(1.0 + x*J(0,2));   // -(1.0 + x^2/z^2)
+    J(0,5) = y*z_inv;             // y/z
+
+    J(1,0) = 0.0;                 // 0
+    J(1,1) = -z_inv;              // -1/z
+    J(1,2) = y*z_inv_2;           // y/z^2
+    J(1,3) = 1.0 + y*J(1,2);      // 1.0 + y^2/z^2
+    J(1,4) = -J(0,3);             // -x*y/z^2
+    J(1,5) = -x*z_inv;            // x/z
+  }
 };
 
 

svo/include/svo/frame_handler_base.h

@@ -112,10 +112,10 @@ protected:
   virtual void resetAll() { resetCommon(); }
 
   /// Set the tracking quality based on the number of tracked features.
-  void setTrackingQuality(const size_t num_observations);
+  virtual void setTrackingQuality(const size_t num_observations);
 
   /// Optimize some of the observed 3D points.
-  void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
+  virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
 };
 
 } // namespace nslam

svo/include/svo/point.h

@@ -75,11 +75,25 @@ public:
   /// Get number of observations.
   inline size_t nRefs() const { return obs_.size(); }
 
-  /// Point jacobian w.r.t frame f.
-  void pointJacobian(const Vector3d& p_in_f, const Matrix3d& R_f_w, Matrix23d& point_jac) const;
-
   /// Optimize point position through minimizing the reprojection error.
   void optimize(const size_t n_iter);
+
+  /// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
+  inline static void jacobian_xyz2uv(
+      const Vector3d& p_in_f,
+      const Matrix3d& R_f_w,
+      Matrix23d& point_jac)
+  {
+    const double z_inv = 1.0/p_in_f[2];
+    const double z_inv_sq = z_inv*z_inv;
+    point_jac(0, 0) = z_inv;
+    point_jac(0, 1) = 0.0;
+    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
+    point_jac(1, 0) = 0.0;
+    point_jac(1, 1) = z_inv;
+    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
+    point_jac = - point_jac * R_f_w;
+  }
 };
 
 } // namespace svo

svo/include/svo/pose_optimizer.h

@@ -34,33 +34,6 @@ class Point;
 /// Motion-only bundle adjustment. Minimize the reprojection error of a single frame.
 namespace pose_optimizer {
 
-inline Matrix26d
-frameJac(const SE3 & T,
-         const Vector3d & xyz,
-         const Vector2d & focal_length)
-{
-  const Vector3d & xyz_trans = T*xyz;
-  double x = xyz_trans[0];
-  double y = xyz_trans[1];
-  double z = xyz_trans[2];
-  double z_2 = z*z;
-  Matrix26d frame_jac;
-  frame_jac(0,0) = -1./z *focal_length[0];
-  frame_jac(0,1) = 0;
-  frame_jac(0,2) = x/z_2 *focal_length[0];
-  frame_jac(0,3) =  x*y/z_2 * focal_length[0];
-  frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length[0];
-  frame_jac(0,5) = y/z *focal_length[0];
-
-  frame_jac(1,0) = 0;
-  frame_jac(1,1) = -1./z *focal_length[1];
-  frame_jac(1,2) = y/z_2 *focal_length[1];
-  frame_jac(1,3) = (1+y*y/z_2) *focal_length[1];
-  frame_jac(1,4) = -x*y/z_2 *focal_length[1];
-  frame_jac(1,5) = -x/z *focal_length[1];
-  return frame_jac;
-}
-
 void optimizeGaussNewton(
     const double reproj_thresh,
     const size_t n_iter,

svo/src/point.cpp

@@ -106,23 +106,6 @@ bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const
   return true;
 }
 
-void Point::pointJacobian(
-    const Vector3d& p_in_f,
-    const Matrix3d& R_f_w,
-    Matrix23d& point_jac) const
-{
-  // we work on the unit plane, therefore focal lenght = 1
-  const double z_inv = 1.0/p_in_f[2];
-  const double z_inv_sq = z_inv*z_inv;
-  point_jac(0, 0) = z_inv;
-  point_jac(0, 1) = 0.0;
-  point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
-  point_jac(1, 0) = 0.0;
-  point_jac(1, 1) = z_inv;
-  point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
-  point_jac = - point_jac * R_f_w;
-}
-
 void Point::optimize(const size_t n_iter)
 {
   Vector3d old_point = pos_;
@@ -141,7 +124,7 @@ void Point::optimize(const size_t n_iter)
     {
       Matrix23d J;
       const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
-      pointJacobian(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
+      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
       const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
       new_chi2 += e.squaredNorm();
       A.noalias() += J.transpose() * J;

svo/src/pose_optimizer.cpp

@@ -37,7 +37,6 @@ void optimizeGaussNewton(
 {
   // init
   double chi2(0.0);
-  Vector2d focal_length(1,1);
   vector<double> chi2_vec_init, chi2_vec_final;
   vk::robust_cost::TukeyWeightFunction weight_function;
   SE3 T_old(frame->T_f_w_);
@@ -79,9 +78,10 @@ void optimizeGaussNewton(
     {
       if((*it)->point == NULL)
         continue;
-      Matrix26d J = frameJac(frame->T_f_w_, (*it)->point->pos_, focal_length);
+      Matrix26d J;
-      Vector2d e = vk::project2d((*it)->f)
+      Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_);
-                 - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+      Frame::jacobian_xyz2uv(xyz_f, J);
+      Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
       double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
       e *= sqrt_inv_cov;
       if(iter == 0)

svo/src/sparse_img_align.cpp

@@ -82,29 +82,6 @@ Matrix<double, 6, 6> SparseImgAlign::getFisherInformation()
   return I;
 }
 
-// TODO take jacobian from Frame Class!
-void frameJac(const Vector3d & xyz, Matrix<double,2,6> & J)
-{
-  const double x = xyz[0];
-  const double y = xyz[1];
-  const double z_inv = 1./xyz[2];
-  const double z_inv_2 = z_inv*z_inv;
-
-  J(0,0) = z_inv;               // 1/z
-  J(0,1) = 0.0;                 // 0
-  J(0,2) = -x*z_inv_2;          // -x/z^2
-  J(0,3) =  J(0,2)*y;           // -x*y/z^2
-  J(0,4) = 1.0 - J(0,2)*x;      // 1.0 + x^2/z^2
-  J(0,5) = -y*z_inv;            // -y/z
-
-  J(1,0) = 0.0;                 // 0
-  J(1,1) = z_inv;               // 1/z
-  J(1,2) = -y*z_inv_2;          // -y/z^2
-  J(1,3) = -1.0 + J(1,2)*y;     // -1.0 - y^2/z^2
-  J(1,4) = -J(0,3);             // x*y/^2
-  J(1,5) = x*z_inv;             // x/z
-}
-
 void SparseImgAlign::precomputeReferencePatches()
 {
   const int border = patch_halfsize_+1;
@@ -157,11 +134,11 @@ void SparseImgAlign::precomputeReferencePatches()
 
         // evaluate jacobian and cache
         Matrix<double,2,6> frame_jac;
-        frameJac(xyz_ref, frame_jac);
+        Frame::jacobian_xyz2uv(xyz_ref, frame_jac);
 
         // cache the jacobian
         jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
-           - (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
+            (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
       }
     }
   }