corrected pixel distance calcualtion

include/msckf_vio/feature.hpp

@@ -788,32 +788,24 @@ cv::Point2f Feature::pixelDistanceAt(
                   Eigen::Vector3d& in_p) const
 {
 
-  Eigen::Isometry3d T_c0_w;
-
-  cv::Point2f out_p;
-
   cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
 
-
-  cv::Point2f surroundingPoint;
   // create vector of patch in pixel plane
-  surroundingPoint = cv::Point2f(cam_p.x+1, cam_p.y+1);
+  std::vector<cv::Point2f> surroundingPoints;
+  surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1));
 
-  cv::Point2f pure_surroundingPoint;
+  std::vector<cv::Point2f> pure;
-  image_handler::undistortPoint(surroundingPoint, 
+  image_handler::undistortPoints(surroundingPoints, 
                                 cam.intrinsics, 
                                 cam.distortion_model, 
                                 cam.distortion_coeffs, 
-                                pure_surroundingPoint);
+                                pure); 
 
-  cv::Point2f pure_Point;
+  // returns the absolute pixel distance at pixels one metres away
-  image_handler::undistortPoint(cam_p, 
+  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
-                                cam.intrinsics, 
-                                cam.distortion_model, 
-                                cam.distortion_coeffs,
-                                pure_Point);
-
-  cv::Point2f distance(fabs(pure_surroundingPoint.x - pure_Point.x), fabs(pure_surroundingPoint.y - pure_Point.y));
 
   return distance;
 }

launch/msckf_vio_tum.launch

@@ -21,7 +21,7 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="3"/>

src/msckf_vio.cpp

@@ -1225,6 +1225,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
   const Feature& feature = map_server[feature_id];
 
+  const StateIDType anchor_state_id = feature.observations.begin()->first;
+  const CAMState anchor_state = state_server.cam_states[anchor_state_id];
+
   // Cam0 pose.
   Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Vector3d& t_c0_w = cam_state.position;
@@ -1253,16 +1256,19 @@ void MsckfVio::PhotometricMeasurementJacobian(
   {
     Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
     cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
-
+    cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
     Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
 
     // add jacobians
+    cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point);
 
-    cv::Point2f pixelDistance = feature.pixelDistanceAt(cam_state, cam_state_id, cam0, point);
+    std::cout << "pixelDistance: \n" << pixelDistance << std::endl;
 
+
+    // calculate derivation for anchor frame, use position for derivation calculation
     // frame derivative calculated convoluting with kernel [-1, 0, 1]
-    dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
+    dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), frame);
-    dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
+    dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), frame);
     dI_dhj(0, 0) = dx/pixelDistance.x;
     dI_dhj(0, 1) = dy/pixelDistance.y;
 
@@ -1690,6 +1696,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 }
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
+  return true;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();