Merge pull request #52 from SPARK/feature/TESSE_0.4

Feature/tesse 0.4

Author: marcusabate

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

param/TESSE/calibration.yaml

@@ -6,16 +6,16 @@ distortion_model: radial-tangential
 camera_rate_hz: 13.0
 camera_resolution: [720, 480] # width, height
 
-left_camera_intrinsics: [700, 700, 360, 240] # fu, fv, cu, cv
+left_camera_intrinsics: [415.69219381653056, 415.69219381653056, 360, 240] # fu, fv, cu, cv
 left_camera_distortion_coefficients: [0.0, 0.0, 0.0, 0.0] # k1, k2, p1, p2
 left_camera_extrinsics: [1.0, 0.0, 0.0, 0.0,
                          0.0, 1.0, 0.0, 0.0,
                          0.0, 0.0, 1.0, 0.0,
                          0.0, 0.0, 0.0, 1.0]
 
-right_camera_intrinsics: [700, 700, 360, 240] # fu, fv, cu, cv
+right_camera_intrinsics: [415.69219381653056, 415.69219381653056, 360, 240] # fu, fv, cu, cv
 right_camera_distortion_coefficients: [0.0, 0.0, 0.0, 0.0] # k1, k2, p1, p2
-right_camera_extrinsics: [1.0, 0.0, 0.0, 0.05,
+right_camera_extrinsics: [1.0, 0.0, 0.0, 0.1,
                           0.0, 1.0, 0.0, 0.0,
                           0.0, 0.0, 1.0, 0.0,
                           0.0, 0.0, 0.0, 1.0]

param/TESSE/flags/StereoVIO.flags

@@ -2,17 +2,17 @@
 --backend_type=1
 --regular_vio_backend_modality=0
 --deterministic_random_number_generator=true
---visualize=true
+--visualize=false
 --visualize_lmk_type=false
---visualize_mesh=true
+--visualize_mesh=false
 --visualize_mesh_with_colored_polygon_clusters=false
---visualize_point_cloud=true
+--visualize_point_cloud=false
 --visualize_convex_hull=false
 --visualize_plane_constraints=false
 --visualize_planes=false
 --visualize_plane_label=false
 --visualize_semantic_mesh=false
---visualize_mesh_in_frustum=true
+--visualize_mesh_in_frustum=false
 #--visualize_load_mesh_filename=/home/tonirv/Downloads/tesse_multiscene_office1_3d_semantic_v4.ply
 --save_frontend_images_option=0
 --viz_type=3

param/TESSE/trackerParameters.yaml

@@ -12,7 +12,7 @@ block_size: 3
 use_harris_detector: 0
 k: 0.04
 equalizeImage: 0
-nominalBaseline: 0.05
+nominalBaseline: 0.1
 toleranceTemplateMatching: 0.15
 templ_cols: 101 #must be odd
 templ_rows: 11

rviz/sparkvio_tesse.rviz renamed to rviz/spark_vio_tesse.rviz

@@ -5,7 +5,6 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /TF1
-        - /TF1/Status1
         - /TF1/Frames1
         - /TF1/Tree1
         - /3D Mesh Textured1
@@ -113,6 +112,8 @@ Visualization Manager:
           Value: true
         base_link_spark:
           Value: true
+        left_cam:
+          Value: true
         right_cam:
           Value: true
         world:
@@ -124,6 +125,11 @@ Visualization Manager:
       Show Names: true
       Tree:
         world:
+          base_link_gt:
+            left_cam:
+              {}
+            right_cam:
+              {}
           base_link_spark:
             {}
       Update Interval: 0
@@ -239,7 +245,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 8.306986808776855
+      Distance: 93.48534393310547
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -254,10 +260,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.784796416759491
+      Pitch: 1.329796552658081
       Target Frame: base_link
       Value: ThirdPersonFollower (rviz)
-      Yaw: 1.6003519296646118
+      Yaw: 4.328537464141846
     Saved:
       - Class: rviz/ThirdPersonFollower
         Distance: 6.814720153808594

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'