From 883a33e8d2f98d8a03810ec79deb92558847e0c0 Mon Sep 17 00:00:00 2001 From: alsora Date: Sat, 27 Oct 2018 12:32:45 -0700 Subject: [PATCH] added stereo node; cleaned other nodes --- CMakeLists.txt | 17 +++- README.md | 10 ++- docker/Dockerfile | 19 ++-- docker/run_dev.sh | 16 ++++ src/monocular/mono.cpp | 4 +- src/monocular/monocular-slam-node.cpp | 2 - src/monocular/monocular-slam-node.hpp | 10 +-- src/rgbd/rgbd-slam-node.cpp | 65 ++++++++++++++ src/rgbd/rgbd-slam-node.hpp | 58 +++++++++++++ src/rgbd/rgbd.cpp | 42 ++++++--- src/stereo/stereo-slam-node.cpp | 120 ++++++++++++++++++++++++++ src/stereo/stereo-slam-node.hpp | 55 ++++++++++++ src/stereo/stereo.cpp | 41 +++++++++ 13 files changed, 424 insertions(+), 35 deletions(-) create mode 100644 docker/run_dev.sh create mode 100644 src/rgbd/rgbd-slam-node.cpp create mode 100644 src/rgbd/rgbd-slam-node.hpp create mode 100644 src/stereo/stereo-slam-node.cpp create mode 100644 src/stereo/stereo-slam-node.hpp create mode 100644 src/stereo/stereo.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 50964c4..5d36afd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ ament_target_dependencies(mono rclcpp sensor_msgs cv_bridge ORB_SLAM2 Pangolin) add_executable(rgbd src/rgbd/rgbd.cpp + src/rgbd/rgbd-slam-node.cpp ) target_link_libraries(rgbd @@ -52,7 +53,21 @@ target_link_libraries(rgbd ament_target_dependencies(rgbd rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM2 Pangolin) -install(TARGETS mono rgbd + +add_executable(stereo + src/stereo/stereo.cpp + src/stereo/stereo-slam-node.cpp +) + +target_link_libraries(stereo + ORB_SLAM2 +) + +ament_target_dependencies(stereo rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM2 Pangolin) + + + +install(TARGETS mono rgbd stereo DESTINATION lib/${PROJECT_NAME}) ament_package() \ No newline at end of file diff --git a/README.md b/README.md index 4b9a2f1..f91d77d 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # ros2-ORB_SLAM2 ROS2 node wrapping the ORB_SLAM2 library +If you want to integrate ORB_SLAM2 inside your ROS2 system, consider trying [this](https://github.com/alsora/ORB_SLAM2) fork of ORB_SLAM2 library which drops Pangolin dependency and streams all SLAM data through ROS2 topics. ### Requirements @@ -43,7 +44,14 @@ For example you can stream frames from your laptop webcam using: $ ros2 run image_tools cam2image -t camera -Similarly you can run the `rgbd` node by using +##### RGBD Node + +You can run the `rgbd` node by using $ ros2 run orbslam rgbd PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE +##### Stereo Node + +You can run the `stereo` node by using + + $ ros2 run orbslam stereo PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY diff --git a/docker/Dockerfile b/docker/Dockerfile index 05b382a..86a22e6 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -114,10 +114,7 @@ RUN bash $HOME/scripts/build_opencv.sh ######## ORBSLAM2 SETUP - -# create a workspace for orbslam -RUN mkdir -p $HOME/orbslam -WORKDIR $HOME/orbslam +WORKDIR $HOME # install pangolin depenendencies RUN apt-get install -y \ @@ -127,14 +124,14 @@ RUN apt-get install -y \ # get pangolin sources RUN git clone https://github.com/stevenlovegrove/Pangolin.git -RUN mkdir -p $HOME/orbslam/Pangolin/build -WORKDIR $HOME/orbslam/Pangolin/build +RUN mkdir -p $HOME/Pangolin/build +WORKDIR $HOME/Pangolin/build # build pangolin RUN cmake .. RUN cmake --build . -WORKDIR $HOME/orbslam +WORKDIR $HOME # get orbslam sources RUN git clone https://github.com/raulmur/ORB_SLAM2.git @@ -144,11 +141,11 @@ RUN apt-get install -y \ libboost-system-dev # build orbslam -WORKDIR $HOME/orbslam/ORB_SLAM2 +WORKDIR $HOME/ORB_SLAM2 RUN rm CMakeLists.txt RUN cp $HOME/scripts/CMakeLists.txt . RUN chmod +x $HOME/scripts/build.sh -RUN /bin/bash -c 'export LD_LIBRARY_PATH=~/orbslam/Pangolin/build/src/:$LD_LIBRARY_PATH; \ +RUN /bin/bash -c 'export LD_LIBRARY_PATH=~/Pangolin/build/src/:$LD_LIBRARY_PATH; \ bash $HOME/scripts/build.sh' @@ -171,7 +168,7 @@ RUN git clone https://github.com/ros2/message_filters src/message_filters # build the workspace RUN /bin/bash -c 'source $HOME/ros2_sdk/install/setup.sh; \ - export LD_LIBRARY_PATH=~/orbslam/Pangolin/build/src/:~/orbslam/ORB_SLAM2/Thirdparty/DBoW2/lib:~/orbslam/ORB_SLAM2/Thirdparty/g2o/lib:$LD_LIBRARY_PATH; \ + export LD_LIBRARY_PATH=~/Pangolin/build/src/:~/ORB_SLAM2/Thirdparty/DBoW2/lib:~/ORB_SLAM2/Thirdparty/g2o/lib:$LD_LIBRARY_PATH; \ colcon build' @@ -188,6 +185,6 @@ source $HOME/ws/install/local_setup.sh' >> $HOME/.bashrc # add orbslam shared libraries to path RUN echo ' \n\ -export LD_LIBRARY_PATH=~/orbslam/Pangolin/build/src/:~/orbslam/ORB_SLAM2/Thirdparty/DBoW2/lib:~/orbslam/ORB_SLAM2/Thirdparty/g2o/lib:~/orbslam/ORB_SLAM2/lib:$LD_LIBRARY_PATH' >> $HOME/.bashrc +export LD_LIBRARY_PATH=~/Pangolin/build/src/:~/ORB_SLAM2/Thirdparty/DBoW2/lib:~/ORB_SLAM2/Thirdparty/g2o/lib:~/ORB_SLAM2/lib:$LD_LIBRARY_PATH' >> $HOME/.bashrc diff --git a/docker/run_dev.sh b/docker/run_dev.sh new file mode 100644 index 0000000..c448b5f --- /dev/null +++ b/docker/run_dev.sh @@ -0,0 +1,16 @@ +#!/bin/bash +# +# @author Alberto Soragna (alberto dot soragna at gmail dot com) +# @2018 + +XSOCK=/tmp/.X11-unix + +# --runtime=nvidia \ +docker run -it --rm \ + -e DISPLAY=$DISPLAY \ + -v $XSOCK:$XSOCK \ + -v $HOME/.Xauthority:/root/.Xauthority \ + -v $PWD/..:/root/VOLUME_ros2-ORB_SLAM2 \ + --privileged \ + --net=host \ + ros2_orbslam2 "$@" diff --git a/src/monocular/mono.cpp b/src/monocular/mono.cpp index 98d0c8f..5c7e4af 100644 --- a/src/monocular/mono.cpp +++ b/src/monocular/mono.cpp @@ -22,12 +22,10 @@ int main(int argc, char **argv) // malloc error using new.. try shared ptr // Create SLAM system. It initializes all system threads and gets ready to process frames. - bool visualization = true; ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, visualization); - std::cout<<"Constructor system ok"<(&SLAM, argv[1], argv[2]); - std::cout<<"Constructor NODE ok"< -#include"MapPoint.h" - using ImageMsg = sensor_msgs::msg::Image; using namespace std; diff --git a/src/monocular/monocular-slam-node.hpp b/src/monocular/monocular-slam-node.hpp index 90e996e..6cec7ac 100644 --- a/src/monocular/monocular-slam-node.hpp +++ b/src/monocular/monocular-slam-node.hpp @@ -1,8 +1,6 @@ +#ifndef __MONOCULAR_SLAM_NODE_HPP__ +#define __MONOCULAR_SLAM_NODE_HPP__ -#include -#include -#include -#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" @@ -37,4 +35,6 @@ class MonocularSlamNode : public rclcpp::Node rclcpp::Subscription::SharedPtr m_image_subscriber; -}; \ No newline at end of file +}; + +#endif \ No newline at end of file diff --git a/src/rgbd/rgbd-slam-node.cpp b/src/rgbd/rgbd-slam-node.cpp new file mode 100644 index 0000000..2b979bb --- /dev/null +++ b/src/rgbd/rgbd-slam-node.cpp @@ -0,0 +1,65 @@ +#include "rgbd-slam-node.hpp" + +#include + + +using ImageMsg = sensor_msgs::msg::Image; + +using namespace std; + +using std::placeholders::_1; + +RgbdSlamNode::RgbdSlamNode(ORB_SLAM2::System* pSLAM, const string &strVocFile, const string &strSettingsFile) +: Node("orbslam") + ,m_SLAM(pSLAM) +{ + + rgb_sub = std::make_shared >(shared_ptr(this), "camera/rgb"); + depth_sub = std::make_shared >(shared_ptr(this), "camera/depth"); + + syncApproximate = std::make_shared >(approximate_sync_policy(10), *rgb_sub, *depth_sub); + syncApproximate->registerCallback(&RgbdSlamNode::GrabRGBD, this); + +} + + +RgbdSlamNode::~RgbdSlamNode() +{ + // Stop all threads + m_SLAM->Shutdown(); + + // Save camera trajectory + m_SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + +} + + +void RgbdSlamNode::GrabRGBD(const ImageMsg::SharedPtr msgRGB, const ImageMsg::SharedPtr msgD) +{ + + // Copy the ros rgb image message to cv::Mat. + try + { + cv_ptrRGB = cv_bridge::toCvShare(msgRGB); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // Copy the ros depth image message to cv::Mat. + try + { + cv_ptrD = cv_bridge::toCvShare(msgD); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + + cv::Mat Tcw = m_SLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, msgRGB->header.stamp.sec); + +} diff --git a/src/rgbd/rgbd-slam-node.hpp b/src/rgbd/rgbd-slam-node.hpp new file mode 100644 index 0000000..edf468b --- /dev/null +++ b/src/rgbd/rgbd-slam-node.hpp @@ -0,0 +1,58 @@ + +#ifndef __RGBD_SLAM_NODE_HPP__ +#define __RGBD_SLAM_NODE_HPP__ + + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" + +#include "message_filters/subscriber.h" +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/approximate_time.h" + +#include + +#include"System.h" +#include"Frame.h" +#include "Map.h" +#include "Tracking.h" + + +typedef message_filters::sync_policies::ApproximateTime approximate_sync_policy; + + +class RgbdSlamNode : public rclcpp::Node +{ + +public: + + RgbdSlamNode(ORB_SLAM2::System* pSLAM, const string &strVocFile, const string &strSettingsFile); + + + ~RgbdSlamNode(); + + +private: + + + void GrabRGBD(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD); + + ORB_SLAM2::System* m_SLAM; + + cv_bridge::CvImageConstPtr cv_ptrRGB; + cv_bridge::CvImageConstPtr cv_ptrD; + + std::shared_ptr > rgb_sub; + std::shared_ptr > depth_sub; + + std::shared_ptr > syncApproximate; +}; + + + +#endif \ No newline at end of file diff --git a/src/rgbd/rgbd.cpp b/src/rgbd/rgbd.cpp index b4c9251..446d6cc 100644 --- a/src/rgbd/rgbd.cpp +++ b/src/rgbd/rgbd.cpp @@ -3,27 +3,44 @@ #include #include -#include - #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/image.hpp" - -#include "message_filters/subscriber.h" -#include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/approximate_time.h" - -#include +#include "rgbd-slam-node.hpp" #include"System.h" using namespace std; -using namespace std::placeholders; -using ImageMsg = sensor_msgs::msg::Image; -rclcpp::Node::SharedPtr g_node = nullptr; +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if(argc != 3) + { + cerr << endl << "Usage: ros2 run orbslam rgbd path_to_vocabulary path_to_settings" << endl; + rclcpp::shutdown(); + return 1; + } + + // malloc error using new.. try shared ptr + // Create SLAM system. It initializes all system threads and gets ready to process frames. + + bool visualization = true; + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, visualization); + + auto node = std::make_shared(&SLAM, argv[1], argv[2]); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} + + +/* class ImageGrabber { @@ -111,3 +128,4 @@ void ImageGrabber::GrabRGBD(const ImageMsg::SharedPtr msgRGB, const ImageMsg::Sh } +*/ \ No newline at end of file diff --git a/src/stereo/stereo-slam-node.cpp b/src/stereo/stereo-slam-node.cpp new file mode 100644 index 0000000..ee6302a --- /dev/null +++ b/src/stereo/stereo-slam-node.cpp @@ -0,0 +1,120 @@ +#include "stereo-slam-node.hpp" + +#include + + +using ImageMsg = sensor_msgs::msg::Image; + +using namespace std; + +using std::placeholders::_1; +using std::placeholders::_2; + +StereoSlamNode::StereoSlamNode(ORB_SLAM2::System* pSLAM, const string &strVocFile, const string &strSettingsFile, const string &strDoRectify) +: Node("orbslam") + ,m_SLAM(pSLAM) +{ + stringstream ss(strDoRectify); + ss >> boolalpha >> doRectify; + + if (doRectify){ + + cv::FileStorage fsSettings(strSettingsFile, cv::FileStorage::READ); + if(!fsSettings.isOpened()){ + cerr << "ERROR: Wrong path to settings" << endl; + assert(0); + } + + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; + + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; + + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int rows_l = fsSettings["LEFT.height"]; + int cols_l = fsSettings["LEFT.width"]; + int rows_r = fsSettings["RIGHT.height"]; + int cols_r = fsSettings["RIGHT.width"]; + + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0){ + cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; + assert(0); + } + + cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); + cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); + } + + + + left_sub = std::make_shared >(shared_ptr(this), "camera/left"); + right_sub = std::make_shared >(shared_ptr(this), "camera/right"); + + syncApproximate = std::make_shared >(approximate_sync_policy(10), *left_sub, *right_sub); + syncApproximate->registerCallback(&StereoSlamNode::GrabStereo, this); + + +} + + +StereoSlamNode::~StereoSlamNode() +{ + // Stop all threads + m_SLAM->Shutdown(); + + // Save camera trajectory + m_SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + +} + + +void StereoSlamNode::GrabStereo(const ImageMsg::SharedPtr msgLeft, const ImageMsg::SharedPtr msgRight) +{ + + // Copy the ros rgb image message to cv::Mat. + try + { + cv_ptrLeft = cv_bridge::toCvShare(msgLeft); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // Copy the ros depth image message to cv::Mat. + try + { + cv_ptrRight = cv_bridge::toCvShare(msgRight); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + cv::Mat Tcw; + + if (doRectify){ + + cv::Mat imLeft, imRight; + cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR); + cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR); + Tcw = m_SLAM->TrackStereo(imLeft, imRight, msgLeft->header.stamp.sec); + + } + else { + + Tcw = m_SLAM->TrackRGBD(cv_ptrLeft->image, cv_ptrRight->image, msgLeft->header.stamp.sec); + + } + +} diff --git a/src/stereo/stereo-slam-node.hpp b/src/stereo/stereo-slam-node.hpp new file mode 100644 index 0000000..2f152e1 --- /dev/null +++ b/src/stereo/stereo-slam-node.hpp @@ -0,0 +1,55 @@ +#ifndef __STEREO_SLAM_NODE_HPP__ +#define __STEREO_SLAM_NODE_HPP__ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" + +#include "message_filters/subscriber.h" +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/approximate_time.h" + +#include + +#include"System.h" +#include"Frame.h" +#include "Map.h" +#include "Tracking.h" + + +typedef message_filters::sync_policies::ApproximateTime approximate_sync_policy; + + +class StereoSlamNode : public rclcpp::Node +{ + +public: + + StereoSlamNode(ORB_SLAM2::System* pSLAM, const string &strVocFile, const string &strSettingsFile, const string &strDoRectify); + + + ~StereoSlamNode(); + + +private: + + + void GrabStereo(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD); + + ORB_SLAM2::System* m_SLAM; + + bool doRectify; + cv::Mat M1l,M2l,M1r,M2r; + + cv_bridge::CvImageConstPtr cv_ptrLeft; + cv_bridge::CvImageConstPtr cv_ptrRight; + + std::shared_ptr > left_sub; + std::shared_ptr > right_sub; + + std::shared_ptr > syncApproximate; +}; + + + + +#endif \ No newline at end of file diff --git a/src/stereo/stereo.cpp b/src/stereo/stereo.cpp new file mode 100644 index 0000000..560667d --- /dev/null +++ b/src/stereo/stereo.cpp @@ -0,0 +1,41 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "stereo-slam-node.hpp" + +#include"System.h" + +using namespace std; + + + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if(argc != 4) + { + cerr << endl << "Usage: ros2 run orbslam stereo path_to_vocabulary path_to_settings do_rectify" << endl; + rclcpp::shutdown(); + return 1; + } + + // malloc error using new.. try shared ptr + // Create SLAM system. It initializes all system threads and gets ready to process frames. + + bool visualization = true; + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, visualization); + + + auto node = std::make_shared(&SLAM, argv[1], argv[2], argv[3]); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} +