return fisher information

svo/include/svo/sparse_img_align.h

@@ -51,6 +51,10 @@ public:
       FramePtr ref_frame,
       FramePtr cur_frame);
 
+  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
+  /// at the converged state.
+  Matrix<double, 6, 6> getFisherInformation();
+
 protected:
   FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
   FramePtr cur_frame_;            //!< only the image is known!

svo/src/sparse_img_align.cpp

@@ -69,9 +69,19 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
     optimize(T_cur_from_ref);
   }
   cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;
+
+
+
   return n_meas_/patch_area_;
 }
 
+Matrix<double, 6, 6> SparseImgAlign::getFisherInformation()
+{
+  double sigma_i_sq = 5e-4*255*255; // image noise
+  Matrix<double,6,6> I = H_/sigma_i_sq;
+  return I;
+}
+
 // TODO take jacobian from Frame Class!
 void frameJac(const Vector3d & xyz, Matrix<double,2,6> & J)
 {