From 9f528c1ea13e00d11c30352e5ce6683a7219838a Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Thu, 27 Jun 2019 18:25:43 +0200
Subject: [PATCH] added support for euroc dataset

---
 launch/msckf_vio_euroc.launch | 11 +++++++++++
 src/image_handler.cpp         | 12 ++++++++----
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch
index 896feff..5f6de1e 100644
--- a/launch/msckf_vio_euroc.launch
+++ b/launch/msckf_vio_euroc.launch
@@ -17,6 +17,17 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="false"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="5"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
diff --git a/src/image_handler.cpp b/src/image_handler.cpp
index 8664922..f174fb7 100644
--- a/src/image_handler.cpp
+++ b/src/image_handler.cpp
@@ -40,6 +40,10 @@ void undistortImage(
       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(
@@ -91,7 +95,7 @@ void undistortPoint(
       pts_out.push_back(newPoint);
     }
    }
-  else if (distortion_model == "pre-equidistant")
+  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++)
@@ -152,7 +156,7 @@ void undistortPoints(
       pts_out.push_back(newPoint);
     }
   }
-  else if (distortion_model == "pre-equidistant")
+  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++)
@@ -205,7 +209,7 @@ std::vector<cv::Point2f> distortPoints(
       pts_out.push_back(newPoint);
     }
   }
-  else if (distortion_model == "pre-equidistant")
+  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++)
@@ -262,7 +266,7 @@ cv::Point2f distortPoint(
       pts_out.push_back(newPoint);
     }
   }
-  else if (distortion_model == "pre-equidistant")
+  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++)