Open source/feature/rviz display

CMakeLists.txt

@@ -20,6 +20,9 @@ cs_add_library(${PROJECT_NAME}
   src/RosDataProviderInterface.cpp
   src/RosBagDataProvider.cpp
   src/RosOnlineDataProvider.cpp
+  src/RosDisplay.cpp
+  src/RosVisualizer.cpp
+  src/utils/UtilsRos.cpp
 )
 target_link_libraries(${PROJECT_NAME} kimera_vio)
 

include/kimera_vio_ros/KimeraVioRos.h

@@ -11,6 +11,8 @@
 #include <kimera-vio/utils/Macros.h>
 
 #include "kimera_vio_ros/RosDataProviderInterface.h"
+#include "kimera_vio_ros/RosDisplay.h"
+#include "kimera_vio_ros/RosVisualizer.h"
 
 namespace VIO {
 
@@ -31,7 +33,7 @@ class KimeraVioRos {
   VIO::RosDataProviderInterface::UniquePtr createDataProvider(
       const VioParams& vio_params);
 
-  void connectVioPipelineAndDataProvider();
+  void connectVIO();
 
   /**
    * @brief restartKimeraVio Callback for the rosservice to restart the pipeline
@@ -52,6 +54,8 @@ class KimeraVioRos {
 
   //! Data provider
   RosDataProviderInterface::UniquePtr data_provider_;
+  RosDisplay::UniquePtr ros_display_;
+  RosVisualizer::UniquePtr ros_visualizer_;
 
   //! ROS Services
   ros::ServiceServer restart_vio_pipeline_srv_;

include/kimera_vio_ros/RosBagDataProvider.h

@@ -8,10 +8,10 @@
 #pragma once
 
 #include <functional>
-#include <opencv2/core/core.hpp>
-#include <opencv2/core/matx.hpp>
 #include <string>
 
+#include <opencv2/opencv.hpp>
+
 #include <nav_msgs/Odometry.h>
 #include <ros/console.h>
 #include <ros/ros.h>
@@ -20,22 +20,22 @@
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 
-#include "kimera_vio_ros/RosDataProviderInterface.h"
+#include <kimera-vio/pipeline/Pipeline-definitions.h>
 
-#include <kimera-vio/pipeline/Pipeline-definitions.h>.h>
+#include "kimera_vio_ros/RosDataProviderInterface.h"
 
 namespace VIO {
 
 struct RosbagData {
-  // IMU messages
+  /// IMU messages
   std::vector<sensor_msgs::ImuConstPtr> imu_msgs_;
-  // The names of the images from left camera
+  /// The names of the images from left camera
   std::vector<sensor_msgs::ImageConstPtr> left_imgs_;
-  // The names of the images from right camera
+  /// The names of the images from right camera
   std::vector<sensor_msgs::ImageConstPtr> right_imgs_;
-  // Vector of timestamps see issue in .cpp file
+  /// Vector of timestamps see issue in .cpp file
   std::vector<Timestamp> timestamps_;
-  // Ground-truth Odometry (only if available)
+  /// Ground-truth Odometry (only if available)
   std::vector<nav_msgs::OdometryConstPtr> gt_odometry_;
 };
 
@@ -64,7 +64,7 @@ class RosbagDataProvider : public RosDataProviderInterface {
   // by the sequential order in the rosbag).
   VioNavState getGroundTruthVioNavState(const size_t& k_frame) const;
 
-  void publishBackendOutput(const BackendOutput::Ptr& output) override;
+  void publishRosbagInfo(const Timestamp& timestamp);
 
   // Publish clock
   void publishClock(const Timestamp& timestamp) const;

include/kimera_vio_ros/RosDataProviderInterface.h

@@ -10,44 +10,18 @@
 #include <functional>
 #include <string>
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/core/matx.hpp>
-
-#define PCL_NO_PRECOMPILE  // Define this before you include any PCL headers
-                           // to include the templated algorithms
-#include <pcl/point_types.h>
-#include <pcl_ros/point_cloud.h>
+#include <opencv2/opencv.hpp>
 
 #include <image_transport/subscriber_filter.h>
-#include <pose_graph_tools/PoseGraph.h>
-#include <pose_graph_tools/PoseGraphEdge.h>
-#include <pose_graph_tools/PoseGraphNode.h>
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 
-#include <kimera-vio/common/vio_types.h>
-#include <kimera-vio/pipeline/Pipeline-definitions.h>
 #include <kimera-vio/dataprovider/DataProviderInterface.h>
-#include <kimera-vio/frontend/StereoFrame.h>
-#include <kimera-vio/frontend/StereoImuSyncPacket.h>
-#include <kimera-vio/frontend/StereoMatchingParams.h>
-#include <kimera-vio/frontend/VisionFrontEndParams.h>
-#include <kimera-vio/loopclosure/LoopClosureDetector-definitions.h>
-#include <kimera-vio/mesh/Mesher-definitions.h>
-#include <kimera-vio/utils/ThreadsafeQueue.h>
+#include <kimera-vio/pipeline/Pipeline-definitions.h>
 
-namespace VIO {
+#include "kimera_vio_ros/RosPublishers.h"
 
-/**
- * @brief The PointNormalUV struct holds mesh vertex data.
- */
-struct PointNormalUV {
-  PCL_ADD_POINT4D;
-  PCL_ADD_NORMAL4D;
-  float u;  // Texture coordinates.
-  float v;
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-} EIGEN_ALIGN16;
+namespace VIO {
 
 class RosDataProviderInterface : public DataProviderInterface {
  public:
@@ -59,32 +33,9 @@ class RosDataProviderInterface : public DataProviderInterface {
   virtual ~RosDataProviderInterface();
 
  public:
-  inline void callbackBackendOutput(const VIO::BackendOutput::Ptr& output) {
-    backend_output_queue_.push(output);
-  }
-
-  inline void callbackFrontendOutput(const VIO::FrontendOutput::Ptr& output) {
-    // TODO(Toni): pushing twice to two different queues bcs we are missing
-    // the functionality to ".front()" a queue, now we can just pop()...
-    // Perhaps we should make all our threadsafe queues temporally aware
-    // (meaning you can query the time of the message directly)...
-    frame_rate_frontend_output_queue_.push(output);
-    if (output && output->is_keyframe_) {
-      keyframe_rate_frontend_output_queue_.push(output);
-    }
-  }
-
-  inline void callbackMesherOutput(const VIO::MesherOutput::Ptr& output) {
-    mesher_output_queue_.push(output);
-  }
-
-  inline void callbackLcdOutput(const VIO::LcdOutput::Ptr& output) {
-    lcd_output_queue_.push(output);
-  }
-
   void shutdown() override {
     DataProviderInterface::shutdown();
-    shutdownQueues();
+    LOG(INFO) << "RosDataProviderInterface shutdown.";
   }
 
  protected:
@@ -93,141 +44,19 @@ class RosDataProviderInterface : public DataProviderInterface {
   const cv::Mat readRosDepthImage(
       const sensor_msgs::ImageConstPtr& img_msg) const;
 
-  // Publish VIO outputs.
-  virtual void publishBackendOutput(const BackendOutput::Ptr& output);
-
-  virtual void publishFrontendOutput(const FrontendOutput::Ptr& output) const;
-
-  virtual void publishMesherOutput(const MesherOutput::Ptr& output) const;
-
-  virtual bool publishSyncedOutputs();
-
-  // Publish all outputs for LCD
-  // TODO(marcus): make like other outputs
-  virtual void publishLcdOutput(const LcdOutput::Ptr& lcd_output);
-
-  // Publish static transforms (for camera frames) to the tf tree
-  void publishStaticTf(const gtsam::Pose3& pose,
-                       const std::string& parent_frame_id,
-                       const std::string& child_frame_id);
-
-  virtual void shutdownQueues() {
-    backend_output_queue_.shutdown();
-    frame_rate_frontend_output_queue_.shutdown();
-    keyframe_rate_frontend_output_queue_.shutdown();
-    mesher_output_queue_.shutdown();
-    lcd_output_queue_.shutdown();
-  }
-
- protected:
-  void msgTFtoPose(const geometry_msgs::Transform& tf, gtsam::Pose3* pose);
-
-  void poseToMsgTF(const gtsam::Pose3& pose, geometry_msgs::Transform* tf);
-
-  void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info,
-                                const std::string& cam_frame_id,
-                                VIO::CameraParams* cam_params);
-
  protected:
   // Define Node Handler for general use (Parameter server)
   ros::NodeHandle nh_;
   ros::NodeHandle nh_private_;
 
-  // Define image transport for this and derived classes.
-  std::unique_ptr<image_transport::ImageTransport> it_;
-
-  // Define frame ids for odometry message
-  std::string world_frame_id_;
-  std::string base_link_frame_id_;
-  std::string map_frame_id_;
-  std::string left_cam_frame_id_;
-  std::string right_cam_frame_id_;
-
-  // Queues to store and retrieve VIO output in a thread-safe way.
-  ThreadsafeQueue<BackendOutput::Ptr> backend_output_queue_;
-  ThreadsafeQueue<FrontendOutput::Ptr> frame_rate_frontend_output_queue_;
-  ThreadsafeQueue<FrontendOutput::Ptr> keyframe_rate_frontend_output_queue_;
-  ThreadsafeQueue<MesherOutput::Ptr> mesher_output_queue_;
-  ThreadsafeQueue<LcdOutput::Ptr> lcd_output_queue_;
-
   // TODO(Toni): technically the dataprovider should not need these, but
   // I still haven't removed the requirement to send camera params with each
   // vio callback...
   // Pipeline params
   VioParams vio_params_;
 
  private:
-  void publishTf(const BackendOutput::Ptr& output);
-
-  void publishTimeHorizonPointCloud(const BackendOutput::Ptr& output) const;
-
-  void publishPerFrameMesh3D(const MesherOutput::Ptr& output) const;
-
-  void publishTimeHorizonMesh3D(const MesherOutput::Ptr& output) const;
-
-  void publishState(const BackendOutput::Ptr& output) const;
-
-  void publishFrontendStats(const FrontendOutput::Ptr& output) const;
-
-  void publishResiliency(const FrontendOutput::Ptr& frontend_output,
-                         const BackendOutput::Ptr& backend_output) const;
-
-  void publishImuBias(const BackendOutput::Ptr& output) const;
-
-  // Publish LCD/PGO outputs.
-  void publishTf(const LcdOutput::Ptr& lcd_output);
-
-  void publishOptimizedTrajectory(const LcdOutput::Ptr& lcd_output) const;
-
-  void publishPoseGraph(const LcdOutput::Ptr& lcd_output);
-
-  void updateNodesAndEdges(const gtsam::NonlinearFactorGraph& nfg,
-                           const gtsam::Values& values);
-
-  void updateRejectedEdges();
-
-  pose_graph_tools::PoseGraph getPosegraphMsg();
-
-  // Publish/print debugging information.
-  void publishDebugImage(const Timestamp& timestamp,
-                         const cv::Mat& debug_image) const;
-
   void printParsedParams() const;
-
- private:
-  // Define publisher for debug images.
-  image_transport::Publisher debug_img_pub_;
-  image_transport::Publisher feature_tracks_pub_;
-
-  // Publishers
-  ros::Publisher pointcloud_pub_;
-  // Published 3d mesh per frame (not time horizon of opitimization!)
-  ros::Publisher mesh_3d_frame_pub_;
-  ros::Publisher odometry_pub_;
-  ros::Publisher resiliency_pub_;
-  ros::Publisher frontend_stats_pub_;
-  ros::Publisher imu_bias_pub_;
-  ros::Publisher trajectory_pub_;
-  ros::Publisher posegraph_pub_;
-
-  // Define tf broadcaster for world to base_link (IMU) and to map (PGO).
-  tf::TransformBroadcaster tf_broadcaster_;
-
-  // Stored pose graph related objects
-  std::vector<pose_graph_tools::PoseGraphEdge> loop_closure_edges_;
-  std::vector<pose_graph_tools::PoseGraphEdge> odometry_edges_;
-  std::vector<pose_graph_tools::PoseGraphEdge> inlier_edges_;
-  std::vector<pose_graph_tools::PoseGraphNode> pose_graph_nodes_;
-
-  // Typedefs
-  typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudXYZRGB;
 };
 
 }  // namespace VIO
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-    VIO::PointNormalUV,
-    (float, x, x)(float, y, y)(float, x, z)(float, normal_x, normal_x)(
-        float,
-        normal_y,
-        normal_y)(float, normal_z, normal_z)(float, u, u)(float, v, v))

include/kimera_vio_ros/RosDisplay.h

@@ -0,0 +1,54 @@
+/**
+ * @file   RosDisplay.h
+ * @brief  Publishes 2D data sent in the display_queue in Kimera. This publishes
+ * images at any rate (frame rate, keyframe rate,...).
+ * @author Antoni Rosinol
+ */
+#pragma once
+
+#include <string>
+
+#include <ros/ros.h>
+
+#include <kimera-vio/visualizer/Display.h>
+
+#include "kimera_vio_ros/RosPublishers.h"
+
+namespace VIO {
+
+class RosDisplay : public DisplayBase {
+ public:
+  KIMERA_POINTER_TYPEDEFS(RosDisplay);
+  KIMERA_DELETE_COPY_CONSTRUCTORS(RosDisplay);
+
+ public:
+  RosDisplay();
+  virtual ~RosDisplay() = default;
+
+ public:
+  /**
+   * @brief spinOnce
+   * Spins the display once to render the visualizer output.
+   * @param viz_output
+   */
+  void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override;
+
+ protected:
+  /**
+   * @brief spin2dWindow Publishes images to ROS
+   * @param viz_output Input from the display queue
+   */
+  void spin2dWindow(const DisplayInputBase& viz_output);
+
+ private:
+  //! Ros
+  ros::NodeHandle nh_private_;
+
+  //! Define image publishers manager
+  std::unique_ptr<ImagePublishers> image_publishers_;
+
+  //! Define frame ids for odometry message
+  std::string base_link_frame_id_;
+};
+
+}  // namespace VIO