3D線(xiàn)激光成像數學(xué)模型簡(jiǎn)析與實(shí)現

點(diǎn)云的獲取方式有多種，比如三維成像傳感器、Lidar激光探測與測量、逆向工程等... 對于尋常百姓家，后2者的成本是十分昂貴的，所以我們可以嘗試玩一下自己搭建三維成像傳感器中的一種——3D線(xiàn)激光，一個(gè)單目相機，一個(gè)激光足矣。

對于3D線(xiàn)掃，要產(chǎn)生點(diǎn)云，首先需要對系統進(jìn)行標定，這里我們需要獲取到的信息有：

1.相機的內外參 2.激光的平面方程   3.移動(dòng)方向的平移矩陣

在計算出這三步驟結果后，我們就可以通過(guò)每一張2D像素圖片，計算出3D點(diǎn)的空間位置。

圖像中每個(gè)點(diǎn)都可以得到一個(gè)像素坐標P(x,y)，這個(gè)點(diǎn)是實(shí)際空間中的某個(gè)點(diǎn)，和相機中心點(diǎn)的連線(xiàn)在相機平面上的投影得到的。因此根據相機內外參的標定結果，我們可以得到相機的中心點(diǎn)C(Xc,Yc,Zc)，以及這個(gè)像素坐標 $P$ 映射到的實(shí)際空間坐標P0(X0,Y0,Z0)，這里的P0理論上應該是一組點(diǎn)，我們可以直接取Z0=0的點(diǎn)來(lái)方便計算。由這兩個(gè)點(diǎn)組成的空間直線(xiàn)與激光平面的交點(diǎn)就是其三維空間中的點(diǎn)坐標。我們可以通過(guò)直線(xiàn)方程和激光平面方程聯(lián)立解出該點(diǎn)的坐標。

1）直線(xiàn)方程：(x-Xc)/(x-X0) ＝ (y-Yc)/(y-Y0) ＝ (z-Zc)/(z-Z0)2）平面方程：Ax+By+Cz+D=0

在一張圖片中的所有點(diǎn)都計算完畢后，再次計算下一張圖片中的所有點(diǎn)。每張圖片的計算結果都要根據移動(dòng)方向的平移矩陣來(lái)進(jìn)行疊加映射。

halcon有非常簡(jiǎn)單實(shí)用的線(xiàn)激光3D點(diǎn)云成像的相關(guān)例程，我們可以通過(guò)例程自己搭建平臺進(jìn)行3D相機的開(kāi)發(fā)和測試，那就是halcon的例程calibrate_sheet_of_line_calplate.hdev，這個(gè)例程就是對自己用線(xiàn)激光和相機搭建的3D平臺進(jìn)行參數標定。

整個(gè)例程的步驟非常明確，每一步在halcon的例程中都有配文字說(shuō)明，但是有些細節和關(guān)鍵參數需要了解透徹，這樣才能方便我們在搭建自己的系統時(shí)可以替換和改變關(guān)鍵的參數。下面我們來(lái)詳細解析這個(gè)例程：

* Part 1: Perform the calibration of the camera

首先第一步，對相機參數的標定，實(shí)際上是外參的標定

gen_cam_par_area_scan_polynomial (0.0125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000006, 0.000006, 376.0, 120.0, 752, 240, StartParameters)

設置相機的內參（焦距，畸變參數K1,K2,K3,P1,P2,像元尺寸X，像元尺寸Y，圖片中點(diǎn)CX，圖片中心點(diǎn)CY，圖片寬W，圖片高K）

這些基礎相機內參可以直接手動(dòng)填寫(xiě)，不需要特別準確，例如鏡頭多少mm，像元尺寸和圖像大小這些都是可以已知的。

CalTabDescription := 'caltab_30mm.descr'

標定板標準文件，實(shí)際上是halcon的圓點(diǎn)標定板的尺寸30mm，如果大家在用不同長(cháng)度的標定板需要在這里修改，這個(gè)文件名實(shí)際上是對應到halcon默認路徑下的標定板描述文件，在halcon安裝目錄下面有個(gè)calib文件夾，而.descr文件實(shí)際上就是對標定板的描述文件，可直接用txt或notepad打開(kāi)，里面對標定板的長(cháng)寬，邊界，圓點(diǎn)半徑等的一些設定，用戶(hù)有需要也可直接找一個(gè)文件自己手動(dòng)修改成自己的標定板尺寸。

CalTabThickness := .00063

標定板厚度，單位是m，halcon有些例程經(jīng)常會(huì )在單位mm和單位m之間來(lái)回切換，大家需要注意

NumCalibImages := 20

用于外參標定的圖片的數目，這里設置為20，大家可以根據自己實(shí)際上可以拍攝的圖片進(jìn)行修改。這里需要拍攝20張標定板的圖片，這20張標定板圖片用來(lái)計算相機的外參。

后面一段代碼就是循環(huán)對標定板圖片進(jìn)行處理，獲取標定板對應的位置坐標。因為是多張圖片進(jìn)行擬合計算，所以理論上圖片數目多，標定板位置變化越大，對于最后的結果越準確，包括標定板需要各種角度的傾斜，在不同平面上拍攝等等。

*Part 2: Calibrate the orientation of the light plane with respect to the world coordinate system

設置標準世界坐標系坐標和激光平面的計算標定

MinThreshold := 80

這里設置最小閾值，是用來(lái)后面計算激光線(xiàn)圖片時(shí)提取激光的有效范圍用的，也可根據實(shí)際情況修改，不過(guò)影響不大，因為激光圖片的黑白分明非常明顯。

Index := 19

這里需要特別注意，在這里設置第19張圖片標定板的坐標系基準世界坐標，后面的點(diǎn)云坐標系就是基于這個(gè)了。所以大家在拍照的時(shí)候第19張（或者可以改成任何你想要的一張）最好選擇一個(gè)平面而且將標定板放正一些，這樣在后續很多調試的時(shí)候你能明白自己的世界坐標大概在什么位置（可以看到halcon例程中這一張圖片也是很正的位置）。

同時(shí)導入與這個(gè)標定板位置對應的一張激光圖像，這張激光圖像的拍攝理論上是跟標定板在同一位置，即拍攝完第19張標定板后，標定板拿開(kāi)，在同樣的位置打上激光，拍攝激光圖片。

Index := 20

這是和之前第19張圖片位置有一定高度差的另外一個(gè)平面的位置拍攝的標定板圖片，同樣在這個(gè)位置拍攝完標定板后需要再拍攝一張激光圖片，這是因為需要確定激光平面的最簡(jiǎn)單方式就是在空間上找到兩條平行線(xiàn)，即可確定一個(gè)平面。這里第19和第20最好是兩個(gè)平行的面，這樣激光打在上面就會(huì )呈現出兩條平行線(xiàn)，這樣擬合出來(lái)的激光平面就更準確。

后面的代碼都是對激光圖片的有效點(diǎn)提取，以及計算擬合激光平面。擬合完成后有一個(gè)RMS值用來(lái)判斷擬合的精度效果：如果擬合RMS大于這個(gè)設定值，就認為擬合失敗，這里單位也是m。

if (MeanResidual > 5e-5)
  return ()
endif

在計算完成后，halcon會(huì )將激光平面的參數show出來(lái)：

這里實(shí)際上是代表兩個(gè)坐標系之間的pose的轉換關(guān)系，type=0表示的是ZYX模式，從這6個(gè)量我們大概能判斷出最后的激光平面是否計算正常，這也是我之前建議第19張標定板圖片盡量擺正，而且激光器打出的線(xiàn)激光也盡量和標定板平行。從這個(gè)結果可以看出beta角接近360°，gamma接近0度，基本上可以理解為激光和世界坐標只有一個(gè)方向的夾角就是alpha，然后這個(gè)角度也是我們安裝激光時(shí)自己可控的。

在上面兩個(gè)步驟完成之后，實(shí)際上就可以通過(guò)某一張激光圖來(lái)計算出這上面的激光點(diǎn)對應的世界坐標系下的3D點(diǎn)的XYZ值了。

* Part 3: Calibration of the movement of the object between the acquisition of two successive profiles

第三部分，很好理解，就是計算出運動(dòng)平面的方向pose，方便每一條激光線(xiàn)的疊加。

因為線(xiàn)激光3D成像最后一定是需要一個(gè)運動(dòng)平臺的，不論是相機動(dòng)還是物體動(dòng)，都一樣，需要將運動(dòng)平臺動(dòng)的方向（這里的方向指的是XYZ三個(gè)方向）計算出來(lái)。

read_image (CaltabImagePos1, 'sheet_of_light/caltab_at_position_1.png')
read_image (CaltabImagePos20, 'sheet_of_light/caltab_at_position_2.png')
StepNumber := 19

這里應該是提前拍好了兩張標定板，這兩張標定板是經(jīng)過(guò)運動(dòng)平面運動(dòng)一定的步長(cháng)之后拍攝的，那么就把這個(gè)步長(cháng)值設置給StepNumber。這個(gè)步長(cháng)值是我們自己可以控制的，根據實(shí)際運動(dòng)平臺和實(shí)際工作時(shí)的觸發(fā)拍照的情況來(lái)設置。

這里在計算完兩張圖片的標定板位置后，需要將它們的坐標轉換到第一步的第19張標定板對應的基準世界坐標位置下，再進(jìn)行兩張標定板的相對關(guān)系計算，最后除以StepNumber即可。

MovementPose := MovementPoseNSteps / StepNumber

從這個(gè)運動(dòng)平面計算結果可以看出，在X,Z兩個(gè)方向上基本上都是0，在Y方向上有一定的位移。這里同樣建議大家在拍攝這兩張運動(dòng)步長(cháng)的標定板時(shí)，盡量將標定板放正，這樣在計算出來(lái)步長(cháng)結果時(shí)可以直接判斷是否準確。比如我們已經(jīng)知道自己的軸或者機械手運動(dòng)一個(gè)單位是多少mm，最后根據計算出來(lái)的結果進(jìn)行比對即可。到這里sheetofline的所有參數就計算完畢了。

最后總結一下：這三個(gè)步驟實(shí)際上需要拍攝N張標定板圖片，2張激光平面圖片，2張運動(dòng)前后的標定板圖片。

那么最合理的標定步驟應該時(shí)怎樣的呢：

1.首先需要用設備拍攝N-2張姿態(tài)、位置和角度不同的標定板圖片。標定板放在視野范圍內，各種平移和傾斜旋轉；2.拍攝倒數第二張標定板圖片，這張標定板圖片將用來(lái)作為后續的基準坐標系用于計算，在拍攝時(shí)盡量將標定板放置于平面，放正，與相機平行，這樣后續方便自我檢查；3.拍攝完后，將標定板拿走，在同一個(gè)位置打上激光線(xiàn)，激光線(xiàn)最好也與相機平行，與標定板的邊也平行；4.重復第2步，拍攝倒數第一張標定板圖片，這張標定板所在的平面最好是與之前的標準平面有一個(gè)臺階落差，同樣盡量放平放正；5.重復第3步拍攝激光圖片；6.再將標定板放在運動(dòng)平面上，盡量放平放正，拍攝一張；7.控制運動(dòng)平面運動(dòng)一定的步長(cháng)N，記錄N，并且拍攝一張運動(dòng)后的標定板圖片，注意在第6步和第7步中間標定板不能移動(dòng)。

OK，到這里halcon搭建3D線(xiàn)激光的原理和步驟都講解完了，看下最終的效果圖吧~