调了一阵子ORB SLAM，不知道放到真的相机里效果好不好。所以想先放到另一个小一点的机器人身上跑跑看。结果发现这机器人身上装备的是两个PointGrey相机，还没双目校正过。所以今天来玩点花活，在ROS里校正一对相机，方便后面的算法使用。
######################################## 安装 pointgrey ROS 驱动包 #####################################
这部分暂时跟校正无关，可以直接看下一部分，我就随手做个记录而已。
ROS wiki 是提供了PointGrey相机的安装包的:
http://wiki.ros.org/pointgrey_camera_driver
但是我怕他给我搞乱了系统配置，所以去github抓了他们的源码下来编译。过程倒也十分简单，见了一个workspace然后编译:
cd ~/point_grey_ws/src
git clone https://github.com/ros-drivers/pointgrey_camera_driver.git
cd ..
catkin_make
记得先在camera.launch里面设置一下相机的***，比如:
<arg name="camera_serial" default="16456235" />
然后用这个命令就能跑单目了:
cd ~/point_grey_ws
. ./devel/setup.bash
roslaunch pointgrey_camera_driver camera.launch
双目的话也是要是在stereo.launch文件里面设置一下相机的***，比如:
<arg name="left_camera_serial" default="16456235" />
<arg name="right_camera_serial" default="16456234" />
然后再运行双目的launch文件：roslaunch pointgrey_camera_driver stereo.launch
没校正的时候其实也只能看到/camera/image_raw这个话题里面有图像，其他的话题都是读不出东西的。所以我们来到下一章，相机校正。
############################################## 在ROS里做相机校正 ##########################################
校正分为两种，单目校正和双目校正。单目校正顾名思义，没有太多需要说的。双目校正在ROS里最后并不会给你fundamental matrix 或者Essential matrix.但双目校正会把图片的极线对齐，也就是y坐标对齐。然后给你一个与baseline有关的量。那么我把单目和双目分开说吧。
单目校正:
举个例子，/camera/image_color 这个话题里的图片没有校正，那我们就用这个ROS自带的程序校正一下：
rosrun camera_calibration cameracalibrator.py --size 9x6 --square 0.0625 image:=/camera/image_color camera:=/camera
上面的9x6是标定板的黑白相间的连接点的数量。打个比方
这个标定板就是7x5
然后--square 0.0625是每个小正方形的边长。其他的不细说了，具体细节在这里：http://wiki.ros.org/camera_calibration
接下来就是慢慢移动标定板，确保图像清晰一点，直到“X”, “Y”, “Size”, “Skew” 底下的长条都变绿了，就可以点击“CALIBRATE”了。计算过程可能需要几分钟，要耐心等一等。
然后就可以点“SAVE”了, 会在这个路径下生成一个yaml文件: ~/.ros/camera_info
所用到的图片SAVE之后会保存到这里，但是不太重要: /tmp/calibrationdata.tar.gz
然后点击 “COMMIT”.在我这里其实有报错，但是没什么影响啊，不管了 。
最后重启一次launch文件，再去rqt里面看看，现在校正后的图像应该有了：/camera/image_rect_color
同样的步骤可以在另一个相机里重复。但是如果要知道两个相机的相对关系，可以不做单目校正，直接进行双目校正。
双目校正:
先运行双目的launch文件，同时开启两个相机：
roslaunch pointgrey_camera_driver stereo.launch
然后，双目校正之前是并不需要做单目校正的。
由于开启双目校正的时候会检查service的状况，然后报错。所以我加了几个flag。话不多说，直接贴代码:
rosrun camera_calibration cameracalibrator.py --size 9x6 --square 0.0625 right:=/camera/right/image_color left:=/camera/left/image_color right_camera:=/camera/right left_camera:=/camera/left --no-service-check --approximate=0.1
和单目一样，确保上面的参数没错就能慢慢移动标定板了。确保图像清晰一点，直到“X”, “Y”, “Size”, “Skew” 底下的长条都变绿了，就可以点击“CALIBRATE”了。计算过程可能需要几分钟，要耐心等一等。
然后就可以点“SAVE”了, 会在这个路径下生成一个yaml文件: ~/.ros/camera_info
所用到的图片SAVE之后会保存到这里，但是不太重要: /tmp/calibrationdata.tar.gz
由于双目其实是对齐了的，所以不用fundamental matrix,可以直接拿camera.bf进行计算。这里的camera.bf是baseline乘以fx。详细解释嘛就是这样：
When using the camera_calibration package for stereo camera calibration the images of right and left camera are rectified and adjusted to become "ideal" stereo images. That means that corresponding points in left and right image have the same value in y (i.e. the epipoles are infinite, epipolar lines are parallel to the x-axis).
In this setting, no fundamental matrix but the baseline (distance from left to right camera center) is needed for 3D triangulation. This information is found in the CameraInfo message of the right camera. Please have a look at the CameraInfo message and at corresponding documentation.
所以最后ORB SLAM2会要你填一些参数，其实都在结果里有了。其中 :
distortion = [k1, k2, p1, p2, k3]
camera matrix =
[fx 0.0 cx
0.0 fy cy
0.0 0.0 1.0]
注意，ROS这个校正程序是把右相机作为默认的，所以填参数的时候要注意看清楚bf和-bf。
projection =
[fx' 0.0 cx' -bf
0.0 fy' cy' 0.0
0.0 0.0 1.0 0.0]
好了，希望采到的数据效果会好。