SLAM入坑之一:用realsense D415跑一下ORB-SLAM在ROS-melodic上

一.安装ROS,realsense-ros,librealsense

这一块有很多大佬们已经分享了的,不再赘述了,ROS,realsense-ros直接按照官网的方法去安装.

librealsense安装如下

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u
sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils
sudo apt-get install librealsense2-dev

如果成功安装，可以使用realsense-viewer命令打开Realsense客户端,然后插上realsense,一定要用USB3,要不各种丢数据整死你.

二.在ROS下安装ORB-SLAM3

官网https://github.com/UZ-SLAMLab/ORB_SLAM3.git

因为是要在ROS下测试,直接

cd ~/catkin_ws/src/
git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git

建立第三方库:

cd ORB_SLAM3
chmod +x build.sh
./build.sh

在此之前,需要安装Pangolin,OpenCV,Eigen3(最少3.1版),DBoW2 and g2o (Included in Thirdparty folder).不会的自己谷歌,要学会自己学习.

然后把ORB-SLAM3写入bash,按照自己的地址改一下:

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
cd
source ~/.bashrc 

然后cd到ORB-SLAM文件下执行ros的sh

chmod +x build_ros.sh
./build_ros.sh

三.获取D415相机内参阵intrinsic:

安装好realsense-ros之后:

roscore
roslaunch realsense2_camera rs_rgbd.launch
rostopic echo /camera/color/camera_info

自己标定也可以,但是fx和fy不知道为啥会相差很大

这是我自己标定的

RGB intrisic matrix is:
[[617.80462732   0.         310.3541441 ]
 [  0.         618.05936895 230.64186799]
 [  0.           0.           1.        ]]
RGB Distortion matrix is:
[[ 1.36745009e-01 -1.85886269e-01  9.91348265e-04  1.19814982e-04
  -3.37693396e-01]]

这是ROS告我的:

K: [924.0258178710938, 0.0, 627.2476806640625, 0.0, 923.494873046875, 344.0141296386719, 0.0, 0.0, 1.0]

没有畸变参数...

好吧,我大概知道了,分辨率的问题,opencv标定的是640*480,ROS是给的1280*720...也许是我错的,有大佬知道正确答案的话可以留言交流一下,谢

还记得要改一下depth factor,要不跑出来的效果自己都想笑

DepthMapFactor: 1000.0

在Asus.yaml下改成自己的相机参数,重命名成D415.yaml,然后放在自己喜欢的目录下.

四:改一下orb-slam3的topic:

在~/catkin_ws/src/ORB_SLAM3/Examples/ROS/ORB_SLAM3/src下的ros_rgbd.cc文件subscribe的名字不对,因为realsense发布的名字和其他的深度相机不一样,要改为

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 100);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image_rect_raw", 100);

如果是ORB-SLAM2,100为1就可.

之后要再运行./build_ros.sh

注意:每次改完ros_rgbd.cc都要build一下,否则无效.

五:编译ORB-SLAM:

这一步有很多坑啊,opencv只能装一个版本,要不会找不到yaml文件(这个问题还么解决,好像是要强行卸载opencv3或4,但是卸载后就会找不到库),所以我决定用ORB-slam2,步骤和上面一样的,但是还是会出现很多头疼的东西:

 

出现错误error: usleep is not declared in this scope

https://github.com/raulmur/ORB_SLAM2/issues/337

出现错误Undefined reference to symbol '_ZN5boost6system15system_categoryEv'

https://github.com/raulmur/ORB_SLAM2/issues/494

六.运行ORB-SLAM2

然后差不多所有配置都搞定了
最后，打开三个终端

roscore
rosrun ORB_SLAM2 RGBD /home/(user name)/catkin_ws/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/(user name)/catkin_ws/ORB_SLAM2/Examples/ROS/ORB_SLAM2/D415.yaml
roslaunch realsense2_camera rs_rgbd.launch

之前就是ORB-SLAM3中无法识别D415.yaml搞得我直接爆炸...只能用orb-SLAM2,效果如下:

七.问题???????欢迎大佬留言讨论

到目前为止,ORB-SLAM运行成功,但是很多细节问题还没有解决

1. realsense的ROS官网上提供了rs_aligned_depth.launch这个launch文件,目的是让RGB和depth重合,网上的所有教程都没有运行这个.论文中好像说ORB-SLAM2的RGBD是用ICP方法解决的T矩阵求解问题,所以...我觉得不运行这个对精度有影响(比如D435的depth和RGB不匹配的话相差十万八千里).D415的depth和RGB不align还是挺接近的,所以差别没有那么大.

如果launch的是rs_aligned_depth.launch,那么在第四步中要把

message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image_rect_raw", 1);

改成

message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);

可以对比上图,还是有区别的,虽然不大:D,改了后再build一下.

2.相机分辨率的问题,realsense的launch分辨率比D415.yaml中的大,影响如何???

3.如何用这个进行稠密建图?