鐳射相機資料融合（6）--鐳射相機標定
轉載於：https://www.cnblogs.com/zoucheng/p/7868264.html
如果自己想要搭建一個鐳射相機系統，那麼首要的就是要確定鐳射與相機的位置關係。那麼下面將介紹一些典型的標定方法歷史。
最早的鐳射相機標定方法，由一些與相機標定類似，只需要一塊標定板，網站提供matlab工具包：http://www.cs.cmu.edu/~ranjith/lcct.html. 演算法思路很簡單，先採集十張左右的資料，分別尋找標定板在相機和鐳射中的對應平面方程，求解約束方程獲取；兩感測器位置方程。這些在作者的主頁有詳細介紹。
KITTI資料集的標定與這個演算法類似，文章發表在2012年的ICRA上Automatic Camera and Range Sensor Calibration using a single Shot.只不過一張影象中出現了多張標定板:
難點在作者如何對應鐳射中某一張的標定板就是影象上的某一張？作者利用了ransac演算法，大致過程就是先假設某三個對應關係是正確的，得出相機與鐳射的位置關係，然後在反求其他標定板對應關係是否正確。不斷迴圈這一操作，最終找出最優關係。
近幾年又出現了，無需標定板，利用鐳射和影象的互資訊標定：http://robots.engin.umich.edu/SoftwareData/ExtrinsicCalib.
不過筆者認為如果想要獲取穩定快捷的標定方法，還是最原始的方法好。下面介紹筆者所用裝置及其標定方法：
筆者所選用的是一架全景相機配合一個旋轉的單線鐳射，單線鐳射的配置教程可以參考：https://jvgomez.github.io/pages/hokuyo-dynamixel-coupling-for-3d-mapping.html 其由一個hokuyo 30米鐳射和一個舵機dynamixel組成。通過舵機旋轉，記錄下每一時刻的點雲來將2D鐳射轉變為3D鐳射。
首先筆者對全景相機進行了標定：http://www-sop.inria.fr/icare/personnel/Christopher.Mei/ChristopherMeiPhDStudentToolbox.html. 如果是其他相機可以自行尋找標定工具包。
這個工具包都直接給出了相機的投影方程，和每個標定板在相機座標系下的方程，我們記為：αm,i: θm,iX+dm,i=0.
另一方面利用PCL中的DoN 特徵（http://pointclouds.org/documentation/tutorials/don_segmentation.php#don-segmentation）分離出標定板在鐳射座標系下的方程：αl,i
可以通過最小化如下方程來約束RT矩陣：
argminR,T∑i=1n1l(i)∑j=1l(i)(θm,i(RX(j)l,i+T)+dm,i)argminR,T∑i=1n1l(i)∑j=1l(i)(θm,i(RXl,i(j)+T)+dm,i)
R和T可以分別求解：
R1=argmaxR∑θTc,i(Rθl,i)R1=argmaxR∑θc,iT(Rθl,i)
t1=argmint∑i(αl,i−(αc,i−θc,it))2t1=argmint∑i(αl,i−(αc,i−θc,it))2
先用USV分解求出近似解：
R1=VUTR1=VUT
θlθtc=USVTθlθct=USVT
再用Levenberg-Marquardt優化求解。
下面給出標定求解程式碼：https://github.com/ZouCheng321/6_calibration
更多詳請下載我的文章：An automatic calibration between an omni-directional camera and a laser rangender for dynamic scenes reconstruction (http://ieeexplore.ieee.org/document/7866544/)
執行calibration.m 進行標定。
執行colorize.m 進行點雲上色。
如果需要看上色後的效果：
cd view mkdir build cmake .. make ./cloud_viewer ../../1.pcd
可以看到影象轉為3D場景：