ORB-SLAM2 在線構建稠密點雲（二）

在之前的一篇博客（ORB-SLAM2 在線構建稠密點雲（一））中我們是把修改後的***ORB_SLAM2_PointCloud***代碼編譯成一個庫，然後新建一個ROS節點調用這個庫，實現利用相機在線建圖的，這種方式有兩個弊端。

問題一：
每次我們修改 ORB_SLAM2_PointCloud 以後，我們需要把 libORB_SLAM2.so 文件重新複製到ROS工作空間中，如果我們修改過 .h　頭文件的化我們還需要把頭文件也複製過去，這就造成了一定的不方便。其實在ORB 中寫好有編譯文件可以支持ROS的，因此我們需要在前面的基礎上進行一定的優化，使用ORB提供的ROS 節點進行修改。

問題二：
ORB_SLAM2_PointCloud 代碼只支持RGBD模式的相機，而雙目、單目計算深度的方式不同，雙目依靠立體匹配來計算深度。

因此接下來的博客我們先解決這兩個問題，對代碼進行優化，我們調整一下系統架構，我們設計三個主要的節點包，第一個節點作爲驅動節點，採集攝像頭傳感器的數據。第二個節點主要做姿態估計，提供Tcw。第三個節點作爲建圖節點，收集第一和第二節點的數據構建點雲地圖或者八叉樹地圖。

首先在編譯原版ORB_SLAM2 ROS節點的時候會遇到一個錯誤（我使用的系統版本是ubuntu1604+ROS Kinetic）,解決這個錯誤以後我們再修改對應的ROS節點，實現實時構建。

1 ORB_SLAM2 ROS 編譯錯誤解決

首先在github上下載ORB_SLAM2的代碼，按照ORB_SLAM2在github上提供的步驟我們先編譯 ORB_SLAM2 ，然後開始編譯ORB_SLAM2 的ROS節點

1、 指向ORB的ROS目錄作爲工作ROS包。

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS

２、運行自帶的腳本

./build_ros.sh

這時候在ubuntu1604+ROS-K版本上會提示以下錯誤，
/usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line

這是由於沒有找到boost庫的原因,參考下列文章進行解決[1]. 將系統中的庫文件直接拷貝出來，然後在 Examples/ROS/ORB_SLAM2 目錄下的　CMakeLists.txt　文件中指向這個庫文件即可。這樣就可以編譯通過了。

３、接下來我們按照github上提供的啓動ROS節點命令進行測試，確認工程編譯沒有問題

• 1）單目

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

• 2）雙目

rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true

rosbag play --pause MH_03_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw

在線運行EUROC數據集

ORB_SLAM2在使用rosbag運行的時候，出現跟蹤丟失的現象比較頻繁，主要集中中相機旋轉的時候。

• 3）RGB-D

修改 ros_rgbd.cc 第69行的接收深度圖topic爲 “/camera/depth/image” ，彩色圖topic改爲 “/camera/rgb/image_color” ，重新用 ./build_ros.sh 腳本編譯，然後啓動RGBD節點

然後啓動RGBD節點

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml 

rosbag play --pause rgbd_dataset_freiburg1_room.bag

使用TUM提供的RGBD rosbag　沒有成功運行，目前只發現了圖像的頻率只有12-13HZ，而深度圖的頻率是13-14HZ，彩色圖和深度圖頻率不一致，不知道是不是RGB圖和Depth圖時間戳的問題(有知道的小夥伴煩請指導一下)。這個在使用真實相機的時候會得到解決。這裏我自己寫了一個節點把TUM的文件夾中的時間對齊以後的圖片發佈在topic上得到了一個比較好的效果：

在線運行TUM數據集

２ RGBD相機在線運行

RGBD相機我們使用的是Astra的RGBD相機，這個相機自帶有ROS節點，驅動可參考wiki進行安裝。

2.1 添加ROS節點文件

修改 Examples/ROS/ORB_SLAM2/src 目錄下的文件。複製 ros_rgbd.cc 文件然後重命名爲 astra.cc ,在 astra.cc 填入以下內容：

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <tf/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

class ImageGrabber
{
public:
	ros::NodeHandle nh;
	ros::Publisher  pub_rgb,pub_depth,pub_tcw,pub_camerapath;
	size_t mcounter=0;	 
	nav_msgs::Path  camerapath;

    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM),nh("~")
    {
	   //創建ROS的發佈節點

	   pub_rgb= nh.advertise<sensor_msgs::Image> ("RGB/Image", 10); 
	   pub_depth= nh.advertise<sensor_msgs::Image> ("Depth/Image", 10); 
	   pub_tcw= nh.advertise<geometry_msgs::PoseStamped> ("CameraPose", 10); 
	   pub_camerapath= nh.advertise<nav_msgs::Path> ("Path", 10); 
    }

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

    ORB_SLAM2::System* mpSLAM;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 3)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;        
        ros::shutdown();
        return 1;
    }    

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 10);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 10);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    bool  isKeyFrame =true;
    cv::Mat Tcw;
    Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    if (!Tcw.empty())
	{
				  //cv::Mat Twc =Tcw.inv();
				  //cv::Mat TWC=orbslam->mpTracker->mCurrentFrame.mTcw.inv();  
				  cv::Mat RWC= Tcw.rowRange(0,3).colRange(0,3);  
				  cv::Mat tWC= Tcw.rowRange(0,3).col(3);

				  tf::Matrix3x3 M(RWC.at<float>(0,0),RWC.at<float>(0,1),RWC.at<float>(0,2),
							      RWC.at<float>(1,0),RWC.at<float>(1,1),RWC.at<float>(1,2),
							      RWC.at<float>(2,0),RWC.at<float>(2,1),RWC.at<float>(2,2));
				  tf::Vector3 V(tWC.at<float>(0), tWC.at<float>(1), tWC.at<float>(2));
				  
				 tf::Quaternion q;
				  M.getRotation(q);
				  
			      tf::Pose tf_pose(q,V);
				  
				   double roll,pitch,yaw;
				   M.getRPY(roll,pitch,yaw);
				   //cout<<"roll: "<<roll<<"  pitch: "<<pitch<<"  yaw: "<<yaw;
				  // cout<<"    t: "<<tWC.at<float>(0)<<"   "<<tWC.at<float>(1)<<"    "<<tWC.at<float>(2)<<endl;
				   
				   if(roll == 0 || pitch==0 || yaw==0)
					return ;
				   // ------
				  
				  std_msgs::Header header ;
				  header.stamp =msgRGB->header.stamp;
				  header.seq = msgRGB->header.seq;
				  header.frame_id="camera";
				  //cout<<"depth type: "<< depth. type()<<endl;
				  
				  sensor_msgs::Image::ConstPtr rgb_msg = msgRGB;
				  sensor_msgs::Image::ConstPtr depth_msg=msgD;
				  
				 geometry_msgs::PoseStamped tcw_msg;
				 tcw_msg.header=header;
				 tf::poseTFToMsg(tf_pose, tcw_msg.pose);
				  
				 camerapath.header =header;
				 camerapath.poses.push_back(tcw_msg);
				  
				 pub_tcw.publish(tcw_msg);	                      //Tcw位姿信息
				 pub_camerapath.publish(camerapath);  //相機軌跡
				 if( isKeyFrame)
				{
					pub_rgb.publish(rgb_msg);
					pub_depth.publish(depth_msg);
				}
	}
	else
	{
	  cout<<"Twc is empty ..."<<endl;
	}
}

2.2 修改配置文件

a. 修改 Examples/ROS/ORB_SLAM2 目錄下的 CMakeLists.txt 文件，增加如下內容：

b. 重新用 ./build_ros.sh 腳本編譯工程

c. 在 ORB_SLAM2/Examples/ROS/ORB_SLAM2 目錄下新建文件 Astra.yaml ，根據你使用的相機矯正參數，修改RGB-D相機的內參數。 （實際上只需要修改　fx fy cx cy 這幾個參數即可）

%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: 1000

# 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

2.3 啓動運行RGBD在線節點

打開兩個終端輸入以下內容，啓動節點

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS
rosrun ORB_SLAM2 astra Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Astra.yaml

啓動相機

roslaunch astra_launch astra.launch

該節點一共會發布4個topic, 以下是測試效果：
/RGBD/CameraPose 和 /RGBD/Path 分別是發佈相機的位姿和軌跡
/RGBD/RGB/Image 和 /RGBD/Depth/Image 這是用於發佈關鍵幀的彩色圖像幀和深度圖像幀

下面分別是用RGB-D相機和TUM數據集作爲演示效果。

ORB-SLAM Astra相機運行

在線運行TUM數據集

讀過代碼的同學可以發現在節點文件中有一個狀態變量“isKeyFrame”，這個是用來指示這個圖像幀是否是關鍵幀用的，因爲在構建點雲的時候我們不需要用每一幀都進行拼接，這樣對點雲的維護會增大計算量，我們只挑選關鍵幀來進行重構點雲。這個功能需要修改原版ORB_SLAM2的接口函數，這裏暫時留給大家，我們在下一篇博客中修改。

總結：　到這裏我們利用ORB_SLAM2提供的ROS節點作爲位姿估計節點，在線運行了RGBD和Stereo的例子，這裏我們還沒有實現點雲地圖的構建，後面一篇博客我們把建圖單獨作爲一個節點，接收位姿估計節點輸出的Tcw數據，實現點雲地圖的構建。

[1] https://blog.csdn.net/sinat_38343378/article/details/78883919

上一篇 ：ORB-SLAM2 在線構建稠密點雲（一）　　　 下一篇 ：ORB-SLAM2 在線構建稠密點雲（三）

如果大家覺得文章對你有所幫助，麻煩大家幫忙點個贊。O(∩_∩)O

歡迎大家在評論區交流討論（[email protected]）