fixed Irradiance jacobain calculation, now division by pixel distance

include/msckf_vio/feature.hpp

@@ -157,6 +157,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
@@ -191,9 +199,7 @@ struct Feature {
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
                   const std::vector<double> photo_r,
-                  std::stringstream& ss,
-                  cv::Point2f gradientVector,
-                  cv::Point2f residualVector) const;
+                  std::stringstream& ss) const;
 
 
   /* @brief takes a pure pixel position (1m from image)
@@ -407,8 +413,11 @@ bool Feature::estimate_FrameIrradiance(
 
   auto anchor = observations.begin();
   if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
+  {
+    std::cout << "anchor not in buffer anymore!" << std::endl;
     return false;
-
+  } 
+  
   double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
   double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;
 
@@ -550,9 +559,7 @@ bool Feature::VisualizePatch(
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
                   const std::vector<double> photo_r,
-                  std::stringstream& ss,
-                  cv::Point2f gradientVector,
-                  cv::Point2f residualVector) const
+                  std::stringstream& ss) const
 {
 
   double rescale = 1;
@@ -615,10 +622,9 @@ bool Feature::VisualizePatch(
                     CV_FILLED);
 
   // irradiance grid projection
-
   namer.str(std::string());
   namer << "projection";
-  cv::putText(irradianceFrame, namer.str() , cvPoint(30, 45+scale*N), 
+  cv::putText(irradianceFrame, namer.str(), cvPoint(30, 45+scale*N), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
   for(int i = 0; i<N; i++)
@@ -682,8 +688,7 @@ bool Feature::VisualizePatch(
                   cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y),
                   cv::Scalar(100, 0, 255),
                   1);
-  */
-
+  
   // residual gradient direction
   cv::arrowedLine(irradianceFrame,
                   cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))),
@@ -691,11 +696,14 @@ bool Feature::VisualizePatch(
                   cv::Scalar(0, 255, 175),
                   3);
                   
-
+  */
   cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
-      
-/*
+  
+    
+
   // visualize position of used observations and resulting feature position
+  /*
+
   cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
   cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
 
@@ -728,7 +736,9 @@ bool Feature::VisualizePatch(
 
 
   cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
-*/
+
+  */
+
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
@@ -771,6 +781,44 @@ 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
+{
+
+  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);
+
+  cv::Point2f pure_surroundingPoint;
+  image_handler::undistortPoint(surroundingPoint, 
+                                cam.intrinsics, 
+                                cam.distortion_model, 
+                                cam.distortion_coeffs,
+                                pure_surroundingPoint);
+
+  cv::Point2f pure_Point;
+  image_handler::undistortPoint(cam_p, 
+                                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;
+}
+
+
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,

include/msckf_vio/math_utils.hpp

@@ -13,6 +13,13 @@
 
 namespace msckf_vio {
 
+
+inline double absoluted(double a){
+  if(a>0)
+    return a;
+  else return -a;
+}
+
 /*
  *  @brief Create a skew-symmetric matrix from a 3-element vector.
  *  @note Performs the operation: