From ee40dff740ff22921f5d778eca18f57d5dd8f7e0 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Thu, 2 May 2019 17:02:44 +0200
Subject: [PATCH] added minor visualization changes

---
 include/msckf_vio/feature.hpp |  6 ++++++
 launch/msckf_vio_tum.launch   |  2 +-
 src/msckf_vio.cpp             | 10 ++++++++++
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index f278b37..76e4eca 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -491,6 +491,12 @@ bool Feature::VisualizePatch(
 
   cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
 
+  // write feature position
+  std::stringstream pos_s;
+  pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
+  cv::putText(cam0.featureVisu, pos_s.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
+    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
+
   // create line?
 
   //save image
diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 0032c70..4b9666d 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -19,7 +19,7 @@
 
       <!-- Photometry Flag-->
       <param name="PHOTOMETRIC" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
 
       <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 965484d..1998f1b 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1836,12 +1836,22 @@ void MsckfVio::publish(const ros::Time& time) {
   Eigen::Vector3d body_velocity =
     IMUState::T_imu_body.linear() * imu_state.velocity;
 
+  // Generate camera frame form IMU Frame
+  Eigen::Isometry3d T_i_c = Eigen::Isometry3d::Identity();
+  T_i_c.linear() =  imu_state.R_imu_cam0;
+  T_i_c.translation() = imu_state.t_cam0_imu;
+
   // Publish tf
   if (publish_tf) {
     tf::Transform T_b_w_tf;
     tf::transformEigenToTF(T_b_w, T_b_w_tf);
     tf_pub.sendTransform(tf::StampedTransform(
           T_b_w_tf, time, fixed_frame_id, child_frame_id));
+
+    tf::Transform T_i_c_tf;
+    tf::transformEigenToTF(T_i_c, T_i_c_tf);
+    tf_pub.sendTransform(tf::StampedTransform(
+          T_i_c_tf, time, child_frame_id, "camera"));
   }
 
   // Publish the odometry