From a2c1b220ec89a5dbf81a258a71352337d9adcbbb Mon Sep 17 00:00:00 2001 From: Toni Date: Sat, 9 May 2020 17:50:54 -0400 Subject: [PATCH 1/8] Add ros publisher manager, not tested --- include/kimera_vio_ros/RosPublishers.h | 88 ++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 include/kimera_vio_ros/RosPublishers.h diff --git a/include/kimera_vio_ros/RosPublishers.h b/include/kimera_vio_ros/RosPublishers.h new file mode 100644 index 00000000..26b3a1bc --- /dev/null +++ b/include/kimera_vio_ros/RosPublishers.h @@ -0,0 +1,88 @@ +#pragma once + +#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. +*/ +template +class RosPublishers { + typedef std::unordered_map RosPubs; + + 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_() {} + 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] = nh_private_.advertise( + base_topic_name_ + std::to_string(idx), queue_size_, latch_msgs_); + } + + // Publish msg (use .at since pub_it might be invalid). + pubs_.at(idx).publish(msg); + } + + 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 stuff + ros::NodeHandle nh_private_; +}; +} // namespace VIO From 48c61dbf60021b2a6875e6b91d4cf8c4837b28b9 Mon Sep 17 00:00:00 2001 From: Toni Date: Sat, 9 May 2020 20:36:36 -0400 Subject: [PATCH 2/8] Add RosPublishers, can handle image_transport as well! --- .../kimera_vio_ros/RosDataProviderInterface.h | 12 ++- .../kimera_vio_ros/RosOnlineDataProvider.h | 3 + include/kimera_vio_ros/RosPublishers.h | 78 +++++++++++++++++-- src/RosDataProviderInterface.cpp | 25 +++--- src/RosOnlineDataProvider.cpp | 3 +- 5 files changed, 95 insertions(+), 26 deletions(-) diff --git a/include/kimera_vio_ros/RosDataProviderInterface.h b/include/kimera_vio_ros/RosDataProviderInterface.h index 4bfb1f00..e390b783 100644 --- a/include/kimera_vio_ros/RosDataProviderInterface.h +++ b/include/kimera_vio_ros/RosDataProviderInterface.h @@ -26,7 +26,6 @@ #include #include -#include #include #include #include @@ -34,8 +33,11 @@ #include #include #include +#include #include +#include "kimera_vio_ros/RosPublishers.h" + namespace VIO { /** @@ -133,9 +135,6 @@ class RosDataProviderInterface : public DataProviderInterface { 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_; @@ -195,9 +194,8 @@ class RosDataProviderInterface : public DataProviderInterface { void printParsedParams() const; private: - // Define publisher for debug images. - image_transport::Publisher debug_img_pub_; - image_transport::Publisher feature_tracks_pub_; + // Define image publishers manager + std::unique_ptr image_publishers_; // Publishers ros::Publisher pointcloud_pub_; diff --git a/include/kimera_vio_ros/RosOnlineDataProvider.h b/include/kimera_vio_ros/RosOnlineDataProvider.h index bbc76d6e..50013a17 100644 --- a/include/kimera_vio_ros/RosOnlineDataProvider.h +++ b/include/kimera_vio_ros/RosOnlineDataProvider.h @@ -75,6 +75,9 @@ class RosOnlineDataProvider : public RosDataProviderInterface { 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_; diff --git a/include/kimera_vio_ros/RosPublishers.h b/include/kimera_vio_ros/RosPublishers.h index 26b3a1bc..a7815748 100644 --- a/include/kimera_vio_ros/RosPublishers.h +++ b/include/kimera_vio_ros/RosPublishers.h @@ -3,6 +3,9 @@ #include #include +#include + +#include #include namespace VIO { @@ -14,10 +17,21 @@ namespace VIO { * 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 +template class RosPublishers { - typedef std::unordered_map RosPubs; + private: + //! Map from unique idx to ros Publisher (either a classic ros::Publisher or + //! a image_transport::Publisher). + using RosPubs = std::unordered_map; public: /** @@ -39,7 +53,11 @@ class RosPublishers { queue_size_(queue_size), latch_msgs_(latch_msgs), nh_private_(nh_private), - pubs_() {} + 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: @@ -62,14 +80,52 @@ class RosPublishers { // If publisher does not exist, we create a new one. if (pub_it == pubs_.end()) { - pubs_[idx] = nh_private_.advertise( - base_topic_name_ + std::to_string(idx), queue_size_, latch_msgs_); + 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_; @@ -82,7 +138,15 @@ class RosPublishers { //! Map of indices to publishers, it contains all our publishers. RosPubs pubs_; - //! Ros stuff - ros::NodeHandle nh_private_; + //! 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/src/RosDataProviderInterface.cpp b/src/RosDataProviderInterface.cpp index 2f58c60e..09d4011c 100644 --- a/src/RosDataProviderInterface.cpp +++ b/src/RosDataProviderInterface.cpp @@ -37,7 +37,6 @@ namespace VIO { RosDataProviderInterface::RosDataProviderInterface(const VioParams& vio_params) : DataProviderInterface(), - it_(nullptr), nh_(), nh_private_("~"), backend_output_queue_("Backend output ROS"), @@ -45,13 +44,14 @@ RosDataProviderInterface::RosDataProviderInterface(const VioParams& vio_params) 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) { + vio_params_(vio_params), + image_publishers_(nullptr) { VLOG(1) << "Initializing RosDataProviderInterface."; // Print parameters to check. if (VLOG_IS_ON(1)) printParsedParams(); - it_ = VIO::make_unique(nh_); + image_publishers_ = VIO::make_unique(nh_private_); // Get ROS params CHECK(nh_private_.getParam("base_link_frame_id", base_link_frame_id_)); @@ -76,8 +76,6 @@ RosDataProviderInterface::RosDataProviderInterface(const VioParams& vio_params) 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_, @@ -170,16 +168,20 @@ void RosDataProviderInterface::publishFrontendOutput( if (frontend_stats_pub_.getNumSubscribers() > 0) { publishFrontendStats(output); } - if (feature_tracks_pub_.getNumSubscribers() > 0) { + + CHECK_NOTNULL(image_publishers_); + if (image_publishers_->getNumSubscribersForPublisher("feature_tracks") > 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( + image_publishers_->publish( + "feature_tracks", cv_bridge::CvImage(h, "bgr8", output->feature_tracks_).toImageMsg()); } else { - LOG(ERROR) << feature_tracks_pub_.getNumSubscribers() + LOG(ERROR) << image_publishers_->getNumSubscribersForPublisher( + "feature_tracks") << " 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?"; @@ -222,7 +224,8 @@ bool RosDataProviderInterface::publishSyncedOutputs() { if (frontend_output && mesher_output) { // Publish 2d mesh debug image - if (debug_img_pub_.getNumSubscribers() > 0) { + CHECK_NOTNULL(image_publishers_); + if (image_publishers_->getNumSubscribersForPublisher("debug_img") > 0) { cv::Mat mesh_2d_img = Visualizer3D::visualizeMesh2D( mesher_output->mesh_2d_for_viz_, frontend_output->stereo_frame_lkf_.getLeftFrame().img_); @@ -333,8 +336,8 @@ void RosDataProviderInterface::publishDebugImage( h.stamp.fromNSec(timestamp); h.frame_id = base_link_frame_id_; // Copies... - debug_img_pub_.publish( - cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); + image_publishers_->publish( + "debug_img", cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); } // void RosBaseDataProvider::publishTimeHorizonMesh3D( diff --git a/src/RosOnlineDataProvider.cpp b/src/RosOnlineDataProvider.cpp index f44287f3..e38ade32 100644 --- a/src/RosOnlineDataProvider.cpp +++ b/src/RosOnlineDataProvider.cpp @@ -20,6 +20,7 @@ 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 +157,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( From 063281790963760b760a6ce17a2ae30c7f92f2cf Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 00:53:22 -0400 Subject: [PATCH 3/8] WIP: make publishing to ROS inherit from DisplayBase --- include/kimera_vio_ros/KimeraVioRos.h | 1 + .../kimera_vio_ros/RosDataProviderInterface.h | 1 - include/kimera_vio_ros/RosDisplay.h | 58 +++++++++++ .../kimera_vio_ros/RosOnlineDataProvider.h | 5 +- src/KimeraVioRos.cpp | 31 +++--- src/RosOnlineDataProvider.cpp | 95 ++++++++----------- 6 files changed, 115 insertions(+), 76 deletions(-) create mode 100644 include/kimera_vio_ros/RosDisplay.h diff --git a/include/kimera_vio_ros/KimeraVioRos.h b/include/kimera_vio_ros/KimeraVioRos.h index 6450c55c..50497707 100644 --- a/include/kimera_vio_ros/KimeraVioRos.h +++ b/include/kimera_vio_ros/KimeraVioRos.h @@ -11,6 +11,7 @@ #include #include "kimera_vio_ros/RosDataProviderInterface.h" +#include "kimera_vio_ros/RosDisplay.h" namespace VIO { diff --git a/include/kimera_vio_ros/RosDataProviderInterface.h b/include/kimera_vio_ros/RosDataProviderInterface.h index e390b783..19f793ee 100644 --- a/include/kimera_vio_ros/RosDataProviderInterface.h +++ b/include/kimera_vio_ros/RosDataProviderInterface.h @@ -187,7 +187,6 @@ class RosDataProviderInterface : public DataProviderInterface { pose_graph_tools::PoseGraph getPosegraphMsg(); - // Publish/print debugging information. void publishDebugImage(const Timestamp& timestamp, const cv::Mat& debug_image) const; diff --git a/include/kimera_vio_ros/RosDisplay.h b/include/kimera_vio_ros/RosDisplay.h new file mode 100644 index 00000000..b4766146 --- /dev/null +++ b/include/kimera_vio_ros/RosDisplay.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#include + +#include +#include +#include + +#include + +namespace VIO { + +class RosDisplay : public DisplayBase { + public: + KIMERA_POINTER_TYPEDEFS(RosDisplay); + KIMERA_DELETE_COPY_CONSTRUCTORS(RosDisplay); + + public: + RosDisplay() : + DisplayBase(), + nh_private_("~"), + image_publishers_(nullptr) { + image_publishers_ = VIO::make_unique(nh_private_); + } + virtual ~RosDisplay() = default; + + public: + // Spins the display once to render the visualizer output. + virtual void spinOnce(VisualizerOutput::UniquePtr&& viz_output) { + CHECK(viz_output); + // Display 2D images. + spin2dWindow(*viz_output); + // ros::spinOnce(); + } + + protected: + void spin2dWindow(const VisualizerOutput& viz_output) { + for (const ImageToDisplay& img : viz_output.images_to_display_) { + std_msgs::Header header; + header.frame_id = "base_link_frame_id_"; // TODO TONI REMOVE! + // header.stamp.fromNSec(ros::Time::now()); + header.stamp = ros::Time::now(); // TODO TONI TIMESTAMP! + image_publishers_->publish( + img.name_, + cv_bridge::CvImage(header, "bgr8", img.image_).toImageMsg()); + } + } + + private: + ros::NodeHandle nh_private_; + // Define image publishers manager + std::unique_ptr image_publishers_; +}; + +} // namespace VIO diff --git a/include/kimera_vio_ros/RosOnlineDataProvider.h b/include/kimera_vio_ros/RosOnlineDataProvider.h index 50013a17..db9b42a7 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 diff --git a/src/KimeraVioRos.cpp b/src/KimeraVioRos.cpp index 48d4accc..77565b83 100644 --- a/src/KimeraVioRos.cpp +++ b/src/KimeraVioRos.cpp @@ -57,7 +57,9 @@ bool KimeraVioRos::runKimeraVio() { // Then, create Kimera-VIO from scratch. VLOG(1) << "Creating Kimera-VIO."; CHECK(vio_params_); - vio_pipeline_ = VIO::make_unique(*vio_params_); + vio_pipeline_ = + VIO::make_unique(*vio_params_, + VIO::make_unique()); CHECK(vio_pipeline_) << "Vio pipeline construction failed."; // Second, destroy dataset parser. @@ -85,20 +87,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."; @@ -112,9 +113,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. @@ -126,7 +127,7 @@ bool KimeraVioRos::spin() { ros::start(); while (ros::ok() && data_provider_->spin() && vio_pipeline_->spin()) { LOG(INFO) << vio_pipeline_->printStatistics(); - continue; + vio_pipeline_->spinViz(); } LOG(INFO) << "Shutting down ROS and VIO pipeline."; ros::shutdown(); @@ -140,8 +141,8 @@ bool KimeraVioRos::spin() { return is_pipeline_successful; } -RosDataProviderInterface::UniquePtr -KimeraVioRos::createDataProvider(const VioParams& vio_params) { +RosDataProviderInterface::UniquePtr KimeraVioRos::createDataProvider( + const VioParams& vio_params) { ros::NodeHandle nh_("~"); bool online_run = false; CHECK(nh_.getParam("online_run", online_run)); diff --git a/src/RosOnlineDataProvider.cpp b/src/RosOnlineDataProvider.cpp index e38ade32..cf05c692 100644 --- a/src/RosOnlineDataProvider.cpp +++ b/src/RosOnlineDataProvider.cpp @@ -188,10 +188,27 @@ RosOnlineDataProvider::RosOnlineDataProvider(const VioParams& vio_params) // 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 @@ -345,60 +362,26 @@ 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."; - - // 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(); // TODO(marcus): need a sequential mode? - } - - 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; -} - -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); - } - - // ros::spinOnce(); // No need because we use an async spinner, see ctor. - - return true; -} +//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); +// // } +// +// // ros::spinOnce(); // No need because we use an async spinner, see ctor. +// +// return true; +//} } // namespace VIO From 8ffd13914111e45898b8bb844aca793af17fe0bb Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 00:53:56 -0400 Subject: [PATCH 4/8] Visualize flag now refers to rviz as well --- launch/kimera_vio_ros.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/kimera_vio_ros.launch b/launch/kimera_vio_ros.launch index 476b82d4..fd42e5a4 100644 --- a/launch/kimera_vio_ros.launch +++ b/launch/kimera_vio_ros.launch @@ -54,9 +54,9 @@ - + - + From 0b1ac0f42e8199306a40a87ba250e4102d0873dc Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 18:21:07 -0400 Subject: [PATCH 5/8] WIP --- CMakeLists.txt | 1 + include/kimera_vio_ros/KimeraVioRos.h | 3 +- include/kimera_vio_ros/RosBagDataProvider.h | 20 +- .../kimera_vio_ros/RosDataProviderInterface.h | 172 +--- include/kimera_vio_ros/RosDisplay.h | 228 ++++- .../kimera_vio_ros/RosOnlineDataProvider.h | 1 + include/kimera_vio_ros/utils/UtilsRos.h | 32 + package.xml | 3 +- src/KimeraVioRos.cpp | 35 +- src/RosBagDataProvider.cpp | 46 +- src/RosDataProviderInterface.cpp | 874 +----------------- src/RosDisplay.cpp | 751 +++++++++++++++ src/utils/UtilsRos.cpp | 101 ++ 13 files changed, 1160 insertions(+), 1107 deletions(-) create mode 100644 include/kimera_vio_ros/utils/UtilsRos.h create mode 100644 src/RosDisplay.cpp create mode 100644 src/utils/UtilsRos.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9120fbc6..21be66f9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ cs_add_library(${PROJECT_NAME} src/RosDataProviderInterface.cpp src/RosBagDataProvider.cpp src/RosOnlineDataProvider.cpp + src/RosDisplay.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 50497707..1f3c3f84 100644 --- a/include/kimera_vio_ros/KimeraVioRos.h +++ b/include/kimera_vio_ros/KimeraVioRos.h @@ -30,7 +30,7 @@ class KimeraVioRos { VIO::RosDataProviderInterface::UniquePtr createDataProvider( const VioParams& vio_params); - void connectVioPipelineAndDataProvider(); + void connectVIO(); bool restartKimeraVio(std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response); @@ -39,6 +39,7 @@ class KimeraVioRos { VioParams::Ptr vio_params_; Pipeline::UniquePtr vio_pipeline_; RosDataProviderInterface::UniquePtr data_provider_; + RosDisplay ros_display_; ros::ServiceServer restart_vio_pipeline_srv_; std::atomic_bool restart_vio_pipeline_; }; diff --git a/include/kimera_vio_ros/RosBagDataProvider.h b/include/kimera_vio_ros/RosBagDataProvider.h index 77ddcd3b..41d14684 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 publishBackendOutput(const BackendOutput::Ptr& output); //override; // 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 19f793ee..024430c8 100644 --- a/include/kimera_vio_ros/RosDataProviderInterface.h +++ b/include/kimera_vio_ros/RosDataProviderInterface.h @@ -10,47 +10,19 @@ #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 "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 RosDataProviderInterface : public DataProviderInterface { public: KIMERA_DELETE_COPY_CONSTRUCTORS(RosDataProviderInterface); @@ -61,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: @@ -95,60 +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 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... @@ -156,75 +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(); - - void publishDebugImage(const Timestamp& timestamp, - const cv::Mat& debug_image) const; - void printParsedParams() const; - - private: - // Define image publishers manager - std::unique_ptr image_publishers_; - - // 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 index b4766146..6d93aba4 100644 --- a/include/kimera_vio_ros/RosDisplay.h +++ b/include/kimera_vio_ros/RosDisplay.h @@ -2,28 +2,104 @@ #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 +#include + +#include 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 RosDisplay : public DisplayBase { public: KIMERA_POINTER_TYPEDEFS(RosDisplay); KIMERA_DELETE_COPY_CONSTRUCTORS(RosDisplay); public: - RosDisplay() : - DisplayBase(), - nh_private_("~"), - image_publishers_(nullptr) { + RosDisplay(const VioParams& vio_params) + : DisplayBase(), + nh_(), + nh_private_("~"), + vio_params_(vio_params), + 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"), + image_publishers_(nullptr) { 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()); + 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); + + 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_); } virtual ~RosDisplay() = default; @@ -36,23 +112,147 @@ class RosDisplay : public DisplayBase { // ros::spinOnce(); } - protected: - void spin2dWindow(const VisualizerOutput& viz_output) { - for (const ImageToDisplay& img : viz_output.images_to_display_) { - std_msgs::Header header; - header.frame_id = "base_link_frame_id_"; // TODO TONI REMOVE! - // header.stamp.fromNSec(ros::Time::now()); - header.stamp = ros::Time::now(); // TODO TONI TIMESTAMP! - image_publishers_->publish( - img.name_, - cv_bridge::CvImage(header, "bgr8", img.image_).toImageMsg()); + 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); + } + + 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: + // 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); + + 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(); + + void publishDebugImage(const Timestamp& timestamp, + const cv::Mat& debug_image) const; + private: + ros::NodeHandle nh_; ros::NodeHandle nh_private_; + + // Just to get left/right cam to body tfs... + VioParams vio_params_; + // Define image publishers manager std::unique_ptr image_publishers_; + + // 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_; + 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_; + + 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/RosOnlineDataProvider.h b/include/kimera_vio_ros/RosOnlineDataProvider.h index db9b42a7..a42c912f 100644 --- a/include/kimera_vio_ros/RosOnlineDataProvider.h +++ b/include/kimera_vio_ros/RosOnlineDataProvider.h @@ -68,6 +68,7 @@ class RosOnlineDataProvider : public RosDataProviderInterface { void callbackReinitPose(const geometry_msgs::PoseStamped& reinitPose); private: + // TODO(Toni): perhaps put in utils void msgGtOdomToVioNavState(const nav_msgs::Odometry::ConstPtr& gt_odom, VioNavState* vio_navstate); diff --git a/include/kimera_vio_ros/utils/UtilsRos.h b/include/kimera_vio_ros/utils/UtilsRos.h new file mode 100644 index 00000000..fa25c535 --- /dev/null +++ b/include/kimera_vio_ros/utils/UtilsRos.h @@ -0,0 +1,32 @@ +/** + * @file UtilsRos.h + * @brief Utilities to convert from/to ROS types + * @author Antoni Rosinol + */ + +#pragma once + +#include +#include +#include + +#include + +namespace VIO { + +namespace utils { + +namespace gtsam { +class Pose3; +} + +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, + CameraParams* cam_params); +} // namespace utils + +} // namespace VIO 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 77565b83..0684b750 100644 --- a/src/KimeraVioRos.cpp +++ b/src/KimeraVioRos.cpp @@ -34,6 +34,7 @@ KimeraVioRos::KimeraVioRos() : vio_params_(nullptr), vio_pipeline_(nullptr), data_provider_(nullptr), + ros_display_(nullptr), restart_vio_pipeline_srv_(), restart_vio_pipeline_(false) { // Add rosservice to restart VIO pipeline if requested. @@ -51,15 +52,21 @@ 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(); + + VLOG(1) << "Creating Ros Display."; + CHECK(vio_params_); + ros_display_ = 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_, - VIO::make_unique()); + CHECK(ros_display_); + vio_pipeline_ = VIO::make_unique(*vio_params_, + std::move(ros_display_)); CHECK(vio_pipeline_) << "Vio pipeline construction failed."; // Second, destroy dataset parser. @@ -73,7 +80,7 @@ bool KimeraVioRos::runKimeraVio() { // Finally, connect data_provider and vio_pipeline VLOG(1) << "Connecting Vio Pipeline and Data Provider."; - connectVioPipelineAndDataProvider(); + connectVIO(); // Run return spin(); @@ -156,7 +163,7 @@ RosDataProviderInterface::UniquePtr KimeraVioRos::createDataProvider( return nullptr; } -void KimeraVioRos::connectVioPipelineAndDataProvider() { +void KimeraVioRos::connectVIO() { CHECK(data_provider_); CHECK(vio_pipeline_); @@ -168,18 +175,18 @@ void KimeraVioRos::connectVioPipelineAndDataProvider() { // Register callback to retrieve vio pipeline output from all modules. vio_pipeline_->registerBackendOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackBackendOutput, - std::ref(*data_provider_), + std::bind(&VIO::RosDisplay::callbackBackendOutput, + std::ref(*ros_display_), std::placeholders::_1)); vio_pipeline_->registerFrontendOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackFrontendOutput, - std::ref(*data_provider_), + std::bind(&VIO::RosDisplay::callbackFrontendOutput, + std::ref(*ros_display_), std::placeholders::_1)); vio_pipeline_->registerMesherOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackMesherOutput, - std::ref(*data_provider_), + std::bind(&VIO::RosDisplay::callbackMesherOutput, + std::ref(*ros_display_), std::placeholders::_1)); bool use_lcd = false; @@ -187,8 +194,8 @@ void KimeraVioRos::connectVioPipelineAndDataProvider() { nh_.getParam("use_lcd", use_lcd); if (use_lcd) { vio_pipeline_->registerLcdOutputCallback( - std::bind(&VIO::RosDataProviderInterface::callbackLcdOutput, - std::ref(*data_provider_), + std::bind(&VIO::RosDisplay::callbackLcdOutput, + std::ref(*ros_display_), std::placeholders::_1)); } diff --git a/src/RosBagDataProvider.cpp b/src/RosBagDataProvider.cpp index d598b3e4..c5a04c32 100644 --- a/src/RosBagDataProvider.cpp +++ b/src/RosBagDataProvider.cpp @@ -111,16 +111,16 @@ 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(); - if (!got_synced_outputs) { - LOG(WARNING) << "Pipeline lagging behind rosbag parser."; - } + // bool got_synced_outputs = publishSyncedOutputs(); + // if (!got_synced_outputs) { + // LOG(WARNING) << "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 { @@ -140,20 +140,20 @@ bool RosbagDataProvider::spin() { LOG(INFO) << "Rosbag processing finished."; // Endless loop until ros dies to publish left-over outputs. - while (nh_.ok() && ros::ok() && !ros::isShuttingDown() && !shutdown_) { - 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); - } - } + // while (nh_.ok() && ros::ok() && !ros::isShuttingDown() && !shutdown_) { + // 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); + // } + // } return true; } diff --git a/src/RosDataProviderInterface.cpp b/src/RosDataProviderInterface.cpp index 09d4011c..6085d3da 100644 --- a/src/RosDataProviderInterface.cpp +++ b/src/RosDataProviderInterface.cpp @@ -13,24 +13,10 @@ #include #include -#include -#include -#include #include -#include #include -#include -#include -#include -#include - -#include -#include -#include #include -#include -#include #include namespace VIO { @@ -39,50 +25,11 @@ RosDataProviderInterface::RosDataProviderInterface(const VioParams& vio_params) : DataProviderInterface(), 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), - image_publishers_(nullptr) { + vio_params_(vio_params) { VLOG(1) << "Initializing RosDataProviderInterface."; // Print parameters to check. if (VLOG_IS_ON(1)) printParsedParams(); - - 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()); - 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); - - 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() { @@ -147,747 +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); - } - - CHECK_NOTNULL(image_publishers_); - if (image_publishers_->getNumSubscribersForPublisher("feature_tracks") > 0) { - if (!output->feature_tracks_.empty()) { - std_msgs::Header h; - h.stamp.fromNSec(output->timestamp_); - h.frame_id = base_link_frame_id_; - // Copies... - image_publishers_->publish( - "feature_tracks", - cv_bridge::CvImage(h, "bgr8", output->feature_tracks_).toImageMsg()); - } else { - LOG(ERROR) << image_publishers_->getNumSubscribersForPublisher( - "feature_tracks") - << " 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 - CHECK_NOTNULL(image_publishers_); - if (image_publishers_->getNumSubscribersForPublisher("debug_img") > 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... - image_publishers_->publish( - "debug_img", 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:"; @@ -905,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..ae260ea9 --- /dev/null +++ b/src/RosDisplay.cpp @@ -0,0 +1,751 @@ +#pragma once + +#include "kimera_vio_ros/RosDisplay.h" + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "kimera_vio_ros/utils/UtilsRos.h" + +namespace VIO { + +void RosDisplay::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 RosDisplay::publishFrontendOutput( + const FrontendOutput::Ptr& output) const { + CHECK(output); + if (frontend_stats_pub_.getNumSubscribers() > 0) { + publishFrontendStats(output); + } +} + +void RosDisplay::publishMesherOutput( + const MesherOutput::Ptr& output) const { + CHECK(output); + if (mesh_3d_frame_pub_.getNumSubscribers() > 0) { + publishPerFrameMesh3D(output); + } +} + +// If we make this a VisualizerModule we get this for free. +bool RosDisplay::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, + "RosDisplay"); + CHECK(frontend_output); + + MesherOutput::Ptr mesher_output = nullptr; + bool get_mesher = + SimpleQueueSynchronizer::getInstance().syncQueue( + ts, &mesher_output_queue_, &mesher_output, "RosDisplay"); + if (mesher_output) { + publishMesherOutput(mesher_output); + } + + if (frontend_output && mesher_output) { + // Publish 2d mesh debug image + CHECK_NOTNULL(image_publishers_); + if (image_publishers_->getNumSubscribersForPublisher("debug_img") > 0) { + // TODO(Toni): this could be done in visualizer and sent to display + // module. + 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 RosDisplay::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 RosDisplay::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 RosDisplay::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( + "debug_img", 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 RosDisplay::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 RosDisplay::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 RosDisplay::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_; + + utils::poseToMsgTF(pose, &odom_tf.transform); + tf_broadcaster_.sendTransform(odom_tf); +} + +void RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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_; + utils::poseToMsgTF(w_Pose_map, &map_tf.transform); + tf_broadcaster_.sendTransform(map_tf); +} + +void RosDisplay::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); +} + +} // namespace VIO diff --git a/src/utils/UtilsRos.cpp b/src/utils/UtilsRos.cpp new file mode 100644 index 00000000..8995dd3c --- /dev/null +++ b/src/utils/UtilsRos.cpp @@ -0,0 +1,101 @@ +/** + * @file UtilsRos.cpp + * @brief Utilities to convert from/to Ros + * @author Antoni Rosinol + */ + +#include "kimera_vio_ros/utils/UtilsRos.h" + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +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& 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 From 883f4c31650e30bdc6a542c6df46243d7c559bc9 Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 23:28:37 -0400 Subject: [PATCH 6/8] Made RosDisplay work, hacked ros_display_2_ for 3d output --- CMakeLists.txt | 1 + include/kimera_vio_ros/KimeraVioRos.h | 3 +- include/kimera_vio_ros/RosDisplay.h | 35 ++- .../kimera_vio_ros/RosOnlineDataProvider.h | 10 + include/kimera_vio_ros/RosVisualizer.h | 236 ++++++++++++++++++ include/kimera_vio_ros/utils/UtilsRos.h | 14 +- src/KimeraVioRos.cpp | 19 +- src/RosBagDataProvider.cpp | 2 +- src/RosDisplay.cpp | 13 - src/RosOnlineDataProvider.cpp | 41 ++- src/utils/UtilsRos.cpp | 15 +- 11 files changed, 323 insertions(+), 66 deletions(-) create mode 100644 include/kimera_vio_ros/RosVisualizer.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 21be66f9..7af2c9f5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ cs_add_library(${PROJECT_NAME} src/RosBagDataProvider.cpp src/RosOnlineDataProvider.cpp src/RosDisplay.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 1f3c3f84..0182d375 100644 --- a/include/kimera_vio_ros/KimeraVioRos.h +++ b/include/kimera_vio_ros/KimeraVioRos.h @@ -39,7 +39,8 @@ class KimeraVioRos { VioParams::Ptr vio_params_; Pipeline::UniquePtr vio_pipeline_; RosDataProviderInterface::UniquePtr data_provider_; - RosDisplay ros_display_; + RosDisplay::UniquePtr ros_display_; + RosDisplay::UniquePtr ros_display_2_; ros::ServiceServer restart_vio_pipeline_srv_; std::atomic_bool restart_vio_pipeline_; }; diff --git a/include/kimera_vio_ros/RosDisplay.h b/include/kimera_vio_ros/RosDisplay.h index 6d93aba4..fa8ceac4 100644 --- a/include/kimera_vio_ros/RosDisplay.h +++ b/include/kimera_vio_ros/RosDisplay.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -75,10 +76,6 @@ class RosDisplay : public DisplayBase { 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); @@ -93,23 +90,16 @@ class RosDisplay : public DisplayBase { pointcloud_pub_ = nh_.advertise("time_horizon_pointcloud", 1, true); mesh_3d_frame_pub_ = nh_.advertise("mesh", 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_); } virtual ~RosDisplay() = default; public: // Spins the display once to render the visualizer output. - virtual void spinOnce(VisualizerOutput::UniquePtr&& viz_output) { + void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override { CHECK(viz_output); // Display 2D images. spin2dWindow(*viz_output); - // ros::spinOnce(); + // No need to run ros::spinOnce(), since it is done with async spinner. } public: @@ -145,6 +135,18 @@ class RosDisplay : public DisplayBase { } protected: + void 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()); + } + } + // Publish VIO outputs. virtual void publishBackendOutput(const BackendOutput::Ptr& output); @@ -158,11 +160,6 @@ class RosDisplay : public DisplayBase { // 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); - private: void publishTf(const BackendOutput::Ptr& output); @@ -233,8 +230,6 @@ class RosDisplay : public DisplayBase { 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_; diff --git a/include/kimera_vio_ros/RosOnlineDataProvider.h b/include/kimera_vio_ros/RosOnlineDataProvider.h index a42c912f..bb3e24bc 100644 --- a/include/kimera_vio_ros/RosOnlineDataProvider.h +++ b/include/kimera_vio_ros/RosOnlineDataProvider.h @@ -67,6 +67,11 @@ 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, @@ -111,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/RosVisualizer.h b/include/kimera_vio_ros/RosVisualizer.h new file mode 100644 index 00000000..93c290ac --- /dev/null +++ b/include/kimera_vio_ros/RosVisualizer.h @@ -0,0 +1,236 @@ +#pragma once + +#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 +#include +#include + +#include + +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 DisplayBase { + public: + KIMERA_POINTER_TYPEDEFS(RosVisualizer); + KIMERA_DELETE_COPY_CONSTRUCTORS(RosVisualizer); + + public: + RosVisualizer(const VioParams& vio_params) + : DisplayBase(), + nh_(), + nh_private_("~"), + vio_params_(vio_params), + 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") { + // 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()); + + // 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); + } + virtual ~RosVisualizer() = default; + + public: + // Spins the display once to render the visualizer output. + void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override { + CHECK(viz_output); + // Display 2D images. + spin2dWindow(*viz_output); + // No need to run ros::spinOnce(), since it is done with async spinner. + } + + 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); + } + + 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: + + // 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); + + 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(); + + void publishDebugImage(const Timestamp& timestamp, + const cv::Mat& debug_image) const; + + private: + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + // Just to get left/right cam to body tfs... + VioParams vio_params_; + + // 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_; + + // 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_; + + 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 index fa25c535..4e8105b1 100644 --- a/include/kimera_vio_ros/utils/UtilsRos.h +++ b/include/kimera_vio_ros/utils/UtilsRos.h @@ -6,9 +6,15 @@ #pragma once -#include +#include + #include +#include #include +#include +#include + +#include #include @@ -16,17 +22,15 @@ namespace VIO { namespace utils { -namespace gtsam { -class Pose3; -} - 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/src/KimeraVioRos.cpp b/src/KimeraVioRos.cpp index 0684b750..646cb777 100644 --- a/src/KimeraVioRos.cpp +++ b/src/KimeraVioRos.cpp @@ -33,8 +33,9 @@ namespace VIO { KimeraVioRos::KimeraVioRos() : vio_params_(nullptr), vio_pipeline_(nullptr), - data_provider_(nullptr), ros_display_(nullptr), + ros_display_2_(nullptr), + data_provider_(nullptr), restart_vio_pipeline_srv_(), restart_vio_pipeline_(false) { // Add rosservice to restart VIO pipeline if requested. @@ -54,10 +55,12 @@ bool KimeraVioRos::runKimeraVio() { // the shutdown function of a deleted object, aka segfault. VLOG(1) << "Destroy Ros Display."; ros_display_.reset(); + ros_display_2_.reset(); VLOG(1) << "Creating Ros Display."; CHECK(vio_params_); ros_display_ = VIO::make_unique(*vio_params_); + ros_display_2_ = VIO::make_unique(*vio_params_); VLOG(1) << "Destroy Vio Pipeline."; vio_pipeline_.reset(); @@ -164,29 +167,29 @@ RosDataProviderInterface::UniquePtr KimeraVioRos::createDataProvider( } void KimeraVioRos::connectVIO() { - CHECK(data_provider_); - CHECK(vio_pipeline_); - // 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. + CHECK(ros_display_2_); vio_pipeline_->registerBackendOutputCallback( std::bind(&VIO::RosDisplay::callbackBackendOutput, - std::ref(*ros_display_), + std::ref(*ros_display_2_), std::placeholders::_1)); vio_pipeline_->registerFrontendOutputCallback( std::bind(&VIO::RosDisplay::callbackFrontendOutput, - std::ref(*ros_display_), + std::ref(*ros_display_2_), std::placeholders::_1)); vio_pipeline_->registerMesherOutputCallback( std::bind(&VIO::RosDisplay::callbackMesherOutput, - std::ref(*ros_display_), + std::ref(*ros_display_2_), std::placeholders::_1)); bool use_lcd = false; @@ -195,7 +198,7 @@ void KimeraVioRos::connectVIO() { if (use_lcd) { vio_pipeline_->registerLcdOutputCallback( std::bind(&VIO::RosDisplay::callbackLcdOutput, - std::ref(*ros_display_), + std::ref(*ros_display_2_), std::placeholders::_1)); } diff --git a/src/RosBagDataProvider.cpp b/src/RosBagDataProvider.cpp index c5a04c32..f5d79632 100644 --- a/src/RosBagDataProvider.cpp +++ b/src/RosBagDataProvider.cpp @@ -312,7 +312,7 @@ void RosbagDataProvider::publishBackendOutput( const BackendOutput::Ptr& output) { CHECK(output); publishInputs(output->timestamp_); - RosDataProviderInterface::publishBackendOutput(output); + // RosDataProviderInterface::publishBackendOutput(output); publishClock(output->timestamp_); } diff --git a/src/RosDisplay.cpp b/src/RosDisplay.cpp index ae260ea9..db2809b0 100644 --- a/src/RosDisplay.cpp +++ b/src/RosDisplay.cpp @@ -734,18 +734,5 @@ void RosDisplay::publishTf(const LcdOutput::Ptr& lcd_output) { tf_broadcaster_.sendTransform(map_tf); } -void RosDisplay::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); -} } // namespace VIO diff --git a/src/RosOnlineDataProvider.cpp b/src/RosOnlineDataProvider.cpp index cf05c692..ca4cf089 100644 --- a/src/RosOnlineDataProvider.cpp +++ b/src/RosOnlineDataProvider.cpp @@ -15,6 +15,9 @@ #include #include #include +#include + +#include "kimera_vio_ros/utils/UtilsRos.h" namespace VIO { @@ -185,6 +188,20 @@ 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); @@ -247,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(); @@ -362,7 +379,21 @@ void RosOnlineDataProvider::msgGtOdomToVioNavState( vio_navstate->imu_bias_ = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias); } -//bool RosOnlineDataProvider::spinOnce() { +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? + // Remove this... Pass timestamp from pose. + 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; diff --git a/src/utils/UtilsRos.cpp b/src/utils/UtilsRos.cpp index 8995dd3c..9c60e568 100644 --- a/src/utils/UtilsRos.cpp +++ b/src/utils/UtilsRos.cpp @@ -6,18 +6,6 @@ #include "kimera_vio_ros/utils/UtilsRos.h" -#include - -#include -#include -#include -#include -#include - -#include - -#include - namespace VIO { namespace utils { @@ -45,6 +33,7 @@ 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) { CHECK_NOTNULL(cam_params); @@ -82,7 +71,7 @@ void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info, static constexpr size_t kTfLookupTimeout = 5u; geometry_msgs::TransformStamped cam_tf; try { - cam_tf = t_buffer.lookupTransform(base_link_frame_id_, + cam_tf = t_buffer.lookupTransform(base_link_frame_id, cam_frame_id, ros::Time(0), ros::Duration(kTfLookupTimeout)); From c6652a34058d9052561fbdba7248557665d0504c Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 01:17:30 -0400 Subject: [PATCH 7/8] Add RosVisualizer, replaces ros_display_2_ --- include/kimera_vio_ros/KimeraVioRos.h | 3 +- include/kimera_vio_ros/RosDisplay.h | 243 +------- include/kimera_vio_ros/RosVisualizer.h | 140 ++--- src/KimeraVioRos.cpp | 36 +- src/RosDisplay.cpp | 740 +------------------------ src/RosVisualizer.cpp | 725 ++++++++++++++++++++++++ 6 files changed, 811 insertions(+), 1076 deletions(-) create mode 100644 src/RosVisualizer.cpp diff --git a/include/kimera_vio_ros/KimeraVioRos.h b/include/kimera_vio_ros/KimeraVioRos.h index 0182d375..5dfdd735 100644 --- a/include/kimera_vio_ros/KimeraVioRos.h +++ b/include/kimera_vio_ros/KimeraVioRos.h @@ -12,6 +12,7 @@ #include "kimera_vio_ros/RosDataProviderInterface.h" #include "kimera_vio_ros/RosDisplay.h" +#include "kimera_vio_ros/RosVisualizer.h" namespace VIO { @@ -40,7 +41,7 @@ class KimeraVioRos { Pipeline::UniquePtr vio_pipeline_; RosDataProviderInterface::UniquePtr data_provider_; RosDisplay::UniquePtr ros_display_; - RosDisplay::UniquePtr ros_display_2_; + RosVisualizer::UniquePtr ros_visualizer_; ros::ServiceServer restart_vio_pipeline_srv_; std::atomic_bool restart_vio_pipeline_; }; diff --git a/include/kimera_vio_ros/RosDisplay.h b/include/kimera_vio_ros/RosDisplay.h index fa8ceac4..c2c13dd2 100644 --- a/include/kimera_vio_ros/RosDisplay.h +++ b/include/kimera_vio_ros/RosDisplay.h @@ -1,253 +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 - -#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 #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 RosDisplay : public DisplayBase { public: KIMERA_POINTER_TYPEDEFS(RosDisplay); KIMERA_DELETE_COPY_CONSTRUCTORS(RosDisplay); public: - RosDisplay(const VioParams& vio_params) - : DisplayBase(), - nh_(), - nh_private_("~"), - vio_params_(vio_params), - 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"), - image_publishers_(nullptr) { - 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()); - - // 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); - } + RosDisplay(); virtual ~RosDisplay() = default; public: - // Spins the display once to render the visualizer output. - void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override { - CHECK(viz_output); - // Display 2D images. - spin2dWindow(*viz_output); - // No need to run ros::spinOnce(), since it is done with async spinner. - } - - 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); - } - - 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(); - } + /** + * @brief spinOnce + * Spins the display once to render the visualizer output. + * @param viz_output + */ + void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override; protected: - void 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()); - } - } - - // 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); + /** + * @brief spin2dWindow Publishes images to ROS + * @param viz_output Input from the display queue + */ + void spin2dWindow(const DisplayInputBase& viz_output); 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(); - - void publishDebugImage(const Timestamp& timestamp, - const cv::Mat& debug_image) const; - - private: - ros::NodeHandle nh_; + //! Ros ros::NodeHandle nh_private_; - // Just to get left/right cam to body tfs... - VioParams vio_params_; - - // Define image publishers manager + //! Define image publishers manager std::unique_ptr image_publishers_; - // 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_; + //! Define frame ids for odometry message std::string base_link_frame_id_; - std::string map_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_; - - 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/RosVisualizer.h b/include/kimera_vio_ros/RosVisualizer.h index 93c290ac..a1dc1587 100644 --- a/include/kimera_vio_ros/RosVisualizer.h +++ b/include/kimera_vio_ros/RosVisualizer.h @@ -1,7 +1,12 @@ +/** + * @file RosVisualizer.h + * @brief Equivalent Kimera Visualizer but in ROS. Publishes 3D data to ROS. + * @author Antoni Rosinol + */ + #pragma once #include -#include #include #define PCL_NO_PRECOMPILE // Define this before you include any PCL headers @@ -13,29 +18,19 @@ #include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include #include #include #include #include -#include -#include +#include +#include +#include +#include #include -#include - namespace VIO { /** @@ -49,90 +44,25 @@ struct PointNormalUV { EIGEN_MAKE_ALIGNED_OPERATOR_NEW } EIGEN_ALIGN16; -class RosVisualizer : public DisplayBase { +class RosVisualizer : public Visualizer3D { public: KIMERA_POINTER_TYPEDEFS(RosVisualizer); KIMERA_DELETE_COPY_CONSTRUCTORS(RosVisualizer); public: - RosVisualizer(const VioParams& vio_params) - : DisplayBase(), - nh_(), - nh_private_("~"), - vio_params_(vio_params), - 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") { - // 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()); - - // 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); - } + RosVisualizer(const VioParams& vio_params); virtual ~RosVisualizer() = default; public: - // Spins the display once to render the visualizer output. - void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override { - CHECK(viz_output); - // Display 2D images. - spin2dWindow(*viz_output); - // No need to run ros::spinOnce(), since it is done with async spinner. - } - - 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); - } - - 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(); - } + /** + * @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::Ptr& output); @@ -140,20 +70,16 @@ class RosVisualizer : public DisplayBase { 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); 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 publishTimeHorizonMesh3D(const MesherOutput::Ptr& output) const; void publishState(const BackendOutput::Ptr& output) const; @@ -164,7 +90,7 @@ class RosVisualizer : public DisplayBase { void publishImuBias(const BackendOutput::Ptr& output) const; - // Publish LCD/PGO outputs. + void publishTf(const BackendOutput::Ptr& output); void publishTf(const LcdOutput::Ptr& lcd_output); void publishOptimizedTrajectory(const LcdOutput::Ptr& lcd_output) const; @@ -182,15 +108,13 @@ class RosVisualizer : public DisplayBase { const cv::Mat& debug_image) const; private: + // ROS handles ros::NodeHandle nh_; ros::NodeHandle nh_private_; - // Just to get left/right cam to body tfs... - VioParams vio_params_; - - // Publishers + // ROS publishers ros::Publisher pointcloud_pub_; - // Published 3d mesh per frame (not time horizon of opitimization!) + //! Published 3d mesh per frame (not time horizon of opitimization!) ros::Publisher mesh_3d_frame_pub_; ros::Publisher odometry_pub_; ros::Publisher resiliency_pub_; @@ -199,27 +123,25 @@ class RosVisualizer : public DisplayBase { ros::Publisher trajectory_pub_; ros::Publisher posegraph_pub_; - // Define tf broadcaster for world to base_link (IMU) and to map (PGO). + //! Define tf broadcaster for world to base_link (IMU) and to map (PGO). tf::TransformBroadcaster tf_broadcaster_; - // Stored pose graph related objects + //! 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 + //! Define frame ids for odometry message std::string world_frame_id_; std::string base_link_frame_id_; std::string map_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_; + cv::Size image_size_; + + //! Whether we publish lcd things or not. (TODO:(Toni) Not used, implement...) + bool use_lcd_; private: // Typedefs diff --git a/src/KimeraVioRos.cpp b/src/KimeraVioRos.cpp index 646cb777..8b5b405f 100644 --- a/src/KimeraVioRos.cpp +++ b/src/KimeraVioRos.cpp @@ -34,7 +34,7 @@ KimeraVioRos::KimeraVioRos() : vio_params_(nullptr), vio_pipeline_(nullptr), ros_display_(nullptr), - ros_display_2_(nullptr), + ros_visualizer_(nullptr), data_provider_(nullptr), restart_vio_pipeline_srv_(), restart_vio_pipeline_(false) { @@ -55,12 +55,12 @@ bool KimeraVioRos::runKimeraVio() { // the shutdown function of a deleted object, aka segfault. VLOG(1) << "Destroy Ros Display."; ros_display_.reset(); - ros_display_2_.reset(); + ros_visualizer_.reset(); VLOG(1) << "Creating Ros Display."; CHECK(vio_params_); - ros_display_ = VIO::make_unique(*vio_params_); - ros_display_2_ = VIO::make_unique(*vio_params_); + ros_display_ = VIO::make_unique(); + ros_visualizer_ = VIO::make_unique(*vio_params_); VLOG(1) << "Destroy Vio Pipeline."; vio_pipeline_.reset(); @@ -69,6 +69,7 @@ bool KimeraVioRos::runKimeraVio() { VLOG(1) << "Creating Kimera-VIO."; 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."; @@ -175,33 +176,6 @@ void KimeraVioRos::connectVIO() { vio_pipeline_->registerShutdownCallback(std::bind( &VIO::DataProviderInterface::shutdown, std::ref(*data_provider_))); - // Register callback to retrieve vio pipeline output from all modules. - CHECK(ros_display_2_); - vio_pipeline_->registerBackendOutputCallback( - std::bind(&VIO::RosDisplay::callbackBackendOutput, - std::ref(*ros_display_2_), - std::placeholders::_1)); - - vio_pipeline_->registerFrontendOutputCallback( - std::bind(&VIO::RosDisplay::callbackFrontendOutput, - std::ref(*ros_display_2_), - std::placeholders::_1)); - - vio_pipeline_->registerMesherOutputCallback( - std::bind(&VIO::RosDisplay::callbackMesherOutput, - std::ref(*ros_display_2_), - std::placeholders::_1)); - - bool use_lcd = false; - ros::NodeHandle nh_("~"); - nh_.getParam("use_lcd", use_lcd); - if (use_lcd) { - vio_pipeline_->registerLcdOutputCallback( - std::bind(&VIO::RosDisplay::callbackLcdOutput, - std::ref(*ros_display_2_), - std::placeholders::_1)); - } - // Register Data Provider callbacks data_provider_->registerImuSingleCallback( std::bind(&VIO::Pipeline::fillSingleImuQueue, diff --git a/src/RosDisplay.cpp b/src/RosDisplay.cpp index db2809b0..da4f2924 100644 --- a/src/RosDisplay.cpp +++ b/src/RosDisplay.cpp @@ -1,738 +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 -#include -#include -#include #include +#include +#include #include +#include "kimera_vio_ros/RosPublishers.h" #include "kimera_vio_ros/utils/UtilsRos.h" namespace VIO { -void RosDisplay::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 RosDisplay::publishFrontendOutput( - const FrontendOutput::Ptr& output) const { - CHECK(output); - if (frontend_stats_pub_.getNumSubscribers() > 0) { - publishFrontendStats(output); - } -} - -void RosDisplay::publishMesherOutput( - const MesherOutput::Ptr& output) const { - CHECK(output); - if (mesh_3d_frame_pub_.getNumSubscribers() > 0) { - publishPerFrameMesh3D(output); - } -} - -// If we make this a VisualizerModule we get this for free. -bool RosDisplay::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, - "RosDisplay"); - CHECK(frontend_output); - - MesherOutput::Ptr mesher_output = nullptr; - bool get_mesher = - SimpleQueueSynchronizer::getInstance().syncQueue( - ts, &mesher_output_queue_, &mesher_output, "RosDisplay"); - if (mesher_output) { - publishMesherOutput(mesher_output); - } - - if (frontend_output && mesher_output) { - // Publish 2d mesh debug image - CHECK_NOTNULL(image_publishers_); - if (image_publishers_->getNumSubscribersForPublisher("debug_img") > 0) { - // TODO(Toni): this could be done in visualizer and sent to display - // module. - 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 RosDisplay::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 RosDisplay::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 RosDisplay::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( - "debug_img", 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 RosDisplay::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 RosDisplay::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 RosDisplay::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_; - - utils::poseToMsgTF(pose, &odom_tf.transform); - tf_broadcaster_.sendTransform(odom_tf); -} - -void RosDisplay::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 RosDisplay::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 RosDisplay::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 RosDisplay::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); +RosDisplay::RosDisplay() + : DisplayBase(), nh_private_("~"), image_publishers_(nullptr) { + image_publishers_ = VIO::make_unique(nh_private_); } -void RosDisplay::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 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::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); +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()); } - - return; -} - -pose_graph_tools::PoseGraph RosDisplay::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 RosDisplay::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 RosDisplay::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_; - utils::poseToMsgTF(w_Pose_map, &map_tf.transform); - tf_broadcaster_.sendTransform(map_tf); -} - - } // namespace VIO diff --git a/src/RosVisualizer.cpp b/src/RosVisualizer.cpp new file mode 100644 index 00000000..af2ead93 --- /dev/null +++ b/src/RosVisualizer.cpp @@ -0,0 +1,725 @@ +/** + * @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_) { + // 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); +} + +virtual RosVisualizer::~RosVisualizer() {} + +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::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 RosVisualizer::publishFrontendOutput( + const FrontendOutput::Ptr& output) const { + CHECK(output); + if (frontend_stats_pub_.getNumSubscribers() > 0) { + publishFrontendStats(output); + } +} + +void RosVisualizer::publishMesherOutput(const MesherOutput::Ptr& output) const { + CHECK(output); + if (mesh_3d_frame_pub_.getNumSubscribers() > 0) { + publishPerFrameMesh3D(output); + } +} + +void RosVisualizer::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 RosVisualizer::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 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( + "debug_img", cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); +} + +void RosVisualizer::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 = 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::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 RosVisualizer::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 RosVisualizer::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 RosVisualizer::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 RosVisualizer::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 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::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 RosVisualizer::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_; + utils::poseToMsgTF(w_Pose_map, &map_tf.transform); + tf_broadcaster_.sendTransform(map_tf); +} + +void RosVisualizer::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_; + + utils::poseToMsgTF(pose, &odom_tf.transform); + tf_broadcaster_.sendTransform(odom_tf); +} + +} // namespace VIO From cdd317cfbb5148a24c93cc65fe142f8fa026f7fb Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 01:37:20 -0400 Subject: [PATCH 8/8] Fix vtable and constptr in RosVisualizer --- CMakeLists.txt | 1 + include/kimera_vio_ros/RosVisualizer.h | 37 +++++++++++++---------- src/RosVisualizer.cpp | 41 ++++++++++++++------------ 3 files changed, 44 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7af2c9f5..295bcb61 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ cs_add_library(${PROJECT_NAME} 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/RosVisualizer.h b/include/kimera_vio_ros/RosVisualizer.h index a1dc1587..e86b93d1 100644 --- a/include/kimera_vio_ros/RosVisualizer.h +++ b/include/kimera_vio_ros/RosVisualizer.h @@ -31,6 +31,8 @@ #include #include +#include "kimera_vio_ros/RosPublishers.h" + namespace VIO { /** @@ -64,38 +66,38 @@ class RosVisualizer : public Visualizer3D { protected: // Publish VIO outputs. - virtual void publishBackendOutput(const BackendOutput::Ptr& output); + virtual void publishBackendOutput(const BackendOutput::ConstPtr& output); - virtual void publishFrontendOutput(const FrontendOutput::Ptr& output) const; + virtual void publishFrontendOutput(const FrontendOutput::ConstPtr& output) const; - virtual void publishMesherOutput(const MesherOutput::Ptr& 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::Ptr& lcd_output); + virtual void publishLcdOutput(const LcdOutput::ConstPtr& lcd_output); private: - void publishTimeHorizonPointCloud(const BackendOutput::Ptr& output) const; + void publishTimeHorizonPointCloud(const BackendOutput::ConstPtr& output) const; - void publishPerFrameMesh3D(const MesherOutput::Ptr& output) const; + void publishPerFrameMesh3D(const MesherOutput::ConstPtr& output) const; - // void publishTimeHorizonMesh3D(const MesherOutput::Ptr& output) const; + // void publishTimeHorizonMesh3D(const MesherOutput::ConstPtr& output) const; - void publishState(const BackendOutput::Ptr& output) const; + void publishState(const BackendOutput::ConstPtr& output) const; - void publishFrontendStats(const FrontendOutput::Ptr& output) const; + void publishFrontendStats(const FrontendOutput::ConstPtr& output) const; - void publishResiliency(const FrontendOutput::Ptr& frontend_output, - const BackendOutput::Ptr& backend_output) const; + void publishResiliency(const FrontendOutput::ConstPtr& frontend_output, + const BackendOutput::ConstPtr& backend_output) const; - void publishImuBias(const BackendOutput::Ptr& output) const; + void publishImuBias(const BackendOutput::ConstPtr& output) const; - void publishTf(const BackendOutput::Ptr& output); - void publishTf(const LcdOutput::Ptr& lcd_output); + void publishTf(const BackendOutput::ConstPtr& output); + void publishTf(const LcdOutput::ConstPtr& lcd_output); - void publishOptimizedTrajectory(const LcdOutput::Ptr& lcd_output) const; + void publishOptimizedTrajectory(const LcdOutput::ConstPtr& lcd_output) const; - void publishPoseGraph(const LcdOutput::Ptr& lcd_output); + void publishPoseGraph(const LcdOutput::ConstPtr& lcd_output); void updateNodesAndEdges(const gtsam::NonlinearFactorGraph& nfg, const gtsam::Values& values); @@ -140,6 +142,9 @@ class RosVisualizer : public Visualizer3D { 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_; diff --git a/src/RosVisualizer.cpp b/src/RosVisualizer.cpp index af2ead93..c5ad6c3f 100644 --- a/src/RosVisualizer.cpp +++ b/src/RosVisualizer.cpp @@ -42,7 +42,12 @@ RosVisualizer::RosVisualizer(const VioParams& vio_params) : Visualizer3D(VisualizationType::kMesh2dTo3dSparse), nh_(), nh_private_("~"), - image_size_(vio_params.camera_params_.at(0).image_size_) { + 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()); @@ -66,8 +71,6 @@ RosVisualizer::RosVisualizer(const VioParams& vio_params) mesh_3d_frame_pub_ = nh_.advertise("mesh", 1, true); } -virtual RosVisualizer::~RosVisualizer() {} - VisualizerOutput::UniquePtr RosVisualizer::spinOnce( const VisualizerInput& viz_input) { publishBackendOutput(viz_input.backend_output_); @@ -78,7 +81,7 @@ VisualizerOutput::UniquePtr RosVisualizer::spinOnce( return VIO::make_unique(); } -void RosVisualizer::publishBackendOutput(const BackendOutput::Ptr& output) { +void RosVisualizer::publishBackendOutput(const BackendOutput::ConstPtr& output) { CHECK(output); publishTf(output); if (odometry_pub_.getNumSubscribers() > 0) { @@ -93,21 +96,21 @@ void RosVisualizer::publishBackendOutput(const BackendOutput::Ptr& output) { } void RosVisualizer::publishFrontendOutput( - const FrontendOutput::Ptr& output) const { + const FrontendOutput::ConstPtr& output) const { CHECK(output); if (frontend_stats_pub_.getNumSubscribers() > 0) { publishFrontendStats(output); } } -void RosVisualizer::publishMesherOutput(const MesherOutput::Ptr& output) const { +void RosVisualizer::publishMesherOutput(const MesherOutput::ConstPtr& output) const { CHECK(output); if (mesh_3d_frame_pub_.getNumSubscribers() > 0) { publishPerFrameMesh3D(output); } } -void RosVisualizer::publishLcdOutput(const LcdOutput::Ptr& lcd_output) { +void RosVisualizer::publishLcdOutput(const LcdOutput::ConstPtr& lcd_output) { CHECK(lcd_output); publishTf(lcd_output); @@ -121,7 +124,7 @@ void RosVisualizer::publishLcdOutput(const LcdOutput::Ptr& lcd_output) { void RosVisualizer::publishTimeHorizonPointCloud( - const BackendOutput::Ptr& output) const { + const BackendOutput::ConstPtr& output) const { CHECK(output); const Timestamp& timestamp = output->timestamp_; const PointsWithIdMap& points_with_id = output->landmarks_with_id_map_; @@ -197,11 +200,11 @@ void RosVisualizer::publishDebugImage(const Timestamp& timestamp, h.frame_id = base_link_frame_id_; // Copies... image_publishers_->publish( - "debug_img", cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); + "mesh_2d", cv_bridge::CvImage(h, "bgr8", debug_image).toImageMsg()); } void RosVisualizer::publishPerFrameMesh3D( - const MesherOutput::Ptr& output) const { + const MesherOutput::ConstPtr& output) const { CHECK(output); const Mesh2D& mesh_2d = output->mesh_2d_; @@ -305,7 +308,7 @@ void RosVisualizer::publishPerFrameMesh3D( return; } // namespace VIO -void RosVisualizer::publishState(const BackendOutput::Ptr& output) const { +void RosVisualizer::publishState(const BackendOutput::ConstPtr& output) const { CHECK(output); // Get latest estimates for odometry. const Timestamp& ts = output->timestamp_; @@ -380,7 +383,7 @@ void RosVisualizer::publishState(const BackendOutput::Ptr& output) const { } void RosVisualizer::publishFrontendStats( - const FrontendOutput::Ptr& output) const { + const FrontendOutput::ConstPtr& output) const { CHECK(output); // Get frontend data for resiliency output @@ -416,8 +419,8 @@ void RosVisualizer::publishFrontendStats( } void RosVisualizer::publishResiliency( - const FrontendOutput::Ptr& frontend_output, - const BackendOutput::Ptr& backend_output) const { + const FrontendOutput::ConstPtr& frontend_output, + const BackendOutput::ConstPtr& backend_output) const { CHECK(frontend_output); CHECK(backend_output); @@ -485,7 +488,7 @@ void RosVisualizer::publishResiliency( resiliency_pub_.publish(resiliency_msg); } -void RosVisualizer::publishImuBias(const BackendOutput::Ptr& output) const { +void RosVisualizer::publishImuBias(const BackendOutput::ConstPtr& output) const { CHECK(output); // Get imu bias to output @@ -516,7 +519,7 @@ void RosVisualizer::publishImuBias(const BackendOutput::Ptr& output) const { } void RosVisualizer::publishOptimizedTrajectory( - const LcdOutput::Ptr& lcd_output) const { + const LcdOutput::ConstPtr& lcd_output) const { CHECK(lcd_output); // Get pgo-optimized trajectory @@ -677,7 +680,7 @@ pose_graph_tools::PoseGraph RosVisualizer::getPosegraphMsg() { return pose_graph; } -void RosVisualizer::publishPoseGraph(const LcdOutput::Ptr& lcd_output) { +void RosVisualizer::publishPoseGraph(const LcdOutput::ConstPtr& lcd_output) { CHECK(lcd_output); // Get the factor graph @@ -691,7 +694,7 @@ void RosVisualizer::publishPoseGraph(const LcdOutput::Ptr& lcd_output) { posegraph_pub_.publish(graph); } -void RosVisualizer::publishTf(const LcdOutput::Ptr& lcd_output) { +void RosVisualizer::publishTf(const LcdOutput::ConstPtr& lcd_output) { CHECK(lcd_output); const Timestamp& ts = lcd_output->timestamp_kf_; @@ -706,7 +709,7 @@ void RosVisualizer::publishTf(const LcdOutput::Ptr& lcd_output) { tf_broadcaster_.sendTransform(map_tf); } -void RosVisualizer::publishTf(const BackendOutput::Ptr& output) { +void RosVisualizer::publishTf(const BackendOutput::ConstPtr& output) { CHECK(output); const Timestamp& timestamp = output->timestamp_;