Added RGBD interface

.gitignore

@@ -29,4 +29,9 @@
 *.app
 
 # Project Files
-.clang_complete
+.clang_complete
+\.project
+
+\.idea/
+
+cmake-build-debug/

CMakeLists.txt

@@ -13,6 +13,7 @@ cs_add_library(orb_slam_2_interface
   src/library/interface.cpp
   src/library/interface_mono.cpp
   src/library/interface_stereo.cpp
+  src/library/interface_rgbd.cpp
 )
 
 cs_add_executable(orb_slam_2_ros_node

include/orb_slam_2_ros/interface.hpp

@@ -8,6 +8,8 @@
 #include <orb_slam_2/System.h>
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
+#include <sensor_msgs/PointCloud.h>
+#include <std_msgs/Bool.h>
 #include <tf/transform_broadcaster.h>
 #include <Eigen/Geometry>
 
@@ -27,6 +29,8 @@ class OrbSlam2Interface {
   OrbSlam2Interface(const ros::NodeHandle& nh,
                     const ros::NodeHandle& nh_private);
 
+  int image_counter=0;
+
  protected:
   // Subscribes and Advertises to the appropriate ROS topics
   void advertiseTopics();
@@ -40,6 +44,14 @@ class OrbSlam2Interface {
                           const std_msgs::Header& header);
   void publishCurrentPoseAsTF(const ros::TimerEvent& event);
 
+  void publishCurrentMap(const std::vector<ORB_SLAM2::MapPoint *> &point_cloud,
+                         const sensor_msgs::ImageConstPtr& msg_rgb);
+
+    void publishGBArunning(bool isGBArunning);
+    void publishLoopClosing(bool loop_closing);
+    void publishEssentialGraphOptimization(bool essential_graph_optimization);
+
+
   // Helper functions
   void convertOrbSlamPoseToKindr(const cv::Mat& T_cv, Transformation* T_kindr);
 
@@ -51,13 +63,26 @@ class OrbSlam2Interface {
   ros::Publisher T_pub_;
   tf::TransformBroadcaster tf_broadcaster_;
   ros::Timer tf_timer_;
+  ros::Publisher Map_pub_;
+    ros::Publisher GBA_info_;
+    ros::Publisher Loop_closing_info_;
+    ros::Publisher Essential_graph_info_;
+
 
   // The orb slam system
   std::shared_ptr<ORB_SLAM2::System> slam_system_;
 
   // The current pose
   Transformation T_W_C_;
 
+  sensor_msgs::PointCloud Map_;
+
+    std_msgs::Bool GBA_running_;
+    std_msgs::Bool loop_closing_;
+    std_msgs::Bool essential_graph_optimization_;
+
+
+
   // Parameters
   bool verbose_;
   std::string vocabulary_file_path_;

include/orb_slam_2_ros/interface_rgbd.hpp

@@ -0,0 +1,46 @@
+#ifndef ORB_SLAM_2_INTERFACE_RGBD
+#define ORB_SLAM_2_INTERFACE_RGBD
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/time_synchronizer.h>
+
+#include "orb_slam_2_ros/interface.hpp"
+
+namespace orb_slam_2_interface {
+
+// The synchronization policy used by the interface to sync rgbd images
+typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
+                                                        sensor_msgs::Image>
+    sync_pol;
+
+// Class handling global alignment calculation and publishing
+class OrbSlam2InterfaceRGBD : public OrbSlam2Interface {
+ public:
+  // Constructor
+  OrbSlam2InterfaceRGBD(const ros::NodeHandle& nh,
+                          const ros::NodeHandle& nh_private);
+
+ protected:
+  // Subscribes to the appropriate ROS topics
+  void subscribeToTopics();
+
+  // Callbacks
+  void rgbdImageCallback(const sensor_msgs::ImageConstPtr& msg_rgb,
+                           const sensor_msgs::ImageConstPtr& msg_depth);
+
+
+
+
+  // Subscribers
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> rgb_sub_;
+  std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_sub_;
+  std::shared_ptr<message_filters::Synchronizer<sync_pol>> sync_;
+};
+
+}  // namespace orb_slam_2_interface
+
+#endif /* ORB_SLAM_2_INTERFACE_RGBD */

launch/run_orb_slam_2.launch

@@ -2,11 +2,16 @@
 <launch>
 
   <node pkg="orb_slam_2_ros" name="orb_slam_2_ros_node" type="orb_slam_2_ros_node" output="screen" >
-    <param name="interface_type" value="stereo" />
-    <param name="vocabulary_file_path" value="/home/millanea/trunk/datasets/kitti/raw/ORBvoc.txt" />
-    <param name="settings_file_path" value="/home/millanea/trunk/datasets/kitti/raw/2011_09_30_drive_18/KITTI00-02_stereo.yaml" />
-    <remap from="camera/left/image_raw" to="/cam00/image_raw" />
-    <remap from="camera/right/image_raw" to="/cam01/image_raw" />
-  </node>
+    <param name="interface_type" value="rgbd" />
+    <param name="vocabulary_file_path" value="" />
+    <param name="settings_file_path" value="" />
+    <!-- optional parameters -->
+    <!-- set frames here
+    <param name="verbose" value="" />
+    <param name="frame_id" value="" />
+    <param name="child_frame_id" value="" />
+	-->
+
+</node>
 
 </launch>

orb_slam_ressources/asus_xtion_settings.yaml

@@ -0,0 +1,69 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters. Adjust them!
+#--------------------------------------------------------------------------------------------
+
+# Camera calibration and distortion parameters (OpenCV) 
+Camera.fx: 535.4
+Camera.fy: 539.2
+Camera.cx: 320.1
+Camera.cy: 247.6
+
+Camera.k1: 0.0
+Camera.k2: 0.0
+Camera.p1: 0.0
+Camera.p2: 0.0
+
+Camera.width: 640
+Camera.height: 480
+
+# Camera frames per second 
+Camera.fps: 30.0
+
+# IR projector baseline times fx (aprox.)
+Camera.bf: 40.0
+
+# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+Camera.RGB: 1
+
+# Close/Far threshold. Baseline times.
+ThDepth: 40.0
+
+# Deptmap values factor
+DepthMapFactor: 1.0
+
+#--------------------------------------------------------------------------------------------
+# ORB Parameters
+#--------------------------------------------------------------------------------------------
+
+# ORB Extractor: Number of features per image
+ORBextractor.nFeatures: 800
+
+# ORB Extractor: Scale factor between levels in the scale pyramid 	
+ORBextractor.scaleFactor: 1.2
+
+# ORB Extractor: Number of levels in the scale pyramid	
+ORBextractor.nLevels: 8
+
+# ORB Extractor: Fast threshold
+# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+# You can lower these values if your images have low contrast			
+ORBextractor.iniThFAST: 20
+ORBextractor.minThFAST: 7
+
+#--------------------------------------------------------------------------------------------
+# Viewer Parameters
+#--------------------------------------------------------------------------------------------
+Viewer.KeyFrameSize: 0.05
+Viewer.KeyFrameLineWidth: 1
+Viewer.GraphLineWidth: 0.9
+Viewer.PointSize:2
+Viewer.CameraSize: 0.08
+Viewer.CameraLineWidth: 3
+Viewer.ViewpointX: 0
+Viewer.ViewpointY: -0.7
+Viewer.ViewpointZ: -1.8
+Viewer.ViewpointF: 500
+

src/library/interface.cpp

@@ -24,6 +24,12 @@ void OrbSlam2Interface::advertiseTopics() {
   // Advertising topics
   T_pub_ = nh_private_.advertise<geometry_msgs::TransformStamped>(
       "transform_cam", 1);
+  Map_pub_ = nh_private_.advertise<sensor_msgs::PointCloud>("slam_map", 1);
+
+    GBA_info_ = nh_private_.advertise<std_msgs::Bool>("GBA_info", 1);
+    Loop_closing_info_ = nh_private_.advertise<std_msgs::Bool>("loop_closing_info",1);
+    Essential_graph_info_ = nh_private_.advertise<std_msgs::Bool>("essential_graph_info",1);
+
   // Creating a callback timer for TF publisher
   tf_timer_ = nh_.createTimer(ros::Duration(0.01),
                               &OrbSlam2Interface::publishCurrentPoseAsTF, this);
@@ -55,6 +61,53 @@ void OrbSlam2Interface::publishCurrentPose(const Transformation& T,
   T_pub_.publish(msg);
 }
 
+ // Publish orbslam2 map to ros point cloud
+void OrbSlam2Interface::publishCurrentMap(const std::vector<ORB_SLAM2::MapPoint *>
+                                          &point_cloud, const sensor_msgs::ImageConstPtr& msg_rgb) {
+
+  // Creating message
+  Map_.points.clear();
+  Map_.header.stamp = msg_rgb->header.stamp;
+  Map_.header.frame_id = "map";
+
+    for(size_t i = 0; i < point_cloud.size(); i++){
+
+      if (point_cloud[i]->isBad()){
+        continue;
+      }
+
+      cv::Mat pos = point_cloud[i]->GetWorldPos();
+      geometry_msgs::Point32 pp;
+      pp.x = pos.at<float> ( 0 );
+      pp.y = pos.at<float> ( 1 );
+      pp.z = pos.at<float> ( 2 );
+
+      Map_.points.push_back ( pp );
+
+    }
+
+      Map_pub_.publish(Map_);
+
+    }
+
+
+    void OrbSlam2Interface::publishGBArunning(bool isGBArunning){
+
+      GBA_running_.data = isGBArunning;
+      GBA_info_.publish(GBA_running_);
+
+    }
+
+    void OrbSlam2Interface::publishLoopClosing(bool loop_closing) {
+        loop_closing_.data = loop_closing;
+        Loop_closing_info_.publish(loop_closing_);
+    }
+
+    void OrbSlam2Interface::publishEssentialGraphOptimization(bool essential_graph_optimization) {
+        essential_graph_optimization_.data = essential_graph_optimization;
+        Essential_graph_info_.publish(essential_graph_optimization_);
+    }
+
 void OrbSlam2Interface::publishCurrentPoseAsTF(const ros::TimerEvent& event) {
   tf::Transform tf_transform;
   tf::transformKindrToTF(T_W_C_, &tf_transform);

src/library/interface_mono.cpp

@@ -47,4 +47,4 @@ void OrbSlam2InterfaceMono::imageCallback(const sensor_msgs::ImageConstPtr& msg)
   }
 }
 
-}  // namespace orb_slam_2_interface
+}  // namespace orb_slam_2_interface

src/library/interface_rgbd.cpp

@@ -0,0 +1,85 @@
+#include <cv_bridge/cv_bridge.h>
+
+#include "orb_slam_2_ros/interface_rgbd.hpp"
+
+namespace orb_slam_2_interface {
+
+OrbSlam2InterfaceRGBD::OrbSlam2InterfaceRGBD(const ros::NodeHandle& nh,
+                                             const ros::NodeHandle& nh_private)
+    : OrbSlam2Interface(nh, nh_private) {
+  // Getting data and params
+  subscribeToTopics();
+  //advertiseTopics();
+  //getParametersFromRos();
+  slam_system_ = std::shared_ptr<ORB_SLAM2::System>(
+      new ORB_SLAM2::System(vocabulary_file_path_, settings_file_path_,
+                            ORB_SLAM2::System::RGBD, false));
+}
+
+void OrbSlam2InterfaceRGBD::subscribeToTopics() {
+  // Subscribing to the rgbd images
+  rgb_sub_ = std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>>(
+      new message_filters::Subscriber<sensor_msgs::Image>(
+          nh_, "/camera/rgb/image_raw", 1));
+  depth_sub_ = std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>>(
+      new message_filters::Subscriber<sensor_msgs::Image>(
+          nh_, "camera/depth_registered/image_raw", 1));
+  // Creating a synchronizer
+  sync_ = std::shared_ptr<message_filters::Synchronizer<sync_pol>>(
+      new message_filters::Synchronizer<sync_pol>(sync_pol(10), *rgb_sub_,
+                                                  *depth_sub_));
+  // Registering the synchronized image callback
+  sync_->registerCallback(
+      boost::bind(&OrbSlam2InterfaceRGBD::rgbdImageCallback, this, _1, _2));
+}
+
+void OrbSlam2InterfaceRGBD::rgbdImageCallback(
+    const sensor_msgs::ImageConstPtr& msg_rgb,
+    const sensor_msgs::ImageConstPtr& msg_depth) {
+  // Copy the ros image message to cv::Mat.
+  cv_bridge::CvImageConstPtr cv_ptr_rgb;
+  try {
+    cv_ptr_rgb = cv_bridge::toCvShare(msg_rgb);
+  } catch (cv_bridge::Exception& e) {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+    return;
+  }
+  cv_bridge::CvImageConstPtr cv_ptr_depth;
+  try {
+    cv_ptr_depth = cv_bridge::toCvShare(msg_depth);
+  } catch (cv_bridge::Exception& e) {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+    return;
+  }
+  // Handing the image to ORB slam for tracking
+  cv::Mat T_C_W_opencv =
+      slam_system_->TrackRGBD(cv_ptr_rgb->image, cv_ptr_depth->image,
+                              cv_ptr_rgb->header.stamp.toSec());
+  // If tracking successfull
+  if (!T_C_W_opencv.empty()) {
+    // Converting to kindr transform and publishing
+    Transformation T_C_W, T_W_C;
+    convertOrbSlamPoseToKindr(T_C_W_opencv, &T_C_W);
+    T_W_C = T_C_W.inverse();
+    publishCurrentPose(T_W_C, msg_rgb->header);
+    // Saving the transform to the member for publishing as a TF
+    T_W_C_ = T_W_C;
+
+    // Publish current ros point cloud (map) every 30 images
+    //TODO improve performance by getting the newly added points only instead of all map points
+    image_counter++;
+    if ( image_counter>30 ) {
+        image_counter = 0;
+        //const std::vector<ORB_SLAM2::MapPoint *> &point_cloud = slam_system_->mpMap->GetAllMapPoints();
+        publishCurrentMap(slam_system_->mpMap->GetAllMapPoints(), msg_rgb);
+    }
+
+  }
+
+    publishGBArunning(slam_system_->IsRunningGBA());
+    publishLoopClosing(slam_system_->IsRunningLoopClosing());
+    publishEssentialGraphOptimization(slam_system_->IsRunningEssentialGraphOptimization());
+
+}
+
+}  // namespace orb_slam_2_interface