Add static tfs for left/right cams, latch mesh 3d and pcl

Author: ToniRV

include/spark-vio-ros/base-data-source.h

@@ -75,6 +75,9 @@ class RosBaseDataProvider : public DataProvider {
   // Publish all outputs by calling individual functions below
   void publishOutput(const SpinOutputPacket& vio_output);
 
+  void publishStaticTf(const gtsam::Pose3& pose,
+                       const std::string& parent_frame_id,
+                       const std::string& child_frame_id);
 
  protected:
   VioFrontEndParams frontend_params_;
@@ -91,6 +94,9 @@ class RosBaseDataProvider : public DataProvider {
   // Define frame ids for odometry message
   std::string world_frame_id_;
   std::string base_link_frame_id_;
+  std::string left_cam_frame_id_;
+  std::string right_cam_frame_id_;
+
 
   // Queue to store and retrieve VIO output in a thread-safe way.
   ThreadsafeQueue<SpinOutputPacket> vio_output_queue_;

launch/spark_vio_ros.launch

@@ -39,14 +39,16 @@
   <!-- Frame IDs -->
   <arg name="world_frame_id"     default="world"/>
   <arg name="base_link_frame_id" default="base_link"/>
+  <arg name="left_cam_frame_id"  default="left_cam"/>
+  <arg name="right_cam_frame_id" default="right_cam"/>
 
   <!-- General parameters -->
   <arg name="verbosity"    default="0" />
   <!-- 0: for vanilla VIO, 1: for regular VIO -->
   <arg name="backend_type" default="1" />
   <!-- Visualize pipeline output in OpenCV. -->
   <!-- Note that this is duplicated from the flags file -->
-  <arg name="visualize"    default="true" />
+  <arg name="visualize"    default="false" />
 
   <!-- Launch main node  -->
   <node name="spark_vio_ros" pkg="spark_vio_ros" type="spark_vio_ros" output="screen"
@@ -70,6 +72,8 @@
     <!-- Frame IDs for Odometry -->
     <param name="world_frame_id"     value="$(arg world_frame_id)"/>
     <param name="base_link_frame_id" value="$(arg base_link_frame_id)"/>
+    <param name="left_cam_frame_id"  value="$(arg left_cam_frame_id)"/>
+    <param name="right_cam_frame_id" value="$(arg right_cam_frame_id)"/>
 
     <!-- Subscriber topics -->
     <!-- if we run online, use remap -->
@@ -84,8 +88,6 @@
     <param name="ground_truth_odometry_rosbag_topic"
              value="$(arg odometry_ground_truth_topic)" unless="$(arg online)"/>
 
-ground_truth_odometry_rosbag_topic
-
     <!-- Other subscription topics -->
     <remap from="reinit_flag" to="/sparkvio/reinit_flag"/>
     <remap from="reinit_pose" to="/sparkvio/reinit_pose"/>

launch/spark_vio_ros_euroc.launch

@@ -25,10 +25,16 @@
   <arg name="base_link_frame_id" value="base_link"/>
 
   <!-- Launch static TF node from IMU to Cam0 for Euroc -->
+  <!-- TODO(Toni): this can be done programatically now, using the parsed
+  calibration files -->
+  <!--
   <node pkg="tf" type="static_transform_publisher" name="left_cam_broadcaster"
     args="-0.0216401454975 -0.064676986768 0.00981073058949
     -0.0077071797555374275 0.010499323370587278 0.7017528002919717 0.7123014606689537
     $(arg base_link_frame_id) cam0 100"/>
+    -->
+  <arg name="left_cam_frame_id"  value="cam0"/>
+  <arg name="right_cam_frame_id" value="cam1"/>
 
   <!-- Launch actual pipeline -->
   <include file="$(find spark_vio_ros)/launch/spark_vio_ros.launch"

launch/spark_vio_ros_tesse.launch

@@ -22,6 +22,8 @@
 
   <!-- Frame IDs -->
   <arg name="base_link_frame_id" value="base_link_spark"/>
+  <arg name="left_cam_frame_id"  value="left_cam_spark"/>
+  <arg name="right_cam_frame_id" value="right_cam_spark"/>
 
     <!-- Subscriber Topics -->
   <arg name="left_cam_topic"   value="/tesse/left_cam"/>

src/base-data-source.cpp

@@ -23,6 +23,8 @@
 #include <pcl_msgs/PolygonMesh.h>
 #include <pcl_ros/point_cloud.h>
 
+#include <tf2_ros/static_transform_broadcaster.h>
+
 namespace VIO {
 
 RosBaseDataProvider::RosBaseDataProvider()
@@ -59,18 +61,34 @@ RosBaseDataProvider::RosBaseDataProvider()
   CHECK(!base_link_frame_id_.empty());
   CHECK(nh_private_.getParam("world_frame_id", world_frame_id_));
   CHECK(!world_frame_id_.empty());
+  CHECK(nh_private_.getParam("left_cam_frame_id", left_cam_frame_id_));
+  CHECK(!left_cam_frame_id_.empty());
+  CHECK(nh_private_.getParam("right_cam_frame_id", right_cam_frame_id_));
+  CHECK(!right_cam_frame_id_.empty());
 
   // Publishers
-  odometry_pub_ = nh_.advertise<nav_msgs::Odometry>("odometry", 10);
+  odometry_pub_ = nh_.advertise<nav_msgs::Odometry>("odometry", 10, true);
   frontend_stats_pub_ =
       nh_.advertise<std_msgs::Float64MultiArray>("frontend_stats", 10);
   resiliency_pub_ =
       nh_.advertise<std_msgs::Float64MultiArray>("resiliency", 10);
   imu_bias_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("imu_bias", 10);
   pointcloud_pub_ =
-      nh_.advertise<PointCloudXYZRGB>("time_horizon_pointcloud", 10);
-  mesh_3d_frame_pub_ = nh_.advertise<pcl_msgs::PolygonMesh>("mesh", 5);
-  debug_img_pub_ = it_->advertise("debug_mesh_img", 10);
+      nh_.advertise<PointCloudXYZRGB>("time_horizon_pointcloud", 10, true);
+  mesh_3d_frame_pub_ = nh_.advertise<pcl_msgs::PolygonMesh>("mesh", 5, true);
+  debug_img_pub_ = it_->advertise("debug_mesh_img", 10, true);
+
+  // Static TFs for left/right cameras
+  const gtsam::Pose3& body_Pose_left_cam =
+      stereo_calib_.left_camera_info_.body_Pose_cam_;
+  const gtsam::Pose3& body_Pose_right_cam =
+      stereo_calib_.right_camera_info_.body_Pose_cam_;
+  publishStaticTf(body_Pose_left_cam,
+                  base_link_frame_id_,
+                  left_cam_frame_id_);
+  publishStaticTf(body_Pose_right_cam,
+                  base_link_frame_id_,
+                  right_cam_frame_id_);
 }
 
 RosBaseDataProvider::~RosBaseDataProvider() {}
@@ -743,6 +761,26 @@ void RosBaseDataProvider::publishImuBias(
   imu_bias_pub_.publish(imu_bias_msg);
 }
 
+void RosBaseDataProvider::publishStaticTf(const gtsam::Pose3& pose,
+                                          const std::string& parent_frame_id,
+                                          const std::string& child_frame_id) {
+  static tf2_ros::StaticTransformBroadcaster static_broadcaster;
+  geometry_msgs::TransformStamped static_transform_stamped;
+  // TODO(Toni): Warning: using ros::Time::now(), will that bring issues?
+  static_transform_stamped.header.stamp = ros::Time::now();
+  static_transform_stamped.header.frame_id = parent_frame_id;
+  static_transform_stamped.child_frame_id = child_frame_id;
+  static_transform_stamped.transform.translation.x = pose.x();
+  static_transform_stamped.transform.translation.y = pose.y();
+  static_transform_stamped.transform.translation.z = pose.z();
+  const gtsam::Quaternion& quat = pose.rotation().toQuaternion();
+  static_transform_stamped.transform.rotation.x = quat.x();
+  static_transform_stamped.transform.rotation.y = quat.y();
+  static_transform_stamped.transform.rotation.z = quat.z();
+  static_transform_stamped.transform.rotation.w = quat.w();
+  static_broadcaster.sendTransform(static_transform_stamped);
+}
+
 void RosBaseDataProvider::printParsedParams() const {
   LOG(INFO) << std::string(80, '=') << '\n'
             << ">>>>>>>>> RosDataProvider::print <<<<<<<<<<<" << '\n'