激光雷达和相机联合标定

激光雷达使用的是Rsruby-Lite
相机使用的是leopard

1、相机内参标定

使用matlab进行标定，本文中打印了一张A2大小的棋盘格。具体标定方法可以参照其他博客，在此不赘述。
棋盘格链接：链接: https://pan.baidu.com/s/1JR3K5cpCJfquSP7ob8vcoA 密码: e3pk
注意：标定一定要选择硬纸板或者不易变形的物体作为底板。另外，标定图像越多越好，我采样了84张图片。标定误差为0.07

2、相机和激光雷达联合标定

github地址
需要在python2环境下运行
播放rosbag可以不使用下面的roslaunch，直接rosbag play xxx.bag即可

(1)录制rosbag包

将激光雷达和摄像头正对着一个板子即可（不需要标定板，板子不应过厚，应保证四个角点清晰可见），我选择的是道路的方形路牌
bag包里面需要有三个类型的消息：
相机的消息通过ros的usb_cam包可以获取

/points_raw    (sensor_msgs/PointCloud2)
/usb_cam/image_raw (sensor_msgs/Image)
/usb_cam/camera_info (sensor_msgs/CameraInfo) (optionally generated by camera_calibration, see below)

注意：录制数据包时我选择了6个地点(如下图，箭头为相机朝向），每个数据包固定录制10s即可

因为使用的是matlab标定，因此需要将matlab标定出来的参数转化为ros标定出来的参数：参考链接

(2)更新camera_info

若相机出厂不带camera_info，则需要根据相机内参标定中的参数进行更新

rosrun lidar_camera_calibration update_camera_info.py <original_file.bag> <calibration_file.yaml>

(3)联合标定

此项目需要自己更新点云的范围
calibrate_camera_lidar.py中的extract_points_3D函数中有以下代码：

# Select points within chessboard range
inrange = np.where((points[:, 0] > 3) &
                   (points[:, 0] < 4) &
                   (np.abs(points[:, 1]) < 2) &
                   (points[:, 2] < 1))

可以自行修改范围，上述代码表示在前方3~4m范围，左右各1m范围，激光雷达上方1m往下的范围内显示激光雷达点。首先需要将范围设置的大一点，保证板子能在点云内显示。然后随意点击几个标定板范围的点云，会显示点击点云的坐标，根据此坐标可以更新点云范围。
例如：
四个坐标为

[[ 7.91058636  0.30166537  0.48335359]
 [ 7.82806826 -0.8249867   0.48059854]
 [ 7.7969718  -0.81680095 -0.14847405]
 [ 7.91867876  0.30688596 -0.15009101]]

那么可以将点云范围设置为

X在(7~9)
Y在(-1.5~1)
Z在(-1~1)中

读取npy文件代码:

import numpy as np

print(np.load("./pcl_corners.npy"))
print(np.load("./img_corners.npy"))
print(np.load("./R.npy"))
print(np.load("./T.npy"))
print(np.load("./euler.npy"))

注意：不能将点云框的太死，否则有些板子的点可能会丢失

播放更新了参数的rosbag包，然后运行以下指令

rosrun lidar_camera_calibration calibrate_camera_lidar.py --calibrate

依次点击板子的四个角点，然后再点击图像中板子的四个角点。然后关闭点云和相机的两个窗口。会在calibration_data/lidar_camera_calibration中生成pcl_corners.npy和img_corners.npy
注意：若点击角点的顺序按照从左上开始，顺时针点击，需要注意输出角点的顺序（因为点云看不出正反，可能是反向点击了四个点），需要读取pcl_corners.npy文件进行确认。按照此顺序，根据激光雷达坐标系，y坐标应该是先减少再近乎不变再增加，z坐标应该是先近乎不变再减少再近乎不变。

一个数据包只采集板子的四个角点的坐标，然后将四个角点的3D和2D坐标统一保存到其他文件后，再删除pcl_corners.npy和img_corners.npy。然后重复以上操作，打开新的数据包进行标定。
因为标定选取了6个位置，每个位置选择了6个角点，最后可以得到24个角点。将24个角点拼接起来。将选取的2D点拼成[24,2]大小的array放入img_corners.npy中，再将选取的3D点拼成[24,3]大小的array放入pcl_corners.npy中。
写入npy文件代码：

import numpy as np

d3_point = np.array([[ 7.91058636 , 0.30166537,  0.48335359],\
 [ 7.82806826, -0.8249867 ,  0.48059854],\
 [ 7.7969718 , -0.81680095, -0.14847405],\
 [ 7.91867876 , 0.30688596 ,-0.15009101],\
 [ 8.40992165, -0.09047135,  0.51365888],\
 [ 8.99070835, -0.88576078,  0.5519231 ],\
 [ 8.94426727, -0.90440661 ,-0.13909627],\
 [ 8.32958412, -0.04713577, -0.14332017],\
 [ 8.34582043 , 1.55940831, 0.51855665],\
 [ 7.85044289 , 0.57428318 , 0.48059854],\
 [ 7.90348864 , 0.58357298 ,-0.12255383],\
 [ 8.4034214 ,  1.52401078 ,-0.1024213 ],\
 [14.31220436, -0.22786899 , 0.45119464],\
 [14.20803356 ,-1.37111962 , 0.44993123],\
 [14.25375748 ,-1.37660742 ,-0.17202684],\
 [14.37234592 ,-0.28008118, -0.17268917],\
 [15.15008068, -0.92981005,  0.47851586],\
 [15.47509003, -1.87597239 , 0.49146581],\
 [15.50647449 ,-1.9237293 , -0.15781347],\
 [15.21449471, -0.88153756, -0.18310592],\
 [14.69152641 , 1.19743121 , 0.41329369],\
 [14.12889671 , 0.21187547 , 0.39615506],\
 [14.27768421 , 0.21057399, -0.22126481],\
 [14.74986744 , 1.14413059, -0.22926421]])
np.save("./pcl_corners.npy", d3_point)

d2_point = np.array([[ 834.27534251 , 352.46530474],\
 [1121.72551051 , 355.13925979],\
 [1119.05155545 , 538.30518079],\
 [ 835.61232004,  539.64215831],\
 [ 915.83097157 , 356.47623731],\
 [1109.69271278 , 377.86787772],\
 [1105.6817802 ,  538.30518079],\
 [ 911.820039 ,   535.63122574],\
 [550.8361071 , 360.48716989],\
 [770.10042129, 345.78041711],\
 [771.43739881, 531.62029316],\
 [554.84703967, 530.28331563],\
 [ 950.59238724 , 438.03186637],\
 [1104.34480267 , 438.03186637],\
 [1103.00782515 , 538.30518079],\
 [ 951.92936476 , 536.96820326],\
 [1028.13708372 , 446.05373153],\
 [1156.48692617 , 451.40164163],\
 [1156.48692617 , 538.30518079],\
 [1026.80010619 , 536.96820326],\
 [760.74157861, 451.40164163],\
 [887.75444354 ,447.39070905],\
 [887.75444354 ,544.99006842],\
 [763.41553366, 544.99006842]])
np.save("./img_corners.npy", d2_point)

最后运行一次标定程序，此次直接关闭激光雷达和相机的窗口就会进行计算。此次标定误差在4.6像素点
最后会生成旋转矩阵、平移向量、欧拉角
读取出来旋转矩阵、平移向量和欧拉角分别为：

[[-0.05071598 -0.99865399  0.01086753]
 [ 0.03579456 -0.01269212 -0.99927857]
 [ 0.99807146 -0.05029039  0.03639007]]
 
[[-0.02557781 -0.09449092  0.14971028]]

[-0.94440713 -1.50868095  2.52699493]

3、3D点投影

根据之前的标定结果可以得到以下参数：

#联合标定得到的[R|T]
lidar2camera = np.array([[-0.05071598,-0.99865399,\
      0.01086753,-0.02557781], [0.03579456,-0.01269212,\
      -0.99927857,-0.09449092], [0.99807146,-0.05029039,\
        0.03639007,0.14971028]] )
#相机内参，不是projection_matrix，注意是3*4的矩阵，用0填充
camera3d_2_2d = [[1991.89468884664, 2.85843784183832,1019.35067539566, 0], [0.,1994.55170758888,453.390674291022, 0], [0, 0, 1,0]]
#相机内参，3*3
camera_intrinsic = np.array([[1991.89468884664, 2.85843784183832, 1019.35067539566], [0.000000, 1994.55170758888, 453.390674291022], [0.000000, 0.000000, 1.000000]])
#相机畸变参数
camera_distortion =np.array([-0.550128832229120, 0.105030329732200, 0.0192641836399498, -0.0105138836942007, 0.000000])

通过lidar2camera 乘上 [4x]的激光雷达点（齐次坐标）会得到[3x]个点，x为点云数
将[3x]个点转化为齐次坐标，维度为[4x]
再用 camera3d_2_2d 乘上 上一步得到的[4x]个点，会得到[3x]个点
假设[3*x]个点中的一个点为(a,b,c)则转化出来的2D点为(a/c,b/c)，并且此2D点为在修正畸变的图像上的坐标
去除畸变代码：

ret,frame = cap.read()
frame = cv2.undistort(frame, camera_intrinsic, camera_distortion)

投影出来的效果图：