文章目錄
- 1，介紹。
- 2，技術原理
- 3，類型。
- 3.1，直射式
- 3.2，斜射式
- 3.3，兩種三角位移傳感器特性的比較
- 4，什么是光片？
- 5，主要的算子。
- 1，create_sheet_of_light_model
- 2，measure_profile_sheet_of_light
- 3，get_sheet_of_light_result
- 6，三角標定測量代碼詳解。
- 6.1， 相機標定參數預設置。
- 6.2，標定相機。
- 6.3，計算LightPanelPose（光平面的位姿）。
- 6.4，計算MovementPose。
- 6.5，激光三角應用。
- 7，完整代碼。
1，介紹。
三角激光測量是一種位移測量方法，其最大的優點是非接觸性測量。通過三維激光掃描獲取的圖像紋理豐富，分辨率高，具有更好的深度和范圍信息，能更好的滿足微小產品的視覺檢測需求，故在工業應用和基礎科學研究中被廣泛使用，對微小產品表面平面度測量技術的研究就顯得尤為重要。
2，技術原理
激光器發出的激光照射到被測物體表面，激光在被測物表面形成反射，返回到成像器，從而計算出物體的高度。由于入射光和反射光構成一個三角形，所以這種方法被稱為三角測量法。如果激光線投射到物體表面的高度不同，則發光線條不會是一條直線，而是一條表現物體表面高度輪廓的線。通過這條輪廓線，就可以得到物體表面的高度差。
視差圖中的每一行存儲一條輪廓線的值，這里的相機必須是固定的，這樣每一行掃描到的輪廓線才能和視差圖中對應的行平行。而被測物體應當是運動，這樣才能獲得完整的輪廓線。如果系統未經過校準，則不會返回點在世界坐標系的三維坐標，但是仍然可以得到視差圖像，以及測量結果的置信分數。注意，這里的視差圖像有所不同，雙目視覺中的視差圖像體現了左右圖中對應的像素灰度值差，而片光測量結果的視差圖中保存的是被檢測到的輪廓線的子像素。
3，類型。
按照發射角度的不同，激光三角法按入射光線與被測對象表面法線方向所成的角度分為直射式與斜射式。
3.1，直射式
激光器發出的光線，經會聚透鏡聚焦后垂直入射到被測物體表面，物體移動或表面變化導致入射光點沿入射光軸移動。接收透鏡接收來自入射光點處的散射光，并將其成像在光點位置探測器（如PSD，CCD）敏感面上，但由于傳感器激光光束與被測面垂直，因此只有一個準確的調焦位置，其余位置的像都處于不同程度的離焦狀態。
若光點在成像面上的位移為x`，利用相似三角形各邊之間的比例關系，可以求出背側面的位移：
x = a x ‘ s i n θ 2 b s i n θ ? x ‘ s i n ( θ 1 ? θ 2 ) x=\frac{ax^`sin\theta_2}{bsin\theta-x^`sin(\theta_1-\theta_2)} x=bsinθ?x‘sin(θ1??θ2?)ax‘sinθ2??
式中，a為激光束光軸和接收光軸的交點到接收透鏡前主面的距離；b為接收透鏡后主面到成像面中心點的距離； θ 1 \theta_1 θ1?為激光束光軸與接收透鏡光軸之間的夾角； θ 2 \theta_2 θ2?為探測器與接收透鏡光軸之間的夾角。
3.2，斜射式
激光器發出的光和被測面的法線方向成一定角度入射到被測面上，同樣用接收透鏡接收光點在被測面的散射光或反射光。
若光點的像在探測器敏感面上移動x`,利用相似三角形的比例關系，則物體表面沿法線方向的移動距離為：
x = a x ‘ s i n θ 3 c o s θ 1 b s i n ( θ 1 + θ 2 ) ? x ‘ s i n ( θ 1 + θ 2 + θ 3 ) x=\frac{ax^`sin\theta_3cos\theta_1}{bsin(\theta_1+\theta_2)-x^`sin(\theta_1+\theta_2+\theta_3)} x=bsin(θ1?+θ2?)?x‘sin(θ1?+θ2?+θ3?)ax‘sinθ3?cosθ1??
式中， θ 1 \theta_1 θ1?為激光束光軸與被測面的法線夾角； θ 2 \theta_2 θ2?為成像透鏡與被測面的法線夾角； θ 3 \theta_3 θ3?為探測器光軸與成像透鏡光軸之間的夾角。
3.3，兩種三角位移傳感器特性的比較
基于三角測量法的傳感器稱為激光三角位移傳感器，具體可以分為直射式激光三角位移傳感器和斜射式激光三角位移傳感器。這兩種傳感器都可以對被測面進行高精度、高速度的非接觸式測量，但比較起來有以下幾點區別：
- 斜射式可以接收來自被測物體的正反射光，比較適合測量表面接近鏡面的物體。直射式由于其接收散射光的特點，適合于測量散射性能好的表面。
- 直射式光斑較小，光強集中，不會因被測面不垂直而擴大光斑，而且一般體積比較小。斜射式傳感器分辨率高于直射式，但他的測量范圍較小，體積較大。
4，什么是光片？
光片技術的基本思想是將一條細的發光直線投影到要重建的物體的表面上，然后用相機對投影線進行成像，光片技術也稱線結構光。
實物圖像
?激光線的投影構成了一個稱為光平面或光片的平面。相機的光軸與光平面形成一個角度 α \alpha α,稱為三角測量角。激光線與相機視圖之間的交點取決于被測物體的高度。因此，如果激光線投射到的物體的高度不同，則該線不會稱為直線，而是表示物體的輪廓。
5，主要的算子。
1，create_sheet_of_light_model
用于光片技術（Sheet-of-Light，亦稱之為線性結構光）3D測量的關鍵算子，主要用于通過激光三角測量技術重建物體表面輪廓。該算子通過分析激光線在物體表面的變形來獲取高度信息，適用于工業檢測、逆向工程等領域。
?核心參數?：
| 參數 | 類型 | 說明 | 典型值 |
|---|---|---|---|
| ProfileRegion | 輸入 | 包含處理輪廓的圖像ROI區域 | 最小外接矩形 |
| GenParamName | 輸入 | 可調整的通用參數名稱 | 'min_gray’等13種選項 |
| GenParamValue | 輸入 | 對應參數的值 | 默認50 |
| SheetOfLightModelID | 輸出 | 光片模型句柄 | 用于后續操作 |
?GenParamName可選值?：
-
‘calibration’：校準相關參數
-
‘method’：輪廓提取方法
-
‘min_gray’：定義輪廓提取的最小灰度閾值，低于此值的像素將被忽略(默認50)。
-
‘score_type’：評分類型
-
‘scale_x/y/z’：各軸向縮放因子
-
‘num_profiles’：指定處理的輪廓數量，影響多幀平均和運動補償效果。靜態測量：設置為1（單幀處理），運動物體：根據運動速度設置（如傳送帶場景常用50-500）
標定要求與area_scan_polynomial模型
線結構光系統標定必須使用area_scan_polynomial相機模型，原因如下：
- ?高精度需求?：多項式模型使用K1-K3三個參數描述徑向畸變，P1-P2兩個參數描述切向畸變，比division單參數模型精度更高
- ?畸變校正?：激光三角測量對鏡頭畸變敏感，polynomial模型能更好校正廣角鏡頭的桶形/枕形畸變
- ?系統架構?：標定過程需要相機內外參和激光平面參數，多項式模型提供更完整的參數空間
2，measure_profile_sheet_of_light
光片測量技術（Sheet-of-Light）中的關鍵操作，主要用于激光條紋剖面數據的精確提取與分析。其核心功能與特性如下：
一、功能定位
-
?核心作用?
- 從光片模型生成的圖像中提取激光條紋的亞像素級剖面數據。
- 輸出剖面點的坐標和灰度值，用于后續三維重建或缺陷檢測。
-
?技術關聯?
- 需配合
create_sheet_of_light_model創建的光片模型句柄使用。
- 需配合
二、參數解析
| 參數類型 | 關鍵參數 | 作用說明 |
|---|---|---|
| ?輸入參數? | SheetOfLightHandle | 已創建的光片模型句柄2 |
ProfileImage | 包含激光條紋的輸入圖像 | |
| ?輸出參數? | Profile | 提取的剖面點坐標序列 |
GrayValues | 對應點的灰度值數組 |
三、典型應用場景
-
?工業三維測量?
- 金屬零件表面輪廓重建（如連接桿的深度測量）
- 注塑模具孔洞檢測（通過剖面灰度突變分析）
-
?流程示例?
halconCopy Code* 創建光片模型 create_sheet_of_light_model(ProfileRegion, ['min_gray'], [80], SheetOfLightHandle)* 處理單幀圖像 read_image(ProfileImage, 'laser_profile_01.png') measure_profile_sheet_of_light(ProfileImage, SheetOfLightHandle, Profile, GrayValues)
四、性能優化建議
- ?參數調優?
- 通過
set_sheet_of_light_param調整'profile_width'（推薦3-10像素）提升條紋定位精度 - 設置
'min_gray'過濾環境光干擾
- 通過
- ?硬件協同?
- 使用高動態范圍相機避免激光過曝
- 標定階段需保證激光平面與相機視角夾角＞30°
該算子是實現激光三角測量三維重建的核心環節，其精度直接影響最終測量結果。
3，get_sheet_of_light_result
是光片測量技術（Sheet-of-Light）的核心數據提取接口，用于獲取線結構光三維測量的標定或未標定結果。其功能特性與使用方法如下：
一、核心功能
-
?數據輸出類型?
- 支持5種結果類型通過
參數指定：'disparity'：未標定的視差圖像（亞像素行坐標）'score'：條紋匹配質量評分（依賴'score_type'參數）'x'/'y'/'z'：標定后的三維坐標點云
- 支持5種結果類型通過
二、參數詳解
| 參數類別 | 參數名 | 作用說明 |
|---|---|---|
| ?輸入參數? | SheetOfLightModelID | 已初始化的光片模型句柄 |
ResultName | 指定輸出數據類型（默認'disparity'） | |
| ?輸出參數? | ResultValue | 根據ResultName返回對應數據 |
三、典型應用場景
- ?工業三維檢測?
- 使用
'z'結果進行零件高度公差驗證 - 通過
'score'分析激光條紋質量，識別表面缺陷
- 使用
- ?代碼示例?
halconCopy Code* 獲取標定后的三維坐標
get_sheet_of_light_result(ResultZ, SheetOfLightHandle, 'z')
* 可視化Z軸深度圖
dev_display(ResultZ)
6，三角標定測量代碼詳解。
6.1， 相機標定參數預設置。
dev_update_off ()
dev_close_window ()
read_image (ProfileImage, 'sheet_of_light/connection_rod_001.png')
get_image_size (ProfileImage, Width, Height)
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
dev_set_draw ('margin')
dev_set_line_width (3)
dev_set_color ('lime green')
dev_set_lut ('default')
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
*
* -------
* Part 1: 執行相機的標定
* -------
*
* 1.1,默認的相機內參
StartParameters := [0.0125,0.0,0.0,0.0,0.0,0.0,0.000006,0.000006,376.0,120.0,752,240]* 1.2,標定板描述文件
* caltab_30mm.descr'：Distance between mark centers [meter]: 0.00375
CalTabDescription := 'caltab_30mm.descr'* 1.3,標定板的厚度（單位：m）
CalTabThickness := .00063* 1.4,標定板圖像數量
NumCalibImages := 20
*
* Initialize a calibration data model
create_calib_data ('calibration_object', 1, 1, CalibDataID)* 1.4，設置標定參數
* 'area_scan_polynomial':復雜畸變、需要亞像素級精度。至少需要15張標定圖像。
* 'division':至少需要5張標定圖像。
* 線結構光系統標定必須使用`area_scan_polynomial`相機模型
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_polynomial', StartParameters)
set_calib_data_calib_object (CalibDataID, 0, CalTabDescription)
釋疑點:
area_scan_polynomial與area_scan_division是兩種不同的面陣相機畸變校正模型，其核心差異可通過以下對比體現：
. 參數結構與畸變建模能力
| 屬性 | division模型 | polynomial模型 |
|---|---|---|
| ?核心參數? | 單參數Kappa描述徑向畸變 | 多參數組合（K1-K3徑向+P1-P2切向畸變） |
| ?畸變類型? | 僅徑向畸變（桶形/枕形） | 徑向畸變 + 切向畸變 + 透視投影畸變6 |
| ?光學中心位置? | 固定在圖像中心 | 可自定義中心位置 |
- 適用場景對比
- ?division模型優勢?
- 計算速度快（直接數學反演即可完成畸變校正）
- 標定穩定性高（尤其適用于少量標定圖像或視野覆蓋不全的情況）
- 典型應用：標準工業鏡頭（畸變率＜5%）的場景
- ?polynomial模型優勢?
- 支持高階非線性畸變校正（如廣角鏡頭/魚眼鏡頭的復雜畸變）
- 可實現亞像素級精度（適用于超高精度測量需求）
- 典型應用：醫療內窺鏡、大視場角工業檢測等場景
- Halcon實現差異
-
?初始化參數示例?
halconCopy Code* division模型參數示例 CameraParamDivision := ['area_scan_division', 0.012, 0, 0.00000375, 0.00000375, 640, 480, 1280, 960]* polynomial模型參數示例 CameraParamPoly := ['area_scan_polynomial', 0.016, 0.000005, 0.000005, 320, 240, 640, 480, 0.1, -0.05, 0.001]- polynomial模型參數包含畸變多項式系數（如K1-K3、P1-P2）且支持自定義光學中心坐標
-
?標定效率?
指標 division模型 polynomial模型 標定數據需求 ≥5張標定圖像8 ≥15張標定圖像 計算耗時 約1-2秒（典型值） 約5-15秒（迭代計算）
- 選型建議
| 場景特征 | 推薦模型 |
|---|---|
| 簡單畸變、實時性要求高 | division模型 |
| 復雜畸變、需要亞像素級精度 | polynomial模型 |
| 廣角鏡頭（視場角＞60°） | polynomial模型 |
實際項目中建議先用division模型快速驗證系統可行性，若精度不達標則切換至polynomial模型進行精細標定。
6.2，標定相機。
* 1.5，標定圖像（包含各個水平角度，傾斜角度，不同位置但是需要全部在視野內的圖像）
* Collect mark positions and estimated poses for all
* calibration images
for Index := 1 to NumCalibImages by 1read_image (Image, 'sheet_of_light/connection_rod_calib_' + Index$'.2')dev_display (Image)* 后續標定板處于不同位置所對應的位姿與該索引對應，該索引不必從0開始find_calib_object (Image, CalibDataID, 0, 0, Index, [], [])get_calib_data_observ_points (CalibDataID, 0, 0, Index, Row, Column, _Index, Pose)get_calib_data_observ_contours (Contours, CalibDataID, 'caltab', 0, 0, Index)dev_set_color ('green')dev_display (Contours)gen_cross_contour_xld (Cross, Row, Column, 6, 0.785398)dev_set_color ('yellow')dev_display (Cross)
endfor
*
* 1.6，執行標定
calibrate_cameras (CalibDataID, Errors)
disp_message (WindowHandle, 'The camera calibration has been performed successfully', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
注意點：
? 標定板的位置可以傾斜,旋轉，但必須在相機視野內。
6.3，計算LightPanelPose（光平面的位姿）。
* 2.1，獲取世界坐標系在相機坐標系的位姿Index := 19get_calib_data (CalibDataID, 'calib_obj_pose', [0,Index], 'pose', CalTabPose)* 相機的位姿即世界坐標系在相機坐標系的位姿set_origin_pose (CalTabPose, 0.0, 0.0, CalTabThickness, CameraPose)read_image (CalTabImage1, 'sheet_of_light/connection_rod_calib_' + Index$'.2')dev_display (CalTabImage1)get_calib_data (CalibDataID, 'camera', 0, 'params', CameraParameters)disp_3d_coord_system (WindowHandle, CameraParameters, CameraPose, .01)disp_message (WindowHandle, 'World coordinate system', 'window', 12, 12, 'black', 'true')disp_continue_message (WindowHandle, 'black', 'true')stop ()
* 2.2，獲取臨時坐標系在相機坐標系的位置（此時的標定板相對于Index：=19的標定板旋轉了90度）
Index := 20
get_calib_data (CalibDataID, 'calib_obj_pose', [0,Index], 'pose', CalTabPose)
set_origin_pose (CalTabPose, 0.0, 0.0, CalTabThickness, TmpCameraPose)
read_image (CalTabImage2, 'sheet_of_light/connection_rod_calib_' + Index$'.2')
dev_display (CalTabImage2)
disp_3d_coord_system (WindowHandle, CameraParameters, TmpCameraPose, .01)
disp_message (WindowHandle, 'Temporary coordinate system', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
read_image (ProfileImage1, 'sheet_of_light/connection_rod_lightline_019.png')* 2.3，計算索引為19被測物體（位于世界坐標系）激光輪廓線點在z=0的世界坐標系中的坐標
compute_3d_coordinates_of_light_line (ProfileImage1, MinThreshold, CameraParameters, [], CameraPose, X19, Y19, Z19)
if (|X19| == 0 or |Y19| == 0 or |Z19| == 0)dev_display (ProfileImage1)disp_message (WindowHandle, 'The profile MUST be oriented horizontally\nfor successfull processing!\nThe program will exit.', 'window', 12, 12, 'black', 'true')return ()
endif
* ProfileImage1
* 2.4，計算索引為20位于被測物體（位于臨時坐標系）激光輪廓線點在z=0的世界坐標系中的坐標
* Compute the 3D coordinates of the light line points
* in the plane z=0 of the TCS
read_image (ProfileImage2, 'sheet_of_light/connection_rod_lightline_020.png')
compute_3d_coordinates_of_light_line (ProfileImage2, MinThreshold, CameraParameters, TmpCameraPose, CameraPose, X20, Y20, Z20)
if (|X20| == 0 or |Y20| == 0 or |Z20| == 0)disp_message (WindowHandle, '為了成功處理，輪廓需要是大致水平!\n程序將退出.', 'window', 12, 12, 'black', 'true')return ()
endif
ProfileImage2
本地算子：compute_3d_coordinates_of_light_line
* coordinates to the plane z=0 of the local coordinate system.
* 計算線結構光打在被測物體上的輪廓線點的世界坐標*警告：如果配置文件不是大致水平方向X， Y和Z返回空元組。
*
* Initialize the output controls
X := []
Y := []
Z := []
*
* Check LocalCameraPose in order to determine if a transform
* has to be applied after the projection from the camera to
* the z=0 plane
if (LocalCameraPose == [])DoTransform := 0LocalCameraPose := ReferenceCameraPose
elseif (LocalCameraPose == ReferenceCameraPose)DoTransform := 0*
elseDoTransform := 1
endif
*
* Compute the pose for the projection of the
* local coordinate system to the plane z=0
pose_to_hom_mat3d (LocalCameraPose, HomMat3D_LocalToCam)
*
* Compute the homography which transform the 3D-coordinates
* of points from the local coordinate system to the reference
* coordinate system
if (DoTransform)* 計算出臨時坐標系在世界坐標系的位姿pose_to_hom_mat3d (ReferenceCameraPose, HomMat3D_ReferenceToCam)hom_mat3d_invert (HomMat3D_ReferenceToCam, HomMat3D_CamToReference)hom_mat3d_compose (HomMat3D_CamToReference, HomMat3D_LocalToCam, HomMat3D_LocalToReference)
endif
*
* Determine the profile region and test if the profile is
* oriented roughly horizontal
threshold (ProfileImage, Region, MinGray, 999999999)
orientation_region (Region, Phi)
if (cos(Phi) > cos(rad(135)) and cos(Phi) < cos(rad(45)))* The detected profile is NOT oriented roughly horizontal,* therefore return empty tuples X, Y and Z.return ()
endif
dilation_circle (Region, RegionDilation, 5.5)
smallest_rectangle1 (RegionDilation, Row1, Column1, Row2, Column2)
gen_rectangle1 (ProfileRegion, Row1, Column1, Row2, Column2)
get_domain (ProfileImage, Domain)
intersection (ProfileRegion, Domain, ProfileRegion)
*
* 獲取光線上各點的坐標
create_sheet_of_light_model (ProfileRegion, ['min_gray','num_profiles'], [MinGray,1], SheetOfLightHandle)
* 從光片模型生成的圖像中提取激光條紋的亞像素剖面數據
* 輸出剖面點的坐標和灰度值，用于后面的三維重建和缺陷檢測
measure_profile_sheet_of_light (ProfileImage, SheetOfLightHandle, [])* 未標定的視差圖像（亞像素行坐標）
get_sheet_of_light_result (Disparity, SheetOfLightHandle, 'disparity')
clear_sheet_of_light_model (SheetOfLightHandle)
*
* Get the 3D-coordinates of the points on the light line
* in the coordinate system defined by ReferenceCameraPose
get_domain (Disparity, DisparityDomain)
get_region_points (DisparityDomain, Rows, Columns)
* Disparities：在這里并非表示常見的灰度值，而是表示激光條紋在亞像素級的行坐標偏移量
get_grayval (Disparity, Rows, Columns, Disparities)
*
* Project those points to the z=0 plane of the WCS
* 將激光條紋的坐標變化到世界坐標系
* Disparities已包含行方向的亞像素的偏移信息（如激光條紋中心的行坐標），因此行參數不需要補償
* 列需要補償的原因是：圖像可能從ROI（感興趣區域）的某一列開始采集，COumn1表示ROI起始列坐標
image_points_to_world_plane (CameraParameters, LocalCameraPose, Disparities, Columns + Column1, 1.0, X, Y)
tuple_gen_const (|Columns|, 0.0, Z)
*
* If necessary, transform the 3D-coordinates from the
* 本地坐標系到參考坐標系
* system
if (DoTransform)* 從臨時坐標系切換到世界坐標系affine_trans_point_3d (HomMat3D_LocalToReference, X, Y, Z, X, Y, Z)* hom_mat3d_to_pose (HomMat3D_LocalToReference, Pose)* pose 的z軸角度為90 度對應了index：=19與index:=20標定板的角度差異
endif
return ()
釋疑點：
? 在Halcon中，get_grayval(Disparity, Rows, Columns, Disparities)獲取的信息取決于輸入圖像Disparity的數據類型和用途，具體可分為以下兩種情況：
1. 常規灰度圖像
當Disparity為普通灰度圖像時：
- ?獲取內容?：指定坐標點（
Rows, Columns）的像素亮度值（灰度值） - ?數值范圍?：通常為0-255（8位圖像）或更大范圍（如16位圖像）
- ?典型應用?：圖像分析、閾值分割等傳統視覺任務
2. 光片測量（Sheet-of-Light）視差圖像
當Disparity由光片測量技術生成時（如通過get_sheet_of_light_result獲取）：
- ?獲取內容?：激光條紋的?亞像素級行坐標偏移量?（非亮度值）
- ?數值特性?：浮點型數據，表示條紋中心相對于參考位置的精確位移
- ?核心用途?：用于三維重建，需配合
image_points_to_world_plane等算子計算世界坐標17
3. 關鍵區別總結
| 特征 | 常規灰度圖像 | 光片測量視差圖像 | 來源 |
|---|---|---|---|
| ?數據本質? | 像素亮度 | 幾何位置偏移量 | |
| ?存儲形式? | 整數（如uint8/uint16） | 浮點數（float） | |
| ?典型操作? | 濾波、直方圖均衡化 | 三維坐標投影 |
4. 使用建議
5. ?確認圖像類型?
通過get_image_type檢查Disparity的圖像類型（如’real’表示浮點型視差數據）
6. ?避免混淆?
光片測量中的“視差”是幾何量，與立體視覺中的視差概念不同
* 2.5，獲取光平面的法向量坐標Nx，Ny,Nz，和重心點坐標
fit_3d_plane_xyz ([X19,X20], [Y19,Y20], [Z19,Z20], Ox, Oy, Oz, Nx, Ny, Nz, MeanResidual)
if (|Nx| == 0 or |Ny| == 0 or |Nz| == 0)disp_message (WindowHandle, '提供的3d點太少，無法擬合光平面,\nor 這些點（幾乎）共線!\n程序將退出.', 'window', 12, 12, 'black', 'true')return ()
endif
if (MeanResidual > 5e-5)disp_message (WindowHandle, '這個光平面不能擬合!\n的平均誤差距離\n高于擬合標準： (' + (MeanResidual * 1000)$'.3' + 'mm). \n請檢查這些點的質量和正確性.\n程序將退出!', 'window', 12, 21, 'black', 'true')return ()endif
本地算子：fit_3d_plane_xyz,獲取擬合光平面所需要的法向量。
*
* WARNING: If the system of equations is under-determined
* (i.e. if it has too few input coordinates in X, Y, Z),
* it cannot be solved and the procedure returns empty tuples
* for X, Y, and Z
*
* Perform some initializations
Ox := []
Oy := []
Oz := []
Nx := []
Ny := []
Nz := []
MeanResidual := []
*
* Test the size of X, Y and Z, and return if necessary
Size := |X|
if (Size < 3 or Size != |Y| or Size != |Z|)return ()
endif
*
* 計算重心點坐標
tuple_mean (X, Ox)
tuple_mean (Y, Oy)
tuple_mean (Z, Oz)
*
*將方程組建立為矩陣M并計算
*其奇異值分解。奇異向量
*M的對應其最小奇異值有
*擬合平面的法向量坐標。
create_matrix (3, |X|, [X - Ox,Y - Oy,Z - Oz], MatrixID_Mt)
* 交換矩陣的行列
transpose_matrix (MatrixID_Mt, MatrixID_M)
* 計算一個矩陣的奇異值分解。
svd_matrix (MatrixID_M, 'reduced', 'right', MatrixID_U, MatrixID_S, MatrixID_V)
get_value_matrix (MatrixID_S, [0,1,2], [0,1,2], SingularvaluesOfM)
tuple_sort_index (SingularvaluesOfM, Indices)
*
* Test if more than one singular value of M is (nearly) equal
* to zero. This indicates that the provided 3d points are
* inappropriate to fit the plane (e.g. they are nearly
* collinear or reduce to a single point).
* 檢驗是否有多個M的奇異值（近似）相等
* 到零。這表明所提供的3d點是
* 不適合擬合成平面（例如它們幾乎是）
* 共線或減少到一個點)。
if (SingularvaluesOfM[Indices[0]] < 1e-9 and SingularvaluesOfM[Indices[1]] < 1e-9)return ()
endif
*
* 得到擬合平面的法向量坐標
get_value_matrix (MatrixID_V, [0,1,2], [Indices[0],Indices[0],Indices[0]], N)
create_matrix (3, 1, N, MatrixID_N)
Nx := N[0]
Ny := N[1]
Nz := N[2]
*
* Compute the mean residual distance between the 3d points
* and the fitted plane, in order to guess the quality of
* the fitted plane:
* 計算三維點之間的平均差異距離
* 和擬合平面，以便猜測質量
* 擬合平面：
mult_matrix (MatrixID_M, MatrixID_N, 'AB', MatrixID_MN)
get_full_matrix (MatrixID_MN, Distances)
Distances := abs(Distances)
* 誤差值
MeanResidual := sum(Distances) / Size
*
* Clear the matrices used in the procedure
clear_matrix ([MatrixID_MN,MatrixID_N,MatrixID_V,MatrixID_S,MatrixID_U,MatrixID_M,MatrixID_Mt])
return ()
* 2.6，根據重心點和法向量計算出光平面在世界坐標系的位姿
* 使得z=0定義的平面get_light_plane_pose (Ox, Oy, Oz, Nx, Ny, Nz, LightPlanePose)
if (|LightPlanePose| != 7)disp_message (WindowHandle, 'The pose of the light plane could not be\ndetermined. Please verify that the vector\npassed at input of the procedure\nget_light_plane_pose() is not null.\nThe program will exit!', 'window', -1, -2, 'black', 'true')return ()
endif* 2.7，顯示光平面位姿信息
String := ['LightPlanePose: ',' Tx = ' + LightPlanePose[0]$'.3' + ' m',' Ty = ' + LightPlanePose[1]$'.3' + ' m',' Tz = ' + LightPlanePose[2]$'.3' + ' m',' alpha = ' + LightPlanePose[3]$'.4' + '°',' beta = ' + LightPlanePose[4]$'.4' + '°',' gamma = ' + LightPlanePose[5]$'.4' + '°',' type = ' + LightPlanePose[6]]
disp_message (WindowHandle, String, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
dev_clear_window ()
光平面位姿：
6.4，計算MovementPose。
* 3.1，計算標定板沿y axis 位移前后的位姿
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 axis 方向)
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_polynomial', CameraParameters)
* Compute the pose of the calibration table in each image
find_calib_object (CaltabImagePos1, CalibDataID, 0, 0, NumCalibImages + 1, [], [])* 從標定數據模型中提取未優化位姿
get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages + 1, Row1, Column1, Index1, CameraPosePos1)
dev_display (CaltabImagePos1)
disp_3d_coord_system (WindowHandle,CameraParameters, CameraPosePos1, 0.01)
* 標定板移動后（位置2相對位置1在y axis 上進行移動的些許）
find_calib_object (CaltabImagePos20, CalibDataID, 0, 0, NumCalibImages + 2, [], [])
get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages + 2, Row1, Column1, Index1, CameraPosePos20)
dev_display (CaltabImagePos20)
disp_3d_coord_system (WindowHandle,CameraParameters, CameraPosePos20, 0.01)
* Clear the model
clear_calib_data (CalibDataID)
位移前：
位移后：
* 3.2，將在相機坐標系中的位姿轉換到世界坐標系
set_origin_pose (CameraPosePos1, 0.0, 0.0, CalTabThickness, CameraPosePos1)
set_origin_pose (CameraPosePos20, 0.0, 0.0, CalTabThickness, CameraPosePos20)
pose_to_hom_mat3d (CameraPosePos1, HomMat3DPos1ToCamera)
pose_to_hom_mat3d (CameraPosePos20, HomMat3DPos20ToCamera)
pose_to_hom_mat3d (CameraPose, HomMat3DWorldToCamera)
hom_mat3d_invert (HomMat3DWorldToCamera, HomMat3DCameraToWorld)
hom_mat3d_compose (HomMat3DCameraToWorld, HomMat3DPos1ToCamera, HomMat3DPos1ToWorld)
hom_mat3d_compose (HomMat3DCameraToWorld, HomMat3DPos20ToCamera, HomMat3DPos20ToWorld)* 3.3，計算觀察點（0，0，0）在位移前后的偏移affine_trans_point_3d (HomMat3DPos1ToWorld, 0,0, 0, StartX, StartY, StartZ)affine_trans_point_3d (HomMat3DPos20ToWorld, 0, 0, 0, EndX, EndY, EndZ)
* 標定板移動的位姿
MovementPoseNSteps := [EndX - StartX,EndY - StartY,EndZ - StartZ,0,0,0,0]
* 計算每一步移動的位姿
MovementPose := MovementPoseNSteps / StepNumberString := ['標定板每一步移動的位姿\n（位置1到位置2共移動了19步）','MovementPose: ',' Tx = ' + MovementPose[0]$'.3' + ' m',' Ty = ' + MovementPose[1]$'.3' + ' m',' Tz = ' + MovementPose[2]$'.3' + ' m',' alpha = ' + MovementPose[3] + '°',' beta = ' + MovementPose[4] + '°',' gamma = ' + MovementPose[5] + '°',' type = ' + MovementPose[6]]
disp_message (WindowHandle, String, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
dev_clear_window ()
MovementPose信息：
釋疑點：
MovementPose 的物理內涵與觸發機制
- 參數定義與觸發邏輯
- ?核心意義?
MovementPose描述的是 ?物體在運動過程中相對于初始位姿的剛體變換?（包含旋轉矩陣 RR 和平移向量 TT），而非嚴格按時間間隔更新。其更新觸發取決于外部運動系統的反饋機制（如編碼器脈沖、機械臂位姿信號等）。 - ?時間關聯性?
- 若運動系統以固定頻率（如每秒）反饋位姿數據，則
MovementPose可能每秒更新一次 - 但在 ?非勻速運動場景? 中，位姿更新頻率會動態調整，與時間間隔無直接綁定
- 若運動系統以固定頻率（如每秒）反饋位姿數據，則
- 典型應用模式
-
?同步觸發模式?
通過外部觸發信號（如機械臂到達指定位置時發送脈沖）更新MovementPose，確保掃描動作與位姿數據嚴格同步。 -
?異步事件驅動?
在自由運動場景（如傳送帶連續運行）中，根據運動系統實時推送的位姿流數據更新MovementPose，此時更新頻率由硬件通信速率決定。
- 技術對比
| 參數更新機制 | 觸發條件 | 適用場景 |
|---|---|---|
| ?時間驅動? | 固定采樣周期（如1秒） | 勻速運動且對實時性要求低的場景 |
| ?事件驅動? | 運動狀態變化（如位移超閾值） | 高精度動態掃描（工業檢測、機器人抓取） |
?實際工程中?，通常采用事件驅動模式以保證點云數據與物體實際運動嚴格匹配。
6.5，激光三角應用。
* Part 4: 三角測量應用：生成3D點云。* Read an already acquired disparity map from file
read_image (Disparity, 'sheet_of_light/connection_rod_disparity.tif')
* Create a model and set the required parameters
gen_rectangle1 (ProfileRegion, 120, 75, 195, 710)
* min_gray:輪廓提取的最小灰度值；num_profiles:提取的輪廓數量;ambiguity_solving:解決深度歧義的方法，值"first":優先選擇第一個有效解
create_sheet_of_light_model (ProfileRegion, ['min_gray','num_profiles','ambiguity_solving'], [70,290,'first'], SheetOfLightModelID)
set_sheet_of_light_param (SheetOfLightModelID, 'calibration', 'xyz')
set_sheet_of_light_param (SheetOfLightModelID, 'scale', 'mm')
set_sheet_of_light_param (SheetOfLightModelID, 'camera_parameter', CameraParameters)
set_sheet_of_light_param (SheetOfLightModelID, 'camera_pose', CameraPose)
* 光平面在世界坐標系的位姿
set_sheet_of_light_param (SheetOfLightModelID, 'lightplane_pose', LightPlanePose)
* 實時更新位姿，確保點云數據對齊世界坐標系
* 即每移動一步的位姿
* 物體在運動過程中相對于初始位姿的剛體變換?set_sheet_of_light_param (SheetOfLightModelID, 'movement_pose', MovementPose)
*
* Apply the calibration transforms and
* get the resulting calibrated coordinates
apply_sheet_of_light_calibration (Disparity, SheetOfLightModelID)
get_sheet_of_light_result (X, SheetOfLightModelID, 'x')
get_sheet_of_light_result (Y, SheetOfLightModelID, 'y')
get_sheet_of_light_result (Z, SheetOfLightModelID, 'z')
clear_sheet_of_light_model (SheetOfLightModelID)
*
* 4.1，顯示生成的 Z-coordinates圖像
*dev_close_window ()
get_image_size (Disparity, Width, Height)
dev_open_window (Height + 10, 0, Width * .5, Height * .5, 'black', WindowHandle3)
set_display_font (WindowHandle3, 14, 'mono', 'true', 'false')
dev_set_lut ('temperature')
dev_display (Z)
disp_message (WindowHandle3, 'Calibrated Z-coordinates', 'window', 12, 12, 'black', 'true')
*
* 4.2，顯示生成的 Y-coordinates圖像
dev_open_window ((Height + 10), Width * .5, Width * .5, Height * .5, 'black', WindowHandle2)
set_display_font (WindowHandle2, 14, 'mono', 'true', 'false')
dev_display (Y)
disp_message (WindowHandle2, 'Calibrated Y-coordinates', 'window', 12, 12, 'black', 'true')
*
* 4.3，顯示生成的 Z-coordinates圖像
dev_open_window (Height*1.5, 0, Width * .5, Height * .5, 'black', WindowHandle1)
dev_display (X)
set_display_font (WindowHandle1, 14, 'mono', 'true', 'false')
disp_message (WindowHandle1, 'Calibrated X-coordinates', 'window', 12, 12, 'black', 'true')
* 合成 3D 點云模型,點云數據默認以世界坐標系為基礎進行渲染
xyz_to_object_model_3d (X, Y, Z, ObjectModel3D)*-----------
* part5： 顯示光平面與3D對象模型
* 光平面,這里的光平面位姿是指在世界坐標系中的位姿
gen_plane_object_model_3d (LightPlanePose, [-80,-80,80,80], [80,-80,-80,80], ObjectModel3D1)
dev_set_color ('red')
visualize_object_model_3d (WindowHandle,[ObjectModel3D,ObjectModel3D1],\[], [], ['lut','intensity','color_1','alpha_1','disp_pose'], ['color1','coord_z','red',0.6,'true'], [], [], [], PoseOut)
釋疑點：
visualize_object_model_3d 算子的默認顯示坐標系：
- ?核心坐標系關系?
- 點云數據默認以?**世界坐標系（場景坐標系 SCS）**?為基礎進行渲染。此時虛擬相機的觀察視角會通過
CamParam和PoseIn參數調整，但點云本身的坐標仍與世界坐標系對齊。
- 點云數據默認以?**世界坐標系（場景坐標系 SCS）**?為基礎進行渲染。此時虛擬相機的觀察視角會通過
- ?默認姿態與相機參數的影響?
- 當
PoseIn參數為空時，算子會自動計算初始位姿，此時點云的顯示位置仍基于世界坐標系，但會通過虛擬相機的視角參數（如CamParam）進行投影變換。 - 若未顯式設置相機內參（
CamParam為空），算子會根據窗口尺寸生成默認參數，但坐標系基準不變。
- 當
- ?用戶交互對坐標系的修正?
- 通過鼠標操作（旋轉、平移）調整點云姿態后，輸出的
PoseOut參數表示點云在世界坐標系下的新位姿，而非相機坐標系下的相對位置。
- 通過鼠標操作（旋轉、平移）調整點云姿態后，輸出的
總結：該算子默認以?世界坐標系?為基準渲染點云，虛擬相機僅提供觀察視角的變換，不會改變點云本身在世界坐標系中的實際坐標。
示意圖
點云效果及光平面顯示
紅色為Light plane，紅軸為 x axis，綠軸為 y axis，藍軸為 z axis。
7，完整代碼。
* 案例庫：cablibrate_sheet_of_light_calplate.hdev* 目的：
* 計算激光三角測量所需的LightPlanePose（光平面位姿）與MovementPose（每步移動位姿），完成激光三角測量* 描述：
* 通過計算結構光在亮處不同位姿的被測物上形成輪廓線的點坐標（世界坐標系）擬合出LightPlanePose
* 通過在y axis移動n步的標定板位姿變化計算出MovementPose（每步移動位姿）
dev_update_off ()
dev_close_window ()
read_image (ProfileImage, 'sheet_of_light/connection_rod_001.png')
get_image_size (ProfileImage, Width, Height)
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
dev_set_draw ('margin')
dev_set_line_width (3)
dev_set_color ('lime green')
dev_set_lut ('default')
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
*
* -------
* Part 1: 執行相機的標定
* -------
*
* 1.1,默認的相機內參
StartParameters := [0.0125,0.0,0.0,0.0,0.0,0.0,0.000006,0.000006,376.0,120.0,752,240]* 1.2,標定板描述文件
* caltab_30mm.descr'：Distance between mark centers [meter]: 0.00375
CalTabDescription := 'caltab_30mm.descr'* 1.3,標定板的厚度（單位：m）
CalTabThickness := .00063* 1.4,標定板圖像數量
NumCalibImages := 20
*
* Initialize a calibration data model
create_calib_data ('calibration_object', 1, 1, CalibDataID)* 1.4，設置標定參數
* 'area_scan_polynomial':復雜畸變、需要亞像素級精度。至少需要15張標定圖像。
* 'division':至少需要5張標定圖像。
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_polynomial', StartParameters)
set_calib_data_calib_object (CalibDataID, 0, CalTabDescription)* 1.5，標定圖像（包含各個水平角度，傾斜角度，不同位置但是需要全部在視野內的圖像）
* Collect mark positions and estimated poses for all
* calibration images
for Index := 1 to NumCalibImages by 1read_image (Image, 'sheet_of_light/connection_rod_calib_' + Index$'.2')dev_display (Image)* 后續標定板處于不同位置所對應的位姿與該索引對應，該索引不必從0開始find_calib_object (Image, CalibDataID, 0, 0, Index, [], [])get_calib_data_observ_points (CalibDataID, 0, 0, Index, Row, Column, _Index, Pose)get_calib_data_observ_contours (Contours, CalibDataID, 'caltab', 0, 0, Index)dev_set_color ('green')dev_display (Contours)gen_cross_contour_xld (Cross, Row, Column, 6, 0.785398)dev_set_color ('yellow')dev_display (Cross)
endfor
*
* 1.6，執行標定
calibrate_cameras (CalibDataID, Errors)
disp_message (WindowHandle, 'The camera calibration has been performed successfully', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*
* -------
* Part 2: 計算光平面相對于世界坐標系的位姿
* -------
dev_set_colored (3)
MinThreshold := 80* 2.1，獲取世界坐標系在相機坐標系的位姿
Index := 19
get_calib_data (CalibDataID, 'calib_obj_pose', [0,Index], 'pose', CalTabPose)
set_origin_pose (CalTabPose, 0.0, 0.0, CalTabThickness, CameraPose)
read_image (CalTabImage1, 'sheet_of_light/connection_rod_calib_' + Index$'.2')
dev_display (CalTabImage1)
get_calib_data (CalibDataID, 'camera', 0, 'params', CameraParameters)
disp_3d_coord_system (WindowHandle, CameraParameters, CameraPose, .01)
disp_message (WindowHandle, 'World coordinate system', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*
* Definition of a temporary coordinate system (TCS):
* The TCS is also defined implicitly by choosing another
* calibration image. Here again we shift the origin of
* the coordinate system in order to take the thickness
* of the calibration table into account.
* 2.2，獲取臨時坐標系在相機坐標系的位置（此時的標定板相對于Index：=19的標定板旋轉了90度）
Index := 20
get_calib_data (CalibDataID, 'calib_obj_pose', [0,Index], 'pose', CalTabPose)
set_origin_pose (CalTabPose, 0.0, 0.0, CalTabThickness, TmpCameraPose)
read_image (CalTabImage2, 'sheet_of_light/connection_rod_calib_' + Index$'.2')
dev_display (CalTabImage2)
disp_3d_coord_system (WindowHandle, CameraParameters, TmpCameraPose, .01)
disp_message (WindowHandle, 'Temporary coordinate system', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()*
* dev_clear_window ()
read_image (ProfileImage1, 'sheet_of_light/connection_rod_lightline_019.png')* 2.3，計算索引為19被測物體（位于世界坐標系）激光輪廓線點在z=0的世界坐標系中的坐標
compute_3d_coordinates_of_light_line (ProfileImage1, MinThreshold, CameraParameters, [], CameraPose, X19, Y19, Z19)
if (|X19| == 0 or |Y19| == 0 or |Z19| == 0)dev_display (ProfileImage1)disp_message (WindowHandle, 'The profile MUST be oriented horizontally\nfor successfull processing!\nThe program will exit.', 'window', 12, 12, 'black', 'true')return ()
endif
*
* 2.4，計算索引為20位于被測物體（位于臨時坐標系）激光輪廓線點在z=0的世界坐標系中的坐標
* Compute the 3D coordinates of the light line points
* in the plane z=0 of the TCS
read_image (ProfileImage2, 'sheet_of_light/connection_rod_lightline_020.png')
compute_3d_coordinates_of_light_line (ProfileImage2, MinThreshold, CameraParameters, TmpCameraPose, CameraPose, X20, Y20, Z20)
if (|X20| == 0 or |Y20| == 0 or |Z20| == 0)disp_message (WindowHandle, '為了成功處理，輪廓需要是大致水平!\n程序將退出.', 'window', 12, 12, 'black', 'true')return ()
endif
* * 2.5，獲取光平面的法向量坐標Nx，Ny,Nz，和重心點坐標
fit_3d_plane_xyz ([X19,X20], [Y19,Y20], [Z19,Z20], Ox, Oy, Oz, Nx, Ny, Nz, MeanResidual)
if (|Nx| == 0 or |Ny| == 0 or |Nz| == 0)disp_message (WindowHandle, '提供的3d點太少，無法擬合光平面,\nor 這些點（幾乎）共線!\n程序將退出.', 'window', 12, 12, 'black', 'true')return ()
endif
if (MeanResidual > 5e-5)disp_message (WindowHandle, '這個光平面不能擬合!\n的平均誤差距離\n高于擬合標準： (' + (MeanResidual * 1000)$'.3' + 'mm). \n請檢查這些點的質量和正確性.\n程序將退出!', 'window', 12, 21, 'black', 'true')return ()endif
*
* 2.6，根據重心點和法向量計算出光平面在世界坐標系的位姿
* 使得z=0定義的平面get_light_plane_pose (Ox, Oy, Oz, Nx, Ny, Nz, LightPlanePose)
if (|LightPlanePose| != 7)disp_message (WindowHandle, 'The pose of the light plane could not be\ndetermined. Please verify that the vector\npassed at input of the procedure\nget_light_plane_pose() is not null.\nThe program will exit!', 'window', -1, -2, 'black', 'true')return ()
endif* 2.7，顯示光平面位姿信息
String := ['LightPlanePose: ',' Tx = ' + LightPlanePose[0]$'.3' + ' m',' Ty = ' + LightPlanePose[1]$'.3' + ' m',' Tz = ' + LightPlanePose[2]$'.3' + ' m',' alpha = ' + LightPlanePose[3]$'.4' + '°',' beta = ' + LightPlanePose[4]$'.4' + '°',' gamma = ' + LightPlanePose[5]$'.4' + '°',' type = ' + LightPlanePose[6]]
disp_message (WindowHandle, String, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* dev_clear_window ()
*
* -------
* Part 3:獲取MovementPose（每移動一步的姿態）* 3.1，計算標定板沿y axis 位移前后的位姿
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 axis 方向)
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_polynomial', CameraParameters)
* Compute the pose of the calibration table in each image
find_calib_object (CaltabImagePos1, CalibDataID, 0, 0, NumCalibImages + 1, [], [])* 從標定數據模型中提取未優化位姿
get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages + 1, Row1, Column1, Index1, CameraPosePos1)
dev_display (CaltabImagePos1)
disp_3d_coord_system (WindowHandle,CameraParameters, CameraPosePos1, 0.01)
* 標定板移動后（位置2相對位置1在y axis 上進行移動的些許）
find_calib_object (CaltabImagePos20, CalibDataID, 0, 0, NumCalibImages + 2, [], [])
get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages + 2, Row1, Column1, Index1, CameraPosePos20)
dev_display (CaltabImagePos20)
disp_3d_coord_system (WindowHandle,CameraParameters, CameraPosePos20, 0.01)
* Clear the model
clear_calib_data (CalibDataID)
*
* 3.2，將在相機坐標系中的位姿轉換到世界坐標系
set_origin_pose (CameraPosePos1, 0.0, 0.0, CalTabThickness, CameraPosePos1)
set_origin_pose (CameraPosePos20, 0.0, 0.0, CalTabThickness, CameraPosePos20)
pose_to_hom_mat3d (CameraPosePos1, HomMat3DPos1ToCamera)
pose_to_hom_mat3d (CameraPosePos20, HomMat3DPos20ToCamera)
pose_to_hom_mat3d (CameraPose, HomMat3DWorldToCamera)
hom_mat3d_invert (HomMat3DWorldToCamera, HomMat3DCameraToWorld)
hom_mat3d_compose (HomMat3DCameraToWorld, HomMat3DPos1ToCamera, HomMat3DPos1ToWorld)
hom_mat3d_compose (HomMat3DCameraToWorld, HomMat3DPos20ToCamera, HomMat3DPos20ToWorld)* 3.3，計算觀察點（0，0，0）在位移前后的偏移affine_trans_point_3d (HomMat3DPos1ToWorld, 0,0, 0, StartX, StartY, StartZ)affine_trans_point_3d (HomMat3DPos20ToWorld, 0, 0, 0, EndX, EndY, EndZ)
* 標定板移動的位姿
MovementPoseNSteps := [EndX - StartX,EndY - StartY,EndZ - StartZ,0,0,0,0]
* 計算每一步移動的位姿
MovementPose := MovementPoseNSteps / StepNumberString := ['標定板每一步移動的位姿\n（位置1到位置2共移動了19步）','MovementPose: ',' Tx = ' + MovementPose[0]$'.3' + ' m',' Ty = ' + MovementPose[1]$'.3' + ' m',' Tz = ' + MovementPose[2]$'.3' + ' m',' alpha = ' + MovementPose[3] + '°',' beta = ' + MovementPose[4] + '°',' gamma = ' + MovementPose[5] + '°',' type = ' + MovementPose[6]]
disp_message (WindowHandle, String, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
dev_clear_window ()
*
* -------
* Part 4: 三角測量應用：生成3D點云。
* -------
*
* Read an already acquired disparity map from file
read_image (Disparity, 'sheet_of_light/connection_rod_disparity.tif')
* Create a model and set the required parameters
gen_rectangle1 (ProfileRegion, 120, 75, 195, 710)
* min_gray:輪廓提取的最小灰度值；num_profiles:提取的輪廓數量;ambiguity_solving:解決深度歧義的方法，值"first":優先選擇第一個有效解
create_sheet_of_light_model (ProfileRegion, ['min_gray','num_profiles','ambiguity_solving'], [70,290,'first'], SheetOfLightModelID)
set_sheet_of_light_param (SheetOfLightModelID, 'calibration', 'xyz')
set_sheet_of_light_param (SheetOfLightModelID, 'scale', 'mm')
set_sheet_of_light_param (SheetOfLightModelID, 'camera_parameter', CameraParameters)
set_sheet_of_light_param (SheetOfLightModelID, 'camera_pose', CameraPose)
* 光平面在世界坐標系的位姿
set_sheet_of_light_param (SheetOfLightModelID, 'lightplane_pose', LightPlanePose)
* 實時更新位姿，確保點云數據對齊世界坐標系
* 即每移動一步的位姿
* 物體在運動過程中相對于初始位姿的剛體變換?set_sheet_of_light_param (SheetOfLightModelID, 'movement_pose', MovementPose)
*
* Apply the calibration transforms and
* get the resulting calibrated coordinates
apply_sheet_of_light_calibration (Disparity, SheetOfLightModelID)
get_sheet_of_light_result (X, SheetOfLightModelID, 'x')
get_sheet_of_light_result (Y, SheetOfLightModelID, 'y')
get_sheet_of_light_result (Z, SheetOfLightModelID, 'z')
clear_sheet_of_light_model (SheetOfLightModelID)
*
* 4.1，顯示生成的 Z-coordinates圖像
*dev_close_window ()
get_image_size (Disparity, Width, Height)
dev_open_window (Height + 10, 0, Width * .5, Height * .5, 'black', WindowHandle3)
set_display_font (WindowHandle3, 14, 'mono', 'true', 'false')
dev_set_lut ('temperature')
dev_display (Z)
disp_message (WindowHandle3, 'Calibrated Z-coordinates', 'window', 12, 12, 'black', 'true')
*
* 4.2，顯示生成的 Y-coordinates圖像
dev_open_window ((Height + 10), Width * .5, Width * .5, Height * .5, 'black', WindowHandle2)
set_display_font (WindowHandle2, 14, 'mono', 'true', 'false')
dev_display (Y)
disp_message (WindowHandle2, 'Calibrated Y-coordinates', 'window', 12, 12, 'black', 'true')
*
* 4.3，顯示生成的 Z-coordinates圖像
dev_open_window (Height*1.5, 0, Width * .5, Height * .5, 'black', WindowHandle1)
dev_display (X)
set_display_font (WindowHandle1, 14, 'mono', 'true', 'false')
disp_message (WindowHandle1, 'Calibrated X-coordinates', 'window', 12, 12, 'black', 'true')
* 合成 3D 點云模型,點云數據默認以世界坐標系為基礎進行渲染
xyz_to_object_model_3d (X, Y, Z, ObjectModel3D)*-----------
* part5： 顯示光平面與3D對象模型
* 光平面,這里的光平面位姿是指在世界坐標系中的位姿
gen_plane_object_model_3d (LightPlanePose, [-80,-80,80,80], [80,-80,-80,80], ObjectModel3D1)
dev_set_color ('red')
visualize_object_model_3d (WindowHandle,[ObjectModel3D,ObjectModel3D1],\[], [], ['lut','intensity','color_1','alpha_1','disp_pose'], ['color1','coord_z','red',0.6,'true'], [], [], [], PoseOut)
統計數據，以及過濾同時為0的數據)
)
