Add openvslam_ros

ros/1/src/openvslam/CMakeLists.txt

@@ -220,7 +220,8 @@ message(STATUS "BoW framework: ${BOW_FRAMEWORK} (found in ${BOW_INCLUDE_DIRS})")
 
 include_directories(
         ${catkin_INCLUDE_DIRS}
-        ${OpenVSLAM_SRC_DIR})
+        ${OpenVSLAM_SRC_DIR}
+        src)
 
 link_libraries(
         ${catkin_LIBRARIES}

ros/1/src/openvslam/src/CMakeLists.txt

@@ -8,10 +8,10 @@ endif()
 
 set(EXECUTABLE_TARGETS "")
 
-add_executable(run_slam run_slam.cc)
+add_executable(run_slam run_slam.cc openvslam_ros.cc)
 list(APPEND EXECUTABLE_TARGETS run_slam)
 
-add_executable(run_localization run_localization.cc)
+add_executable(run_localization run_localization.cc openvslam_ros.cc)
 list(APPEND EXECUTABLE_TARGETS run_localization)
 
 foreach(EXECUTABLE_TARGET IN LISTS EXECUTABLE_TARGETS)

ros/1/src/openvslam/src/openvslam_ros.cc

@@ -0,0 +1,89 @@
+#include <openvslam_ros.h>
+
+#include <chrono>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+
+namespace openvslam_ros {
+system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
+    : SLAM_(cfg, vocab_file_path), cfg_(cfg), it_(nh_), tp_0_(std::chrono::steady_clock::now()),
+      mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)) {}
+
+mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
+    : system(cfg, vocab_file_path, mask_img_path) {
+    sub_ = it_.subscribe("camera/image_raw", 1, &mono::callback, this);
+}
+void mono::callback(const sensor_msgs::ImageConstPtr& msg) {
+    const auto tp_1 = std::chrono::steady_clock::now();
+    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();
+
+    // input the current frame and estimate the camera pose
+    SLAM_.feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);
+
+    const auto tp_2 = std::chrono::steady_clock::now();
+
+    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
+    track_times_.push_back(track_time);
+}
+
+stereo::stereo(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path,
+               const bool rectify)
+    : system(cfg, vocab_file_path, mask_img_path),
+      rectifier_(rectify ? std::make_shared<openvslam::util::stereo_rectifier>(cfg) : nullptr),
+      left_sf_(it_, "camera/left/image_raw", 1),
+      right_sf_(it_, "camera/right/image_raw", 1),
+      sync_(SyncPolicy(10), left_sf_, right_sf_) {
+    sync_.registerCallback(&stereo::callback, this);
+}
+
+void stereo::callback(const sensor_msgs::ImageConstPtr& left, const sensor_msgs::ImageConstPtr& right) {
+    auto leftcv = cv_bridge::toCvShare(left)->image;
+    auto rightcv = cv_bridge::toCvShare(right)->image;
+    if (leftcv.empty() || rightcv.empty()) {
+        return;
+    }
+
+    if (rectifier_) {
+        rectifier_->rectify(leftcv, rightcv, leftcv, rightcv);
+    }
+
+    const auto tp_1 = std::chrono::steady_clock::now();
+    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();
+
+    // input the current frame and estimate the camera pose
+    SLAM_.feed_stereo_frame(leftcv, rightcv, timestamp, mask_);
+
+    const auto tp_2 = std::chrono::steady_clock::now();
+
+    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
+    track_times_.push_back(track_time);
+}
+
+rgbd::rgbd(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
+    : system(cfg, vocab_file_path, mask_img_path),
+      color_sf_(it_, "camera/color/image_raw", 1),
+      depth_sf_(it_, "camera/depth/image_raw", 1),
+      sync_(SyncPolicy(10), color_sf_, depth_sf_) {
+    sync_.registerCallback(&rgbd::callback, this);
+}
+
+void rgbd::callback(const sensor_msgs::ImageConstPtr& color, const sensor_msgs::ImageConstPtr& depth) {
+    auto colorcv = cv_bridge::toCvShare(color)->image;
+    auto depthcv = cv_bridge::toCvShare(depth)->image;
+    if (colorcv.empty() || depthcv.empty()) {
+        return;
+    }
+
+    const auto tp_1 = std::chrono::steady_clock::now();
+    const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0_).count();
+
+    // input the current frame and estimate the camera pose
+    SLAM_.feed_RGBD_frame(colorcv, depthcv, timestamp, mask_);
+
+    const auto tp_2 = std::chrono::steady_clock::now();
+
+    const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
+    track_times_.push_back(track_time);
+}
+} // namespace openvslam_ros

ros/1/src/openvslam/src/openvslam_ros.h

@@ -0,0 +1,60 @@
+#ifndef OPENVSLAM_ROS_H
+#define OPENVSLAM_ROS_H
+
+#include <openvslam/system.h>
+#include <openvslam/config.h>
+#include <openvslam/util/stereo_rectifier.h>
+
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <opencv2/core/core.hpp>
+
+namespace openvslam_ros {
+class system {
+public:
+    system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path);
+    openvslam::system SLAM_;
+    std::shared_ptr<openvslam::config> cfg_;
+    ros::NodeHandle nh_;
+    image_transport::ImageTransport it_;
+    std::chrono::steady_clock::time_point tp_0_;
+    cv::Mat mask_;
+    std::vector<double> track_times_;
+};
+
+class mono : public system {
+public:
+    mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path);
+    void callback(const sensor_msgs::ImageConstPtr& msg);
+
+    image_transport::Subscriber sub_;
+};
+
+class stereo : public system {
+public:
+    stereo(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path,
+           const bool rectify);
+    void callback(const sensor_msgs::ImageConstPtr& left, const sensor_msgs::ImageConstPtr& right);
+
+    std::shared_ptr<openvslam::util::stereo_rectifier> rectifier_;
+    image_transport::SubscriberFilter left_sf_, right_sf_;
+    using SyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image>;
+    message_filters::Synchronizer<SyncPolicy> sync_;
+};
+
+class rgbd : public system {
+public:
+    rgbd(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path);
+    void callback(const sensor_msgs::ImageConstPtr& color, const sensor_msgs::ImageConstPtr& depth);
+
+    image_transport::SubscriberFilter color_sf_, depth_sf_;
+    using SyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image>;
+    message_filters::Synchronizer<SyncPolicy> sync_;
+};
+} // namespace openvslam_ros
+
+#endif // OPENVSLAM_ROS_H

ros/1/src/openvslam/src/run_localization.cc

@@ -6,17 +6,12 @@
 
 #include <openvslam/system.h>
 #include <openvslam/config.h>
+#include <openvslam_ros.h>
 
 #include <iostream>
 #include <chrono>
 #include <numeric>
 
-#include <ros/ros.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/imgcodecs.hpp>
 #include <spdlog/spdlog.h>
 #include <popl.hpp>
 
@@ -28,13 +23,24 @@
 #include <gperftools/profiler.h>
 #endif
 
-void mono_localization(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path,
-                       const std::string& mask_img_path, const std::string& map_db_path, const bool mapping) {
-    // load the mask image
-    const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE);
+void localization(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path,
+                  const std::string& mask_img_path, const std::string& map_db_path, const bool mapping,
+                  const bool rectify) {
+    std::shared_ptr<openvslam_ros::system> ros;
+    if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {
+        ros = std::make_shared<openvslam_ros::mono>(cfg, vocab_file_path, mask_img_path);
+    }
+    else if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Stereo) {
+        ros = std::make_shared<openvslam_ros::stereo>(cfg, vocab_file_path, mask_img_path, rectify);
+    }
+    else if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::RGBD) {
+        ros = std::make_shared<openvslam_ros::rgbd>(cfg, vocab_file_path, mask_img_path);
+    }
+    else {
+        throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());
+    }
 
-    // build a SLAM system
-    openvslam::system SLAM(cfg, vocab_file_path);
+    auto& SLAM = ros->SLAM_;
     // load the prebuilt map
     SLAM.load_map_database(map_db_path);
     // startup the SLAM process (it does not need initialization of a map)
@@ -55,27 +61,6 @@ void mono_localization(const std::shared_ptr<openvslam::config>& cfg, const std:
     socket_publisher::publisher publisher(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher());
 #endif
 
-    std::vector<double> track_times;
-    const auto tp_0 = std::chrono::steady_clock::now();
-
-    // initialize this node
-    const ros::NodeHandle nh;
-    image_transport::ImageTransport it(nh);
-
-    // run the SLAM as subscriber
-    image_transport::Subscriber sub = it.subscribe("camera/image_raw", 1, [&](const sensor_msgs::ImageConstPtr& msg) {
-        const auto tp_1 = std::chrono::steady_clock::now();
-        const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0).count();
-
-        // input the current frame and estimate the camera pose
-        SLAM.feed_monocular_frame(cv_bridge::toCvShare(msg, "bgr8")->image, timestamp, mask);
-
-        const auto tp_2 = std::chrono::steady_clock::now();
-
-        const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
-        track_times.push_back(track_time);
-    });
-
     // run the viewer in another thread
 #ifdef USE_PANGOLIN_VIEWER
     std::thread thread([&]() {
@@ -115,6 +100,7 @@ void mono_localization(const std::shared_ptr<openvslam::config>& cfg, const std:
     // shutdown the SLAM process
     SLAM.shutdown();
 
+    auto& track_times = ros->track_times_;
     if (track_times.size()) {
         std::sort(track_times.begin(), track_times.end());
         const auto total_track_time = std::accumulate(track_times.begin(), track_times.end(), 0.0);
@@ -139,6 +125,7 @@ int main(int argc, char* argv[]) {
     auto mapping = op.add<popl::Switch>("", "mapping", "perform mapping as well as localization");
     auto mask_img_path = op.add<popl::Value<std::string>>("", "mask", "mask image path", "");
     auto debug_mode = op.add<popl::Switch>("", "debug", "debug mode");
+    auto rectify = op.add<popl::Switch>("r", "rectify", "rectify stereo image");
     try {
         op.parse(argc, argv);
     }
@@ -185,12 +172,7 @@ int main(int argc, char* argv[]) {
 #endif
 
     // run localization
-    if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {
-        mono_localization(cfg, vocab_file_path->value(), mask_img_path->value(), map_db_path->value(), mapping->is_set());
-    }
-    else {
-        throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());
-    }
+    localization(cfg, vocab_file_path->value(), mask_img_path->value(), map_db_path->value(), mapping->is_set(), rectify->value());
 
 #ifdef USE_GOOGLE_PERFTOOLS
     ProfilerStop();

ros/1/src/openvslam/src/run_slam.cc

@@ -6,17 +6,12 @@
 
 #include <openvslam/system.h>
 #include <openvslam/config.h>
+#include <openvslam_ros.h>
 
 #include <iostream>
 #include <chrono>
 #include <numeric>
 
-#include <ros/ros.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/imgcodecs.hpp>
 #include <spdlog/spdlog.h>
 #include <popl.hpp>
 
@@ -28,13 +23,24 @@
 #include <gperftools/profiler.h>
 #endif
 
-void mono_tracking(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path,
-                   const std::string& mask_img_path, const bool eval_log, const std::string& map_db_path) {
-    // load the mask image
-    const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE);
+void tracking(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path,
+              const std::string& mask_img_path, const bool eval_log, const std::string& map_db_path,
+              const bool rectify) {
+    std::shared_ptr<openvslam_ros::system> ros;
+    if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {
+        ros = std::make_shared<openvslam_ros::mono>(cfg, vocab_file_path, mask_img_path);
+    }
+    else if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Stereo) {
+        ros = std::make_shared<openvslam_ros::stereo>(cfg, vocab_file_path, mask_img_path, rectify);
+    }
+    else if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::RGBD) {
+        ros = std::make_shared<openvslam_ros::rgbd>(cfg, vocab_file_path, mask_img_path);
+    }
+    else {
+        throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());
+    }
 
-    // build a SLAM system
-    openvslam::system SLAM(cfg, vocab_file_path);
+    auto& SLAM = ros->SLAM_;
     // startup the SLAM process
     SLAM.startup();
 
@@ -46,27 +52,6 @@ void mono_tracking(const std::shared_ptr<openvslam::config>& cfg, const std::str
     socket_publisher::publisher publisher(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher());
 #endif
 
-    std::vector<double> track_times;
-    const auto tp_0 = std::chrono::steady_clock::now();
-
-    // initialize this node
-    const ros::NodeHandle nh;
-    image_transport::ImageTransport it(nh);
-
-    // run the SLAM as subscriber
-    image_transport::Subscriber sub = it.subscribe("camera/image_raw", 1, [&](const sensor_msgs::ImageConstPtr& msg) {
-        const auto tp_1 = std::chrono::steady_clock::now();
-        const auto timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(tp_1 - tp_0).count();
-
-        // input the current frame and estimate the camera pose
-        SLAM.feed_monocular_frame(cv_bridge::toCvShare(msg, "bgr8")->image, timestamp, mask);
-
-        const auto tp_2 = std::chrono::steady_clock::now();
-
-        const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
-        track_times.push_back(track_time);
-    });
-
     // run the viewer in another thread
 #ifdef USE_PANGOLIN_VIEWER
     std::thread thread([&]() {
@@ -106,6 +91,7 @@ void mono_tracking(const std::shared_ptr<openvslam::config>& cfg, const std::str
     // shutdown the SLAM process
     SLAM.shutdown();
 
+    auto& track_times = ros->track_times_;
     if (eval_log) {
         // output the trajectories for evaluation
         SLAM.save_frame_trajectory("frame_trajectory.txt", "TUM");
@@ -149,6 +135,7 @@ int main(int argc, char* argv[]) {
     auto debug_mode = op.add<popl::Switch>("", "debug", "debug mode");
     auto eval_log = op.add<popl::Switch>("", "eval-log", "store trajectory and tracking times for evaluation");
     auto map_db_path = op.add<popl::Value<std::string>>("", "map-db", "store a map database at this path after SLAM", "");
+    auto rectify = op.add<popl::Switch>("r", "rectify", "rectify stereo image");
     try {
         op.parse(argc, argv);
     }
@@ -195,12 +182,7 @@ int main(int argc, char* argv[]) {
 #endif
 
     // run tracking
-    if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {
-        mono_tracking(cfg, vocab_file_path->value(), mask_img_path->value(), eval_log->is_set(), map_db_path->value());
-    }
-    else {
-        throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());
-    }
+    tracking(cfg, vocab_file_path->value(), mask_img_path->value(), eval_log->is_set(), map_db_path->value(), rectify->value());
 
 #ifdef USE_GOOGLE_PERFTOOLS
     ProfilerStop();