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

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

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

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

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

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

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

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

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

整個(gè)例程的步驟非常明確，每一步在halcon的例程中都有配文字說明，但是有些細(xì)節(jié)和關(guān)鍵參數(shù)需要了解透徹，這樣才能方便我們?cè)诖罱ㄗ约旱南到y(tǒng)時(shí)可以替換和改變關(guān)鍵的參數(shù)。下面我們來詳細(xì)解析這個(gè)例程：

* Part 1: Perform the calibration of the camera

首先第一步，對(duì)相機(jī)參數(shù)的標(biāo)定，實(shí)際上是外參的標(biāo)定

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)

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

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

CalTabDescription := 'caltab_30mm.descr'

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

CalTabThickness := .00063

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

NumCalibImages := 20

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

后面一段代碼就是循環(huán)對(duì)標(biāo)定板圖片進(jìn)行處理，獲取標(biāo)定板對(duì)應(yīng)的位置坐標(biāo)。因?yàn)槭嵌鄰垐D片進(jìn)行擬合計(jì)算，所以理論上圖片數(shù)目多，標(biāo)定板位置變化越大，對(duì)于最后的結(jié)果越準(zhǔn)確，包括標(biāo)定板需要各種角度的傾斜，在不同平面上拍攝等等。

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

設(shè)置標(biāo)準(zhǔn)世界坐標(biāo)系坐標(biāo)和激光平面的計(jì)算標(biāo)定

MinThreshold := 80

這里設(shè)置最小閾值，是用來后面計(jì)算激光線圖片時(shí)提取激光的有效范圍用的，也可根據(jù)實(shí)際情況修改，不過影響不大，因?yàn)榧す鈭D片的黑白分明非常明顯。

Index := 19

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

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

Index := 20

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

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

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

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

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

在上面兩個(gè)步驟完成之后，實(shí)際上就可以通過某一張激光圖來計(jì)算出這上面的激光點(diǎn)對(duì)應(yīng)的世界坐標(biāo)系下的3D點(diǎn)的XYZ值了。

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

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

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

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

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

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

MovementPose := MovementPoseNSteps / StepNumber

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

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

那么最合理的標(biāo)定步驟應(yīng)該時(shí)怎樣的呢：

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

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