added check if pre-undistorted, works for still distorted

include/msckf_vio/feature.hpp

@@ -969,7 +969,7 @@ cv::Point2f Feature::projectPositionToCamera(
   Eigen::Isometry3d T_c0_w;
 
   cv::Point2f out_p;
-
+  cv::Point2f my_p;
   // transfrom position to camera frame
   Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Eigen::Vector3d& t_c0_w = cam_state.position;
@@ -980,11 +980,15 @@ cv::Point2f Feature::projectPositionToCamera(
  // if(cam_state_id == observations.begin()->first)
       //printf("undist:\n  \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
  
-  cv::Point2f my_p = image_handler::distortPoint(out_p, 
+  if (cam.distortion_model.substr(0,3) == "pre-")
+    my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]);
+  else
+    my_p = image_handler::distortPoint(out_p, 
                                                  cam.intrinsics, 
                                                  cam.distortion_model, 
                                                  cam.distortion_coeffs);
 
+
   // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
   // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
   // printf("Photo projection: %f, %f\n", my_p[0].x,  my_p[0].y);
@@ -1008,8 +1012,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal
 bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {
 
-
-
   //initialize patch Size
   int n = (int)(N-1)/2;
 
@@ -1037,12 +1039,26 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
   auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
   
-  //testing
-  undist_anchor_center_pos = cv::Point2f(u,v);
 
-  //for NxN patch pixels around feature
+  // check if image has been pre-undistorted
-  int count = 0;
+  if(cam.distortion_model.substr(0,3) == "pre-")
+  {
+    std::cout << "is a pre" << std::endl;
+    //project onto pixel plane
+    undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
 
+    // create vector of patch in pixel plane
+    for(double u_run = -n; u_run <= n; u_run++)
+      for(double v_run = -n; v_run <= n; v_run++)
+        anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
+
+    //project back into u,v
+    for(int i = 0; i < N*N; i++)
+      anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));
+  }
+
+  else
+  {
     // get feature in undistorted pixel space
     // this only reverts from 'pure' space into undistorted pixel space using camera matrix
     cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
@@ -1062,13 +1078,14 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
                                   cam.distortion_coeffs,
                                   anchorPatch_ideal);
 
+  }
 
   // save anchor position for later visualisaztion
   anchor_center_pos = anchorPatch_real[(N*N-1)/2];
     
   // 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))
+    if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1)
       return false;
     
   for(auto point : anchorPatch_real)
@@ -1079,9 +1096,11 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
     anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
 
   is_anchored = true;
+
   return true;
 }
 
+
 bool Feature::initializeRho(const CamStateServer& cam_states) {
 
   // Organize camera poses and feature observations properly.

src/msckf_vio.cpp

@@ -546,10 +546,20 @@ void MsckfVio::manageMovingWindow(
   cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
 
+  image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+
+
+
   // save image information into moving window
   cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
   cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
 
+
+  
+
+
+
   //TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
   while(cam0.moving_window.size() > 100)
   {