diff --git a/CMakeLists.txt b/CMakeLists.txt index 9120fbc6..295bcb61 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/kimera_vio_ros/KimeraVioRos.h b/include/kimera_vio_ros/KimeraVioRos.h index 8b5ce4c6..94bc4f71 100644 --- a/include/kimera_vio_ros/KimeraVioRos.h +++ b/include/kimera_vio_ros/KimeraVioRos.h @@ -11,6 +11,8 @@ #include #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_; diff --git a/include/kimera_vio_ros/RosBagDataProvider.h b/include/kimera_vio_ros/RosBagDataProvider.h index 8d2b119c..686d0482 100644 --- a/include/kimera_vio_ros/RosBagDataProvider.h +++ b/include/kimera_vio_ros/RosBagDataProvider.h @@ -8,10 +8,10 @@ #pragma once #include -#include -#include #include +#include + #include #include #include @@ -20,22 +20,22 @@ #include #include -#include "kimera_vio_ros/RosDataProviderInterface.h" +#include -#include .h> +#include "kimera_vio_ros/RosDataProviderInterface.h" namespace VIO { struct RosbagData { - // IMU messages + /// IMU messages std::vector imu_msgs_; - // The names of the images from left camera + /// The names of the images from left camera std::vector left_imgs_; - // The names of the images from right camera + /// The names of the images from right camera std::vector right_imgs_; - // Vector of timestamps see issue in .cpp file + /// Vector of timestamps see issue in .cpp file std::vector timestamps_; - // Ground-truth Odometry (only if available) + /// Ground-truth Odometry (only if available) std::vector 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; diff --git a/include/kimera_vio_ros/RosDataProviderInterface.h b/include/kimera_vio_ros/RosDataProviderInterface.h index 4bfb1f00..024430c8 100644 --- a/include/kimera_vio_ros/RosDataProviderInterface.h +++ b/include/kimera_vio_ros/RosDataProviderInterface.h @@ -10,44 +10,18 @@ #include #include -#include -#include - -#define PCL_NO_PRECOMPILE // Define this before you include any PCL headers - // to include the templated algorithms -#include -#include +#include #include -#include -#include -#include #include #include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include +#include -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,63 +44,11 @@ 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 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 backend_output_queue_; - ThreadsafeQueue frame_rate_frontend_output_queue_; - ThreadsafeQueue keyframe_rate_frontend_output_queue_; - ThreadsafeQueue mesher_output_queue_; - ThreadsafeQueue 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... @@ -157,77 +56,7 @@ class RosDataProviderInterface : public DataProviderInterface { 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 loop_closure_edges_; - std::vector odometry_edges_; - std::vector inlier_edges_; - std::vector pose_graph_nodes_; - - // Typedefs - typedef pcl::PointCloud 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)) diff --git a/include/kimera_vio_ros/RosDisplay.h b/include/kimera_vio_ros/RosDisplay.h new file mode 100644 index 00000000..c2c13dd2 --- /dev/null +++ b/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 + +#include + +#include + +#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 image_publishers_; + + //! Define frame ids for odometry message + std::string base_link_frame_id_; +}; + +} // namespace VIO diff --git a/include/kimera_vio_ros/RosOnlineDataProvider.h b/include/kimera_vio_ros/RosOnlineDataProvider.h index bbc76d6e..bb3e24bc 100644 --- a/include/kimera_vio_ros/RosOnlineDataProvider.h +++ b/include/kimera_vio_ros/RosOnlineDataProvider.h @@ -29,10 +29,7 @@ class RosOnlineDataProvider : public RosDataProviderInterface { virtual ~RosOnlineDataProvider(); - bool spin() override; - - bool spinOnce(); - + public: // Checks the current status of reinitialization flag inline bool getReinitFlag() const { return reinit_flag_; } // Resets the current status of reinitialization flag @@ -70,11 +67,20 @@ class RosOnlineDataProvider : public RosDataProviderInterface { // Reinitialization pose void callbackReinitPose(const geometry_msgs::PoseStamped& reinitPose); + // 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); + private: + // TODO(Toni): perhaps put in utils void msgGtOdomToVioNavState(const nav_msgs::Odometry::ConstPtr& gt_odom, VioNavState* vio_navstate); private: + // Define image transport for this and derived classes. + std::unique_ptr it_; + // Message filters and to sync stereo images typedef image_transport::SubscriberFilter ImageSubscriber; ImageSubscriber left_img_subscriber_; @@ -110,6 +116,11 @@ class RosOnlineDataProvider : public RosDataProviderInterface { // Ground-truth initialization pose received flag bool gt_init_pose_received_ = false; bool camera_info_received_ = false; + + // Frame ids + std::string base_link_frame_id_; + std::string left_cam_frame_id_; + std::string right_cam_frame_id_; }; } // namespace VIO diff --git a/include/kimera_vio_ros/RosPublishers.h b/include/kimera_vio_ros/RosPublishers.h new file mode 100644 index 00000000..a7815748 --- /dev/null +++ b/include/kimera_vio_ros/RosPublishers.h @@ -0,0 +1,152 @@ +#pragma once + +#include +#include + +#include + +#include +#include + +namespace VIO { + +/** +* @brief The RosPublishers class +* This is a convenience class to allow for publishing msgs of the same type in +* different ROS topics. +* IdxType: typically the name of the topic (which we use to index the map of +* publishers). It must be UNIQUE, or you will override previous publishers. +* MsgType: type of the message to be sent via the publishers. +* PublisherType = ros::Publisher, the actual ros publisher, it can be the +* classic +* ros::Publisher (default), or an image_transport::Publisher. +* NodeHandleType = ros::NodeHandle, the actual ros node handle, it can be the +* classic ros::NodeHandle (default), or a image_transport::ImageTransport. +*/ +template +class RosPublishers { + private: + //! Map from unique idx to ros Publisher (either a classic ros::Publisher or + //! a image_transport::Publisher). + using RosPubs = std::unordered_map; + + public: + /** + * @brief RosPublishers Publishes a msg in a different ROS topic + * defined by the LabelType. This is a convenience class + * to allow for publishing msgs of the same type in different ROS topics. + * @param nh_private Node handle to create the ROS publishers. + * @param base_topic_name String to be prepended to all the ROS topics + * advertised. It can be empty if need be. + * @param queue_size Size of the publishers' queues (default: 1). + * @param latch_msgs Set to true to send last msg to new subscribers (default: + * true). + */ + RosPublishers(const ros::NodeHandle& nh_private, + const std::string& base_topic_name = "", + const size_t& queue_size = 1u, + const bool& latch_msgs = true) + : base_topic_name_(base_topic_name), + queue_size_(queue_size), + latch_msgs_(latch_msgs), + nh_private_(nh_private), + pubs_() { + // TODO(Toni): add check on PublisherType and NodeHandleType, they can only + // be either ros::Publisher + ros::NodeHandle or image_transport::Publisher + // + image_transport::ImageTransport. Not other combinations. + } + virtual ~RosPublishers() = default; + + public: + // We don't pass queue_size, latch_msgs per function call bcs this is meant + // to be called even when the publisher already exists. + /** + * @brief publish + * Publishes a given ROS msg for a given semantic_label under the + * same ROS topic. + * If the given semantic_label has not been published before, a new ROS + * topic + * is advertised with the name given by topic_name_. + * @param idx Publisher index, this will be used as well in the topic + * name. + * @param msg Actual msg to be published + */ + void publish(const IdxType& idx, const MsgType& msg) { + // Try to find the publisher. + const auto& pub_it = pubs_.find(idx); + + // If publisher does not exist, we create a new one. + if (pub_it == pubs_.end()) { + pubs_[idx] = createPublisher(idx, &nh_private_); + } + + // Publish msg (use .at since pub_it might be invalid). + pubs_.at(idx).publish(msg); + } + + /** + * @brief getNumSubscribersForPublisher Get the number of subscribers to a + * given + * publisher + * @param idx Index of the publisher in the map (unique id). + * @return The number of subscribers to the publisher (NOTE: + * if there is no such publisher with index `idx` in our map of publishers, + * we will create one! + */ + size_t getNumSubscribersForPublisher(const IdxType& idx) { + const auto& pub_it = pubs_.find(idx); + if (pub_it == pubs_.end()) { + pubs_[idx] = createPublisher(idx, &nh_private_); + } + // Use .at(0 since pub_it might be invalid. + return pubs_.at(idx).getNumSubscribers(); + } + + protected: + // We need the two below because one advertise is templated, the other is not. + PublisherType createPublisher(const IdxType& idx, ros::NodeHandle* nh) const { + return CHECK_NOTNULL(nh)->advertise( + base_topic_name_ + safeToString(idx), queue_size_, latch_msgs_); + } + PublisherType createPublisher(const IdxType& idx, + image_transport::ImageTransport* it) const { + return CHECK_NOTNULL(it)->advertise( + base_topic_name_ + safeToString(idx), queue_size_, latch_msgs_); + } + + private: + // The two below is to get around the lack of template for std::to_string + // for an actual string :/ + static std::string to_string(const std::string& value) { return value; } + std::string safeToString(const IdxType& idx) const { + using namespace std; + return to_string(idx); + } + + private: + //! Prepended string to the ROS topic that is to be advertised. + std::string base_topic_name_; + //! Latch msgs on publishing or not: set to true if you want to send last + //! msg to new subscribers. + bool latch_msgs_; + //! Size of the publisher queue. + size_t queue_size_; + + //! Map of indices to publishers, it contains all our publishers. + RosPubs pubs_; + + //! Ros handle: can be the classic ros::NodeHandle, or a + //! image_transport::ImageTransport + NodeHandleType nh_private_; +}; + +//! Define some classics: +using ImagePublishers = RosPublishers; + +} // namespace VIO diff --git a/include/kimera_vio_ros/RosVisualizer.h b/include/kimera_vio_ros/RosVisualizer.h new file mode 100644 index 00000000..e86b93d1 --- /dev/null +++ b/include/kimera_vio_ros/RosVisualizer.h @@ -0,0 +1,163 @@ +/** + * @file RosVisualizer.h + * @brief Equivalent Kimera Visualizer but in ROS. Publishes 3D data to ROS. + * @author Antoni Rosinol + */ + +#pragma once + +#include +#include + +#define PCL_NO_PRECOMPILE // Define this before you include any PCL headers + // to include the templated algorithms +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "kimera_vio_ros/RosPublishers.h" + +namespace VIO { + +/** + * @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; + +class RosVisualizer : public Visualizer3D { + public: + KIMERA_POINTER_TYPEDEFS(RosVisualizer); + KIMERA_DELETE_COPY_CONSTRUCTORS(RosVisualizer); + + public: + RosVisualizer(const VioParams& vio_params); + virtual ~RosVisualizer() = default; + + public: + /** + * @brief spinOnce + * Spins the display once to render the visualizer output. + * @param viz_input + */ + VisualizerOutput::UniquePtr spinOnce( + const VisualizerInput& viz_input) override; + + protected: + // Publish VIO outputs. + virtual void publishBackendOutput(const BackendOutput::ConstPtr& output); + + virtual void publishFrontendOutput(const FrontendOutput::ConstPtr& output) const; + + virtual void publishMesherOutput(const MesherOutput::ConstPtr& output) const; + + // Publish all outputs for LCD + // TODO(marcus): make like other outputs + virtual void publishLcdOutput(const LcdOutput::ConstPtr& lcd_output); + + private: + void publishTimeHorizonPointCloud(const BackendOutput::ConstPtr& output) const; + + void publishPerFrameMesh3D(const MesherOutput::ConstPtr& output) const; + + // void publishTimeHorizonMesh3D(const MesherOutput::ConstPtr& output) const; + + void publishState(const BackendOutput::ConstPtr& output) const; + + void publishFrontendStats(const FrontendOutput::ConstPtr& output) const; + + void publishResiliency(const FrontendOutput::ConstPtr& frontend_output, + const BackendOutput::ConstPtr& backend_output) const; + + void publishImuBias(const BackendOutput::ConstPtr& output) const; + + void publishTf(const BackendOutput::ConstPtr& output); + void publishTf(const LcdOutput::ConstPtr& lcd_output); + + void publishOptimizedTrajectory(const LcdOutput::ConstPtr& lcd_output) const; + + void publishPoseGraph(const LcdOutput::ConstPtr& lcd_output); + + void updateNodesAndEdges(const gtsam::NonlinearFactorGraph& nfg, + const gtsam::Values& values); + + void updateRejectedEdges(); + + pose_graph_tools::PoseGraph getPosegraphMsg(); + + void publishDebugImage(const Timestamp& timestamp, + const cv::Mat& debug_image) const; + + private: + // ROS handles + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + // ROS 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 loop_closure_edges_; + std::vector odometry_edges_; + std::vector inlier_edges_; + std::vector pose_graph_nodes_; + + private: + //! Define frame ids for odometry message + std::string world_frame_id_; + std::string base_link_frame_id_; + std::string map_frame_id_; + + cv::Size image_size_; + + //! Define image publishers manager + std::unique_ptr image_publishers_; + + //! Whether we publish lcd things or not. (TODO:(Toni) Not used, implement...) + bool use_lcd_; + + private: + // Typedefs + typedef pcl::PointCloud 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)) diff --git a/include/kimera_vio_ros/utils/UtilsRos.h b/include/kimera_vio_ros/utils/UtilsRos.h new file mode 100644 index 00000000..4e8105b1 --- /dev/null +++ b/include/kimera_vio_ros/utils/UtilsRos.h @@ -0,0 +1,36 @@ +/** + * @file UtilsRos.h + * @brief Utilities to convert from/to ROS types + * @author Antoni Rosinol + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +namespace VIO { + +namespace utils { + +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& base_link_frame_id, + const std::string& cam_frame_id, + CameraParams* cam_params); + +} // namespace utils + +} // namespace VIO diff --git a/launch/kimera_vio_ros.launch b/launch/kimera_vio_ros.launch index 3d7a1002..db31ba7d 100644 --- a/launch/kimera_vio_ros.launch +++ b/launch/kimera_vio_ros.launch @@ -54,9 +54,9 @@ - + - + diff --git a/package.xml b/package.xml index 331a8019..d956d02b 100644 --- a/package.xml +++ b/package.xml @@ -1,10 +1,9 @@ kimera_vio_ros - 4.0.0 + 5.0.0 KimeraVIO ROS Wrapper - Yun Chang Marcus Abate Antoni Rosinol diff --git a/src/KimeraVioRos.cpp b/src/KimeraVioRos.cpp index 45f24523..4e510f50 100644 --- a/src/KimeraVioRos.cpp +++ b/src/KimeraVioRos.cpp @@ -34,6 +34,8 @@ KimeraVioRos::KimeraVioRos() : nh_private_("~"), vio_params_(nullptr), vio_pipeline_(nullptr), + ros_display_(nullptr), + ros_visualizer_(nullptr), data_provider_(nullptr), restart_vio_pipeline_srv_(), restart_vio_pipeline_(false) { @@ -53,13 +55,24 @@ bool KimeraVioRos::runKimeraVio() { // the data provider. // NOTE: had the data provider been destroyed before, the vio would be calling // the shutdown function of a deleted object, aka segfault. + VLOG(1) << "Destroy Ros Display."; + ros_display_.reset(); + ros_visualizer_.reset(); + + VLOG(1) << "Creating Ros Display."; + CHECK(vio_params_); + ros_display_ = VIO::make_unique(); + ros_visualizer_ = VIO::make_unique(*vio_params_); + VLOG(1) << "Destroy Vio Pipeline."; vio_pipeline_.reset(); // Then, create Kimera-VIO from scratch. VLOG(1) << "Creating Kimera-VIO."; - CHECK(vio_params_); - vio_pipeline_ = VIO::make_unique(*vio_params_); + CHECK(ros_display_); + vio_pipeline_ = VIO::make_unique(*vio_params_, + std::move(ros_visualizer_), + std::move(ros_display_)); CHECK(vio_pipeline_) << "Vio pipeline construction failed."; // Second, destroy dataset parser. @@ -73,7 +86,7 @@ bool KimeraVioRos::runKimeraVio() { // Finally, connect data_provider and vio_pipeline VLOG(1) << "Connecting Vio Pipeline and Data Provider."; - connectVioPipelineAndDataProvider(); + connectVIO(); // Run return spin(); @@ -87,20 +100,19 @@ bool KimeraVioRos::spin() { auto tic = VIO::utils::Timer::tic(); bool is_pipeline_successful = false; if (vio_params_->parallel_run_) { - std::future data_provider_handle = + std::future vio_viz_handle = std::async(std::launch::async, - &VIO::RosDataProviderInterface::spin, - data_provider_.get()); + &VIO::Pipeline::spinViz, + vio_pipeline_.get()); std::future vio_pipeline_handle = std::async( std::launch::async, &VIO::Pipeline::spin, vio_pipeline_.get()); // Run while ROS is ok and vio pipeline is not shutdown. // Ideally make a thread that shutdowns pipeline if ros is not ok. - ros::Rate rate (10); // Check pipeline status at 10Hz - while (ros::ok() && - !restart_vio_pipeline_) { //&& vio_pipeline.spinViz()) { - LOG_EVERY_N(INFO, 5) << vio_pipeline_->printStatistics(); + ros::Rate rate(20); // Check pipeline status at 20Hz + while (ros::ok() && !restart_vio_pipeline_) { + // Print stats at 10hz + LOG_EVERY_N(INFO, 10) << vio_pipeline_->printStatistics(); rate.sleep(); - continue; } if (!restart_vio_pipeline_) { LOG(INFO) << "Shutting down ROS and Kimera-VIO."; @@ -114,9 +126,9 @@ bool KimeraVioRos::spin() { LOG(INFO) << "Joining Kimera-VIO thread."; vio_pipeline_handle.get(); LOG(INFO) << "Kimera-VIO thread joined successfully."; - LOG(INFO) << "Joining DataProvider thread."; - is_pipeline_successful = !data_provider_handle.get(); - LOG(INFO) << "DataProvider thread joined successfully."; + LOG(INFO) << "Joining RosDisplay thread."; + is_pipeline_successful = !vio_viz_handle.get(); + LOG(INFO) << "RosDisplay thread joined successfully."; if (restart_vio_pipeline_) { // Mind that this is a recursive call! As we call this function // inside runKimeraVio. Sorry, couldn't find a better way. @@ -129,7 +141,7 @@ bool KimeraVioRos::spin() { while (ros::ok() && data_provider_->spin() && vio_pipeline_->spin()) { // TODO(Toni): right now this will loop forwever unless ROS dies or Ctrl+C LOG(INFO) << vio_pipeline_->printStatistics(); - continue; + vio_pipeline_->spinViz(); } LOG(INFO) << "Shutting down ROS and VIO pipeline."; ros::shutdown(); @@ -157,41 +169,15 @@ KimeraVioRos::createDataProvider(const VioParams& vio_params) { return nullptr; } -void KimeraVioRos::connectVioPipelineAndDataProvider() { - CHECK(data_provider_); - CHECK(vio_pipeline_); - +void KimeraVioRos::connectVIO() { // Register VIO pipeline callbacks // Register callback to shutdown data provider in case VIO pipeline // shutsdown. + CHECK(data_provider_); + CHECK(vio_pipeline_); vio_pipeline_->registerShutdownCallback(std::bind( &VIO::DataProviderInterface::shutdown, std::ref(*data_provider_))); - // Register callback to retrieve vio pipeline output from all modules. - vio_pipeline_->registerBackendOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackBackendOutput, - std::ref(*data_provider_), - std::placeholders::_1)); - - vio_pipeline_->registerFrontendOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackFrontendOutput, - std::ref(*data_provider_), - std::placeholders::_1)); - - vio_pipeline_->registerMesherOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackMesherOutput, - std::ref(*data_provider_), - std::placeholders::_1)); - - bool use_lcd = false; - CHECK(nh_private_.getParam("use_lcd", use_lcd)); - if (use_lcd) { - vio_pipeline_->registerLcdOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackLcdOutput, - std::ref(*data_provider_), - std::placeholders::_1)); - } - // Register Data Provider callbacks data_provider_->registerImuSingleCallback( std::bind(&VIO::Pipeline::fillSingleImuQueue, diff --git a/src/RosBagDataProvider.cpp b/src/RosBagDataProvider.cpp index a1ae71b0..32219f8e 100644 --- a/src/RosBagDataProvider.cpp +++ b/src/RosBagDataProvider.cpp @@ -120,15 +120,12 @@ bool RosbagDataProvider::spin() { // Publish VIO output if any. // TODO(Toni) this could go faster if running in another thread or // node... - bool got_synced_outputs = publishSyncedOutputs(); - VLOG_IF(5, !got_synced_outputs) - << "Pipeline lagging behind rosbag parser..."; - - // Publish LCD output if any. - LcdOutput::Ptr lcd_output = nullptr; - if (lcd_output_queue_.pop(lcd_output)) { - publishLcdOutput(lcd_output); - } + + // // Publish LCD output if any. + // LcdOutput::Ptr lcd_output = nullptr; + // if (lcd_output_queue_.pop(lcd_output)) { + // publishLcdOutput(lcd_output); + // } timestamp_last_frame_ = timestamp_frame_k; } else { @@ -139,6 +136,7 @@ bool RosbagDataProvider::spin() { << " Timestamp Last Frame: " << timestamp_last_frame_; } + publishRosbagInfo(timestamp_frame_k); ros::spinOnce(); } else { LOG(ERROR) << "ROS SHUTDOWN requested, stopping rosbag spin."; @@ -156,34 +154,9 @@ bool RosbagDataProvider::spin() { } // End of for loop over rosbag images. LOG(INFO) << "Rosbag processing finished."; - if (vio_params_.parallel_run_) { - // Endless loop until ros dies to publish left-over outputs. - while (nh_.ok() && ros::ok() && !ros::isShuttingDown() && !shutdown_) { - publishOutputs(); - } - } else { - // Publish just once otw we will be blocking kimera-vio spin. - publishOutputs(); - } - return true; } -void RosbagDataProvider::publishOutputs() { - FrontendOutput::Ptr frame_rate_frontend_output = nullptr; - if (frame_rate_frontend_output_queue_.pop(frame_rate_frontend_output)) { - publishFrontendOutput(frame_rate_frontend_output); - } - - // Publish backend, mesher, etc output - publishSyncedOutputs(); - - LcdOutput::Ptr lcd_output = nullptr; - if (lcd_output_queue_.pop(lcd_output)) { - publishLcdOutput(lcd_output); - } -} - bool RosbagDataProvider::parseRosbag(const std::string& bag_path, RosbagData* rosbag_data) { LOG(INFO) << "Parsing rosbag data."; @@ -340,12 +313,9 @@ VioNavState RosbagDataProvider::getGroundTruthVioNavState( return gt_init; } -void RosbagDataProvider::publishBackendOutput( - const BackendOutput::Ptr& output) { - CHECK(output); - publishInputs(output->timestamp_); - RosDataProviderInterface::publishBackendOutput(output); - publishClock(output->timestamp_); +void RosbagDataProvider::publishRosbagInfo(const Timestamp& timestamp) { + publishInputs(timestamp); + publishClock(timestamp); } void RosbagDataProvider::publishClock(const Timestamp& timestamp) const { diff --git a/src/RosDataProviderInterface.cpp b/src/RosDataProviderInterface.cpp index 2f58c60e..6085d3da 100644 --- a/src/RosDataProviderInterface.cpp +++ b/src/RosDataProviderInterface.cpp @@ -13,78 +13,23 @@ #include #include -#include -#include -#include #include -#include #include -#include -#include -#include -#include - -#include -#include -#include #include -#include -#include #include namespace VIO { RosDataProviderInterface::RosDataProviderInterface(const VioParams& vio_params) : DataProviderInterface(), - it_(nullptr), nh_(), nh_private_("~"), - backend_output_queue_("Backend output ROS"), - frame_rate_frontend_output_queue_("Frame Rate Frontend output ROS"), - keyframe_rate_frontend_output_queue_("Keyframe Rate Frontend output ROS"), - mesher_output_queue_("Mesher output ROS"), - lcd_output_queue_("LCD output ROS"), vio_params_(vio_params) { VLOG(1) << "Initializing RosDataProviderInterface."; // Print parameters to check. if (VLOG_IS_ON(1)) printParsedParams(); - - it_ = VIO::make_unique(nh_); - - // Get ROS params - CHECK(nh_private_.getParam("base_link_frame_id", base_link_frame_id_)); - 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("map_frame_id", map_frame_id_)); - CHECK(!map_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("odometry", 1, true); - frontend_stats_pub_ = - nh_.advertise("frontend_stats", 1); - resiliency_pub_ = nh_.advertise("resiliency", 1); - imu_bias_pub_ = nh_.advertise("imu_bias", 1); - trajectory_pub_ = nh_.advertise("optimized_trajectory", 1); - posegraph_pub_ = nh_.advertise("pose_graph", 1); - pointcloud_pub_ = - nh_.advertise("time_horizon_pointcloud", 1, true); - mesh_3d_frame_pub_ = nh_.advertise("mesh", 1, true); - debug_img_pub_ = it_->advertise("debug_mesh_img/image_raw", 1, true); - feature_tracks_pub_ = it_->advertise("feature_tracks/image_raw", 1, true); - - publishStaticTf(vio_params_.camera_params_.at(0).body_Pose_cam_, - base_link_frame_id_, - left_cam_frame_id_); - publishStaticTf(vio_params_.camera_params_.at(1).body_Pose_cam_, - base_link_frame_id_, - right_cam_frame_id_); } RosDataProviderInterface::~RosDataProviderInterface() { @@ -149,742 +94,6 @@ const cv::Mat RosDataProviderInterface::readRosDepthImage( return img_depth; } -void RosDataProviderInterface::publishBackendOutput( - const BackendOutput::Ptr& output) { - CHECK(output); - publishTf(output); - if (odometry_pub_.getNumSubscribers() > 0) { - publishState(output); - } - if (imu_bias_pub_.getNumSubscribers() > 0) { - publishImuBias(output); - } - if (pointcloud_pub_.getNumSubscribers() > 0) { - publishTimeHorizonPointCloud(output); - } -} - -void RosDataProviderInterface::publishFrontendOutput( - const FrontendOutput::Ptr& output) const { - CHECK(output); - if (frontend_stats_pub_.getNumSubscribers() > 0) { - publishFrontendStats(output); - } - if (feature_tracks_pub_.getNumSubscribers() > 0) { - if (!output->feature_tracks_.empty()) { - std_msgs::Header h; - h.stamp.fromNSec(output->timestamp_); - h.frame_id = base_link_frame_id_; - // Copies... - feature_tracks_pub_.publish( - cv_bridge::CvImage(h, "bgr8", output->feature_tracks_).toImageMsg()); - } else { - LOG(ERROR) << feature_tracks_pub_.getNumSubscribers() - << " nodes subscribed to the feature tracks topic, but the " - "feature tracks image is empty. Did you set the gflag " - "visualize_feature_tracks in Kimera-VIO to true?"; - } - } -} - -void RosDataProviderInterface::publishMesherOutput( - const MesherOutput::Ptr& output) const { - CHECK(output); - if (mesh_3d_frame_pub_.getNumSubscribers() > 0) { - publishPerFrameMesh3D(output); - } -} - -bool RosDataProviderInterface::publishSyncedOutputs() { - // First acquire a backend output packet, as it is slowest. - BackendOutput::Ptr backend_output = nullptr; - if (backend_output_queue_.pop(backend_output)) { - CHECK(backend_output); - publishBackendOutput(backend_output); - const Timestamp& ts = backend_output->timestamp_; - - FrontendOutput::Ptr frontend_output = nullptr; - bool get_frontend = - SimpleQueueSynchronizer::getInstance().syncQueue( - ts, - &keyframe_rate_frontend_output_queue_, - &frontend_output, - "RosDataProvider"); - CHECK(frontend_output); - - MesherOutput::Ptr mesher_output = nullptr; - bool get_mesher = - SimpleQueueSynchronizer::getInstance().syncQueue( - ts, &mesher_output_queue_, &mesher_output, "RosDataProvider"); - if (mesher_output) { - publishMesherOutput(mesher_output); - } - - if (frontend_output && mesher_output) { - // Publish 2d mesh debug image - if (debug_img_pub_.getNumSubscribers() > 0) { - cv::Mat mesh_2d_img = Visualizer3D::visualizeMesh2D( - mesher_output->mesh_2d_for_viz_, - frontend_output->stereo_frame_lkf_.getLeftFrame().img_); - publishDebugImage(backend_output->timestamp_, mesh_2d_img); - } - } - - if (frontend_output && backend_output) { - // Publish Resiliency - if (resiliency_pub_.getNumSubscribers() > 0) { - publishResiliency(frontend_output, backend_output); - } - } - - return get_frontend && get_mesher; - } - - return false; -} - -void RosDataProviderInterface::publishLcdOutput( - const LcdOutput::Ptr& lcd_output) { - CHECK(lcd_output); - - publishTf(lcd_output); - if (trajectory_pub_.getNumSubscribers() > 0) { - publishOptimizedTrajectory(lcd_output); - } - if (posegraph_pub_.getNumSubscribers() > 0) { - publishPoseGraph(lcd_output); - } -} - -void RosDataProviderInterface::publishTimeHorizonPointCloud( - const BackendOutput::Ptr& output) const { - CHECK(output); - const Timestamp& timestamp = output->timestamp_; - const PointsWithIdMap& points_with_id = output->landmarks_with_id_map_; - const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map = - output->lmk_id_to_lmk_type_map_; - - PointCloudXYZRGB::Ptr msg(new PointCloudXYZRGB); - msg->header.frame_id = world_frame_id_; - msg->is_dense = true; - msg->height = 1; - msg->width = points_with_id.size(); - msg->points.resize(points_with_id.size()); - - bool color_the_cloud = false; - if (lmk_id_to_lmk_type_map.size() != 0) { - color_the_cloud = true; - CHECK_EQ(points_with_id.size(), lmk_id_to_lmk_type_map.size()); - } - - if (points_with_id.size() == 0) { - // No points to visualize. - return; - } - - // Populate cloud structure with 3D points. - size_t i = 0; - for (const std::pair& id_point : points_with_id) { - const gtsam::Point3 point_3d = id_point.second; - msg->points[i].x = static_cast(point_3d.x()); - msg->points[i].y = static_cast(point_3d.y()); - msg->points[i].z = static_cast(point_3d.z()); - if (color_the_cloud) { - DCHECK(lmk_id_to_lmk_type_map.find(id_point.first) != - lmk_id_to_lmk_type_map.end()); - switch (lmk_id_to_lmk_type_map.at(id_point.first)) { - case LandmarkType::SMART: { - // point_cloud_color.col(i) = cv::viz::Color::white(); - msg->points[i].r = 0; - msg->points[i].g = 255; - msg->points[i].b = 0; - break; - } - case LandmarkType::PROJECTION: { - // point_cloud_color.col(i) = cv::viz::Color::green(); - msg->points[i].r = 0; - msg->points[i].g = 0; - msg->points[i].b = 255; - break; - } - default: { - // point_cloud_color.col(i) = cv::viz::Color::white(); - msg->points[i].r = 255; - msg->points[i].g = 0; - msg->points[i].b = 0; - break; - } - } - } - i++; - } - - ros::Time ros_timestamp; - ros_timestamp.fromNSec(timestamp); - pcl_conversions::toPCL(ros_timestamp, msg->header.stamp); - pointcloud_pub_.publish(msg); -} - -void RosDataProviderInterface::publishDebugImage( - const Timestamp& timestamp, - const cv::Mat& debug_image) const { - // CHECK(debug_image.type(), CV_8UC1); - std_msgs::Header h; - h.stamp.fromNSec(timestamp); - h.frame_id = base_link_frame_id_; - // Copies... - debug_img_pub_.publish( - cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); -} - -// void RosBaseDataProvider::publishTimeHorizonMesh3D( -// const MesherOutput::Ptr& output) { -// const Mesh3D& mesh_3d = output->mesh_3d_; -// size_t number_mesh_3d_polygons = mesh_3d.getNumberOfPolygons(); -//} - -void RosDataProviderInterface::publishPerFrameMesh3D( - const MesherOutput::Ptr& output) const { - CHECK(output); - - const Mesh2D& mesh_2d = output->mesh_2d_; - const Mesh3D& mesh_3d = output->mesh_3d_; - size_t number_mesh_2d_polygons = mesh_2d.getNumberOfPolygons(); - size_t mesh_2d_poly_dim = mesh_2d.getMeshPolygonDimension(); - - static const size_t cam_width = - vio_params_.camera_params_.at(0).image_size_.width; - static const size_t cam_height = - vio_params_.camera_params_.at(0).image_size_.height; - DCHECK_GT(cam_width, 0); - DCHECK_GT(cam_height, 0); - - pcl_msgs::PolygonMesh::Ptr msg(new pcl_msgs::PolygonMesh()); - msg->header.stamp.fromNSec(output->timestamp_); - msg->header.frame_id = world_frame_id_; - - // Create point cloud to hold vertices. - pcl::PointCloud cloud; - cloud.points.reserve(number_mesh_2d_polygons * mesh_2d_poly_dim); - msg->polygons.reserve(number_mesh_2d_polygons); - - Mesh2D::Polygon polygon; - for (size_t i = 0; i < number_mesh_2d_polygons; i++) { - CHECK(mesh_2d.getPolygon(i, &polygon)) << "Could not retrieve 2d polygon."; - const LandmarkId& lmk0_id = polygon.at(0).getLmkId(); - const LandmarkId& lmk1_id = polygon.at(1).getLmkId(); - const LandmarkId& lmk2_id = polygon.at(2).getLmkId(); - - // Returns indices of points in the 3D mesh corresponding to the - // vertices - // in the 2D mesh. - int p0_id, p1_id, p2_id; - Mesh3D::VertexType vtx0, vtx1, vtx2; - if (mesh_3d.getVertex(lmk0_id, &vtx0, &p0_id) && - mesh_3d.getVertex(lmk1_id, &vtx1, &p1_id) && - mesh_3d.getVertex(lmk2_id, &vtx2, &p2_id)) { - // Get pixel coordinates of the vertices of the 2D mesh. - const Vertex2D& px0 = polygon.at(0).getVertexPosition(); - const Vertex2D& px1 = polygon.at(1).getVertexPosition(); - const Vertex2D& px2 = polygon.at(2).getVertexPosition(); - - // Get 3D coordinates of the vertices of the 3D mesh. - const Vertex3D& lmk0_pos = vtx0.getVertexPosition(); - const Vertex3D& lmk1_pos = vtx1.getVertexPosition(); - const Vertex3D& lmk2_pos = vtx2.getVertexPosition(); - - // Get normals of the vertices of the 3D mesh. - const Mesh3D::VertexNormal& normal0 = vtx0.getVertexNormal(); - const Mesh3D::VertexNormal& normal1 = vtx1.getVertexNormal(); - const Mesh3D::VertexNormal& normal2 = vtx2.getVertexNormal(); - - // FILL POINTCLOUD - // clang-format off - PointNormalUV pn0, pn1, pn2; - pn0.x = lmk0_pos.x; pn1.x = lmk1_pos.x; pn2.x = lmk2_pos.x; - pn0.y = lmk0_pos.y; pn1.y = lmk1_pos.y; pn2.y = lmk2_pos.y; - pn0.z = lmk0_pos.z; pn1.z = lmk1_pos.z; pn2.z = lmk2_pos.z; - // OpenGL textures range from 0 to 1. - pn0.u = px0.x / cam_width; pn1.u = px1.x / cam_width; pn2.u = px2.x / cam_width; - pn0.v = px0.y / cam_height; pn1.v = px1.y / cam_height; pn2.v = px2.y / cam_height; - pn0.normal_x = normal0.x; pn1.normal_x = normal1.x; pn2.normal_x = normal2.x; - pn0.normal_y = normal0.y; pn1.normal_y = normal1.y; pn2.normal_y = normal2.y; - pn0.normal_z = normal0.z; pn1.normal_z = normal1.z; pn2.normal_z = normal2.z; - // clang-format on - - // TODO(Toni): we are adding repeated vertices!! - cloud.points.push_back(pn0); - cloud.points.push_back(pn1); - cloud.points.push_back(pn2); - - // Store polygon connectivity - pcl_msgs::Vertices vtx_ii; - vtx_ii.vertices.resize(3); - size_t idx = i * mesh_2d_poly_dim; - // Store connectivity CCW bcs of RVIZ - vtx_ii.vertices[0] = idx + 2; - vtx_ii.vertices[1] = idx + 1; - vtx_ii.vertices[2] = idx; - msg->polygons.push_back(vtx_ii); - } else { - // LOG_EVERY_N(ERROR, 1000) << "Polygon in 2d mesh did not have a - // corresponding polygon in" - // " 3d mesh!"; - } - } - - cloud.is_dense = false; - cloud.width = cloud.points.size(); - cloud.height = 1; - pcl::toROSMsg(cloud, msg->cloud); - - // NOTE: Header fields need to be filled in after pcl::toROSMsg() call. - msg->cloud.header = std_msgs::Header(); - msg->cloud.header.stamp = msg->header.stamp; - msg->cloud.header.frame_id = msg->header.frame_id; - - if (msg->polygons.size() > 0) { - mesh_3d_frame_pub_.publish(msg); - } - - return; -} // namespace VIO - -void RosDataProviderInterface::publishState( - const BackendOutput::Ptr& output) const { - CHECK(output); - // Get latest estimates for odometry. - const Timestamp& ts = output->timestamp_; - const gtsam::Pose3& pose = output->W_State_Blkf_.pose_; - const gtsam::Rot3& rotation = pose.rotation(); - const gtsam::Quaternion& quaternion = rotation.toQuaternion(); - const gtsam::Vector3& velocity = output->W_State_Blkf_.velocity_; - const gtsam::Matrix6& pose_cov = - gtsam::sub(output->state_covariance_lkf_, 0, 6, 0, 6); - const gtsam::Matrix3& vel_cov = - gtsam::sub(output->state_covariance_lkf_, 6, 9, 6, 9); - - // First publish odometry estimate - nav_msgs::Odometry odometry_msg; - - // Create header. - odometry_msg.header.stamp.fromNSec(ts); - odometry_msg.header.frame_id = world_frame_id_; - odometry_msg.child_frame_id = base_link_frame_id_; - - // Position - odometry_msg.pose.pose.position.x = pose.x(); - odometry_msg.pose.pose.position.y = pose.y(); - odometry_msg.pose.pose.position.z = pose.z(); - - // Orientation - odometry_msg.pose.pose.orientation.w = quaternion.w(); - odometry_msg.pose.pose.orientation.x = quaternion.x(); - odometry_msg.pose.pose.orientation.y = quaternion.y(); - odometry_msg.pose.pose.orientation.z = quaternion.z(); - - // Remap covariance from GTSAM convention - // to odometry convention and fill in covariance - static const std::vector remapping{3, 4, 5, 0, 1, 2}; - - // Position covariance first, angular covariance after - DCHECK_EQ(pose_cov.rows(), remapping.size()); - DCHECK_EQ(pose_cov.rows() * pose_cov.cols(), - odometry_msg.pose.covariance.size()); - for (int i = 0; i < pose_cov.rows(); i++) { - for (int j = 0; j < pose_cov.cols(); j++) { - odometry_msg.pose - .covariance[remapping[i] * pose_cov.cols() + remapping[j]] = - pose_cov(i, j); - } - } - - // Linear velocities, trivial values for angular - const gtsam::Matrix3& inversed_rotation = rotation.transpose(); - const Vector3 velocity_body = inversed_rotation * velocity; - odometry_msg.twist.twist.linear.x = velocity_body(0); - odometry_msg.twist.twist.linear.y = velocity_body(1); - odometry_msg.twist.twist.linear.z = velocity_body(2); - - // Velocity covariance: first linear - // and then angular (trivial values for angular) - const gtsam::Matrix3 vel_cov_body = - inversed_rotation.matrix() * vel_cov * rotation.matrix(); - DCHECK_EQ(vel_cov_body.rows(), 3); - DCHECK_EQ(vel_cov_body.cols(), 3); - DCHECK_EQ(odometry_msg.twist.covariance.size(), 36); - for (int i = 0; i < vel_cov_body.rows(); i++) { - for (int j = 0; j < vel_cov_body.cols(); j++) { - odometry_msg.twist - .covariance[i * static_cast( - sqrt(odometry_msg.twist.covariance.size())) + - j] = vel_cov_body(i, j); - } - } - // Publish message - odometry_pub_.publish(odometry_msg); -} - -void RosDataProviderInterface::publishTf(const BackendOutput::Ptr& output) { - CHECK(output); - - const Timestamp& timestamp = output->timestamp_; - const gtsam::Pose3& pose = output->W_State_Blkf_.pose_; - const gtsam::Quaternion& quaternion = pose.rotation().toQuaternion(); - // Publish base_link TF. - geometry_msgs::TransformStamped odom_tf; - odom_tf.header.stamp.fromNSec(timestamp); - odom_tf.header.frame_id = world_frame_id_; - odom_tf.child_frame_id = base_link_frame_id_; - - poseToMsgTF(pose, &odom_tf.transform); - tf_broadcaster_.sendTransform(odom_tf); -} - -void RosDataProviderInterface::publishFrontendStats( - const FrontendOutput::Ptr& output) const { - CHECK(output); - - // Get frontend data for resiliency output - const DebugTrackerInfo& debug_tracker_info = output->getTrackerInfo(); - - // Create message type - std_msgs::Float64MultiArray frontend_stats_msg; - - // Build Message Layout - frontend_stats_msg.data.resize(13); - frontend_stats_msg.data[0] = debug_tracker_info.nrDetectedFeatures_; - frontend_stats_msg.data[1] = debug_tracker_info.nrTrackerFeatures_; - frontend_stats_msg.data[2] = debug_tracker_info.nrMonoInliers_; - frontend_stats_msg.data[3] = debug_tracker_info.nrMonoPutatives_; - frontend_stats_msg.data[4] = debug_tracker_info.nrStereoInliers_; - frontend_stats_msg.data[5] = debug_tracker_info.nrStereoPutatives_; - frontend_stats_msg.data[6] = debug_tracker_info.monoRansacIters_; - frontend_stats_msg.data[7] = debug_tracker_info.stereoRansacIters_; - frontend_stats_msg.data[8] = debug_tracker_info.nrValidRKP_; - frontend_stats_msg.data[9] = debug_tracker_info.nrNoLeftRectRKP_; - frontend_stats_msg.data[10] = debug_tracker_info.nrNoRightRectRKP_; - frontend_stats_msg.data[11] = debug_tracker_info.nrNoDepthRKP_; - frontend_stats_msg.data[12] = debug_tracker_info.nrFailedArunRKP_; - frontend_stats_msg.layout.dim.resize(1); - frontend_stats_msg.layout.dim[0].size = frontend_stats_msg.data.size(); - frontend_stats_msg.layout.dim[0].stride = 1; - frontend_stats_msg.layout.dim[0].label = - "FrontEnd: nrDetFeat, nrTrackFeat, nrMoIn, nrMoPu, nrStIn, nrStPu, " - "moRaIt, stRaIt, nrVaRKP, nrNoLRKP, nrNoRRKP, nrNoDRKP nrFaARKP"; - - // Publish Message - frontend_stats_pub_.publish(frontend_stats_msg); -} - -void RosDataProviderInterface::publishResiliency( - const FrontendOutput::Ptr& frontend_output, - const BackendOutput::Ptr& backend_output) const { - CHECK(frontend_output); - CHECK(backend_output); - - // Get frontend and velocity covariance data for resiliency output - const DebugTrackerInfo& debug_tracker_info = - frontend_output->getTrackerInfo(); - const gtsam::Matrix6& pose_cov = - gtsam::sub(backend_output->state_covariance_lkf_, 0, 6, 0, 6); - const gtsam::Matrix3& vel_cov = - gtsam::sub(backend_output->state_covariance_lkf_, 6, 9, 6, 9); - - // Create message type for quality of SparkVIO - std_msgs::Float64MultiArray resiliency_msg; - - // Publishing extra information: - // cov_v_det and nrStIn should be the most relevant! - resiliency_msg.layout.dim[0].label = - "Values: cbrtPDet, cbrtVDet, nrStIn, nrMoIn. " - "Thresholds : cbrtPDet, cbrtVDet, nrStIn, nrMoIn."; - - CHECK_EQ(pose_cov.size(), 36); - gtsam::Matrix3 position_cov = gtsam::sub(pose_cov, 3, 6, 3, 6); - CHECK_EQ(position_cov.size(), 9); - - // Compute eigenvalues and determinant of velocity covariance - gtsam::Matrix U; - gtsam::Matrix V; - gtsam::Vector cov_v_eigv; - gtsam::svd(vel_cov, U, cov_v_eigv, V); - CHECK_EQ(cov_v_eigv.size(), 3); - - // Compute eigenvalues and determinant of position covariance - gtsam::Vector cov_p_eigv; - gtsam::svd(position_cov, U, cov_p_eigv, V); - CHECK_EQ(cov_p_eigv.size(), 3); - - // Quality statistics to publish - resiliency_msg.data.resize(8); - resiliency_msg.data[0] = - std::cbrt(cov_p_eigv(0) * cov_p_eigv(1) * cov_p_eigv(2)); - resiliency_msg.data[1] = - std::cbrt(cov_v_eigv(0) * cov_v_eigv(1) * cov_v_eigv(2)); - resiliency_msg.data[2] = debug_tracker_info.nrStereoInliers_; - resiliency_msg.data[3] = debug_tracker_info.nrMonoInliers_; - - // Publish thresholds for statistics - float pos_det_threshold, vel_det_threshold; - int mono_ransac_theshold, stereo_ransac_threshold; - CHECK(nh_private_.getParam("velocity_det_threshold", vel_det_threshold)); - CHECK(nh_private_.getParam("position_det_threshold", pos_det_threshold)); - CHECK( - nh_private_.getParam("stereo_ransac_threshold", stereo_ransac_threshold)); - CHECK(nh_private_.getParam("mono_ransac_threshold", mono_ransac_theshold)); - resiliency_msg.data[4] = pos_det_threshold; - resiliency_msg.data[5] = vel_det_threshold; - resiliency_msg.data[6] = stereo_ransac_threshold; - resiliency_msg.data[7] = mono_ransac_theshold; - - // Build Message Layout - resiliency_msg.layout.dim.resize(1); - resiliency_msg.layout.dim[0].size = resiliency_msg.data.size(); - resiliency_msg.layout.dim[0].stride = 1; - - // Publish Message - resiliency_pub_.publish(resiliency_msg); -} - -void RosDataProviderInterface::publishImuBias( - const BackendOutput::Ptr& output) const { - CHECK(output); - - // Get imu bias to output - const ImuBias& imu_bias = output->W_State_Blkf_.imu_bias_; - const Vector3& accel_bias = imu_bias.accelerometer(); - const Vector3& gyro_bias = imu_bias.gyroscope(); - - // Create message type - std_msgs::Float64MultiArray imu_bias_msg; - - // Get Imu Bias to Publish - imu_bias_msg.data.resize(6); - imu_bias_msg.data.at(0) = gyro_bias[0]; - imu_bias_msg.data.at(1) = gyro_bias[1]; - imu_bias_msg.data.at(2) = gyro_bias[2]; - imu_bias_msg.data.at(3) = accel_bias[0]; - imu_bias_msg.data.at(4) = accel_bias[1]; - imu_bias_msg.data.at(5) = accel_bias[2]; - - // Build Message Layout - imu_bias_msg.layout.dim.resize(1); - imu_bias_msg.layout.dim[0].size = imu_bias_msg.data.size(); - imu_bias_msg.layout.dim[0].stride = 1; - imu_bias_msg.layout.dim[0].label = "Gyro Bias: x,y,z. Accel Bias: x,y,z"; - - // Publish Message - imu_bias_pub_.publish(imu_bias_msg); -} - -void RosDataProviderInterface::publishOptimizedTrajectory( - const LcdOutput::Ptr& lcd_output) const { - CHECK(lcd_output); - - // Get pgo-optimized trajectory - const Timestamp& ts = lcd_output->timestamp_kf_; - const gtsam::Values& trajectory = lcd_output->states_; - // Create message type - nav_msgs::Path path; - - // Fill path poses - path.poses.reserve(trajectory.size()); - for (size_t i = 0; i < trajectory.size(); i++) { - gtsam::Pose3 pose = trajectory.at(i); - gtsam::Point3 trans = pose.translation(); - gtsam::Quaternion quat = pose.rotation().toQuaternion(); - - geometry_msgs::PoseStamped ps_msg; - ps_msg.header.frame_id = world_frame_id_; - ps_msg.pose.position.x = trans.x(); - ps_msg.pose.position.y = trans.y(); - ps_msg.pose.position.z = trans.z(); - ps_msg.pose.orientation.x = quat.x(); - ps_msg.pose.orientation.y = quat.y(); - ps_msg.pose.orientation.z = quat.z(); - ps_msg.pose.orientation.w = quat.w(); - - path.poses.push_back(ps_msg); - } - - // Publish path message - path.header.stamp.fromNSec(ts); - path.header.frame_id = world_frame_id_; - trajectory_pub_.publish(path); -} - -void RosDataProviderInterface::updateRejectedEdges() { - // first update the rejected edges - for (pose_graph_tools::PoseGraphEdge& loop_closure_edge : - loop_closure_edges_) { - bool is_inlier = false; - for (pose_graph_tools::PoseGraphEdge& inlier_edge : inlier_edges_) { - if (loop_closure_edge.key_from == inlier_edge.key_from && - loop_closure_edge.key_to == inlier_edge.key_to) { - is_inlier = true; - continue; - } - } - if (!is_inlier) { - // set as rejected loop closure - loop_closure_edge.type = - pose_graph_tools::PoseGraphEdge::REJECTED_LOOPCLOSE; - } - } - - // Then update the loop edges - for (pose_graph_tools::PoseGraphEdge& inlier_edge : inlier_edges_) { - bool previously_stored = false; - for (pose_graph_tools::PoseGraphEdge& loop_closure_edge : - loop_closure_edges_) { - if (inlier_edge.key_from == loop_closure_edge.key_from && - inlier_edge.key_to == loop_closure_edge.key_to) { - previously_stored = true; - continue; - } - } - if (!previously_stored) { - // add to the vector of all loop clousres - loop_closure_edges_.push_back(inlier_edge); - } - } -} - -void RosDataProviderInterface::updateNodesAndEdges( - const gtsam::NonlinearFactorGraph& nfg, - const gtsam::Values& values) { - inlier_edges_.clear(); - odometry_edges_.clear(); - // first store the factors as edges - for (size_t i = 0; i < nfg.size(); i++) { - // check if between factor - if (boost::dynamic_pointer_cast >( - nfg[i])) { - // convert to between factor - const gtsam::BetweenFactor& factor = - *boost::dynamic_pointer_cast >( - nfg[i]); - // convert between factor to PoseGraphEdge type - pose_graph_tools::PoseGraphEdge edge; - edge.header.frame_id = world_frame_id_; - edge.key_from = factor.front(); - edge.key_to = factor.back(); - if (edge.key_to == edge.key_from + 1) { // check if odom - edge.type = pose_graph_tools::PoseGraphEdge::ODOM; - } else { - edge.type = pose_graph_tools::PoseGraphEdge::LOOPCLOSE; - } - // transforms - translation - const gtsam::Point3& translation = factor.measured().translation(); - edge.pose.position.x = translation.x(); - edge.pose.position.y = translation.y(); - edge.pose.position.z = translation.z(); - // transforms - rotation (to quaternion) - const gtsam::Quaternion& quaternion = - factor.measured().rotation().toQuaternion(); - edge.pose.orientation.x = quaternion.x(); - edge.pose.orientation.y = quaternion.y(); - edge.pose.orientation.z = quaternion.z(); - edge.pose.orientation.w = quaternion.w(); - - // TODO: add covariance - if (edge.type == pose_graph_tools::PoseGraphEdge::ODOM) { - odometry_edges_.push_back(edge); - } else { - inlier_edges_.push_back(edge); - } - } - } - - // update inliers and rejected closures - updateRejectedEdges(); - - pose_graph_nodes_.clear(); - // Then store the values as nodes - gtsam::KeyVector key_list = values.keys(); - for (size_t i = 0; i < key_list.size(); i++) { - pose_graph_tools::PoseGraphNode node; - node.key = key_list[i]; - - const gtsam::Pose3& value = values.at(i); - const gtsam::Point3& translation = value.translation(); - const gtsam::Quaternion& quaternion = value.rotation().toQuaternion(); - - // pose - translation - node.pose.position.x = translation.x(); - node.pose.position.y = translation.y(); - node.pose.position.z = translation.z(); - // pose - rotation (to quaternion) - node.pose.orientation.x = quaternion.x(); - node.pose.orientation.y = quaternion.y(); - node.pose.orientation.z = quaternion.z(); - node.pose.orientation.w = quaternion.w(); - - pose_graph_nodes_.push_back(node); - } - - return; -} - -pose_graph_tools::PoseGraph RosDataProviderInterface::getPosegraphMsg() { - // pose graph getter - pose_graph_tools::PoseGraph pose_graph; - pose_graph.edges = odometry_edges_; // add odometry edges to pg - // then add loop closure edges to pg - pose_graph.edges.insert(pose_graph.edges.end(), - loop_closure_edges_.begin(), - loop_closure_edges_.end()); - // then add the nodes - pose_graph.nodes = pose_graph_nodes_; - - return pose_graph; -} - -void RosDataProviderInterface::publishPoseGraph( - const LcdOutput::Ptr& lcd_output) { - CHECK(lcd_output); - - // Get the factor graph - const Timestamp& ts = lcd_output->timestamp_kf_; - const gtsam::NonlinearFactorGraph& nfg = lcd_output->nfg_; - const gtsam::Values& values = lcd_output->states_; - updateNodesAndEdges(nfg, values); - pose_graph_tools::PoseGraph graph = getPosegraphMsg(); - graph.header.stamp.fromNSec(ts); - graph.header.frame_id = world_frame_id_; - posegraph_pub_.publish(graph); -} - -void RosDataProviderInterface::publishTf(const LcdOutput::Ptr& lcd_output) { - CHECK(lcd_output); - - const Timestamp& ts = lcd_output->timestamp_kf_; - const gtsam::Pose3& w_Pose_map = lcd_output->W_Pose_Map_; - const gtsam::Quaternion& w_Quat_map = w_Pose_map.rotation().toQuaternion(); - // Publish map TF. - geometry_msgs::TransformStamped map_tf; - map_tf.header.stamp.fromNSec(ts); - map_tf.header.frame_id = world_frame_id_; - map_tf.child_frame_id = map_frame_id_; - poseToMsgTF(w_Pose_map, &map_tf.transform); - tf_broadcaster_.sendTransform(map_tf); -} - -void RosDataProviderInterface::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; - poseToMsgTF(pose, &static_transform_stamped.transform); - static_broadcaster.sendTransform(static_transform_stamped); -} - void RosDataProviderInterface::printParsedParams() const { static constexpr int kSeparatorWidth = 40; LOG(INFO) << std::string(kSeparatorWidth, '=') << " - Left camera info:"; @@ -902,82 +111,4 @@ void RosDataProviderInterface::printParsedParams() const { LOG(INFO) << std::string(kSeparatorWidth, '='); } -void RosDataProviderInterface::msgTFtoPose(const geometry_msgs::Transform& tf, - gtsam::Pose3* pose) { - CHECK_NOTNULL(pose); - - *pose = gtsam::Pose3( - gtsam::Rot3(gtsam::Quaternion( - tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z)), - gtsam::Point3(tf.translation.x, tf.translation.y, tf.translation.z)); -} - -void RosDataProviderInterface::poseToMsgTF(const gtsam::Pose3& pose, - geometry_msgs::Transform* tf) { - CHECK_NOTNULL(tf); - - tf->translation.x = pose.x(); - tf->translation.y = pose.y(); - tf->translation.z = pose.z(); - const gtsam::Quaternion& quat = pose.rotation().toQuaternion(); - tf->rotation.w = quat.w(); - tf->rotation.x = quat.x(); - tf->rotation.y = quat.y(); - tf->rotation.z = quat.z(); -} - -void RosDataProviderInterface::msgCamInfoToCameraParams( - const sensor_msgs::CameraInfoConstPtr& cam_info, - const std::string& cam_frame_id, - VIO::CameraParams* cam_params) { - CHECK_NOTNULL(cam_params); - - // Get intrinsics from incoming CameraInfo messages: - cam_params->camera_id_ = cam_info->header.frame_id; - CHECK(!cam_params->camera_id_.empty()); - - cam_params->distortion_model_ = cam_info->distortion_model; - CHECK(cam_params->distortion_model_ == "radtan" || - cam_params->distortion_model_ == "radial-tangential" || - cam_params->distortion_model_ == "equidistant"); - - const std::vector& distortion_coeffs = cam_info->D; - CHECK_EQ(distortion_coeffs.size(), 4); - VIO::CameraParams::convertDistortionVectorToMatrix( - distortion_coeffs, &cam_params->distortion_coeff_); - - cam_params->image_size_ = cv::Size(cam_info->width, cam_info->height); - - cam_params->frame_rate_ = 0; // TODO(marcus): is there a way to get this? - - std::array intrinsics = { - cam_info->K[0], cam_info->K[4], cam_info->K[2], cam_info->K[5]}; - cam_params->intrinsics_ = intrinsics; - VIO::CameraParams::convertIntrinsicsVectorToMatrix(cam_params->intrinsics_, - &cam_params->K_); - - VIO::CameraParams::createGtsamCalibration(cam_params->distortion_coeff_, - cam_params->intrinsics_, - &cam_params->calibration_); - - // Get extrinsics from the TF tree: - tf2_ros::Buffer t_buffer; - tf2_ros::TransformListener tf_listener(t_buffer); - static constexpr size_t kTfLookupTimeout = 5u; - geometry_msgs::TransformStamped cam_tf; - - try { - cam_tf = t_buffer.lookupTransform(base_link_frame_id_, - cam_frame_id, - ros::Time(0), - ros::Duration(kTfLookupTimeout)); - } catch (tf2::TransformException& ex) { - ROS_FATAL( - "TF for left/right camera frames not available. Either publish to " - "tree or provide CameraParameter yaml files."); - } - - msgTFtoPose(cam_tf.transform, &cam_params->body_Pose_cam_); -} - } // namespace VIO diff --git a/src/RosDisplay.cpp b/src/RosDisplay.cpp new file mode 100644 index 00000000..da4f2924 --- /dev/null +++ b/src/RosDisplay.cpp @@ -0,0 +1,50 @@ +/** + * @file RosDisplay.cpp + * @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 "kimera_vio_ros/RosDisplay.h" + +#include + +#include + +#include +#include +#include + +#include + +#include "kimera_vio_ros/RosPublishers.h" +#include "kimera_vio_ros/utils/UtilsRos.h" + +namespace VIO { + +RosDisplay::RosDisplay() + : DisplayBase(), nh_private_("~"), image_publishers_(nullptr) { + image_publishers_ = VIO::make_unique(nh_private_); +} + +void RosDisplay::spinOnce(DisplayInputBase::UniquePtr&& viz_output) { + CHECK(viz_output); + // Display 2D images. + spin2dWindow(*viz_output); + // No need to run ros::spinOnce(), since it is done with async spinner. +} + +void RosDisplay::spin2dWindow(const DisplayInputBase& viz_output) { + for (const ImageToDisplay& img_to_display : viz_output.images_to_display_) { + std_msgs::Header header; + header.stamp.fromNSec(viz_output.timestamp_); + header.frame_id = base_link_frame_id_; + // Copies... + image_publishers_->publish( + img_to_display.name_, + cv_bridge::CvImage(header, "bgr8", img_to_display.image_).toImageMsg()); + } +} + +} // namespace VIO diff --git a/src/RosOnlineDataProvider.cpp b/src/RosOnlineDataProvider.cpp index be8dfaf0..0085fe92 100644 --- a/src/RosOnlineDataProvider.cpp +++ b/src/RosOnlineDataProvider.cpp @@ -15,11 +15,15 @@ #include #include #include +#include + +#include "kimera_vio_ros/utils/UtilsRos.h" namespace VIO { RosOnlineDataProvider::RosOnlineDataProvider(const VioParams& vio_params) : RosDataProviderInterface(vio_params), + it_(nullptr), frame_count_(FrameId(0)), left_img_subscriber_(), right_img_subscriber_(), @@ -156,7 +160,7 @@ RosOnlineDataProvider::RosOnlineDataProvider(const VioParams& vio_params) // We set the queue to only 1, since we prefer to drop messages to reach // real-time than to be delayed... static constexpr size_t kMaxImagesQueueSize = 1u; - CHECK(it_); + it_ = VIO::make_unique(nh_); left_img_subscriber_.subscribe( *it_, "left_cam/image_raw", kMaxImagesQueueSize); right_img_subscriber_.subscribe( @@ -184,13 +188,44 @@ RosOnlineDataProvider::RosOnlineDataProvider(const VioParams& vio_params) &RosOnlineDataProvider::callbackReinitPose, this); + CHECK(nh_private_.getParam("base_link_frame_id", base_link_frame_id_)); + CHECK(!base_link_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()); + + publishStaticTf(vio_params_.camera_params_.at(0).body_Pose_cam_, + base_link_frame_id_, + left_cam_frame_id_); + publishStaticTf(vio_params_.camera_params_.at(1).body_Pose_cam_, + base_link_frame_id_, + right_cam_frame_id_); + // This async spinner will process the regular Global callback queue of ROS. static constexpr size_t kGlobalSpinnerThreads = 2u; async_spinner_ = VIO::make_unique(kGlobalSpinnerThreads); + + // Start async spinners to get input data. + if (!shutdown_) { + CHECK(imu_async_spinner_); + imu_async_spinner_->start(); + CHECK(async_spinner_); + async_spinner_->start(); + } } RosOnlineDataProvider::~RosOnlineDataProvider() { - VLOG(1) << "RosDataProvider destructor called."; + VLOG(1) << "RosOnlineDataProvider destructor called."; + imu_queue_.disable(); + + LOG(INFO) << "Shutting down queues ROS Async Spinner."; + CHECK(imu_async_spinner_); + imu_async_spinner_->stop(); + CHECK(async_spinner_); + async_spinner_->stop(); + + LOG(INFO) << "RosOnlineDataProvider successfully shutdown."; } // TODO(marcus): with the readRosImage, this is a slow callback. Might be too @@ -229,10 +264,10 @@ void RosOnlineDataProvider::callbackCameraInfo( CHECK_GE(vio_params_.camera_params_.size(), 2u); // Initialize CameraParams for pipeline. - msgCamInfoToCameraParams( - left_msg, left_cam_frame_id_, &vio_params_.camera_params_.at(0)); - msgCamInfoToCameraParams( - right_msg, right_cam_frame_id_, &vio_params_.camera_params_.at(1)); + utils::msgCamInfoToCameraParams( + left_msg, base_link_frame_id_, left_cam_frame_id_, &vio_params_.camera_params_.at(0)); + utils::msgCamInfoToCameraParams( + right_msg, base_link_frame_id_, right_cam_frame_id_, &vio_params_.camera_params_.at(1)); vio_params_.camera_params_.at(0).print(); vio_params_.camera_params_.at(1).print(); @@ -344,62 +379,18 @@ void RosOnlineDataProvider::msgGtOdomToVioNavState( vio_navstate->imu_bias_ = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias); } -bool RosOnlineDataProvider::spin() { - CHECK_EQ(vio_params_.camera_params_.size(), 2u); - - LOG(INFO) << "Spinning RosOnlineDataProvider."; - - // TODO(Toni): add sequential mode! - // Start async spinners to get input data. - if (!shutdown_) { - CHECK(imu_async_spinner_); - imu_async_spinner_->start(); - CHECK(async_spinner_); - async_spinner_->start(); - } - - // Start our own spin to publish output data to ROS - // Pop and send output at a 30Hz rate. - ros::Rate rate(30); - while (ros::ok() && !shutdown_) { - rate.sleep(); - spinOnce(); - } - - imu_queue_.disable(); - - LOG(INFO) << "Shutting down queues ROS Async Spinner."; - CHECK(imu_async_spinner_); - imu_async_spinner_->stop(); - CHECK(async_spinner_); - async_spinner_->stop(); - - LOG(INFO) << "RosOnlineDataProvider successfully shutdown."; - - return false; +void RosOnlineDataProvider::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; + utils::poseToMsgTF(pose, &static_transform_stamped.transform); + static_broadcaster.sendTransform(static_transform_stamped); } -bool RosOnlineDataProvider::spinOnce() { - // Publish frontend output at frame rate - LOG_EVERY_N(INFO, 100) << "Spinning RosOnlineDataProvider."; - FrontendOutput::Ptr frame_rate_frontend_output = nullptr; - if (frame_rate_frontend_output_queue_.pop(frame_rate_frontend_output)) { - publishFrontendOutput(frame_rate_frontend_output); - } - - // Publish all output at keyframe rate (backend, mesher, etc) - publishSyncedOutputs(); - - // Publish lcd output at whatever frame rate it might go - LcdOutput::Ptr lcd_output = nullptr; - if (lcd_output_queue_.pop(lcd_output)) { - publishLcdOutput(lcd_output); - } - - // TODO(Toni): add sequential mode! - // ros::spinOnce(); // No need because we use an async spinner, see ctor. - - return true; -} } // namespace VIO diff --git a/src/RosVisualizer.cpp b/src/RosVisualizer.cpp new file mode 100644 index 00000000..c5ad6c3f --- /dev/null +++ b/src/RosVisualizer.cpp @@ -0,0 +1,728 @@ +/** + * @file RosDisplay.cpp + * @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 "kimera_vio_ros/RosVisualizer.h" + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "kimera_vio_ros/utils/UtilsRos.h" + +namespace VIO { + +RosVisualizer::RosVisualizer(const VioParams& vio_params) + // I'm not sure we use this flag in ROS? + : Visualizer3D(VisualizationType::kMesh2dTo3dSparse), + nh_(), + nh_private_("~"), + image_size_(vio_params.camera_params_.at(0).image_size_), + image_publishers_(nullptr) { + + //! To publish 2d images + image_publishers_ = VIO::make_unique(nh_private_); + + // Get ROS params + CHECK(nh_private_.getParam("base_link_frame_id", base_link_frame_id_)); + 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("map_frame_id", map_frame_id_)); + CHECK(!map_frame_id_.empty()); + + nh_private_.getParam("use_lcd", use_lcd_); + + // Publishers + odometry_pub_ = nh_.advertise("odometry", 1, true); + frontend_stats_pub_ = + nh_.advertise("frontend_stats", 1); + resiliency_pub_ = nh_.advertise("resiliency", 1); + imu_bias_pub_ = nh_.advertise("imu_bias", 1); + trajectory_pub_ = nh_.advertise("optimized_trajectory", 1); + posegraph_pub_ = nh_.advertise("pose_graph", 1); + pointcloud_pub_ = + nh_.advertise("time_horizon_pointcloud", 1, true); + mesh_3d_frame_pub_ = nh_.advertise("mesh", 1, true); +} + +VisualizerOutput::UniquePtr RosVisualizer::spinOnce( + const VisualizerInput& viz_input) { + publishBackendOutput(viz_input.backend_output_); + publishFrontendOutput(viz_input.frontend_output_); + publishMesherOutput(viz_input.mesher_output_); + // publishLcdOutput(viz_input->lcd_output_); // missing this one... + // Return empty output, since in ROS, we only publish, not display... + return VIO::make_unique(); +} + +void RosVisualizer::publishBackendOutput(const BackendOutput::ConstPtr& output) { + CHECK(output); + publishTf(output); + if (odometry_pub_.getNumSubscribers() > 0) { + publishState(output); + } + if (imu_bias_pub_.getNumSubscribers() > 0) { + publishImuBias(output); + } + if (pointcloud_pub_.getNumSubscribers() > 0) { + publishTimeHorizonPointCloud(output); + } +} + +void RosVisualizer::publishFrontendOutput( + const FrontendOutput::ConstPtr& output) const { + CHECK(output); + if (frontend_stats_pub_.getNumSubscribers() > 0) { + publishFrontendStats(output); + } +} + +void RosVisualizer::publishMesherOutput(const MesherOutput::ConstPtr& output) const { + CHECK(output); + if (mesh_3d_frame_pub_.getNumSubscribers() > 0) { + publishPerFrameMesh3D(output); + } +} + +void RosVisualizer::publishLcdOutput(const LcdOutput::ConstPtr& lcd_output) { + CHECK(lcd_output); + + publishTf(lcd_output); + if (trajectory_pub_.getNumSubscribers() > 0) { + publishOptimizedTrajectory(lcd_output); + } + if (posegraph_pub_.getNumSubscribers() > 0) { + publishPoseGraph(lcd_output); + } +} + + +void RosVisualizer::publishTimeHorizonPointCloud( + const BackendOutput::ConstPtr& output) const { + CHECK(output); + const Timestamp& timestamp = output->timestamp_; + const PointsWithIdMap& points_with_id = output->landmarks_with_id_map_; + const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map = + output->lmk_id_to_lmk_type_map_; + + PointCloudXYZRGB::Ptr msg(new PointCloudXYZRGB); + msg->header.frame_id = world_frame_id_; + msg->is_dense = true; + msg->height = 1; + msg->width = points_with_id.size(); + msg->points.resize(points_with_id.size()); + + bool color_the_cloud = false; + if (lmk_id_to_lmk_type_map.size() != 0) { + color_the_cloud = true; + CHECK_EQ(points_with_id.size(), lmk_id_to_lmk_type_map.size()); + } + + if (points_with_id.size() == 0) { + // No points to visualize. + return; + } + + // Populate cloud structure with 3D points. + size_t i = 0; + for (const std::pair& id_point : points_with_id) { + const gtsam::Point3 point_3d = id_point.second; + msg->points[i].x = static_cast(point_3d.x()); + msg->points[i].y = static_cast(point_3d.y()); + msg->points[i].z = static_cast(point_3d.z()); + if (color_the_cloud) { + DCHECK(lmk_id_to_lmk_type_map.find(id_point.first) != + lmk_id_to_lmk_type_map.end()); + switch (lmk_id_to_lmk_type_map.at(id_point.first)) { + case LandmarkType::SMART: { + // point_cloud_color.col(i) = cv::viz::Color::white(); + msg->points[i].r = 0; + msg->points[i].g = 255; + msg->points[i].b = 0; + break; + } + case LandmarkType::PROJECTION: { + // point_cloud_color.col(i) = cv::viz::Color::green(); + msg->points[i].r = 0; + msg->points[i].g = 0; + msg->points[i].b = 255; + break; + } + default: { + // point_cloud_color.col(i) = cv::viz::Color::white(); + msg->points[i].r = 255; + msg->points[i].g = 0; + msg->points[i].b = 0; + break; + } + } + } + i++; + } + + ros::Time ros_timestamp; + ros_timestamp.fromNSec(timestamp); + pcl_conversions::toPCL(ros_timestamp, msg->header.stamp); + pointcloud_pub_.publish(msg); +} + +void RosVisualizer::publishDebugImage(const Timestamp& timestamp, + const cv::Mat& debug_image) const { + // CHECK(debug_image.type(), CV_8UC1); + std_msgs::Header h; + h.stamp.fromNSec(timestamp); + h.frame_id = base_link_frame_id_; + // Copies... + image_publishers_->publish( + "mesh_2d", cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); +} + +void RosVisualizer::publishPerFrameMesh3D( + const MesherOutput::ConstPtr& output) const { + CHECK(output); + + const Mesh2D& mesh_2d = output->mesh_2d_; + const Mesh3D& mesh_3d = output->mesh_3d_; + size_t number_mesh_2d_polygons = mesh_2d.getNumberOfPolygons(); + size_t mesh_2d_poly_dim = mesh_2d.getMeshPolygonDimension(); + + static const size_t cam_width = image_size_.width; + static const size_t cam_height = image_size_.height; + DCHECK_GT(cam_width, 0); + DCHECK_GT(cam_height, 0); + + pcl_msgs::PolygonMesh::Ptr msg(new pcl_msgs::PolygonMesh()); + msg->header.stamp.fromNSec(output->timestamp_); + msg->header.frame_id = world_frame_id_; + + // Create point cloud to hold vertices. + pcl::PointCloud cloud; + cloud.points.reserve(number_mesh_2d_polygons * mesh_2d_poly_dim); + msg->polygons.reserve(number_mesh_2d_polygons); + + Mesh2D::Polygon polygon; + for (size_t i = 0; i < number_mesh_2d_polygons; i++) { + CHECK(mesh_2d.getPolygon(i, &polygon)) << "Could not retrieve 2d polygon."; + const LandmarkId& lmk0_id = polygon.at(0).getLmkId(); + const LandmarkId& lmk1_id = polygon.at(1).getLmkId(); + const LandmarkId& lmk2_id = polygon.at(2).getLmkId(); + + // Returns indices of points in the 3D mesh corresponding to the + // vertices + // in the 2D mesh. + int p0_id, p1_id, p2_id; + Mesh3D::VertexType vtx0, vtx1, vtx2; + if (mesh_3d.getVertex(lmk0_id, &vtx0, &p0_id) && + mesh_3d.getVertex(lmk1_id, &vtx1, &p1_id) && + mesh_3d.getVertex(lmk2_id, &vtx2, &p2_id)) { + // Get pixel coordinates of the vertices of the 2D mesh. + const Vertex2D& px0 = polygon.at(0).getVertexPosition(); + const Vertex2D& px1 = polygon.at(1).getVertexPosition(); + const Vertex2D& px2 = polygon.at(2).getVertexPosition(); + + // Get 3D coordinates of the vertices of the 3D mesh. + const Vertex3D& lmk0_pos = vtx0.getVertexPosition(); + const Vertex3D& lmk1_pos = vtx1.getVertexPosition(); + const Vertex3D& lmk2_pos = vtx2.getVertexPosition(); + + // Get normals of the vertices of the 3D mesh. + const Mesh3D::VertexNormal& normal0 = vtx0.getVertexNormal(); + const Mesh3D::VertexNormal& normal1 = vtx1.getVertexNormal(); + const Mesh3D::VertexNormal& normal2 = vtx2.getVertexNormal(); + + // FILL POINTCLOUD + // clang-format off + PointNormalUV pn0, pn1, pn2; + pn0.x = lmk0_pos.x; pn1.x = lmk1_pos.x; pn2.x = lmk2_pos.x; + pn0.y = lmk0_pos.y; pn1.y = lmk1_pos.y; pn2.y = lmk2_pos.y; + pn0.z = lmk0_pos.z; pn1.z = lmk1_pos.z; pn2.z = lmk2_pos.z; + // OpenGL textures range from 0 to 1. + pn0.u = px0.x / cam_width; pn1.u = px1.x / cam_width; pn2.u = px2.x / cam_width; + pn0.v = px0.y / cam_height; pn1.v = px1.y / cam_height; pn2.v = px2.y / cam_height; + pn0.normal_x = normal0.x; pn1.normal_x = normal1.x; pn2.normal_x = normal2.x; + pn0.normal_y = normal0.y; pn1.normal_y = normal1.y; pn2.normal_y = normal2.y; + pn0.normal_z = normal0.z; pn1.normal_z = normal1.z; pn2.normal_z = normal2.z; + // clang-format on + + // TODO(Toni): we are adding repeated vertices!! + cloud.points.push_back(pn0); + cloud.points.push_back(pn1); + cloud.points.push_back(pn2); + + // Store polygon connectivity + pcl_msgs::Vertices vtx_ii; + vtx_ii.vertices.resize(3); + size_t idx = i * mesh_2d_poly_dim; + // Store connectivity CCW bcs of RVIZ + vtx_ii.vertices[0] = idx + 2; + vtx_ii.vertices[1] = idx + 1; + vtx_ii.vertices[2] = idx; + msg->polygons.push_back(vtx_ii); + } else { + // LOG_EVERY_N(ERROR, 1000) << "Polygon in 2d mesh did not have a + // corresponding polygon in" + // " 3d mesh!"; + } + } + + cloud.is_dense = false; + cloud.width = cloud.points.size(); + cloud.height = 1; + pcl::toROSMsg(cloud, msg->cloud); + + // NOTE: Header fields need to be filled in after pcl::toROSMsg() call. + msg->cloud.header = std_msgs::Header(); + msg->cloud.header.stamp = msg->header.stamp; + msg->cloud.header.frame_id = msg->header.frame_id; + + if (msg->polygons.size() > 0) { + mesh_3d_frame_pub_.publish(msg); + } + + return; +} // namespace VIO + +void RosVisualizer::publishState(const BackendOutput::ConstPtr& output) const { + CHECK(output); + // Get latest estimates for odometry. + const Timestamp& ts = output->timestamp_; + const gtsam::Pose3& pose = output->W_State_Blkf_.pose_; + const gtsam::Rot3& rotation = pose.rotation(); + const gtsam::Quaternion& quaternion = rotation.toQuaternion(); + const gtsam::Vector3& velocity = output->W_State_Blkf_.velocity_; + const gtsam::Matrix6& pose_cov = + gtsam::sub(output->state_covariance_lkf_, 0, 6, 0, 6); + const gtsam::Matrix3& vel_cov = + gtsam::sub(output->state_covariance_lkf_, 6, 9, 6, 9); + + // First publish odometry estimate + nav_msgs::Odometry odometry_msg; + + // Create header. + odometry_msg.header.stamp.fromNSec(ts); + odometry_msg.header.frame_id = world_frame_id_; + odometry_msg.child_frame_id = base_link_frame_id_; + + // Position + odometry_msg.pose.pose.position.x = pose.x(); + odometry_msg.pose.pose.position.y = pose.y(); + odometry_msg.pose.pose.position.z = pose.z(); + + // Orientation + odometry_msg.pose.pose.orientation.w = quaternion.w(); + odometry_msg.pose.pose.orientation.x = quaternion.x(); + odometry_msg.pose.pose.orientation.y = quaternion.y(); + odometry_msg.pose.pose.orientation.z = quaternion.z(); + + // Remap covariance from GTSAM convention + // to odometry convention and fill in covariance + static const std::vector remapping{3, 4, 5, 0, 1, 2}; + + // Position covariance first, angular covariance after + DCHECK_EQ(pose_cov.rows(), remapping.size()); + DCHECK_EQ(pose_cov.rows() * pose_cov.cols(), + odometry_msg.pose.covariance.size()); + for (int i = 0; i < pose_cov.rows(); i++) { + for (int j = 0; j < pose_cov.cols(); j++) { + odometry_msg.pose + .covariance[remapping[i] * pose_cov.cols() + remapping[j]] = + pose_cov(i, j); + } + } + + // Linear velocities, trivial values for angular + const gtsam::Matrix3& inversed_rotation = rotation.transpose(); + const Vector3 velocity_body = inversed_rotation * velocity; + odometry_msg.twist.twist.linear.x = velocity_body(0); + odometry_msg.twist.twist.linear.y = velocity_body(1); + odometry_msg.twist.twist.linear.z = velocity_body(2); + + // Velocity covariance: first linear + // and then angular (trivial values for angular) + const gtsam::Matrix3 vel_cov_body = + inversed_rotation.matrix() * vel_cov * rotation.matrix(); + DCHECK_EQ(vel_cov_body.rows(), 3); + DCHECK_EQ(vel_cov_body.cols(), 3); + DCHECK_EQ(odometry_msg.twist.covariance.size(), 36); + for (int i = 0; i < vel_cov_body.rows(); i++) { + for (int j = 0; j < vel_cov_body.cols(); j++) { + odometry_msg.twist + .covariance[i * static_cast( + sqrt(odometry_msg.twist.covariance.size())) + + j] = vel_cov_body(i, j); + } + } + // Publish message + odometry_pub_.publish(odometry_msg); +} + +void RosVisualizer::publishFrontendStats( + const FrontendOutput::ConstPtr& output) const { + CHECK(output); + + // Get frontend data for resiliency output + const DebugTrackerInfo& debug_tracker_info = output->getTrackerInfo(); + + // Create message type + std_msgs::Float64MultiArray frontend_stats_msg; + + // Build Message Layout + frontend_stats_msg.data.resize(13); + frontend_stats_msg.data[0] = debug_tracker_info.nrDetectedFeatures_; + frontend_stats_msg.data[1] = debug_tracker_info.nrTrackerFeatures_; + frontend_stats_msg.data[2] = debug_tracker_info.nrMonoInliers_; + frontend_stats_msg.data[3] = debug_tracker_info.nrMonoPutatives_; + frontend_stats_msg.data[4] = debug_tracker_info.nrStereoInliers_; + frontend_stats_msg.data[5] = debug_tracker_info.nrStereoPutatives_; + frontend_stats_msg.data[6] = debug_tracker_info.monoRansacIters_; + frontend_stats_msg.data[7] = debug_tracker_info.stereoRansacIters_; + frontend_stats_msg.data[8] = debug_tracker_info.nrValidRKP_; + frontend_stats_msg.data[9] = debug_tracker_info.nrNoLeftRectRKP_; + frontend_stats_msg.data[10] = debug_tracker_info.nrNoRightRectRKP_; + frontend_stats_msg.data[11] = debug_tracker_info.nrNoDepthRKP_; + frontend_stats_msg.data[12] = debug_tracker_info.nrFailedArunRKP_; + frontend_stats_msg.layout.dim.resize(1); + frontend_stats_msg.layout.dim[0].size = frontend_stats_msg.data.size(); + frontend_stats_msg.layout.dim[0].stride = 1; + frontend_stats_msg.layout.dim[0].label = + "FrontEnd: nrDetFeat, nrTrackFeat, nrMoIn, nrMoPu, nrStIn, nrStPu, " + "moRaIt, stRaIt, nrVaRKP, nrNoLRKP, nrNoRRKP, nrNoDRKP nrFaARKP"; + + // Publish Message + frontend_stats_pub_.publish(frontend_stats_msg); +} + +void RosVisualizer::publishResiliency( + const FrontendOutput::ConstPtr& frontend_output, + const BackendOutput::ConstPtr& backend_output) const { + CHECK(frontend_output); + CHECK(backend_output); + + // Get frontend and velocity covariance data for resiliency output + const DebugTrackerInfo& debug_tracker_info = + frontend_output->getTrackerInfo(); + const gtsam::Matrix6& pose_cov = + gtsam::sub(backend_output->state_covariance_lkf_, 0, 6, 0, 6); + const gtsam::Matrix3& vel_cov = + gtsam::sub(backend_output->state_covariance_lkf_, 6, 9, 6, 9); + + // Create message type for quality of SparkVIO + std_msgs::Float64MultiArray resiliency_msg; + + // Publishing extra information: + // cov_v_det and nrStIn should be the most relevant! + resiliency_msg.layout.dim[0].label = + "Values: cbrtPDet, cbrtVDet, nrStIn, nrMoIn. " + "Thresholds : cbrtPDet, cbrtVDet, nrStIn, nrMoIn."; + + CHECK_EQ(pose_cov.size(), 36); + gtsam::Matrix3 position_cov = gtsam::sub(pose_cov, 3, 6, 3, 6); + CHECK_EQ(position_cov.size(), 9); + + // Compute eigenvalues and determinant of velocity covariance + gtsam::Matrix U; + gtsam::Matrix V; + gtsam::Vector cov_v_eigv; + gtsam::svd(vel_cov, U, cov_v_eigv, V); + CHECK_EQ(cov_v_eigv.size(), 3); + + // Compute eigenvalues and determinant of position covariance + gtsam::Vector cov_p_eigv; + gtsam::svd(position_cov, U, cov_p_eigv, V); + CHECK_EQ(cov_p_eigv.size(), 3); + + // Quality statistics to publish + resiliency_msg.data.resize(8); + resiliency_msg.data[0] = + std::cbrt(cov_p_eigv(0) * cov_p_eigv(1) * cov_p_eigv(2)); + resiliency_msg.data[1] = + std::cbrt(cov_v_eigv(0) * cov_v_eigv(1) * cov_v_eigv(2)); + resiliency_msg.data[2] = debug_tracker_info.nrStereoInliers_; + resiliency_msg.data[3] = debug_tracker_info.nrMonoInliers_; + + // Publish thresholds for statistics + float pos_det_threshold, vel_det_threshold; + int mono_ransac_theshold, stereo_ransac_threshold; + CHECK(nh_private_.getParam("velocity_det_threshold", vel_det_threshold)); + CHECK(nh_private_.getParam("position_det_threshold", pos_det_threshold)); + CHECK( + nh_private_.getParam("stereo_ransac_threshold", stereo_ransac_threshold)); + CHECK(nh_private_.getParam("mono_ransac_threshold", mono_ransac_theshold)); + resiliency_msg.data[4] = pos_det_threshold; + resiliency_msg.data[5] = vel_det_threshold; + resiliency_msg.data[6] = stereo_ransac_threshold; + resiliency_msg.data[7] = mono_ransac_theshold; + + // Build Message Layout + resiliency_msg.layout.dim.resize(1); + resiliency_msg.layout.dim[0].size = resiliency_msg.data.size(); + resiliency_msg.layout.dim[0].stride = 1; + + // Publish Message + resiliency_pub_.publish(resiliency_msg); +} + +void RosVisualizer::publishImuBias(const BackendOutput::ConstPtr& output) const { + CHECK(output); + + // Get imu bias to output + const ImuBias& imu_bias = output->W_State_Blkf_.imu_bias_; + const Vector3& accel_bias = imu_bias.accelerometer(); + const Vector3& gyro_bias = imu_bias.gyroscope(); + + // Create message type + std_msgs::Float64MultiArray imu_bias_msg; + + // Get Imu Bias to Publish + imu_bias_msg.data.resize(6); + imu_bias_msg.data.at(0) = gyro_bias[0]; + imu_bias_msg.data.at(1) = gyro_bias[1]; + imu_bias_msg.data.at(2) = gyro_bias[2]; + imu_bias_msg.data.at(3) = accel_bias[0]; + imu_bias_msg.data.at(4) = accel_bias[1]; + imu_bias_msg.data.at(5) = accel_bias[2]; + + // Build Message Layout + imu_bias_msg.layout.dim.resize(1); + imu_bias_msg.layout.dim[0].size = imu_bias_msg.data.size(); + imu_bias_msg.layout.dim[0].stride = 1; + imu_bias_msg.layout.dim[0].label = "Gyro Bias: x,y,z. Accel Bias: x,y,z"; + + // Publish Message + imu_bias_pub_.publish(imu_bias_msg); +} + +void RosVisualizer::publishOptimizedTrajectory( + const LcdOutput::ConstPtr& lcd_output) const { + CHECK(lcd_output); + + // Get pgo-optimized trajectory + const Timestamp& ts = lcd_output->timestamp_kf_; + const gtsam::Values& trajectory = lcd_output->states_; + // Create message type + nav_msgs::Path path; + + // Fill path poses + path.poses.reserve(trajectory.size()); + for (size_t i = 0; i < trajectory.size(); i++) { + gtsam::Pose3 pose = trajectory.at(i); + gtsam::Point3 trans = pose.translation(); + gtsam::Quaternion quat = pose.rotation().toQuaternion(); + + geometry_msgs::PoseStamped ps_msg; + ps_msg.header.frame_id = world_frame_id_; + ps_msg.pose.position.x = trans.x(); + ps_msg.pose.position.y = trans.y(); + ps_msg.pose.position.z = trans.z(); + ps_msg.pose.orientation.x = quat.x(); + ps_msg.pose.orientation.y = quat.y(); + ps_msg.pose.orientation.z = quat.z(); + ps_msg.pose.orientation.w = quat.w(); + + path.poses.push_back(ps_msg); + } + + // Publish path message + path.header.stamp.fromNSec(ts); + path.header.frame_id = world_frame_id_; + trajectory_pub_.publish(path); +} + +void RosVisualizer::updateRejectedEdges() { + // first update the rejected edges + for (pose_graph_tools::PoseGraphEdge& loop_closure_edge : + loop_closure_edges_) { + bool is_inlier = false; + for (pose_graph_tools::PoseGraphEdge& inlier_edge : inlier_edges_) { + if (loop_closure_edge.key_from == inlier_edge.key_from && + loop_closure_edge.key_to == inlier_edge.key_to) { + is_inlier = true; + continue; + } + } + if (!is_inlier) { + // set as rejected loop closure + loop_closure_edge.type = + pose_graph_tools::PoseGraphEdge::REJECTED_LOOPCLOSE; + } + } + + // Then update the loop edges + for (pose_graph_tools::PoseGraphEdge& inlier_edge : inlier_edges_) { + bool previously_stored = false; + for (pose_graph_tools::PoseGraphEdge& loop_closure_edge : + loop_closure_edges_) { + if (inlier_edge.key_from == loop_closure_edge.key_from && + inlier_edge.key_to == loop_closure_edge.key_to) { + previously_stored = true; + continue; + } + } + if (!previously_stored) { + // add to the vector of all loop clousres + loop_closure_edges_.push_back(inlier_edge); + } + } +} + +void RosVisualizer::updateNodesAndEdges(const gtsam::NonlinearFactorGraph& nfg, + const gtsam::Values& values) { + inlier_edges_.clear(); + odometry_edges_.clear(); + // first store the factors as edges + for (size_t i = 0; i < nfg.size(); i++) { + // check if between factor + if (boost::dynamic_pointer_cast >( + nfg[i])) { + // convert to between factor + const gtsam::BetweenFactor& factor = + *boost::dynamic_pointer_cast >( + nfg[i]); + // convert between factor to PoseGraphEdge type + pose_graph_tools::PoseGraphEdge edge; + edge.header.frame_id = world_frame_id_; + edge.key_from = factor.front(); + edge.key_to = factor.back(); + if (edge.key_to == edge.key_from + 1) { // check if odom + edge.type = pose_graph_tools::PoseGraphEdge::ODOM; + } else { + edge.type = pose_graph_tools::PoseGraphEdge::LOOPCLOSE; + } + // transforms - translation + const gtsam::Point3& translation = factor.measured().translation(); + edge.pose.position.x = translation.x(); + edge.pose.position.y = translation.y(); + edge.pose.position.z = translation.z(); + // transforms - rotation (to quaternion) + const gtsam::Quaternion& quaternion = + factor.measured().rotation().toQuaternion(); + edge.pose.orientation.x = quaternion.x(); + edge.pose.orientation.y = quaternion.y(); + edge.pose.orientation.z = quaternion.z(); + edge.pose.orientation.w = quaternion.w(); + + // TODO: add covariance + if (edge.type == pose_graph_tools::PoseGraphEdge::ODOM) { + odometry_edges_.push_back(edge); + } else { + inlier_edges_.push_back(edge); + } + } + } + + // update inliers and rejected closures + updateRejectedEdges(); + + pose_graph_nodes_.clear(); + // Then store the values as nodes + gtsam::KeyVector key_list = values.keys(); + for (size_t i = 0; i < key_list.size(); i++) { + pose_graph_tools::PoseGraphNode node; + node.key = key_list[i]; + + const gtsam::Pose3& value = values.at(i); + const gtsam::Point3& translation = value.translation(); + const gtsam::Quaternion& quaternion = value.rotation().toQuaternion(); + + // pose - translation + node.pose.position.x = translation.x(); + node.pose.position.y = translation.y(); + node.pose.position.z = translation.z(); + // pose - rotation (to quaternion) + node.pose.orientation.x = quaternion.x(); + node.pose.orientation.y = quaternion.y(); + node.pose.orientation.z = quaternion.z(); + node.pose.orientation.w = quaternion.w(); + + pose_graph_nodes_.push_back(node); + } + + return; +} + +pose_graph_tools::PoseGraph RosVisualizer::getPosegraphMsg() { + // pose graph getter + pose_graph_tools::PoseGraph pose_graph; + pose_graph.edges = odometry_edges_; // add odometry edges to pg + // then add loop closure edges to pg + pose_graph.edges.insert(pose_graph.edges.end(), + loop_closure_edges_.begin(), + loop_closure_edges_.end()); + // then add the nodes + pose_graph.nodes = pose_graph_nodes_; + + return pose_graph; +} + +void RosVisualizer::publishPoseGraph(const LcdOutput::ConstPtr& lcd_output) { + CHECK(lcd_output); + + // Get the factor graph + const Timestamp& ts = lcd_output->timestamp_kf_; + const gtsam::NonlinearFactorGraph& nfg = lcd_output->nfg_; + const gtsam::Values& values = lcd_output->states_; + updateNodesAndEdges(nfg, values); + pose_graph_tools::PoseGraph graph = getPosegraphMsg(); + graph.header.stamp.fromNSec(ts); + graph.header.frame_id = world_frame_id_; + posegraph_pub_.publish(graph); +} + +void RosVisualizer::publishTf(const LcdOutput::ConstPtr& lcd_output) { + CHECK(lcd_output); + + const Timestamp& ts = lcd_output->timestamp_kf_; + const gtsam::Pose3& w_Pose_map = lcd_output->W_Pose_Map_; + const gtsam::Quaternion& w_Quat_map = w_Pose_map.rotation().toQuaternion(); + // Publish map TF. + geometry_msgs::TransformStamped map_tf; + map_tf.header.stamp.fromNSec(ts); + map_tf.header.frame_id = world_frame_id_; + map_tf.child_frame_id = map_frame_id_; + utils::poseToMsgTF(w_Pose_map, &map_tf.transform); + tf_broadcaster_.sendTransform(map_tf); +} + +void RosVisualizer::publishTf(const BackendOutput::ConstPtr& output) { + CHECK(output); + + const Timestamp& timestamp = output->timestamp_; + const gtsam::Pose3& pose = output->W_State_Blkf_.pose_; + // const gtsam::Quaternion& quaternion = pose.rotation().toQuaternion(); + // Publish base_link TF. + geometry_msgs::TransformStamped odom_tf; + odom_tf.header.stamp.fromNSec(timestamp); + odom_tf.header.frame_id = world_frame_id_; + odom_tf.child_frame_id = base_link_frame_id_; + + utils::poseToMsgTF(pose, &odom_tf.transform); + tf_broadcaster_.sendTransform(odom_tf); +} + +} // namespace VIO diff --git a/src/utils/UtilsRos.cpp b/src/utils/UtilsRos.cpp new file mode 100644 index 00000000..9c60e568 --- /dev/null +++ b/src/utils/UtilsRos.cpp @@ -0,0 +1,90 @@ +/** + * @file UtilsRos.cpp + * @brief Utilities to convert from/to Ros + * @author Antoni Rosinol + */ + +#include "kimera_vio_ros/utils/UtilsRos.h" + +namespace VIO { + +namespace utils { + +void msgTFtoPose(const geometry_msgs::Transform& tf, gtsam::Pose3* pose) { + CHECK_NOTNULL(pose); + + *pose = gtsam::Pose3( + gtsam::Rot3(gtsam::Quaternion( + tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z)), + gtsam::Point3(tf.translation.x, tf.translation.y, tf.translation.z)); +} + +void poseToMsgTF(const gtsam::Pose3& pose, geometry_msgs::Transform* tf) { + CHECK_NOTNULL(tf); + + tf->translation.x = pose.x(); + tf->translation.y = pose.y(); + tf->translation.z = pose.z(); + const gtsam::Quaternion& quat = pose.rotation().toQuaternion(); + tf->rotation.w = quat.w(); + tf->rotation.x = quat.x(); + tf->rotation.y = quat.y(); + tf->rotation.z = quat.z(); +} + +void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info, + const std::string& base_link_frame_id, + const std::string& cam_frame_id, + CameraParams* cam_params) { + CHECK_NOTNULL(cam_params); + + // Get intrinsics from incoming CameraInfo messages: + cam_params->camera_id_ = cam_info->header.frame_id; + CHECK(!cam_params->camera_id_.empty()); + + cam_params->distortion_model_ = cam_info->distortion_model; + CHECK(cam_params->distortion_model_ == "radtan" || + cam_params->distortion_model_ == "radial-tangential" || + cam_params->distortion_model_ == "equidistant"); + + const std::vector& distortion_coeffs = cam_info->D; + CHECK_EQ(distortion_coeffs.size(), 4); + VIO::CameraParams::convertDistortionVectorToMatrix( + distortion_coeffs, &cam_params->distortion_coeff_); + + cam_params->image_size_ = cv::Size(cam_info->width, cam_info->height); + + cam_params->frame_rate_ = 0; // TODO(marcus): is there a way to get this? + + std::array intrinsics = { + cam_info->K[0], cam_info->K[4], cam_info->K[2], cam_info->K[5]}; + cam_params->intrinsics_ = intrinsics; + VIO::CameraParams::convertIntrinsicsVectorToMatrix(cam_params->intrinsics_, + &cam_params->K_); + + VIO::CameraParams::createGtsamCalibration(cam_params->distortion_coeff_, + cam_params->intrinsics_, + &cam_params->calibration_); + + // Get extrinsics from the TF tree: + tf2_ros::Buffer t_buffer; + static constexpr size_t kTfLookupTimeout = 5u; + geometry_msgs::TransformStamped cam_tf; + try { + cam_tf = t_buffer.lookupTransform(base_link_frame_id, + cam_frame_id, + ros::Time(0), + ros::Duration(kTfLookupTimeout)); + } catch (tf2::TransformException& ex) { + LOG(FATAL) + << "TF for left/right camera frames not available. Either publish to " + "tree or provide CameraParameter yaml files. Error: \n" + << ex.what(); + } + + msgTFtoPose(cam_tf.transform, &cam_params->body_Pose_cam_); +} + +} // namespace utils + +} // namespace VIO