halcon激光三角测量（二十一）calibrate_sheet_of_light_calplate

目录

• 一、calibrate_sheet_of_light_calplate例程代码
• 二、为什么把视差图作为Rows输入
• 三、坐标系和点变换区分

一、calibrate_sheet_of_light_calplate例程代码

1、第一部分：标定相机。

2、第二部分：标定光平面相对于世界坐标系的位姿。

1）设定位置1为世界坐标系，获得位置1的CameraPose

2）设定位置2为临时坐标系，获得位置2的TmpCameraPose

3）把轮廓坐标投影到参考坐标系

4）拟合平面

3、第三部分：标定两次连续采集轮廓图像，目标对象的移动。

4、第四部分：将校准变换应用于已获得的视差图像。

CamParam、CamPose、LightplanePose、MovementPose怎么得到的

通过例程reconstruct_connection_rod_calib标定得到的。

*第一部分

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')

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)
CalTabDescription := 'caltab_30mm.descr'
* Note that the thickness of the calibration target used for this example is 0.63 mm.
* If you adapt this example program to your application, it is necessary to determine
* the thickness of your specific calibration target and to use this value instead.
CalTabThickness := .00063
NumCalibImages := 20

create_calib_data ('calibration_object', 1, 1, CalibDataID)
set_calib_data_cam_param (CalibDataID, 0, [], StartParameters)
set_calib_data_calib_object (CalibDataID, 0, CalTabDescription)
* 

for Index := 1 to NumCalibImages by 1
    read_image (Image, 'sheet_of_light/connection_rod_calib_' + Index$'.2')
    dev_display (Image)
    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

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 ()
*第二部分
dev_set_colored (3)
MinThreshold := 80

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 ()

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')
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

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, 'The profile MUST be oriented horizontally\nfor successfull processing!\nThe program will exit.', 'window', 12, 12, 'black', 'true')
    return ()
endif

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, 'Too few 3d points have been provided to fit the light plane,\nor the points are (nearly) collinear!\nThe program will exit.', 'window', 12, 12, 'black', 'true')
    return ()
endif
if (MeanResidual > 5e-5)
    disp_message (WindowHandle, 'The light plane could not be fitted accurately!\nThe mean residual distance between the 3d-points and the\nfitted plane is too high (' + (MeanResidual * 1000)$'.3' + 'mm). Please check the\nquality and the correctness of those points.\nThe program will exit!', 'window', 12, 21, 'black', 'true')
    return ()
endif

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', 12, 12, 'black', 'true')
    return ()
endif
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 ()
*第三部分
read_image (CaltabImagePos1, 'sheet_of_light/caltab_at_position_1.png')
read_image (CaltabImagePos20, 'sheet_of_light/caltab_at_position_2.png')
StepNumber := 19

set_calib_data_cam_param (CalibDataID, 0, [], CameraParameters)
find_calib_object (CaltabImagePos1, CalibDataID, 0, 0, NumCalibImages + 1, [], [])
get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages + 1, Row1, Column1, Index1, CameraPosePos1)
find_calib_object (CaltabImagePos20, CalibDataID, 0, 0, NumCalibImages + 2, [], [])
get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages + 2, Row1, Column1, Index1, CameraPosePos20)

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)
affine_trans_point_3d (HomMat3DPos1ToWorld, 0, 0, 0, StartX, StartY, StartZ)
affine_trans_point_3d (HomMat3DPos20ToWorld, 0, 0, 0, EndX, EndY, EndZ)
create_pose (EndX - StartX, EndY - StartY, EndZ - StartZ, 0, 0, 0, 'Rp+T', 'gba', 'point', MovementPoseNSteps)
MovementPose := MovementPoseNSteps / StepNumber
String := ['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 ()
*第四部分
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)
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, '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_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')
* 
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')
* 
* Display the resulting Y-coordinates
dev_open_window ((Height + 10) * .5, 0, 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')
* 
* Display the resulting X-coordinates
dev_open_window (0, 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')
* 

get_sheet_of_light_result_object_model_3d (SheetOfLightModelID, ObjectModel3D)
gen_sheet_of_light_object_model_3d (SheetOfLightModelID, 0.1, 0.05, 0.3, OM3DLightPlane, OM3DMovement, OM3DCamera, OM3DCone)
dev_open_window (0, Width * .5 + 10, Width, Height * 1.5, 'black', WindowHandle)
dev_set_lut ('default')
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
visualize_object_model_3d (WindowHandle, [ObjectModel3D,OM3DLightPlane,OM3DMovement,OM3DCamera,OM3DCone], [], [-0.002989894371, 0.1325031046, 8.667736001, 288.0583956, 2.798360231, 297.2537796, 0], ['alpha_1', 'alpha_3', 'alpha_4', 'alpha_5', 'color_0', 'color_3', 'color_4', 'color_5'], [0.5, 0.5, 0.5, 0.5, 'blue', 'green', 'green', 'green'], 'Setup with reconstructed object', [], [], PoseOut)

二、为什么把视差图作为Rows输入

get_sheet_of_light_result (Disparity, SheetOfLightHandle, 'disparity')

返回每个像素的测量视差，即检测到轮廓的亚像素Row值。

image_points_to_world_plane (CameraParameters, LocalCameraPose, Disparities, Columns + Column1, 1.0, X, Y)

三、坐标系和点变换区分

1、ReferenceCameraPose：参考坐标系在相机坐标系下的姿态

HomMat3D_ReferenceToCam：参考坐标系的点到相机坐标系的点的变换（相机坐标系到参考坐标系）

HomMat3D_LocalToReference：本地坐标系的点到参考坐标系的点的变换

HomMat3D_LocalToCam：本地坐标系的点到相机坐标系的点的变换

pose_to_hom_mat3d (ReferenceCameraPose, HomMat3D_ReferenceToCam)
hom_mat3d_invert (HomMat3D_ReferenceToCam, HomMat3D_CamToReference)
hom_mat3d_compose (HomMat3D_CamToReference, HomMat3D_LocalToCam, HomMat3D_LocalToReference)
**这里用坐标系去理解，就是Reference到相机x相机到Local=Reference到Local

2、pose_compose 和hom_mat3d_compose

下面代码中得到的PoseCompose=PoseComposeAlternative

create_pose (0.1, 0.1, 0.1, 77, 0, 0, 'Rp+T', 'gba', 'point', PoseLeft)
create_pose (0.2, 0.2, 0.2, 0, 90, 0, 'Rp+T', 'gba', 'point', PoseRight)
pose_compose (PoseLeft, PoseRight, PoseCompose)
pose_to_hom_mat3d (PoseLeft, HomMat3DLeft)
pose_to_hom_mat3d (PoseRight, HomMat3DRight)
hom_mat3d_compose (HomMat3DLeft, HomMat3DRight, HomMat3DCompose)
hom_mat3d_to_pose (HomMat3DCompose, PoseComposeAlternative)