added position calculation

src/image_handler.cpp

@@ -0,0 +1,75 @@
+#include <msckf_vio/image_handler.h>
+
+namespace msckf_vio {
+namespace image_handler {
+
+
+void undistortPoints(
+    const std::vector<cv::Point2f>& pts_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    std::vector<cv::Point2f>& pts_out,
+    const cv::Matx33d &rectification_matrix,
+    const cv::Vec4d &new_intrinsics) {
+
+  if (pts_in.size() == 0) return;
+
+  const cv::Matx33d K(
+      intrinsics[0], 0.0, intrinsics[2],
+      0.0, intrinsics[1], intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  const cv::Matx33d K_new(
+      new_intrinsics[0], 0.0, new_intrinsics[2],
+      0.0, new_intrinsics[1], new_intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  if (distortion_model == "radtan") {
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                                 rectification_matrix, K_new);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
+                  distortion_model.c_str());
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  }
+
+  return;
+}
+
+std::vector<cv::Point2f> distortPoints(
+    const std::vector<cv::Point2f>& pts_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs) {
+
+  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
+                      0.0, intrinsics[1], intrinsics[3],
+                      0.0, 0.0, 1.0);
+
+  std::vector<cv::Point2f> pts_out;
+  if (distortion_model == "radtan") {
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
+                  distortion_model.c_str());
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  }
+
+  return pts_out;
+}
+
+	} // namespace image_handler
+} // namespace msckf_vio

src/image_processor.cpp

@@ -17,6 +17,7 @@
 #include <msckf_vio/TrackingInfo.h>
 #include <msckf_vio/image_processor.h>
 #include <msckf_vio/utils.h>
+#include <msckf_vio/image_handler.h>
 
 using namespace std;
 using namespace cv;
@@ -618,10 +619,10 @@ void ImageProcessor::stereoMatch(
     // rotation from stereo extrinsics
     const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
     vector<cv::Point2f> cam0_points_undistorted;
-    undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
+    image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
                     cam0_distortion_coeffs, cam0_points_undistorted,
                     R_cam0_cam1);
-    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
+    cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics,
                                 cam1_distortion_model, cam1_distortion_coeffs);
   }
 
@@ -662,10 +663,10 @@ void ImageProcessor::stereoMatch(
   // essential matrix.
   vector<cv::Point2f> cam0_points_undistorted(0);
   vector<cv::Point2f> cam1_points_undistorted(0);
-  undistortPoints(
+  image_handler::undistortPoints(
       cam0_points, cam0_intrinsics, cam0_distortion_model,
       cam0_distortion_coeffs, cam0_points_undistorted);
-  undistortPoints(
+  image_handler::undistortPoints(
       cam1_points, cam1_intrinsics, cam1_distortion_model,
       cam1_distortion_coeffs, cam1_points_undistorted);
 
@@ -1009,10 +1010,10 @@ void ImageProcessor::twoPointRansac(
   // Undistort all the points.
   vector<Point2f> pts1_undistorted(pts1.size());
   vector<Point2f> pts2_undistorted(pts2.size());
-  undistortPoints(
+  image_handler::undistortPoints(
       pts1, intrinsics, distortion_model,
       distortion_coeffs, pts1_undistorted);
-  undistortPoints(
+  image_handler::undistortPoints(
       pts2, intrinsics, distortion_model,
       distortion_coeffs, pts2_undistorted);
 
@@ -1250,10 +1251,10 @@ void ImageProcessor::publish() {
   vector<Point2f> curr_cam0_points_undistorted(0);
   vector<Point2f> curr_cam1_points_undistorted(0);
 
-  undistortPoints(
+  image_handler::undistortPoints(
       curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
       cam0_distortion_coeffs, curr_cam0_points_undistorted);
-  undistortPoints(
+  image_handler::undistortPoints(
       curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
       cam1_distortion_coeffs, curr_cam1_points_undistorted);
 

src/msckf_vio.cpp

@@ -117,6 +117,54 @@ bool MsckfVio::loadParameters() {
   nh.param<double>("initial_covariance/extrinsic_translation_cov",
       extrinsic_translation_cov, 1e-4);
 
+  // get camera information (used for back projection)
+  nh.param<string>("cam0/distortion_model",
+      cam0_distortion_model, string("radtan"));
+  nh.param<string>("cam1/distortion_model",
+      cam1_distortion_model, string("radtan"));
+
+  vector<int> cam0_resolution_temp(2);
+  nh.getParam("cam0/resolution", cam0_resolution_temp);
+  cam0_resolution[0] = cam0_resolution_temp[0];
+  cam0_resolution[1] = cam0_resolution_temp[1];
+
+  vector<int> cam1_resolution_temp(2);
+  nh.getParam("cam1/resolution", cam1_resolution_temp);
+  cam1_resolution[0] = cam1_resolution_temp[0];
+  cam1_resolution[1] = cam1_resolution_temp[1];
+
+  vector<double> cam0_intrinsics_temp(4);
+  nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
+  cam0_intrinsics[0] = cam0_intrinsics_temp[0];
+  cam0_intrinsics[1] = cam0_intrinsics_temp[1];
+  cam0_intrinsics[2] = cam0_intrinsics_temp[2];
+  cam0_intrinsics[3] = cam0_intrinsics_temp[3];
+
+  vector<double> cam1_intrinsics_temp(4);
+  nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
+  cam1_intrinsics[0] = cam1_intrinsics_temp[0];
+  cam1_intrinsics[1] = cam1_intrinsics_temp[1];
+  cam1_intrinsics[2] = cam1_intrinsics_temp[2];
+  cam1_intrinsics[3] = cam1_intrinsics_temp[3];
+
+  vector<double> cam0_distortion_coeffs_temp(4);
+  nh.getParam("cam0/distortion_coeffs",
+      cam0_distortion_coeffs_temp);
+  cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
+  cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
+  cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
+  cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
+
+  vector<double> cam1_distortion_coeffs_temp(4);
+  nh.getParam("cam1/distortion_coeffs",
+      cam1_distortion_coeffs_temp);
+  cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
+  cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
+  cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
+  cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
+
+
+
   state_server.state_cov = MatrixXd::Zero(21, 21);
   for (int i = 3; i < 6; ++i)
     state_server.state_cov(i, i) = gyro_bias_cov;
@@ -1114,7 +1162,7 @@ void MsckfVio::removeLostFeatures() {
       }
     }
 
-    if(!feature.initializeAnchor(cam0_moving_window))
+    if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs))
     {
       invalid_feature_ids.push_back(feature.id);
       continue;