fixed pixel position return value

include/msckf_vio/feature.hpp

@@ -127,9 +127,7 @@ struct Feature {
 
   bool initializeAnchor(
    const movingWindow& cam0_moving_window,
-   const cv::Vec4d& intrinsics,
+   const CameraCalibration& cam);
-   const std::string& distortion_model,
-   const cv::Vec4d& distortion_coeffs);
 
 
   /*
@@ -150,9 +148,7 @@ struct Feature {
   inline cv::Point2f projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  const cv::Vec4d& intrinsics,
+                  const CameraCalibration& cam,
-                  const std::string& distortion_model,
-                  const cv::Vec4d& distortion_coeffs,
                   Eigen::Vector3d& in_p) const;
 
   /*
@@ -164,9 +160,7 @@ struct Feature {
   bool IrradianceOfAnchorPatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  const cv::Vec4d& intrinsics,
+                  const CameraCalibration& cam,
-                  const std::string& distortion_model,
-                  const cv::Vec4d& distortion_coeffs,
                   const movingWindow& cam0_moving_window,
                   std::vector<uint8_t>& anchorPatch_measurement) const;
 
@@ -174,11 +168,8 @@ struct Feature {
   * @brief projectPixelToPosition uses the calcualted pixels
   *     of the anchor patch to generate 3D positions of all of em
   */
-  inline bool projectPixelToPosition(cv::Point2f in_p,
+inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
-          Eigen::Vector3d& out_p,
+          const CameraCalibration& cam);
-          const cv::Vec4d& intrinsics,
-          const std::string& distortion_model,
-          const cv::Vec4d& distortion_coeffs);
 
   /*
   * @brief Irradiance returns irradiance value of a pixel
@@ -362,16 +353,14 @@ bool Feature::checkMotion(
 bool Feature::IrradianceOfAnchorPatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  const cv::Vec4d& intrinsics,
+                  const CameraCalibration& cam,
-                  const std::string& distortion_model,
-                  const cv::Vec4d& distortion_coeffs,
                   const movingWindow& cam0_moving_window,
                   std::vector<uint8_t>& anchorPatch_measurement) const
 {
   //project every point in anchorPatch_3d.
   for (auto point : anchorPatch_3d)
   {
-    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, intrinsics, distortion_model, distortion_coeffs, point);
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); 
     uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second);
     anchorPatch_measurement.push_back(irradiance);
   }
@@ -385,9 +374,7 @@ uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  const cv::Vec4d& intrinsics,
+                  const CameraCalibration& cam,
-                  const std::string& distortion_model,
-                  const cv::Vec4d& distortion_coeffs,
                   Eigen::Vector3d& in_p) const
 {
   Eigen::Isometry3d T_c0_w;
@@ -402,23 +389,17 @@ cv::Point2f Feature::projectPositionToCamera(
   out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
   std::vector<cv::Point2f> out_v;
   out_v.push_back(out_p);
-  std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
+  std::vector<cv::Point2f> my_p = image_handler::distortPoints(out_v, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
-    intrinsics,
-    distortion_model,
-    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);
   
-  return out_p;
+  return my_p[0];
 }
 
-bool Feature::projectPixelToPosition(cv::Point2f in_p,
+Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
-          Eigen::Vector3d& out_p,
+          const CameraCalibration& cam)
-          const cv::Vec4d& intrinsics,
-          const std::string& distortion_model,
-          const cv::Vec4d& distortion_coeffs)
 {
   // use undistorted position of point of interest
   // project it back into 3D space using pinhole model
@@ -426,7 +407,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
 
   Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho);
   Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
-  anchorPatch_3d.push_back(PositionInWorld);
+  return PositionInWorld;
   //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
 }
 
@@ -434,9 +415,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
 //@test center projection must always be initial feature projection
 bool Feature::initializeAnchor(
    const movingWindow& cam0_moving_window,
-   const cv::Vec4d& intrinsics,
+   const CameraCalibration& cam)
-   const std::string& distortion_model,
-   const cv::Vec4d& distortion_coeffs)
 {
 
   int N = 3;
@@ -447,8 +426,8 @@ bool Feature::initializeAnchor(
     return false;
 
   cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second;
-  auto u = anchor->second(0)*intrinsics[0] + intrinsics[2];
+  auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
-  auto v = anchor->second(1)*intrinsics[1] + intrinsics[3];
+  auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
   int count = 0;
 
   //go through surrounding pixels 
@@ -457,12 +436,9 @@ bool Feature::initializeAnchor(
     for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1)
     {
       anchorPatch.push_back(anchorImage.at<uint8_t>((int)u_run,(int)v_run));
-      Eigen::Vector3d Npose;
+      cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]);
-      projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]),
+      Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam);
-                             Npose,
+      anchorPatch_3d.push_back(Npose);
-                             intrinsics,
-                             distortion_model,
-                             distortion_coeffs);
     }
   }
 

src/msckf_vio.cpp

@@ -904,8 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //photometric observation
   std::vector<uint8_t> photo_z;
-  feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z);
+  feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
-  
 
   // Convert the feature position from the world frame to
   // the cam0 and cam1 frame.
@@ -1316,7 +1315,7 @@ void MsckfVio::removeLostFeatures() {
       }
     }
 
-    if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs))
+    if(!feature.initializeAnchor(cam0_moving_window, cam0))
     {
       invalid_feature_ids.push_back(feature.id);
       continue;
@@ -1470,7 +1469,7 @@ void MsckfVio::pruneCamStateBuffer() {
       }
     }
 
-    if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs))
+    if(!feature.initializeAnchor(cam0_moving_window, cam0))
     {
       for (const auto& cam_id : involved_cam_state_ids)
             feature.observations.erase(cam_id);