added stereo node; cleaned other nodes

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()

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

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
 
 

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 "$@"

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"<<std::endl;
+
     auto node = std::make_shared<MonocularSlamNode>(&SLAM, argv[1], argv[2]);
-    std::cout<<"Constructor NODE ok"<<std::endl;
 
     rclcpp::spin(node);
 

src/monocular/monocular-slam-node.cpp

@@ -2,8 +2,6 @@
 
 #include<opencv2/core/core.hpp>
 
-#include"MapPoint.h"
-
 using ImageMsg = sensor_msgs::msg::Image;
 
 using namespace std;

src/monocular/monocular-slam-node.hpp

@@ -1,8 +1,6 @@
+#ifndef __MONOCULAR_SLAM_NODE_HPP__
+#define __MONOCULAR_SLAM_NODE_HPP__
 
-#include<iostream>
-#include<algorithm>
-#include<fstream>
-#include<chrono>
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/image.hpp"
@@ -37,4 +35,6 @@ class MonocularSlamNode : public rclcpp::Node
 
     rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr m_image_subscriber;
 
-};
+};
+
+#endif

src/rgbd/rgbd-slam-node.cpp

@@ -0,0 +1,65 @@
+#include "rgbd-slam-node.hpp"
+
+#include<opencv2/core/core.hpp>
+
+
+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<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/rgb");
+    depth_sub = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/depth");
+
+    syncApproximate = std::make_shared<message_filters::Synchronizer<approximate_sync_policy> >(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);
+
+}

src/rgbd/rgbd-slam-node.hpp

@@ -0,0 +1,58 @@
+
+#ifndef __RGBD_SLAM_NODE_HPP__
+#define __RGBD_SLAM_NODE_HPP__
+
+
+#include<iostream>
+#include<algorithm>
+#include<fstream>
+#include<chrono>
+
+#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 <cv_bridge/cv_bridge.h>
+
+#include"System.h"
+#include"Frame.h"
+#include "Map.h"
+#include "Tracking.h"
+
+
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> 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<message_filters::Subscriber<sensor_msgs::msg::Image> > rgb_sub;
+    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > depth_sub;
+
+    std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy> > syncApproximate;
+};
+
+
+
+#endif

src/rgbd/rgbd.cpp

@@ -3,27 +3,44 @@
 #include<fstream>
 #include<chrono>
 
-#include<opencv2/core/core.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 <cv_bridge/cv_bridge.h>
+#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<RgbdSlamNode>(&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
     
 }
 
+*/