From 5c764ae201be9e9b31de184b154f1d51af4fb575 Mon Sep 17 00:00:00 2001
From: Zachary Taylor <zacharyjeremytaylor@gmail.com>
Date: Sun, 31 Mar 2019 20:09:05 +0200
Subject: [PATCH 1/2] src/undistorter.cpp

---
 launch/dense_stereo.launch | 6 +++---
 src/undistorter.cpp        | 1 +
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/launch/dense_stereo.launch b/launch/dense_stereo.launch
index 1436d39..6a5e38c 100644
--- a/launch/dense_stereo.launch
+++ b/launch/dense_stereo.launch
@@ -3,7 +3,7 @@
 <arg name="mav_name" default="ibis" />
 <arg name="namespace" default="$(arg mav_name)" />
 <arg name="process_every_nth_frame" default="1" />
-<arg name="stereo_params_camchain" default="$(find mav_startup)/parameters/mavs/ibis/camchain_p22031.yaml"/>
+<arg name="stereo_params_camchain" default="/home/z/Downloads/pinhole-equi-512/camchain-imucam-imucalib.yaml"/>
 
 <group ns="$(arg namespace)">
  
@@ -20,8 +20,8 @@
       <param name="depth/use_mode_HH" value="true"/>
       <rosparam file="$(arg stereo_params_camchain)"/>
   
-      <remap from="raw/first/image" to="/$(arg mav_name)/cam0/image_raw"/>
-      <remap from="raw/second/image" to="/$(arg mav_name)/cam1/image_raw"/>
+      <remap from="raw/first/image" to="/cam0/image_raw"/>
+      <remap from="raw/second/image" to="/cam1/image_raw"/>
       <remap from="raw/first/camera_info" to="/$(arg mav_name)/cam0/camera_info"/>
       <remap from="raw/second/camera_info" to="/$(arg mav_name)/cam1/camera_info"/>
     </node>
diff --git a/src/undistorter.cpp b/src/undistorter.cpp
index 9d02809..3a3c7f7 100644
--- a/src/undistorter.cpp
+++ b/src/undistorter.cpp
@@ -85,6 +85,7 @@ void Undistorter::distortPixel(const Eigen::Matrix<double, 3, 3>& K_in,
   // Transform image coordinates to be size and focus independent
   Eigen::Vector3d norm_pixel_location =
       R_in * P_out.topLeftCorner<3, 3>().inverse() * pixel_location_3;
+  norm_pixel_location /= norm_pixel_location.z();
 
   const double& x = norm_pixel_location.x();
   const double& y = norm_pixel_location.y();

From 41a93253a1615591ac3b65ab4b471cd224ac4f79 Mon Sep 17 00:00:00 2001
From: Zachary Taylor <zacharyjeremytaylor@gmail.com>
Date: Sun, 31 Mar 2019 20:12:26 +0200
Subject: [PATCH 2/2] undo launchfile changes

---
 launch/dense_stereo.launch | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/launch/dense_stereo.launch b/launch/dense_stereo.launch
index 6a5e38c..1436d39 100644
--- a/launch/dense_stereo.launch
+++ b/launch/dense_stereo.launch
@@ -3,7 +3,7 @@
 <arg name="mav_name" default="ibis" />
 <arg name="namespace" default="$(arg mav_name)" />
 <arg name="process_every_nth_frame" default="1" />
-<arg name="stereo_params_camchain" default="/home/z/Downloads/pinhole-equi-512/camchain-imucam-imucalib.yaml"/>
+<arg name="stereo_params_camchain" default="$(find mav_startup)/parameters/mavs/ibis/camchain_p22031.yaml"/>
 
 <group ns="$(arg namespace)">
  
@@ -20,8 +20,8 @@
       <param name="depth/use_mode_HH" value="true"/>
       <rosparam file="$(arg stereo_params_camchain)"/>
   
-      <remap from="raw/first/image" to="/cam0/image_raw"/>
-      <remap from="raw/second/image" to="/cam1/image_raw"/>
+      <remap from="raw/first/image" to="/$(arg mav_name)/cam0/image_raw"/>
+      <remap from="raw/second/image" to="/$(arg mav_name)/cam1/image_raw"/>
       <remap from="raw/first/camera_info" to="/$(arg mav_name)/cam0/camera_info"/>
       <remap from="raw/second/camera_info" to="/$(arg mav_name)/cam1/camera_info"/>
     </node>