added minor visualization changes

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

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 -->

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