changed anchor feature position calculation

include/msckf_vio/feature.hpp

@@ -121,6 +121,15 @@ struct Feature {
   inline bool checkMotion(
       const CamStateServer& cam_states) const;
 
+
+  /*
+   * @brief AnchorPixelToPosition projects an undistorted point in the
+   *        anchor frame back into the real world using the rho calculated 
+   *        based on featur position
+   */
+  inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam);
+
+
   /*
    * @brief InitializeAnchor generates the NxN patch around the
    *        feature in the Anchor image
@@ -745,6 +754,20 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCa
   //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
 }
 
+
+Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
+{
+  // use undistorted position of point of interest
+  // project it back into 3D space using pinhole model
+  // save resulting NxN positions for this feature
+
+  Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
+  Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
+  return PositionInWorld;
+  //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
+}
+
+
 //@test center projection must always be initial feature projection
 bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {
@@ -772,27 +795,19 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
                                                     cam.intrinsics, 
                                                     cam.distortion_model, 
-                                                    0);
+                                                    cam.distortion_coeffs);
   // create vector of patch in pixel plane
-  std::vector<cv::Point2f>und_pix_v;
   for(double u_run = -n; u_run <= n; u_run++)
     for(double v_run = -n; v_run <= n; v_run++)
-      und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
+      anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
 
 
   //create undistorted pure points    
-  std::vector<cv::Point2f> und_v;
+  image_handler::undistortPoints(anchorPatch_real,
-  image_handler::undistortPoints(und_pix_v,
                                 cam.intrinsics,
                                 cam.distortion_model,
-                                0,
+                                cam.distortion_coeffs,
-                                und_v);
+                                anchorPatch_ideal);
-
-  //create distorted pixel points
-  anchorPatch_real = image_handler::distortPoints(und_v,
-                                                   cam.intrinsics,
-                                                   cam.distortion_model,
-                                                   cam.distortion_coeffs);
 
 
   // save anchor position for later visualisaztion
@@ -800,19 +815,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 
   // save true pixel Patch position
   for(auto point : anchorPatch_real)
-  {
     if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
       return false;
-  }
+
   for(auto point : anchorPatch_real)
     anchorPatch.push_back(PixelIrradiance(point, anchorImage));
 
   // project patch pixel to 3D space in camera  coordinate system
-  for(auto point : und_v)
+  for(auto point : anchorPatch_ideal)
-  {
+    anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
-    anchorPatch_ideal.push_back(point);
+
-    anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
-  }
   is_anchored = true;
   return true;
 }