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,