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

2、运行自带的脚本

./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 文件中指向这个库文件即可。这样就可以编译通过了。
在这里插入图片描述
3、接下来我们按照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数据集

2 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

欢迎大家在评论区交流讨论(cenruping@vip.qq.com)

©️2020 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客 返回首页