ORBSLAM2构建稠密点云图及小觅相机的使用
一、 ORB-SLAM2 稠密点云构图
上一篇文章我们尝试复现了基本的 ORB SLAM2，其中构建的地图为稀疏的特征点地图. 这次，我们尝试复现高翔博士关于ORB SLAM2 + 稠密的点云地图的工作
高博的工作是对基本 ORB SLAM2 的扩展，基本思想是为每个关键帧构造相应的点云，然后依据从 ORB SLAM2 中获取的关键帧位置信息将所有的点云拼接起来，形成一个全局点云地图。
1. 代码下载
先下载高博的原文件到自己的工作区间（在主目录中建一个文件夹，我的是SLAM/src）
cd ~/SLAM/src
git clone https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git
原本的 ORB SLAM2 中的 Vocabulary 文件夹比较大，高博没有将它放入自己的 repo 中，因此我们还要做些补充工作，把原本 ORB SLAM2 repo 中的 Vocabulary 文件夹和它里面的 ORBvoc.txt.tar.gz 文件拷贝过来，放到 ORB_SLAM2_modified 路径下。
做完以上工作就可以编译了。先把 /ORB_SLAM2_modified/build, /ORB_SLAM2_modified/Thirdparty/DBoW2/build 和 /ORB_SLAM2_modified/Thirdparty/g2o/build 这三个 build 文件夹删掉，然后运行 ORB_SLAM2_modified 中的 build.sh。
chmod u+x build.sh
./build.sh
2.ROS设置
我们要以 ROS 的形式运行，还需要对 repo 中的 ROS 文件进行编译。这里也要做一些额外设置。
首先是将 ROS 文件所在路径加入到 ROS_PACKAGE_PATH 环境变量中。
即在 ~/.bashrc 文件末尾加入ROS所在路径：
具体操作是
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/SLAM/src/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS
并且新开一个命令行窗口或者在当前窗口中运行 source ~/.bashrc 使改变生效。
第二个设置是修改 ROS 文件夹中的 CMakeLists.txt 文件。 ROS 文件夹中的 CMakeLists.txt 文件没加入PCL 相关的设置。编译 ROS 文件（运行 build_ros.sh）就会报错。我们需要参照 /ORB_SLAM2_modified/CMakeLists.txt 文件，把 PCL 相关的设置添加到 /ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt 文件中。
具体要修改的 5 个位置如下：
...
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
find_package( PCL 1.7 REQUIRED ) ####### 1
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} ####### 2
)
add_definitions( ${PCL_DEFINITIONS} ) ####### 3
link_directories( ${PCL_LIBRARY_DIRS} ) ####### 4
set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
${PCL_LIBRARIES} ####### 5
)
...
随后再删掉 /ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/build 文件夹，就可以运行 build_ros.sh 编译 ROS 文件了。
3.运行
下载 TUM 数据集中比较简单的 fr1/xyz 以及相应的 rosbag数据集,并且放在放在新建的/ORB_SLAM2_modified/datasets 目录中。
先对 rgbd_dataset_freiburg1_xyz 中的 RGB 文件和 Depth 文件进行匹配。
python associate.py rgb.txt depth.txt > association.txt
然后，在/ORB_SLAM2_modified 文件夹下运行如下命令：
./bin/rgbd_tum Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml datasets/rgbd_dataset_freiburg1_xyz datasets/rgbd_dataset_freiburg1_xyz/association.txt
奇妙的BUG
在这一步时，找到的参考命令为
./bin/rgbd_tum Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml datasets/rgbd_dataset_freiburg1_xyz datasets/rgbd_dataset_freiburg1_xyz/associations.txt
在最后一个文件处多了一个s，导致无法运行，振宇学长发现错误后删掉bulid文件夹重新编译，然后依旧无法运行，跑0-7张图之后就陷入死循环不跑了。查看代码未能找到原因。错误如下图
随后学长打开bulid.sh文件发现cmake指令不是默认的debug模式，而是以Release模式编译的（会自动进行一些优化）
于是清空build文件夹之后，以Release模式编译，问题便解决了
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8
结果如下
上述显示中，点云地图是黑白的，视觉效果不太好，在高博的 github issue 中有建立彩色点云地图的方法，按照步骤修改 ~/ORB_SLAM2_modified/include/Tracking.h 和 ~/ORB_SLAM2_modified/src/Tracking.cc，重新编译系统
cd ~/SLAM/src/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/build
make -j8
然后再次运行前述 SLAM 命令，可以得到以下的彩色点云地图
4.地图保存
高博的程序只能实时查看点云地图，不能保存。我们可以稍微修改一下文件 /ORB_SLAM2_modified/src/pointcloudmapping.cc，在其中调用 PCL 库的 pcl::io::savePCDFileBinary 函数就可以保存点云地图了。
具体修改如下：
加入头文件
#include <pcl/io/pcd_io.h>
在 void PointCloudMapping::viewer() 函数中（ 123 行附近）加入保存地图的命令，最后样式如下：
...
for ( size_t i=lastKeyframeSize; i<N ; i++ )
{
PointCloud::Ptr p = generatePointCloud( keyframes[i], colorImgs[i], depthImgs[i] );
*globalMap += *p;
}
pcl::io::savePCDFileBinary("vslam.pcd", *globalMap); // 只需要加入这一句
...
修改之后重新编译程序
并且安装相应的工具，就可以查看生成的文件
sudo apt-get install pcl-tools //安装
pcl_viewer vslam.pcd //查看
二、小觅相机的使用
1.关键文件
参照网上诸多相机使用之后，确定了关键文件：相机的配置文件：小觅版的ORB-SLAM2内有现成的：./config/mynteye_d_stereo.yaml，直接将config文件夹复制进来即可。 发布节点的启动文件，在小觅的SDK内可以找到mynteye_wrapper_d orb_slam2.launch，以及ROS节点文件，节点文件的修改参考了熊猫飞天的博客。
2.添加ROS节点文件
修改一下 Examples/ROS/ORB_SLAM2/src 目录下的文件。我们复制 ros_stereo.cc 文件然后重命名为 mynt.cc
随后在 mynt.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> ("Left/Image", 10);
pub_depth= nh.advertise<sensor_msgs::Image> ("Right/Image", 10);
pub_tcw= nh.advertise<geometry_msgs::PoseStamped> ("CameraPose", 10);
pub_camerapath= nh.advertise<nav_msgs::Path> ("Path", 10);
}
void GrabStereo(const sensor_msgs::Image::ConstPtr msgLeft,const sensor_msgs::Image::ConstPtr msgRight);
ORB_SLAM2::System* mpSLAM;
bool do_rectify;
cv::Mat M1l,M2l,M1r,M2r;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "STEREO");
ros::start();
if(argc != 4)
{
cerr << endl << "Usage: rosrun ORB_SLAM2 Stereo path_to_vocabulary path_to_settings do_rectify" << 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::STEREO,true);
ImageGrabber igb(&SLAM);
stringstream ss(argv[3]);
ss >> boolalpha >> igb.do_rectify;
if(igb.do_rectify)
{
// Load settings related to stereo calibration
cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "ERROR: Wrong path to settings" << endl;
return -1;
}
cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
{
cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
return -1;
}
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);
}
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/mynteye/left/image_mono", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/mynteye/right/image_mono", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));
ros::spin();
// Stop all threads
SLAM.Shutdown();
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");
ros::shutdown();
return 0;
}
void ImageGrabber::GrabStereo(const sensor_msgs::Image::ConstPtr msgLeft,const sensor_msgs::Image::ConstPtr msgRight)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptrLeft;
try
{
cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv_bridge::CvImageConstPtr cv_ptrRight;
try
{
cv_ptrRight = cv_bridge::toCvShare(msgRight);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
bool isKeyFrame =true;
cv::Mat Tcw;
if(do_rectify)
{
cv::Mat imLeft, imRight;
cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR);
Tcw=mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
// cv::imshow("left",imLeft);
// cv::imshow("right",imRight);
// cv::waitKey(1);
}
else
{
Tcw=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->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 =msgLeft->header.stamp;
header.seq = msgLeft->header.seq;
header.frame_id="camera";
//cout<<"depth type: "<< depth. type()<<endl;
sensor_msgs::Image::ConstPtr rgb_msg = msgLeft;
sensor_msgs::Image::ConstPtr depth_msg=msgRight;
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;
}
}
3.修改配置文件
a.修改 Examples/ROS/ORB_SLAM2 目录下的 CMakeLists.txt 文件，更改为mynt camera（.cc文件名及target文件名也需更改为你用的那个）：
b. 修改接收的topic为 "/indemind/left/image_raw " 和 “/indemind/right/image_raw”
c. 重新用 ./build_ros.sh 脚本编译
d. 添加相机的配置文件：小觅版的ORB-SLAM2内有现成的：./config/mynteye_d_stereo.yaml，直接将config文件夹复制进来即可。
4.运行双目在线节点
打开两个终端输入以下内容，启动节点
cd SDK路径
source ./wrappers/ros/devel/setup.bash
roslaunch mynteye_wrapper_d orb_slam2.launch
cd ORB-SLAM2路径
rosrun ORB_SLAM2 mynt Vocabulary/ORBvoc.txt ./config/mynteye_d_stereo.yaml true
可以看到关键帧的捕捉效果并不理想，需要继续探索，以及可能存在其他问题，需要继续实验。