added scaling changes to dI/dh for pixel size

include/msckf_vio/feature.hpp

@@ -148,6 +148,14 @@ struct Feature {
   inline bool initializePosition(
       const CamStateServer& cam_states);
 
+  cv::Point2f pixelDistanceAt(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const;
+
+
+
 /*
 *  @brief project PositionToCamera Takes a 3d position in a world frame
 *         and projects it into the passed camera frame using pinhole projection
@@ -737,6 +745,37 @@ float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
   return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
 }
 
+cv::Point2f Feature::pixelDistanceAt(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const
+{
+
+  cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
+
+  // create vector of patch in pixel plane
+  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));
+
+  std::vector<cv::Point2f> pure;
+  image_handler::undistortPoints(surroundingPoints, 
+                                cam.intrinsics, 
+                                cam.distortion_model, 
+                                cam.distortion_coeffs, 
+                                pure); 
+
+  // returns the absolute pixel distance at pixels one metres away
+  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
+
+  return distance;
+}
+
+
+
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,