From e94d4a06b50b1069863351b44b5323375fda619d Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Tue, 16 Jul 2019 09:53:30 +0200
Subject: [PATCH] added image rescaling factor

---
 include/msckf_vio/msckf_vio.h         |  3 ++-
 launch/image_processor_tinytum.launch |  6 +++---
 launch/msckf_vio_euroc.launch         |  7 +++---
 launch/msckf_vio_tinytum.launch       |  4 +++-
 src/msckf_vio.cpp                     | 31 +++++++++++++++------------
 5 files changed, 29 insertions(+), 22 deletions(-)

diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h
index c7fdcea..adbf6cc 100644
--- a/include/msckf_vio/msckf_vio.h
+++ b/include/msckf_vio/msckf_vio.h
@@ -286,7 +286,8 @@ class MsckfVio {
 
     // Patch size for Photometry
     int N;
-
+    // Image rescale
+    int SCALE; 
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 
diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch
index 26ceacd..ec463d9 100644
--- a/launch/image_processor_tinytum.launch
+++ b/launch/image_processor_tinytum.launch
@@ -27,9 +27,9 @@
       <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"/>
+      <remap from="~imu" to="/imu02"/>
+      <remap from="~cam0_image" to="/cam0/image_raw2"/>
+      <remap from="~cam1_image" to="/cam1/image_raw2"/>
 
 
     </node>
diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch
index 9e8ef7a..0df8102 100644
--- a/launch/msckf_vio_euroc.launch
+++ b/launch/msckf_vio_euroc.launch
@@ -19,14 +19,15 @@
 
 
       <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
-      <param name="FILTER" value="2"/>
+      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="5"/>
+      <param name="image_scale" value ="1"/>
 
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch
index 28ae944..196c72e 100644
--- a/launch/msckf_vio_tinytum.launch
+++ b/launch/msckf_vio_tinytum.launch
@@ -25,7 +25,9 @@
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="3"/>
+      <param name="image_scale" value ="1"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index eb33e53..bfbe609 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -137,6 +137,8 @@ bool MsckfVio::loadParameters() {
   // Get the photometric patch size
   nh.param<int>("patch_size_n",
       N, 3);
+  // Get rescaling factor for image
+  nh.param<int>("image_scale", SCALE, 1);
 
   // get camera information (used for back projection)
   nh.param<string>("cam0/distortion_model",
@@ -425,10 +427,10 @@ void MsckfVio::imageCallback(
   // the origin.
   if (is_first_img) {
     is_first_img = false;
-    cam0.intrinsics *=0.5;
-    cam1.intrinsics *=0.5;
-    cam0.resolution *=0.5;
-    cam1.resolution *=0.5;
+    //cam0.intrinsics *=SCALE;
+    //cam1.intrinsics *=SCALE;
+    //cam0.resolution *=SCALE;
+    //cam1.resolution *=SCALE;
     state_server.imu_state.time = feature_msg->header.stamp.toSec();
   }
   static double max_processing_time = 0.0;
@@ -550,22 +552,23 @@ void MsckfVio::manageMovingWindow(
 
   cv::Mat new_cam0;
   cv::Mat new_cam1;
-  cam0.intrinsics *=2;
-  cam1.intrinsics *=2;
-  cam0.resolution *=2;
-  cam1.resolution *=2;
+  //cam0.intrinsics *=float(1./SCALE);
+  //cam1.intrinsics *=float(1./SCALE);
+  //cam0.resolution *=float(1./SCALE);
+  //cam1.resolution *=float(1./SCALE);
   image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
   image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
-  cam0.intrinsics *=0.5;
-  cam1.intrinsics *=0.5;
-  cam0.resolution *=0.5;
-  cam1.resolution *=0.5;
+  //cam0.intrinsics *=SCALE;
+  //cam1.intrinsics *=SCALE;
+  //cam0.resolution *=SCALE;
+  //cam1.resolution *=SCALE;
   //resize
 
+  int SCALE = 1;yy
   cv::Mat new_cam02;
   cv::Mat new_cam12;
-  cv::resize(new_cam0, new_cam02, cv::Size(), 0.5, 0.5);
-  cv::resize(new_cam1, new_cam12, cv::Size(), 0.5, 0.5);
+  cv::resize(new_cam0, new_cam02, cv::Size(), SCALE, SCALE);
+  cv::resize(new_cam1, new_cam12, cv::Size(), SCALE, SCALE);
 
   // save image information into moving window
   cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone();