在之前的一篇博客（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]）