removed scaling

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "intelliSenseMode": "clang-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "core": "cpp",
+        "sparsecore": "cpp"
+    }
+}

config/camchain-imucam-euroc.yaml

@@ -9,7 +9,7 @@ cam0:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
-  distortion_model: radtan
+  distortion_model: pre-radtan
   intrinsics: [458.654, 457.296, 367.215, 248.375]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0
@@ -26,7 +26,7 @@ cam1:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
-  distortion_model: radtan
+  distortion_model: pre-radtan
   intrinsics: [457.587, 456.134, 379.999, 255.238]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0

config/camchain-imucam-tum-scaled.yaml

@@ -0,0 +1,36 @@
+cam0:
+  T_cam_imu:
+      [-0.9995378259923383,   0.02917807204183088,  -0.008530798463872679, 0.047094247958417004,
+      0.007526588843243184,   -0.03435493139706542, -0.9993813532126198,   -0.04788273017221637,
+      -0.029453096117288798,  -0.9989836729399656,  0.034119442089241274,  -0.0697294754693238,
+      0.0,                    0.0,                  0.0,                   1.0]
+  camera_model: pinhole
+  distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
+    0.00020293673591811182]
+  distortion_model: pre-equidistant
+  intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
+  resolution: [512, 512]
+  rostopic: /cam0/image_raw
+cam1:
+  T_cam_imu:
+    [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335,
+    0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889,
+    -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645,
+    0.0, 0.0, 0.0, 1.0]
+  T_cn_cnm1:
+    [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335,
+    0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977,
+    0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
+    0.0, 0.0, 0.0, 1.0]
+  camera_model: pinhole
+  distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
+    0.0003299517423931039]
+  distortion_model: pre-equidistant
+  intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
+  resolution: [512, 512]
+  rostopic: /cam1/image_raw
+T_imu_body:
+  [1.0000, 0.0000, 0.0000, 0.0000,
+  0.0000, 1.0000, 0.0000, 0.0000,
+  0.0000, 0.0000, 1.0000, 0.0000,
+  0.0000, 0.0000, 0.0000, 1.0000]

include/msckf_vio/cam_state.h

@@ -18,6 +18,8 @@ namespace msckf_vio {
 
 struct Frame{
   cv::Mat image;
+  cv::Mat dximage;
+  cv::Mat dyimage;
   double exposureTime_ms;
 };
 
@@ -39,6 +41,7 @@ struct CameraCalibration{
   cv::Vec4d distortion_coeffs;
   movingWindow moving_window;
   cv::Mat featureVisu;
+  int id;
 };
 
 

include/msckf_vio/feature.hpp

@@ -70,6 +70,11 @@ struct Feature {
     position(Eigen::Vector3d::Zero()),
     is_initialized(false), is_anchored(false) {}
 
+
+void Rhocost(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    double& e) const;
+
   /*
    * @brief cost Compute the cost of the camera observations
    * @param T_c0_c1 A rigid body transformation takes
@@ -82,6 +87,13 @@ struct Feature {
       const Eigen::Vector3d& x, const Eigen::Vector2d& z,
       double& e) const;
 
+  bool initializeRho(const CamStateServer& cam_states);
+
+  inline void RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    Eigen::Matrix<double, 2, 1>& J, Eigen::Vector2d& r,
+    double& w) const; 
+
   /*
    * @brief jacobian Compute the Jacobian of the camera observation
    * @param T_c0_c1 A rigid body transformation takes
@@ -97,6 +109,10 @@ struct Feature {
       Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
       double& w) const;
 
+  inline double generateInitialDepth(
+    const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
+    const Eigen::Vector2d& z2) const;
+
   /*
    * @brief generateInitialGuess Compute the initial guess of
    *    the feature's 3d position using only two views.
@@ -122,14 +138,6 @@ struct Feature {
       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
@@ -157,8 +165,7 @@ struct Feature {
   inline bool initializePosition(
       const CamStateServer& cam_states);
 
-
+  cv::Point2f pixelDistanceAt(
-cv::Point2f pixelDistanceAt(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   const CameraCalibration& cam,
@@ -177,6 +184,21 @@ cv::Point2f pixelDistanceAt(
                   const CameraCalibration& cam,
                   Eigen::Vector3d& in_p) const;
 
+  double CompleteCvKernel(
+                const cv::Point2f pose,
+                const StateIDType& cam_state_id,
+                CameraCalibration& cam,
+                std::string type) const;
+
+  double cvKernel(
+            const cv::Point2f pose,
+            std::string type) const;
+
+  double Kernel(
+            const cv::Point2f pose,
+            const cv::Mat frame,
+            std::string type) const;
+
   /*
   * @brief IrradianceAnchorPatch_Camera returns irradiance values
   *        of the Anchor Patch position in a camera frame
@@ -186,32 +208,31 @@ cv::Point2f pixelDistanceAt(
   bool estimate_FrameIrradiance(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
                   std::vector<double>& anchorPatch_estimate,
                   IlluminationParameter& estimatedIllumination) const;
 
-  bool MarkerGeneration(
+bool MarkerGeneration(
                   ros::Publisher& marker_pub,
                   const CamStateServer& cam_states) const;
 
+bool VisualizeKernel(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0) const;
+
   bool VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
-                  const std::vector<double> photo_r,
+                  const Eigen::VectorXd& photo_r,
                   std::stringstream& ss) const;
-
+  /*
-
+  * @brief AnchorPixelToPosition uses the calcualted pixels
-  /* @brief takes a pure pixel position (1m from image)
+  *     of the anchor patch to generate 3D positions of all of em
-  * converts to actual pixel value and returns patch irradiance
-  * around this pixel
   */
-  void PatchAroundPurePixel(cv::Point2f p, 
+inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
-                               int N,
+          const CameraCalibration& cam);
-                               const CameraCalibration& cam, 
-                               const StateIDType& cam_state_id,
-                               std::vector<float>& return_i) const;
-
 
   /*
   * @brief Irradiance returns irradiance value of a pixel
@@ -256,6 +277,14 @@ cv::Point2f pixelDistanceAt(
   bool is_initialized;
   bool is_anchored;
 
+
+  cv::Mat abs_xderImage;
+  cv::Mat abs_yderImage;
+
+  cv::Mat xderImage;
+  cv::Mat yderImage;
+
+  cv::Mat anchorImage_blurred;
   cv::Point2f anchor_center_pos;
   cv::Point2f undist_anchor_center_pos;
   // Noise for a normalized feature measurement.
@@ -271,6 +300,26 @@ typedef std::map<FeatureIDType, Feature, std::less<int>,
         Eigen::aligned_allocator<
         std::pair<const FeatureIDType, Feature> > > MapServer;
 
+void Feature::Rhocost(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    double& e) const 
+{
+  // Compute hi1, hi2, and hi3 as Equation (37).
+  const double& rho = x;
+
+  Eigen::Vector3d h = T_c0_ci.linear()*
+    Eigen::Vector3d(z1(0), z1(1), 1.0) + rho*T_c0_ci.translation();
+  double& h1 = h(0);
+  double& h2 = h(1);
+  double& h3 = h(2);
+
+  // Predict the feature observation in ci frame.
+  Eigen::Vector2d z_hat(h1/h3, h2/h3);
+
+  // Compute the residual.
+  e = (z_hat-z2).squaredNorm();
+  return;
+}
 
 void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
@@ -283,9 +332,9 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
 
   Eigen::Vector3d h = T_c0_ci.linear()*
     Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation();
-  double& h1 = h(0);
+  double h1 = h(0);
-  double& h2 = h(1);
+  double h2 = h(1);
-  double& h3 = h(2);
+  double h3 = h(2);
 
   // Predict the feature observation in ci frame.
   Eigen::Vector2d z_hat(h1/h3, h2/h3);
@@ -295,6 +344,42 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
   return;
 }
 
+void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    Eigen::Matrix<double, 2, 1>& J, Eigen::Vector2d& r,
+    double& w) const 
+{
+
+  const double& rho = x;
+
+  Eigen::Vector3d h = T_c0_ci.linear()*
+    Eigen::Vector3d(z1(0), z2(1), 1.0) + rho*T_c0_ci.translation();
+  double& h1 = h(0);
+  double& h2 = h(1);
+  double& h3 = h(2);
+
+  // Compute the Jacobian.
+  Eigen::Matrix3d W;
+  W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
+  W.rightCols<1>() = T_c0_ci.translation();
+
+  J(0,0) = -h1/(h3*h3);
+  J(1,0) = -h2/(h3*h3);
+
+  // Compute the residual.
+  Eigen::Vector2d z_hat(h1/h3, h2/h3);
+  r = z_hat - z2;
+
+  // Compute the weight based on the residual.
+  double e = r.norm();
+  if (e <= optimization_config.huber_epsilon)
+    w = 1.0;
+  else
+    w = optimization_config.huber_epsilon / (2*e);
+
+  return;
+}
+
 void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
     Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
@@ -334,9 +419,9 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
   return;
 }
 
-void Feature::generateInitialGuess(
+double Feature::generateInitialDepth(
     const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
-    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const 
+    const Eigen::Vector2d& z2) const 
 {
   // Construct a least square problem to solve the depth.
   Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);
@@ -351,6 +436,15 @@ void Feature::generateInitialGuess(
 
   // Solve for the depth.
   double depth = (A.transpose() * A).inverse() * A.transpose() * b;
+  return depth;
+}
+
+
+void Feature::generateInitialGuess(
+    const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
+    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const 
+{
+  double depth = generateInitialDepth(T_c1_c2, z1, z2);
   p(0) = z1(0) * depth;
   p(1) = z1(1) * depth;
   p(2) = depth;
@@ -400,10 +494,62 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
   else return false;
 }
 
+double Feature::CompleteCvKernel(
+                const cv::Point2f pose,
+                const StateIDType& cam_state_id,
+                CameraCalibration& cam,
+                std::string type) const
+{
+
+  double delta = 0;
+
+  if(type == "Sobel_x")
+    delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at<short>(pose.y, pose.x))/255.;
+  else if (type == "Sobel_y")
+    delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
+
+  return delta;
+}
+
+double Feature::cvKernel(
+                const cv::Point2f pose,
+                std::string type) const
+{
+
+  double delta = 0;
+
+  if(type == "Sobel_x")
+    delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
+  else if (type == "Sobel_y")
+    delta = ((double)yderImage.at<short>(pose.y, pose.x))/255.;
+  return delta;
+
+}
+
+double Feature::Kernel(
+                const cv::Point2f pose,
+                const cv::Mat frame,
+                std::string type) const
+{
+Eigen::Matrix<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::Zero();
+if(type == "Sobel_x")
+  kernel  << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.;
+else if(type == "Sobel_y")
+  kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
+
+double delta = 0;
+int offs = (int)(kernel.rows()-1)/2;
+
+for(int i = 0; i < kernel.rows(); i++)
+  for(int j = 0; j < kernel.cols(); j++)
+    delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255. * (float)kernel(j,i);
+
+return delta;
+}
 bool Feature::estimate_FrameIrradiance(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
                   std::vector<double>& anchorPatch_estimate,
                   IlluminationParameter& estimated_illumination) const
 {
@@ -412,14 +558,11 @@ bool Feature::estimate_FrameIrradiance(
   // muliply by a and add b of this frame
 
   auto anchor = observations.begin();
-  if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
+  if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
-  {
-    std::cout << "anchor not in buffer anymore!" << std::endl;
     return false;
-  } 
 
-  double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
+  double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms;
-  double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;
+  double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms;
 
 
   double a_A = anchorExposureTime_ms;
@@ -554,11 +697,47 @@ bool Feature::MarkerGeneration(
   marker_pub.publish(ma);
 }
 
+
+bool Feature::VisualizeKernel(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0) const
+{
+  auto anchor = observations.begin();
+  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
+
+  //cv::Mat xderImage;
+  //cv::Mat yderImage;
+
+  //cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3);
+  //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
+
+
+  // cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
+  // cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
+
+  /*
+  for(int i = 1; i < anchorImage.rows-1; i++)
+    for(int j = 1; j < anchorImage.cols-1; j++)
+      xderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x"));
+  
+
+  for(int i = 1; i < anchorImage.rows-1; i++)
+    for(int j = 1; j < anchorImage.cols-1; j++)
+      yderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y"));
+  */
+  //cv::imshow("anchor", anchorImage);
+  cv::imshow("xder2", xderImage);
+  cv::imshow("yder2", yderImage);
+
+  cvWaitKey(0);
+} 
+
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
-                  CameraCalibration& cam0,
+                  CameraCalibration& cam,
-                  const std::vector<double> photo_r,
+                  const Eigen::VectorXd& photo_r,
                   std::stringstream& ss) const
 {
 
@@ -566,7 +745,7 @@ bool Feature::VisualizePatch(
 
   //visu - anchor
   auto anchor = observations.begin();
-  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
+  cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
   cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
   cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
 
@@ -578,10 +757,10 @@ bool Feature::VisualizePatch(
     cv::Point ys(point.x, point.y);
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
   }  
-  cam0.featureVisu = dottedFrame.clone(); 
+  cam.featureVisu = dottedFrame.clone(); 
 
   // visu - feature
-  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
+  cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image;
   cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
   
   // set position in frame 
@@ -589,7 +768,7 @@ bool Feature::VisualizePatch(
   std::vector<double> projectionPatch;
   for(auto point : anchorPatch_3d)
   {
-    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
     projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
     // visu - feature
     cv::Point xs(p_in_c0.x, p_in_c0.y);
@@ -597,7 +776,7 @@ bool Feature::VisualizePatch(
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
   }  
 
-  cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
+  cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu);
 
 
   // patches visualization
@@ -622,9 +801,10 @@ bool Feature::VisualizePatch(
                     CV_FILLED);
 
   // irradiance grid projection
+
   namer.str(std::string());
   namer << "projection";
-  cv::putText(irradianceFrame, namer.str(), cvPoint(30, 45+scale*N), 
+  cv::putText(irradianceFrame, namer.str() , cvPoint(30, 45+scale*N), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
   for(int i = 0; i<N; i++)
@@ -643,10 +823,14 @@ bool Feature::VisualizePatch(
   cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
-  cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
+  cv::Point2f p_f;
+  if(cam.id == 0)
+    p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
+  else if(cam.id == 1)
+    p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3));
   // move to real pixels
-  p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
 
+  p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
   for(int i = 0; i<N; i++)
   {
     for(int j = 0; j<N ; j++)
@@ -663,22 +847,23 @@ bool Feature::VisualizePatch(
   // residual grid projection, positive - red, negative - blue colored 
   namer.str(std::string());
   namer << "residual";
+  std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl;
   cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 
   for(int i = 0; i<N; i++)
     for(int j = 0; j<N; j++)
-      if(photo_r[i*N+j]>0)
+      if(photo_r(2*(i*N+j))>0)
         cv::rectangle(irradianceFrame, 
                       cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                       cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255),  
+                      cv::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255),  
                       CV_FILLED);
       else
         cv::rectangle(irradianceFrame, 
                       cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                       cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), 
+                      cv::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), 
                       CV_FILLED);
 
   // gradient arrow
@@ -688,22 +873,21 @@ bool Feature::VisualizePatch(
                   cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y),
                   cv::Scalar(100, 0, 255),
                   1);
+  */
 
   // residual gradient direction
+      /*
   cv::arrowedLine(irradianceFrame,
                   cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))),
                   cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y),
                   cv::Scalar(0, 255, 175),
                   3);
+       */           
 
-  */
+  cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu);
-  cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
-  
-    
       
+/*
   // visualize position of used observations and resulting feature position
-  /*
-
   cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
   cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
 
@@ -732,17 +916,15 @@ bool Feature::VisualizePatch(
 
   // draw, x y position and arrow with direction - write z next to it
 
-  cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
+  cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale);
 
 
-  cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
+  cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu);
-
+*/
-  */
-
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
-  cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
+  cv::putText(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
   // create line?
 
@@ -750,35 +932,16 @@ bool Feature::VisualizePatch(
 
   std::stringstream loc;
  // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
-  //cv::imwrite(loc.str(), cam0.featureVisu);
+  //cv::imwrite(loc.str(), cam.featureVisu);
-
-  cv::imshow("patch", cam0.featureVisu);
-  cvWaitKey(0);
-}
-
-void Feature::PatchAroundPurePixel(cv::Point2f p, 
-                               int N,
-                               const CameraCalibration& cam, 
-                               const StateIDType& cam_state_id,
-                               std::vector<float>& return_i) const
-{
-  int n = (int)(N-1)/2;
-  cv::Mat image = cam.moving_window.find(cam_state_id)->second.image;
-  cv::Point2f img_p = image_handler::distortPoint(p, 
-                                                cam.intrinsics, 
-                                                cam.distortion_model, 
-                                                cam.distortion_coeffs);
-
-  for(double u_run = -n; u_run <= n; u_run++)
-    for(double v_run = -n; v_run <= n; v_run++)
-      return_i.push_back(PixelIrradiance(cv::Point2f(img_p.x+u_run, img_p.y+v_run), image));
 
+  cv::imshow("patch", cam.featureVisu);
+  cvWaitKey(1);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
 {
 
-  return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
+  return ((float)image.at<uint8_t>(pose.y, pose.x))/255.;
 }
 
 cv::Point2f Feature::pixelDistanceAt(
@@ -804,13 +967,18 @@ cv::Point2f Feature::pixelDistanceAt(
                                 cam.distortion_coeffs, 
                                 pure); 
 
-  // returns the absolute pixel distance at pixels one metres away
+  // transfrom position to camera frame
+  // to get distance multiplier
+  Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
+  const Eigen::Vector3d& t_c0_w = cam_state.position;
+  Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
+
+  // returns the distance between the pixel points in space
   cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
 
   return distance;
 }
 
-
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -820,26 +988,40 @@ cv::Point2f Feature::projectPositionToCamera(
   Eigen::Isometry3d T_c0_w;
 
   cv::Point2f out_p;
-
+  cv::Point2f my_p;
   // transfrom position to camera frame
+  
+  // cam0 position
   Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Eigen::Vector3d& t_c0_w = cam_state.position;
-  Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
 
-  out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
+  // project point according to model
+  if(cam.id == 0)
+  {
+    Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
+    out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
 
- // 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);
+  // if camera is one, calcualte the cam1 position from cam0 position first
+  else if(cam.id == 1)
+  {  
+    // cam1 position
+    Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
+    Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0;
+    Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
 
-  cv::Point2f my_p = image_handler::distortPoint(out_p, 
+    Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w);
-                                                 cam.intrinsics, 
+    out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
-                                                 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);
 
+  // undistort point according to camera model
+  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);
   return my_p;
 }
 
@@ -864,45 +1046,75 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 
   auto anchor = observations.begin();
   if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
+  {
     return false;
-
+  }
   cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
+  cv::Mat anchorImage_deeper;
+  anchorImage.convertTo(anchorImage_deeper,CV_16S);
+  //TODO remove this?
+
+
+  cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3);
+  cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3);
+
+  xderImage/=8.;
+  yderImage/=8.;
+
+  cv::convertScaleAbs(xderImage, abs_xderImage);
+  cv::convertScaleAbs(yderImage, abs_yderImage);
+  
+  cv::GaussianBlur(anchorImage, anchorImage_blurred, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT);
 
   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")
-
+  {
-  // get feature in undistorted pixel space
+    //project onto pixel plane
-  // this only reverts from 'pure' space into undistorted pixel space using camera matrix
+    undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
-  cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
+    // create vector of patch in pixel plane
-                                                    cam.intrinsics, 
+    for(double u_run = -n; u_run <= n; u_run++)
-                                                    cam.distortion_model, 
+      for(double v_run = -n; v_run <= n; v_run++)
-                                                    cam.distortion_coeffs);
+        anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
-  // 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    
+    //project back into u,v
-  image_handler::undistortPoints(anchorPatch_real,
+    for(int i = 0; i < N*N; i++)
-                                cam.intrinsics,
+      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]));
-                                cam.distortion_model,
+  }
-                                cam.distortion_coeffs,
-                                anchorPatch_ideal);
 
+  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    
+    image_handler::undistortPoints(anchorPatch_real,
+                                  cam.intrinsics,
+                                  cam.distortion_model,
+                                  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)
@@ -913,9 +1125,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.
@@ -925,9 +1139,6 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
     Eigen::aligned_allocator<Eigen::Vector2d> > measurements(0);
 
   for (auto& m : observations) {
-    // TODO: This should be handled properly. Normally, the
-    //    required camera states should all be available in
-    //    the input cam_states buffer.
     auto cam_state_iter = cam_states.find(m.first);
     if (cam_state_iter == cam_states.end()) continue;
 
@@ -958,13 +1169,11 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
     pose = pose.inverse() * T_c0_w;
 
   // Generate initial guess
-  Eigen::Vector3d initial_position(0.0, 0.0, 0.0);
+  double initial_depth = 0;
-  generateInitialGuess(cam_poses[cam_poses.size()-1], measurements[0],
+  initial_depth = generateInitialDepth(cam_poses[cam_poses.size()-1], measurements[0],
-      measurements[measurements.size()-1], initial_position);
+      measurements[measurements.size()-1]);
-  Eigen::Vector3d solution(
+
-      initial_position(0)/initial_position(2),
+  double solution = 1.0/initial_depth;
-      initial_position(1)/initial_position(2),
-      1.0/initial_position(2));
 
   // Apply Levenberg-Marquart method to solve for the 3d position.
   double lambda = optimization_config.initial_damping;
@@ -977,21 +1186,21 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
   double total_cost = 0.0;
   for (int i = 0; i < cam_poses.size(); ++i) {
     double this_cost = 0.0;
-    cost(cam_poses[i], solution, measurements[i], this_cost);
+    Rhocost(cam_poses[i], solution, measurements[0], measurements[i], this_cost);
     total_cost += this_cost;
   }
 
   // Outer loop.
   do {
-    Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
+    Eigen::Matrix<double, 1, 1> A = Eigen::Matrix<double, 1, 1>::Zero();
-    Eigen::Vector3d b = Eigen::Vector3d::Zero();
+    Eigen::Matrix<double, 1, 1>  b = Eigen::Matrix<double, 1, 1>::Zero();
 
     for (int i = 0; i < cam_poses.size(); ++i) {
-      Eigen::Matrix<double, 2, 3> J;
+      Eigen::Matrix<double, 2, 1> J;
       Eigen::Vector2d r;
       double w;
 
-      jacobian(cam_poses[i], solution, measurements[i], J, r, w);
+      RhoJacobian(cam_poses[i], solution, measurements[0], measurements[i], J, r, w);
 
       if (w == 1) {
         A += J.transpose() * J;
@@ -1006,15 +1215,15 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
     // Inner loop.
     // Solve for the delta that can reduce the total cost.
     do {
-      Eigen::Matrix3d damper = lambda * Eigen::Matrix3d::Identity();
+      Eigen::Matrix<double, 1, 1> damper = lambda*Eigen::Matrix<double, 1, 1>::Identity();
-      Eigen::Vector3d delta = (A+damper).ldlt().solve(b);
+      Eigen::Matrix<double, 1, 1> delta = (A+damper).ldlt().solve(b);
-      Eigen::Vector3d new_solution = solution - delta;
+      double new_solution = solution - delta(0,0);
       delta_norm = delta.norm();
 
       double new_cost = 0.0;
       for (int i = 0; i < cam_poses.size(); ++i) {
         double this_cost = 0.0;
-        cost(cam_poses[i], new_solution, measurements[i], this_cost);
+        Rhocost(cam_poses[i], new_solution, measurements[0], measurements[i], this_cost);
         new_cost += this_cost;
       }
 
@@ -1039,8 +1248,8 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
 
   // Covert the feature position from inverse depth
   // representation to its 3d coordinate.
-  Eigen::Vector3d final_position(solution(0)/solution(2),
+  Eigen::Vector3d final_position(measurements[0](0)/solution,
-      solution(1)/solution(2), 1.0/solution(2));
+      measurements[0](1)/solution, 1.0/solution);
 
   // Check if the solution is valid. Make sure the feature
   // is in front of every camera frame observing it.
@@ -1055,7 +1264,7 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
   }
 
   //save inverse depth distance from camera
-  anchor_rho = solution(2);
+  anchor_rho = solution;
 
   // Convert the feature position to the world frame.
   position = T_c0_w.linear()*final_position + T_c0_w.translation();
@@ -1065,10 +1274,12 @@ bool Feature::initializeRho(const CamStateServer& cam_states) {
 
   return is_valid_solution;
 }
-*/
+
+
 bool Feature::initializePosition(const CamStateServer& cam_states) {
 
   // Organize camera poses and feature observations properly.
+
   std::vector<Eigen::Isometry3d,
     Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
   std::vector<Eigen::Vector2d,
@@ -1206,6 +1417,7 @@ bool Feature::initializePosition(const CamStateServer& cam_states) {
 
   //save inverse depth distance from camera
   anchor_rho = solution(2);
+  std::cout << "from feature: " << anchor_rho << std::endl;
 
   // Convert the feature position to the world frame.
   position = T_c0_w.linear()*final_position + T_c0_w.translation();

include/msckf_vio/image_handler.h

@@ -16,6 +16,16 @@ namespace msckf_vio {
  */
 namespace image_handler {
 
+cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
+cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
+
+void undistortImage(
+  cv::InputArray src,
+  cv::OutputArray dst,
+  const std::string& distortion_model,
+  const cv::Vec4d& intrinsics,
+  const cv::Vec4d& distortion_coeffs);
+
 void undistortPoints(
     const std::vector<cv::Point2f>& pts_in,
     const cv::Vec4d& intrinsics,

include/msckf_vio/image_processor.h

@@ -320,6 +320,8 @@ private:
     return;
   }
 
+  bool STREAMPAUSE;
+
   // Indicate if this is the first image message.
   bool is_first_img;
 

include/msckf_vio/math_utils.hpp

@@ -13,13 +13,6 @@
 
 namespace msckf_vio {
 
-
-inline double absoluted(double a){
-  if(a>0)
-    return a;
-  else return -a;
-}
-
 /*
  *  @brief Create a skew-symmetric matrix from a 3-element vector.
  *  @note Performs the operation:

include/msckf_vio/msckf_vio.h

@@ -202,53 +202,111 @@ class MsckfVio {
         Eigen::Vector4d& r);
     // This function computes the Jacobian of all measurements viewed
     // in the given camera states of this feature.
-    void featureJacobian(const FeatureIDType& feature_id,
+    bool featureJacobian(
+        const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
 
 
-    void PhotometricMeasurementJacobian(
+    void twodotMeasurementJacobian(
-    const StateIDType& cam_state_id,
+        const StateIDType& cam_state_id,
-    const FeatureIDType& feature_id,
+        const FeatureIDType& feature_id,
-    Eigen::MatrixXd& H_x,
+        Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
-    Eigen::MatrixXd& H_y,
-    Eigen::VectorXd& r);
 
-    void PhotometricFeatureJacobian(
+    bool ConstructJacobians(
-    const FeatureIDType& feature_id,
+        Eigen::MatrixXd& H_rho,
-    const std::vector<StateIDType>& cam_state_ids,
+        Eigen::MatrixXd& H_pl,
-    Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+        Eigen::MatrixXd& H_pA,
+        const Feature& feature,
+        const StateIDType&  cam_state_id,
+        Eigen::MatrixXd& H_xl,
+        Eigen::MatrixXd& H_yl);
 
+    bool PhotometricPatchPointResidual(
+        const StateIDType& cam_state_id,
+        const Feature& feature, 
+        Eigen::VectorXd& r);
+
+    bool PhotometricPatchPointJacobian(
+        const CAMState& cam_state,
+        const StateIDType& cam_state_id,
+        const Feature& feature,
+        Eigen::Vector3d point,
+        int count,
+        Eigen::Matrix<double, 2, 1>& H_rhoj,
+        Eigen::Matrix<double, 2, 6>& H_plj,
+        Eigen::Matrix<double, 2, 6>& H_pAj,
+        Eigen::Matrix<double, 2, 4>& dI_dhj);
+
+    bool PhotometricMeasurementJacobian(
+        const StateIDType& cam_state_id,
+        const FeatureIDType& feature_id,
+        Eigen::MatrixXd& H_x,
+        Eigen::MatrixXd& H_y,
+        Eigen::VectorXd& r);
+
+
+    bool twodotFeatureJacobian(
+        const FeatureIDType& feature_id,
+        const std::vector<StateIDType>& cam_state_ids,
+        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
+    bool PhotometricFeatureJacobian(
+        const FeatureIDType& feature_id,
+        const std::vector<StateIDType>& cam_state_ids,
+        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
+    void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
     void measurementUpdate(const Eigen::MatrixXd& H,
         const Eigen::VectorXd& r);
+    void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
+
     bool gatingTest(const Eigen::MatrixXd& H,
-        const Eigen::VectorXd&r, const int& dof);
+        const Eigen::VectorXd&r, const int& dof, int filter=0);
     void removeLostFeatures();
     void findRedundantCamStates(
         std::vector<StateIDType>& rm_cam_state_ids);
+
+    void pruneLastCamStateBuffer();
     void pruneCamStateBuffer();
     // Reset the system online if the uncertainty is too large.
     void onlineReset();
 
     // Photometry flag
-    bool PHOTOMETRIC;
+    int FILTER;
 
     // debug flag
+    bool STREAMPAUSE;
     bool PRINTIMAGES;
     bool GROUNDTRUTH;
 
     bool nan_flag;
     bool play;
     double last_time_bound;
+    double time_offset;
 
     // Patch size for Photometry
     int N;
-
+    // Image rescale
+    int SCALE; 
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 
+    double eval_time;
+
+    IMUState timed_old_imu_state;
+    IMUState timed_old_true_state;
+
+    IMUState old_imu_state;
+    IMUState old_true_state;
+
+    // change in position
+    Eigen::Vector3d delta_position;
+    Eigen::Vector3d delta_orientation;
+
     // State vector
     StateServer state_server;
+    StateServer photometric_state_server;
 
     // Ground truth state vector 
     StateServer true_state_server;
@@ -311,6 +369,7 @@ class MsckfVio {
     // Subscribers and publishers
     ros::Subscriber imu_sub;
     ros::Subscriber truth_sub;
+    ros::Publisher truth_odom_pub;
     ros::Publisher odom_pub;
     ros::Publisher marker_pub;
     ros::Publisher feature_pub;

launch/image_processor_tinytum.launch

@@ -0,0 +1,38 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="image_processor"
+      args="standalone msckf_vio/ImageProcessorNodelet"
+      output="screen"
+       >
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+
+      <rosparam command="load" file="$(arg calibration_file)"/>
+      <param name="grid_row" value="4"/>
+      <param name="grid_col" value="4"/>
+      <param name="grid_min_feature_num" value="3"/>
+      <param name="grid_max_feature_num" value="5"/>
+      <param name="pyramid_levels" value="3"/>
+      <param name="patch_size" value="15"/>
+      <param name="fast_threshold" value="10"/>
+      <param name="max_iteration" value="30"/>
+      <param name="track_precision" value="0.01"/>
+      <param name="ransac_threshold" value="3"/>
+      <param name="stereo_threshold" value="5"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+
+    </node>
+  </group>
+
+</launch>

launch/image_processor_tum.launch

@@ -11,6 +11,9 @@
       output="screen"
        >
 
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+
       <rosparam command="load" file="$(arg calibration_file)"/>
       <param name="grid_row" value="4"/>
       <param name="grid_col" value="4"/>

launch/msckf_vio_debug_tum.launch

@@ -22,10 +22,10 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="7"/>
+      <param name="patch_size_n" value="3"/>
 
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>

launch/msckf_vio_euroc.launch

@@ -17,6 +17,18 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="1"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="true"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="5"/>
+      <param name="image_scale" value ="1"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

launch/msckf_vio_tinytum.launch

@@ -0,0 +1,77 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_tinytum.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen">
+
+      <param name="FILTER" value="0"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="3"/>
+      <param name="image_scale" value ="1"/>
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="12"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+
+</launch>

launch/msckf_vio_tum.launch

@@ -17,14 +17,16 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-      <!-- Photometry Flag-->
+
-      <param name="PHOTOMETRIC" value="true"/>
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="7"/>
+      <param name="patch_size_n" value="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -71,4 +73,6 @@
     </node>
   </group>
 
+  <!--node name="player" pkg="bagcontrol" type="control.py" /-->
+
 </launch>

log

@@ -0,0 +1,73 @@
+# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>
+# name: Hx
+# type: matrix
+# rows: 18
+# columns: 49
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 313.795 -58.7912 139.778 -46.7616 144.055 86.9644 0 -314.123 55.6434 -140.648 46.7616 -144.055 -86.9644 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 441.06 -94.50069999999999 174.424 -53.7653 204.822 120.248 0 -441.685 90.1101 -175.657 53.7653 -204.822 -120.248 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 225.35 -54.5629 77.60599999999999 -21.1425 105.886 60.3706 0 -225.756 52.3373 -78.2406 21.1425 -105.886 -60.3706 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 175.128 20.6203 175.127 -79.63939999999999 73.245 62.1868 0 -174.573 -22.5235 -175.576 79.63939999999999 -73.245 -62.1868 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 296.962 43.5469 311.307 -143.667 123.399 108.355 0 -295.905 -46.7952 -312.063 143.667 -123.399 -108.355 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 126.283 117.889 311.864 -161.264 38.8521 71.8019 0 -124.464 -119.541 -312.118 161.264 -38.8521 -71.8019 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49.2502 63.7166 155.071 -80.81950000000001 12.7732 32.1826 0 -48.2934 -64.4113 -155.157 80.81950000000001 -12.7732 -32.1826 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 69.59699999999999 154.579 335.384 -179.355 9.212580000000001 62.0364 0 -67.35599999999999 -155.735 -335.462 179.355 -9.212580000000001 -62.0364 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -66.6965 304.947 500.218 -285.589 -71.31010000000001 55.5058 0 70.8009 -305.077 -499.831 285.589 71.31010000000001 -55.5058 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 323.404 -62.2043 141.092 -46.6015 148.737 89.1108 0 0 0 0 0 0 0 0 -324.336 57.8552 -141.991 46.6015 -148.737 -89.1108 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 454.208 -99.3095 175.986 -53.4094 211.276 123.174 0 0 0 0 0 0 0 0 -455.779 93.0992 -177.158 53.4094 -211.276 -123.174 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 231.884 -57.0266 78.2559 -20.8926 109.118 61.8183 0 0 0 0 0 0 0 0 -232.824 53.8025 -78.80719999999999 20.8926 -109.118 -61.8183 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 181.715 18.8525 177.045 -80.08499999999999 76.3716 63.8254 0 0 0 0 0 0 0 0 -181.07 -20.839 -177.959 80.08499999999999 -76.3716 -63.8254 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 308.249 40.5812 314.711 -144.494 128.766 111.207 0 0 0 0 0 0 0 0 -306.972 -43.8825 -316.328 144.494 -128.766 -111.207 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 133.309 116.865 315.474 -162.598 42.0763 73.8353 0 0 0 0 0 0 0 0 -130.603 -117.454 -316.931 162.598 -42.0763 -73.8353 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 52.4426 63.3607 156.902 -81.5288 14.2139 33.1222 0 0 0 0 0 0 0 0 -50.9976 -63.4393 -157.607 81.5288 -14.2139 -33.1222 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 75.5508 154.234 339.369 -180.995 11.859 63.8956 0 0 0 0 0 0 0 0 -72.1041 -153.816 -340.865 180.995 -11.859 -63.8956 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -62.0551 306.357 506.351 -288.542 -69.50409999999999 57.4562 0 0 0 0 0 0 0 0 68.6262 -303.028 -508.423 288.542 69.50409999999999 -57.4562 1 0 0 0 0 0 0 0
+
+
+# name: Hy
+# type: matrix
+# rows: 18
+# columns: 14
+ 1.56267 0 0 0 0 0 0 0 0 0 0.252873 0 0 0.110387
+ 0 1.56267 0 0 0 0 0 0 0 0 0.453168 0 0 0.162961
+ 0 0 1.56267 0 0 0 0 0 0 0 0.638441 0 0 0.0873758
+ 0 0 0 1.56267 0 0 0 0 0 0 0.21031 0 0 0.0321517
+ 0 0 0 0 1.56267 0 0 0 0 0 0.375554 0 0 0.0505151
+ 0 0 0 0 0 1.56267 0 0 0 0 0.638441 0 0 -0.0336034
+ 0 0 0 0 0 0 1.56267 0 0 0 0.157733 0 0 -0.0232704
+ 0 0 0 0 0 0 0 1.56267 0 0 0.212814 0 0 -0.0688343
+ 0 0 0 0 0 0 0 0 1.56267 0 0.360532 0 0 -0.187745
+ 1.56267 0 0 0 0 0 0 0 0 0 0 0.252873 0 0.322811
+ 0 1.56267 0 0 0 0 0 0 0 0 0 0.453168 0 0.467319
+ 0 0 1.56267 0 0 0 0 0 0 0 0 0.638441 0 0.245907
+ 0 0 0 1.56267 0 0 0 0 0 0 0 0.21031 0 0.135725
+ 0 0 0 0 1.56267 0 0 0 0 0 0 0.375554 0 0.225277
+ 0 0 0 0 0 1.56267 0 0 0 0 0 0.638441 0 0.012926
+ 0 0 0 0 0 0 1.56267 0 0 0 0 0.157733 0 -0.0108962
+ 0 0 0 0 0 0 0 1.56267 0 0 0 0.212814 0 -0.06974909999999999
+ 0 0 0 0 0 0 0 0 1.56267 0 0 0.360532 0 -0.318846
+
+
+# name: r
+# type: matrix
+# rows: 18
+# columns: 1
+ 0.0354809
+ 0.0153183
+ 0.0570191
+ -0.0372801
+ 0.0878601
+ 0.06811780000000001
+ -0.00426164
+ 5.162985999041026e-321
+ 6.927779999999998e-310
+ 0
+ 2.121999999910509e-314
+ 0
+ 0
+ 0
+ 3.6073900000086e-313
+ 4.446590812571219e-323
+ 3.952525166729972e-323
+ 3.952525166729972e-323
+
+

src/bagcontrol.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+import rosbag
+import rospy
+from sensor_msgs.msg import Imu, Image
+from geometry_msgs.msg import TransformStamped
+import time
+import signal
+import sys
+
+
+def signal_handler(sig, frame):
+        print('gracefully exiting the program.')
+        bag.close()
+        sys.exit(0)
+
+def main():
+    global bag
+
+    cam0_topic = '/cam0/image_raw'
+    cam1_topic = '/cam1/image_raw'
+    imu0_topic = '/imu0'
+    grnd_topic = '/vrpn_client/raw_transform'
+
+    rospy.init_node('controlbag')
+    rospy.set_param('play_bag', False)
+
+    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
+    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
+    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
+    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
+
+    signal.signal(signal.SIGINT, signal_handler)
+
+    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
+    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
+        
+        # pause if parameter set to false
+        flag = False
+        while not rospy.get_param('/play_bag'):
+            time.sleep(0.01)
+            if not flag:
+                print("stopped playback")
+                flag = not flag
+
+        if flag:
+            print("resume playback")
+
+        if topic == cam0_topic:
+            cam0_pub.publish(msg)
+
+        elif topic == cam1_topic:
+            cam1_pub.publish(msg)
+
+        elif topic == imu0_topic:
+            imu0_pub.publish(msg)
+
+        elif topic ==grnd_topic:
+            grnd_pub.publish(msg)
+
+        #print msg
+    bag.close()
+
+if __name__== "__main__":
+    main()

src/control.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+import rosbag
+import rospy
+from sensor_msgs.msg import Imu, Image
+from geometry_msgs.msg import TransformStamped
+import time
+import signal
+import sys
+
+
+def signal_handler(sig, frame):
+        print('gracefully exiting the program.')
+        bag.close()
+        sys.exit(0)
+
+def main():
+    global bag
+
+    cam0_topic = '/cam0/image_raw'
+    cam1_topic = '/cam1/image_raw'
+    imu0_topic = '/imu0'
+    grnd_topic = '/vrpn_client/raw_transform'
+
+    rospy.init_node('controlbag')
+    rospy.set_param('play_bag', False)
+
+    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
+    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
+    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
+    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
+
+    signal.signal(signal.SIGINT, signal_handler)
+
+    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
+    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
+        
+        # pause if parameter set to false
+        flag = False
+        while not rospy.get_param('/play_bag'):
+            time.sleep(0.01)
+            if not flag:
+                print("stopped playback")
+                flag = not flag
+
+        if flag:
+            print("resume playback")
+
+        if topic == cam0_topic:
+            cam0_pub.publish(msg)
+
+        elif topic == cam1_topic:
+            cam1_pub.publish(msg)
+
+        elif topic == imu0_topic:
+            imu0_pub.publish(msg)
+
+        elif topic ==grnd_topic:
+            grnd_pub.publish(msg)
+
+        #print msg
+    bag.close()
+
+if __name__== "__main__":
+    main()

src/image_handler.cpp

@@ -14,6 +14,40 @@ namespace msckf_vio {
 namespace image_handler {
 
 
+cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
+{
+  return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]);
+}
+
+cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
+{
+  return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]);
+}
+
+void undistortImage(
+  cv::InputArray src,
+  cv::OutputArray dst,
+  const std::string& distortion_model,
+  const cv::Vec4d& intrinsics,
+  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);
+
+  if (distortion_model == "pre-equidistant")
+      cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K);
+  else if (distortion_model == "equidistant")
+      src.copyTo(dst);
+
+  else if (distortion_model == "pre-radtan")
+      cv::undistort(src, dst, K, distortion_coeffs, K);
+  else if (distortion_model == "radtan")
+      src.copyTo(dst);
+
+}
+
 void undistortPoint(
     const cv::Point2f& pt_in,
     const cv::Vec4d& intrinsics,
@@ -42,10 +76,37 @@ void undistortPoint(
   if (distortion_model == "radtan") {
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                         rectification_matrix, K_new);
-  } else if (distortion_model == "equidistant") {
+  } 
+  // equidistant
+  else if (distortion_model == "equidistant") {
     cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                  rectification_matrix, K_new);
-  } else {
+  }
+  // fov
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      float omega = distortion_coeffs[0];
+      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float ru = tan(rd * omega)/(2 * tan(omega / 2));
+
+      cv::Point2f newPoint(
+              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
+              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
+
+      pts_out.push_back(newPoint);
+    }
+   }
+
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+  } 
+   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,
@@ -79,10 +140,35 @@ void undistortPoints(
   if (distortion_model == "radtan") {
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                         rectification_matrix, K_new);
-  } else if (distortion_model == "equidistant") {
+  } 
+  else if (distortion_model == "equidistant") {
     cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                  rectification_matrix, K_new);
-  } else {
+  } 
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      float omega = distortion_coeffs[0];
+      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float ru = tan(rd * omega)/(2 * tan(omega / 2));
+
+      cv::Point2f newPoint(
+              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
+              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
+
+      pts_out.push_back(newPoint);
+    }
+  }
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+
+  }  
+  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,
@@ -110,7 +196,31 @@ std::vector<cv::Point2f> distortPoints(
                       distortion_coeffs, pts_out);
   } else if (distortion_model == "equidistant") {
     cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  } else {
+  }
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      // based on 'straight lines have to be straight'
+      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float omega = distortion_coeffs[0];
+      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
+      
+      cv::Point2f newPoint(
+              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
+              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
+
+      pts_out.push_back(newPoint);
+    }
+  }
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+  }   
+   else {
     ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                   distortion_model.c_str());
     std::vector<cv::Point3f> homogenous_pts;
@@ -143,7 +253,31 @@ cv::Point2f distortPoint(
                       distortion_coeffs, pts_out);
   } else if (distortion_model == "equidistant") {
     cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  } else {
+  }
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      // based on 'straight lines have to be straight'
+      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float omega = distortion_coeffs[0];
+      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
+      
+      cv::Point2f newPoint(
+              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
+              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
+
+      pts_out.push_back(newPoint);
+    }
+  }
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+  }    
+   else {
     ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                   distortion_model.c_str());
     std::vector<cv::Point3f> homogenous_pts;
@@ -155,5 +289,5 @@ cv::Point2f distortPoint(
   return pts_out[0];
 }
 
-	} // namespace image_handler
+  } // namespace image_handler
 } // namespace msckf_vio

src/image_processor.cpp

@@ -42,6 +42,9 @@ ImageProcessor::~ImageProcessor() {
 }
 
 bool ImageProcessor::loadParameters() {
+
+  // debug parameters
+  nh.param<bool>("StreamPause", STREAMPAUSE, false);
   // Camera calibration parameters
   nh.param<string>("cam0/distortion_model",
       cam0.distortion_model, string("radtan"));
@@ -211,7 +214,9 @@ void ImageProcessor::stereoCallback(
     const sensor_msgs::ImageConstPtr& cam0_img,
     const sensor_msgs::ImageConstPtr& cam1_img) {
 
-  //cout << "==================================" << endl;
+    // stop playing bagfile if printing images
+  //if(STREAMPAUSE)
+   // nh.setParam("/play_bag_image", false);
 
   // Get the current image.
   cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -219,12 +224,27 @@ void ImageProcessor::stereoCallback(
   cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
 
+  ros::Time start_time = ros::Time::now();
+
+
+  cv::Mat new_cam0;
+  cv::Mat new_cam1;
+
+  image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  
+  new_cam0.copyTo(cam0_curr_img_ptr->image);
+  new_cam1.copyTo(cam1_curr_img_ptr->image);
+
+  //ROS_INFO("Publishing: %f",
+  //    (ros::Time::now()-start_time).toSec());
+
   // Build the image pyramids once since they're used at multiple places
   createImagePyramids();
 
   // Detect features in the first frame.
   if (is_first_img) {
-    ros::Time start_time = ros::Time::now();
+    start_time = ros::Time::now();
     initializeFirstFrame();
     //ROS_INFO("Detection time: %f",
     //    (ros::Time::now()-start_time).toSec());
@@ -237,7 +257,7 @@ void ImageProcessor::stereoCallback(
     //    (ros::Time::now()-start_time).toSec());
   } else {
     // Track the feature in the previous image.
-    ros::Time start_time = ros::Time::now();
+    start_time = ros::Time::now();
     trackFeatures();
     //ROS_INFO("Tracking time: %f",
     //    (ros::Time::now()-start_time).toSec());
@@ -245,6 +265,7 @@ void ImageProcessor::stereoCallback(
     // Add new features into the current image.
     start_time = ros::Time::now();
     addNewFeatures();
+    
     //ROS_INFO("Addition time: %f",
     //    (ros::Time::now()-start_time).toSec());
 
@@ -267,16 +288,18 @@ void ImageProcessor::stereoCallback(
   //    (ros::Time::now()-start_time).toSec());
 
   // Publish features in the current image.
-  ros::Time start_time = ros::Time::now();
+  start_time = ros::Time::now();
   publish();
   //ROS_INFO("Publishing: %f",
   //    (ros::Time::now()-start_time).toSec());
 
   // Update the previous image and previous features.
+  
   cam0_prev_img_ptr = cam0_curr_img_ptr;
   prev_features_ptr = curr_features_ptr;
   std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
 
+
   // Initialize the current features to empty vectors.
   curr_features_ptr.reset(new GridFeatures());
   for (int code = 0; code <
@@ -284,6 +307,10 @@ void ImageProcessor::stereoCallback(
     (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
   }
 
+    // stop playing bagfile if printing images
+  //if(STREAMPAUSE)
+  //  nh.setParam("/play_bag_image", true);
+
   return;
 }
 
@@ -580,6 +607,7 @@ void ImageProcessor::trackFeatures() {
     ++after_ransac;
   }
 
+
   // Compute the tracking rate.
   int prev_feature_num = 0;
   for (const auto& item : *prev_features_ptr)
@@ -659,6 +687,8 @@ void ImageProcessor::stereoMatch(
 
   // Further remove outliers based on the known
   // essential matrix.
+
+
   vector<cv::Point2f> cam0_points_undistorted(0);
   vector<cv::Point2f> cam1_points_undistorted(0);
   image_handler::undistortPoints(
@@ -668,6 +698,7 @@ void ImageProcessor::stereoMatch(
       cam1_points, cam1.intrinsics, cam1.distortion_model,
       cam1.distortion_coeffs, cam1_points_undistorted);
 
+
   double norm_pixel_unit = 4.0 / (
       cam0.intrinsics[0]+cam0.intrinsics[1]+
       cam1.intrinsics[0]+cam1.intrinsics[1]);

src/shrinkImage.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import sys
+import rospy
+import cv2
+from std_msgs.msg import String
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+
+class image_converter:
+
+  def __init__(self):
+    self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10)
+    self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10)
+
+    self.bridge = CvBridge()
+    self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0)
+    self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1)
+
+  def callback_cam0(self,data):
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+      print(e)
+
+    imgScale = 0.25
+    newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
+    newimg = cv2.resize(cv_image,(int(newX),int(newY)))
+
+    newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
+    newdata = data
+    newdata.height = newpub.height
+    newdata.width = newpub.width
+    newdata.step = newpub.step
+    newdata.data = newpub.data
+    try:
+      self.image0_pub.publish(newdata)
+    except CvBridgeError as e:
+      print(e)
+
+  def callback_cam1(self,data):
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+      print(e)
+
+    imgScale = 0.25
+    newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
+    newimg = cv2.resize(cv_image,(int(newX),int(newY)))
+
+    newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
+    newdata = data
+    newdata.height = newpub.height
+    newdata.width = newpub.width
+    newdata.step = newpub.step
+    newdata.data = newpub.data
+
+    try:
+      self.image1_pub.publish(newdata)
+    except CvBridgeError as e:
+      print(e)
+
+
+def main(args):
+  ic = image_converter()
+  rospy.init_node('image_converter', anonymous=True)
+  try:
+    rospy.spin()
+  except KeyboardInterrupt:
+    print("Shutting down")
+  cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    main(sys.argv)