RGBD相机重建点云
更新日志
- 2021-4-26 更新 pointcloud_mapping 节点，整理评论区提出的错误信息。
- 2020-2-3 创建 pointcloud_mapping 节点，并修改了ORB-SLAM2在ROS中编译错误的情况。
在之前的一篇博客（ORB-SLAM2 在线构建稠密点云（一））中我们是把修改后的 ORB_SLAM2_PointCloud 代码编译成一个库，然后新建一个ROS节点调用这个库，实现利用RGBD相机在线建图的，这种方式有以下个弊端。
问题一：
每次我们修改 ORB_SLAM2_PointCloud 以后，我们需要把 libORB_SLAM2.so 文件重新复制到ROS工作空间中，如果我们修改过 .h 头文件的化我们还需要把头文件也复制过去，这就造成了一定的不方便。其实在ORB 中写好有编译文件可以支持ROS的，因此我们需要在前面的基础上进行一定的优化，使用ORB提供的ROS 节点进行修改。
问题二：
ORB-SLAM2的核心是估计位姿，后续也推出了VINS-mono 和ORB-SLAM3这些系统，如果每次我们想使用其他的SLAM系统或者是新推出SLAM系统。我们又需要重新去读这个SLAM系统的代码，并重复修改建图和地图管理程序，则工作量很繁琐。
问题三：
ORB_SLAM2_PointCloud 代码只支持RGBD模式的相机，而双目、单目计算深度的方式不同，双目依靠立体匹配来计算深度。后续使用双目计算深度则很难在现有的代码上进行修改。
因此接下来的博客我们先解决这两个问题，对代码结构进行优化，我们使用ROS环境，把系统分为3个节点，数据获取（驱动）、位姿估计（SLAM系统）、地图构建（点云和八叉树。
修改思路
把建图系统分为了三个节点，如下图所示。第一个节点作为驱动节点，采集摄像头传感器的数据。第二个节点利用ORB-SLAM主要做姿态估计，提供Tcw。第三个节点作为建图节点，收集第一和第二节点的建图节点接收图像数据和位姿数据，进行点云的拼接。
修改以后的代码：
- ORB-SLAM2 修改后的代码 必须切换到V1.0.0这个分支，master分支是原版的ORB-SLAM2
- pointcloud_mapping ROS节点 [1]
1、 ORB_SLAM2 ROS 编译错误解决
首先在编译原版ORB_SLAM2 ROS节点的时候会遇到一个错误（我使用的系统版本是ubuntu1604+ROS Kinetic）,解决这个错误以后我们再修改对应的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 - 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相机在线运行
RGBD相机我们使用的是Astra的RGBD相机，这个相机自带有ROS节点，驱动可参考wiki进行安装。
2.1 添加ROS节点文件
修改 Examples/ROS/ORB_SLAM2/src 目录下的文件。复制 ros_rgbd.cc 文件然后重命名为 astra.cc ,在 astra.cc 填入以下内容：
#include
// cout<<" t: "<
(0)<<" "<
(1)<<" "<
(2)<
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()<
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 这几个参数即可）
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 下面分别是用RGB-D相机和TUM数据集作为演示效果。
ORB-SLAM Astra相机运行
在线运行TUM数据集
读过代码的同学可以发现在节点文件中有一个状态变量“isKeyFrame”，这个是用来指示这个图像帧是否是关键帧用的，因为在构建点云的时候我们不需要用每一帧都进行拼接，这样对点云的维护会增大计算量，我们只挑选关键帧来进行重构点云。这个功能需要修改原版ORB_SLAM2的接口函数，这里暂时留给大家，我们在下一篇博客中修改。
3、在ORB-SLAM2上增加Keyframe状态接口
由于我们建图的线程更新需要很大的计算量。其次，有些图像之间的重叠度很大，我们没有必要都为其计算点云，因此我们利用ORB-SLAM2挑选的关键帧来进行建图。
增加keyframe的接口需要自己弄清楚ORB_SLAM2的调用过程，然后逐层添加关键帧状态标志位。在ROS节点中我们是通过System::TrackRGBD() 这个接口函数实现调用ORB_SLAM库
，在System::TrackRGBD() 又调用了mpTracker->GrabImageRGBD()函数, 在Tracking这个类中，mpTracker->GrabImageRGBD() 最终调用函数Tracking::Track() 计算位姿态，并用函数NeedNewKeyFrame() 决定是否插入关键帧。
- cv::Mat System::TrackRGBD(im,depthmap, timestamp)
-
- cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
-
-
- Tracking::Track()
-
-
-
-
- NeedNewKeyFrame()
-
-
在函数Tracking::Track() 中有一个字段标记了是否产生新的关键帧（如下），我们使用这个字段来判断是否有新关键帧产生
// Check if we need to insert a new keyframe if(NeedNewKeyFrame()) CreateNewKeyFrame(); cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp,bool& isKeyframe) step2: 同样对函数Tracking::GrabImageRGBD() 增加一个状态变量 isKeyframe
cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp,bool& isKeyframe); step３: 同样对函数Tracking::Track() 增加一个状态变量 isKeyframe
void Track(bool& isKeyframe); step４: 同样对函数Tracking::Track()中的代码段
// Check if we need to insert a new keyframe if(NeedNewKeyFrame()) CreateNewKeyFrame(); Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 改为:
Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec(),isKeyFrame); 由于我调整了一下Tcw的发布位置，因此修改后的函数为：
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(),isKeyFrame); 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: "<
// cout<<" t: "<
(0)<<" "<
(1)<<" "<
(2)<
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()<
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_camerapath
.
publish
(camerapath
)
;
//相机轨迹
if
( isKeyFrame
)
{
pub_tcw
.
publish
(tcw_msg
)
;
//Tcw位姿信息 pub_rgb
.
publish
(rgb_msg
)
; pub_depth
.
publish
(depth_msg
)
;
}
}
else
{
cout
<<
"Twc is empty ..."
<<endl
;
}
}
4、 pointcloud_mapping 节点
首先 pointcloud_mapping 节点的代码可以在github上下载 [1]，使用的时候请切换到V1.0.0分支。这个代码包的功能主要是利用接收ORB-SLAM2输出的位姿和关键帧这两个话题进行建图。
int main(int argc, char **argv) {
std::string cameraParamFile; ros::init(argc, argv, "pointcloud_mapping", ros::init_options::AnonymousName); if(!ros::ok()) {
cout<<"ros init error..."<<endl; return 0; } float fx =515.2888; //Astra camera float cx =317.9098; float fy =517.6610; float cy =241.5734; float resolution =0.01; Mapping::PointCloudMapper mapper(fx,fy,cx,cy,resolution);; mapper.viewer(); cout<<"ros shutdown ..."<<endl; return 0; } 首先把 pointcloud_mapping 这个ROS包拷贝到的你的ROS工作空间
cd catkin_ws/src git clone -b v1.0.0 https://github.com/RuPingCen/pointcloud_mapping.git cd ../ catkin_make 4.1 利用TUM的RGB-D bag包运行
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/TUM1_ROSbag.yaml step2： 启动 pointcloud_mapping 节点 RGBD建图
source devel/setup.bash roslaunch pointcloud_mapping tum1.launch step3： 播放TUM bag包
rosbag play rgbd_dataset_freiburg1_room.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth/image 下图是在TUM数据集上运行的一个效果图，从图中可以看到点云有明显的没有对齐的现象，我猜测主要是由于深度图和位姿之间的时间戳没有对齐的原因，需要我们用一些额外的方法解决这个问题。
ORB-SLAM分布式建图效果
4.2 利用Astra RGB-D相机运行
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
step2： 启动 pointcloud_mapping 节点 RGBD建图
source devel/setup.bash
roslaunch pointcloud_mapping astra.launch
step3： 启动相机
roslaunch astra_launch astra.launch
下图是一个用Astra RGBD相机构建的3D点云的过程，实测发现真实的相机运行效果会比用TUM的ROS bag包数据好。
ORB-SLAM Astra相机在线构建octomap
关于八叉树地图和点云地图之间在ROS里面在线转换可以参考我之前的博客 Octomap 在ROS环境下实时显示
修改后的 ORB-SLAM 部分的代码位于Github [2]上，使用的时候请切换到V1.0.0分支
修改后的 pointcloud_mapping 部分的代码位于Github [1]上，使用的时候请切换到V1.0.0分支
参考资料
上一篇 ：ORB-SLAM2 在线构建稠密点云（一） 下一篇 ：ORB-SLAM2 在线构建稠密点云（三）
如果大家觉得文章对你有所帮助，麻烦大家帮忙点个赞。O(∩_∩)O
欢迎大家在评论区交流讨论（cenruping@vip..com）
发布者：全栈程序员-站长，转载请注明出处：https://javaforall.net/227943.html原文链接：https://javaforall.net
