利用普通双目摄像头和rtabmap做立体建图

手头有一个淘宝够的普通双目摄像头，尝试用其做了手持建图的测试，现记录步骤及注意事项如下，供以后参考。

运行环境为基于Ubuntu16.04的ROS kinetic；
建图使用rtabmap_ros，有官方提供的参考文档：rtabmap_ros手持建图。
主要步骤有一下几项：
1、通过ROS提供的标定程序对相机进行标定，得到一组标定参数，标定过程参见标定过程；
2、创建一个能够读取摄像头数据的节点，并切分出左右图像；然后将图像数据和标定参数分别发布到如下几个话题（括号内为话题的消息类型）：

	left/image_raw (sensor_msgs/Image)
	right/image_raw (sensor_msgs/Image)
	left/camera_info (sensor_msgs/CameraInfo)
	right/camera_info (sensor_msgs/CameraInfo) 

3、以上话题发布之后可以启动stereo_image_proc节点，该节点会接收以上几个话题的消息，并进行标定，标定完成后再发布如下几个话题：

	left/image_rect_color (sensor_msgs/Image)
	right/image_rect_color (sensor_msgs/Image)
	left/image_rect (sensor_msgs/Image)
	right/image_rect (sensor_msgs/Image)
	points2 (sensor_msgs/PointCloud2)

4、之后启动建图程序即可。
有两种启动方式，分别为ratbmap自带的查看工具和rviz方式的工具。
rtabmapviz方式启动：

	roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rtabmap_args:="--delete_db_on_start"

rviz方式启动：

	roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rtabmap_args:="--delete_db_on_start" rviz:=true rtabmapviz:=false

启动会后应该能看到建图效果，我测试时的效果如下：



可以看到，其建立的图像应该是特征点地图，属于稀疏图，局限性较大。

测试时遇到了一些坑，现记录如下；
注意事项：
1、注意各交接的模块之间话题一定要匹配，比如命名空间、话题名称等等；
2、相机标定时，要根据自己标定时实际的方格大小设置标定参数，比如我的是棋盘格式9x7的，角点数是8x6，则size参数应该为8*6，方格边长为0.0234m,则square参数应该填0.0234；
另外，启动了相机左右图发布程序之后，若直接按官方文档里的指令进行校正启动，则会报如下错误：

('Waiting for service', '/my_stereo/left/set_camera_info', '...')
Service not found
('Waiting for service', '/my_stereo/right/set_camera_info', '...')
Service not found

我们现在的校准不关心这个服务，所以可以加一个参数“–no-service-check”，忽略这个检查。
我的实际标定命令如下：

rosrun camera_calibration cameracalibrator.py --approximate 0.1 --size 8x6 --square 0.0234 right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw right_camera:=/my_stereo/right left_camera:=/my_stereo/left --no-service-check

3、标定的数据存放在/tmp/calibrationdata.tar.gz中，注意另存一下，/tmp下的数据会被定期的清理。
4、标定结果样例：

Left:
('D = ', [0.15700041597711392, -0.5816125132681096, 0.009177113974845021, 0.005776352945253162, 0.0])
('K = ', [822.8451233408556, 0.0, 370.08821607444503, 0.0, 825.808578427481, 250.51637252807234, 0.0, 0.0, 1.0])
('R = ', [0.9836573919866463, -0.021172332581402865, 0.17880119553037635, 0.019478031858069712, 0.9997472565724339, 0.011226275020146684, -0.17899369113174196, -0.0075601130252407, 0.983821174414375])
('P = ', [912.7466605309611, 0.0, 187.60241508483887, 0.0, 0.0, 912.7466605309611, 247.2319736480713, 0.0, 0.0, 0.0, 1.0, 0.0])

Right:
('D = ', [0.1354083371969516, -0.29786559274219837, 0.0037615861459039377, 0.011028808525337682, 0.0])
('K = ', [814.6806768195991, 0.0, 361.07143461165, 0.0, 815.3846497136162, 241.12566679257992, 0.0, 0.0, 1.0])
('R = ', [0.9787886987615322, -0.018091906982014988, 0.20407196298967845, 0.020026149786696177, 0.9997719452110938, -0.007416932893486832, -0.20389123694123637, 0.011346385793714001, 0.9789277925506011])
('P = ', [912.7466605309611, 0.0, 187.60241508483887, -54.26348966505485, 0.0, 912.7466605309611, 247.2319736480713, 0.0, 0.0, 0.0, 1.0, 0.0])
('self.T ', [-0.058189739536951474, 0.001075577758858753, -0.012132234861507028])
('self.R ', [0.9996780538028401, 0.000839289247992199, -0.025359107605041616, -0.0003535797125895043, 0.9998165273355033, 0.01915167481348713, 0.025370528696752066, -0.01913654253863317, 0.9994949369623212])
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo/left]

camera matrix
822.845123 0.000000 370.088216
0.000000 825.808578 250.516373
0.000000 0.000000 1.000000

distortion
0.157000 -0.581613 0.009177 0.005776 0.000000

rectification
0.983657 -0.021172 0.178801
0.019478 0.999747 0.011226
-0.178994 -0.007560 0.983821

projection
912.746661 0.000000 187.602415 0.000000
0.000000 912.746661 247.231974 0.000000
0.000000 0.000000 1.000000 0.000000

# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo/right]

camera matrix
814.680677 0.000000 361.071435
0.000000 815.384650 241.125667
0.000000 0.000000 1.000000

distortion
0.135408 -0.297866 0.003762 0.011029 0.000000

rectification
0.978789 -0.018092 0.204072
0.020026 0.999772 -0.007417
-0.203891 0.011346 0.978928

projection
912.746661 0.000000 187.602415 -54.263490
0.000000 912.746661 247.231974 0.000000
0.000000 0.000000 1.000000 0.000000

最后，提供部分参考源码：相机数据发布及标定结果样例