接上一篇博客： 博客鏈接
參考開源實現一和2004年論文（Extrinsic Calibration of a Camera and Laser Range Finder (improves camera calibration)）
調試優化總結：
1、標定時使用單個二維碼（AprilTag Family 36h11），標定分別測試了6組, 10組, 12組, 15組標定板位姿，利用15組位姿時，標定效果最好（理論上越多越好，但是沒有測試更多姿態），將算法標定結果與結構外參對比，旋轉角相差1°以內，平移向量相差1~3cm（有點大）。
2、在獲取標定板上的激光點時，首先截取激光點前方90度範圍（應保證標定板在激光前方90度範圍內），減少激光點數量，然後利用RANSAC擬合直線，求出標定板的直線點（即直線內點），最後爲了消除與直線內點在同一條直線上的離羣點（或噪點），通過從激光點中間點向兩邊遍歷，求相鄰激光點距離，若距離大於一個閾值，則移除遠離中間點的離羣點。
代碼如下：
void FilterLaserPoints(
const Eigen::Matrix3Xd& laser_points_origin,
Eigen::Matrix3Xd& laser_points_filtered) {
// filter noise:
// from middle column to both ends to find noise point and remove it
unsigned int forward_col = laser_points_filtered.cols() / 2;
unsigned int backward_col = laser_points_filtered.cols() / 2;
while(backward_col < laser_points_filtered.cols() - 1 || forward_col > 0) {
// if distance of both points > 5cm, abandon the outliner
if(backward_col < laser_points_filtered.cols() - 1) {
double backward_dis = (laser_points_filtered.col(backward_col) -
laser_points_filtered.col(backward_col + 1)).norm();
if(backward_dis > 0.05) {
Utilities::RemoveColumn(laser_points_filtered, backward_col + 1);
} else {
++backward_col;
}
}
if(forward_col > 0) {
double forward_dis = (laser_points_filtered.col(forward_col) -
laser_points_filtered.col(forward_col - 1)).norm();
if (forward_dis > 0.05) {
Utilities::RemoveColumn(laser_points_filtered, forward_col - 1);
--backward_col;
}
--forward_col;
}
} // end while loop
}
3、算法改進：
（1）利用單個標定板上多個二維碼標定（如下圖），可以提高相機位姿計算精度，從而提高標定板平面方程計算精度；
（2）原04年論文方法是將標定板上的所有激光點到平面的距離作爲優化對象，實現時可以改爲標定板上激光線的兩端點到平面的距離作爲優化對象（效果更好），進一步優化可以將下圖中的標定板的黑色邊緣線檢測出來，然後將激光線兩端點到黑色邊緣線的距離作爲優化對象（理論上效果會更好，參見論文：An Algorithm for Extrinsic Parameters Calibration of a Camera and a Laser Range Finder Using Line Features）。
4、新想法：
（1）接着上面3中第（2）條討論，進一步優化可以將圖中的標定板的黑色邊緣線檢測出來，然後將激光線兩端點到黑色邊緣線的距離作爲優化對象（理論上效果會更好，參見論文：An Algorithm for Extrinsic Parameters Calibration of a Camera and a Laser Range Finder Using Line Features）。怎麼實現呢？
1）首先二維碼座標系是定義好的，所以4條邊緣線的直線方程也是可以得到的（比如標定板最下面那條直線方向向量爲
[1, 0, 0]），那麼通過二維碼座標系與相機座標系的變換可以將4條直線的方程轉到相機座標系下（類似將標定板平面方程從二維碼座標系轉到相機座標系，見博客），然後就可以求激光兩端點到標定板邊緣線的距離，進行優化。
2）那麼怎麼判斷激光點與哪兩條邊緣線相交呢？首先激光線打在標定板平面時，與邊緣線只有兩個交點，所以只需分別計算激光兩端點到四條邊緣線的距離，對每個端點取最小的距離即可。