From 010db87e4b1b9f69d38e19a1b1627a20cb2d0e79 Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Thu, 27 Jun 2019 16:28:45 +0200
Subject: [PATCH] tinytum works completely, image_handler equidistant
 distort/undistort work

---
 config/camchain-imucam-tum-scaled.yaml | 18 +++++++++---------
 include/msckf_vio/image_handler.h      | 10 ++++++++++
 launch/image_processor_tinytum.launch  |  4 ++--
 launch/msckf_vio_tinytum.launch        | 13 +++++--------
 launch/msckf_vio_tum.launch            |  6 +++---
 5 files changed, 29 insertions(+), 22 deletions(-)

diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml
index 529828c..4facacc 100644
--- a/config/camchain-imucam-tum-scaled.yaml
+++ b/config/camchain-imucam-tum-scaled.yaml
@@ -5,11 +5,11 @@ cam0:
       -0.029453096117288798,  -0.9989836729399656,  0.034119442089241274,  -0.0697294754693238,
       0.0,                    0.0,                  0.0,                   1.0]
   camera_model: pinhole
-  distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
-    -0.001662284667857643]
+  distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
+    0.00020293673591811182]
   distortion_model: equidistant
-  intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763]
-  resolution: [256, 256]
+  intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
+  resolution: [512, 512]
   rostopic: /cam0/image_raw
 cam1:
   T_cam_imu:
@@ -23,14 +23,14 @@ cam1:
     0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
     0.0, 0.0, 0.0, 1.0]
   camera_model: pinhole
-  distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
-    -0.002347858896562788]
+  distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
+    0.0003299517423931039]
   distortion_model: equidistant
-  intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044]
-  resolution: [256, 256]
+  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]
\ No newline at end of file
+  0.0000, 0.0000, 0.0000, 1.0000]
diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h
index d664be6..3cf2ed2 100644
--- a/include/msckf_vio/image_handler.h
+++ b/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,
diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch
index 6fe2855..53dd99a 100644
--- a/launch/image_processor_tinytum.launch
+++ b/launch/image_processor_tinytum.launch
@@ -25,8 +25,8 @@
       <param name="stereo_threshold" value="5"/>
 
       <remap from="~imu" to="/imu0"/>
-      <remap from="~cam0_image" to="/cam0/new_image_raw"/>
-      <remap from="~cam1_image" to="/cam1/new_image_raw"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
 
     </node>
   </group>
diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch
index 94deb7c..06d4c7f 100644
--- a/launch/msckf_vio_tinytum.launch
+++ b/launch/msckf_vio_tinytum.launch
@@ -6,7 +6,7 @@
     default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
 
   <!-- Image Processor Nodelet  -->
-  <include file="$(find msckf_vio)/launch/image_processor_tum.launch">
+  <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>
@@ -18,11 +18,11 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="5"/>
@@ -64,16 +64,13 @@
 
       <remap from="~imu" to="/imu0"/>
       <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
-      <remap from="~cam0_image" to="/cam0/new_image_raw"/>
-      <remap from="~cam1_image" to="/cam1/new_image_raw"/>
+      <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>
 
-  <node name="scaler" pkg="msckf_vio" type="shrinkImage.py"/>
-  <node name="player" pkg="bagcontrol" type="control.py" />
-
 
 </launch>
diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 390ab03..0240de3 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -18,11 +18,11 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="5"/>
@@ -72,6 +72,6 @@
     </node>
   </group>
 
-  <node name="player" pkg="bagcontrol" type="control.py" />
+  <!--node name="player" pkg="bagcontrol" type="control.py" /-->
 
 </launch>