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,51 +1039,68 @@ 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]);
 
-  // get feature in undistorted pixel space
+    // create vector of patch in pixel plane
-  // this only reverts from 'pure' space into undistorted pixel space using camera matrix
+    for(double u_run = -n; u_run <= n; u_run++)
-  cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
+      for(double v_run = -n; v_run <= n; v_run++)
-                                                    cam.intrinsics, 
+        anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
-                                                    cam.distortion_model, 
+
-                                                    cam.distortion_coeffs);
+    //project back into u,v
-  // create vector of patch in pixel plane
+    for(int i = 0; i < N*N; i++)
-  for(double u_run = -n; u_run <= n; u_run++)
+      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]));
-    for(double v_run = -n; v_run <= n; v_run++)
+  }
-      anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
+
+  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), 
+                                                      cam.intrinsics, 
+                                                      cam.distortion_model, 
+                                                      cam.distortion_coeffs);
+    // 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(und_pix_p.x+u_run, und_pix_p.y+v_run));
 
 
-  //create undistorted pure points    
+    //create undistorted pure points    
-  image_handler::undistortPoints(anchorPatch_real,
+    image_handler::undistortPoints(anchorPatch_real,
-                                cam.intrinsics,
+                                  cam.intrinsics,
-                                cam.distortion_model,
+                                  cam.distortion_model,
-                                cam.distortion_coeffs,
+                                  cam.distortion_coeffs,
-                                anchorPatch_ideal);
+                                  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)
     anchorPatch.push_back(PixelIrradiance(point, anchorImage));
-
+    
   // project patch pixel to 3D space in camera  coordinate system
   for(auto point : anchorPatch_ideal)
     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)
   {