calibrate_sheet_of_light_calplate------------激光三角测量系统标定

• This program demonstrates how to perform the calibration of a sheet-of-light measurement system.
• 用halcon标定板标定激光三角测量系统
• The measurement system consists of an area scan camera and这个测量系统包括面扫描相机和激光线投影器
• a light line projector (e.g. a laser line projector).
  *The position and the orientation of the projector with respect激光线投影器相对于这个相机的位置和方向是固定的
• to the camera are fixed. In order to reconstruct the surface为了重建整个物体表面，物体必须在测量系统下移动
• of the whole object (and not only single profiles), the
• object must be moved under the measurement system,
• or alternatively the measurement system over the object.
• • Therefore, the calibration is performed in three steps:标定的三步骤
• • At first we determine the internal and external 首先，确定用标准的相机标定流程确定相机内外参数
• parameters of the camera by using the standard camera

• calibration procedure.

• • Then, we determine the orientation of the light plane with respect to the world coordinate system.
    *其次，确定激光投影平面相对于世界坐标系的方向

• This is done by computing a pose which is oriented such that the plane defined by z=0 coincides with the light plane.
  *通过计算一个面向z=0的平面的位姿来确定

• • At last, we calibrate the movement of the object between然后，从两连续的profiles文件中标定物体的运动
• the acquisition of two successive profiles. The transform 此时的物体的运动是相对世界坐标系而言的
• corresponding to this movement is also expressed with

• respect to the world coordinate system.

• • Finally, we show how to apply the calibration transforms to an already acquired disparity image.
• 最后，通过差异图像进行标定变换
• 
  
  • Perform some initializations
    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: Perform the calibration of the camera，相机标定
• ——-
• 
  
  • Initialize some parameters required for the camera calibration
    *初始化相机参数
    StartParameters := [0.0125,0.0,0.0,0.0,0.0,0.0,0.000006,0.000006,376.0,120.0,752,240]
    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
• 
  
  • Initialize a calibration data model，初始化标定数据模型
    create_calib_data (‘calibration_object’, 1, 1, CalibDataID)
    **在校准数据模型中设置摄像机的类型和初始参数，依次为标定数据模型的句柄，相机索引，相机类型，初始的相机内参数
    set_calib_data_cam_param (CalibDataID, 0, ‘area_scan_polynomial’, StartParameters)
    *定义在标定模型中的标定对象，依次为标定数据模型的句柄，标定对象索引，标定板的描述文件
    set_calib_data_calib_object (CalibDataID, 0, CalTabDescription)
• 
  
  • Collect mark positions and estimated poses for allcalibration images，提取标记点位置，并估计所有的图像的位姿
    for Index := 1 to NumCalibImages by 1
    read_image (Image, ‘sheet_of_light/connection_rod_calib_’ + Index$’.2’)
    dev_display (Image)
    *找出标定板，参数依次为：标定图像，标定数据模型的句柄，相机索引，标定板索引，标定板pose索 引， [], []
    find_calib_object (Image, CalibDataID, 0, 0, Index, [], [])
    *从校准数据模型中获得基于点的观测数据，依次是标定数据模型的句柄，相机索引，标定板索引，标定板pose索引，检测到的点的行坐标
    *列坐标，标定板中检测到的点数，Pose标定板相对于相机的姿态Pose
    get_calib_data_observ_points (CalibDataID, 0, 0, Index, Row, Column, _Index, Pose)
    *取得标定板的轮廓，依次是轮廓变量，标定数据模型的句柄，轮廓名称，相机索引，标定板索引，标定板pose索引
    get_calib_data_observ_contours (Contours, CalibDataID, ‘caltab’, 0, 0, Index)
    dev_set_color (‘green’)
    dev_display (Contours)
    *为每个输入点生成一个十字形的XLD轮廓
    gen_cross_contour_xld (Cross, Row, Column, 6, 0.785398)
    dev_set_color (‘yellow’)
    dev_display (Cross)
    endfor
• 
  
  • Perform the actual calibration，标定
    *通过同步最小化过程来确定所有相机参数
    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: Calibrate the orientation of the light plane确定激光平面相对于世界坐标系的方向
• with respect to the world coordinate system
• ——-
  dev_set_colored (3)
  MinThreshold := 80
• 
  
  • Definition of the world coordinate system (WCS):定义世界坐标系
• Here, the WCS is defined implicitly by choosing one选择一个标定图像定义世界坐标系，
• specific calibration image. In order to take the thickness考虑到标定板的厚度
• of the calibration table into account, we shift the origin把选择的标定图像的位姿进行转换
• of the pose that corresponds to the chosen calibration image
• (and that was computed during the calibration).
  Index := 19
  **在校准数据模型中存储数据，标定数据模型的句柄，校准数据项类型，Index of the affected item，数据名称，标定板对于相机坐标系的位姿。
  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)
  *在校准数据模型中存储数据，标定数据模型的句柄，依次校准数据项类型，0，数据名称，输出的相机参数。
  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.
  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 ()
• 
  
  *
• Compute the 3D coordinates of the light line points在世界坐标系z=0的平面上计算光线点的三维坐标
• in the plane z=0 of the WCS
  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
• 
  
  • Compute the 3D coordinates of the light line points在临时坐标系z=0的平面上计算光线点的三维坐标
• 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, ‘The profile MUST be oriented horizontally\nfor successfull processing!\nThe program will exit.’, ‘window’, 12, 12, ‘black’, ‘true’)
  return ()
  endif
• 
  
  • Fit the light plane in the 3D coordinates of the line points computed previously.
• 通过前面计算出的光线点的坐标来拟合激光平面（不同高度的光线点）
• Note that this requires nearly coplanar points.注意这需要近共面点
• We must provide line points recorded at -at least- two different heights, 需要提供两种不同高度的点
• in order to get an unambiguous solution. To obtain stable and为了获取稳定和准确的结果，需要得到在测量区的顶部和底部的光线点
• accurate results, acquire the light line points at the
• bottom and at the top of the measurement volume.
  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
• 计算光平面的位姿，这个位姿必须面向z=0的平面
• Compute the light plane pose: this pose must be oriented
• such that the plane defined by z=0 coincides with the
• light plane.
  *得到光平面的位姿
  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
  String := [‘LightPlanePose: ‘,’ Tx = ’ + LightPlanePose[0] ′.3′+′m′,′Ty=′+LightPlanePose[1] ′ .3 ′ + ′ m ′ , ′ T y = ′ + L i g h t P l a n e P o s e [ 1 ] ’.3’ + ’ m’,’ Tz = ’ + LightPlanePose[2] ′.3′+′m′,′alpha=′+LightPlanePose[3] ′ .3 ′ + ′ m ′ , ′ a l p h a = ′ + L i g h t P l a n e P o s e [ 3 ] ’.4’ + ‘°’,’ beta = ’ + LightPlanePose[4] ′.4′+′°′,′gamma=′+LightPlanePose[5] ′ .4 ′ + ′ ° ′ , ′ g a m m a = ′ + L i g h t P l a n e P o s e [ 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: Calibration of the movement of the object between
• the acquisition of two successive profiles
• ——-获取物体的移动方向
• 
  
  • In order to determine the movement of the object between为了在两个连续的profile图像中确定物体的运动，
• two successive profile images, we acquire two images of a我们需要一个标定板在相同运动中的两张图像
• calibration table which describe the same movement.（标定板放在传送带上？）
• In order to get a good accuracy, we usually move the为了得到较高的额准确度，需要不止一次移动标定板
• calibration table by more than one step.
  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 the optimized camera parameter as new start camera parameters for the
• calibration data model to extract the following poses using
• these calibrated parameters.
  *设置优化的相机参数作为一个新的开始相机参数
  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, [], [])
• Extract the unoptimized pose from the calibration data model从校准数据模型中提取未优化的姿势
  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)
* Clear the model
clear_calib_data (CalibDataID)
*
* Compute the coordinates of the origin of the calibration计算在世界坐标系下标定板在两个位置下的坐标
* table in the two positions with respect to the world
* coordinate system and determine the coordinates of the确定其对应的平移向量
* corresponding translation vector
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)
*CameraPose表示世界坐标系到到相机坐标系的关系
pose_to_hom_mat3d (CameraPose, HomMat3DWorldToCamera)
*hom_mat3d_invert进行坐标系关系的转换，相机坐标系到世界坐标系的关系
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)
MovementPoseNSteps := [EndX - StartX,EndY - StartY,EndZ - StartZ,0,0,0,0]
MovementPose := MovementPoseNSteps / StepNumber
String := [‘MovementPose: ‘,’ Tx = ’ + MovementPose[0] ′.3′+′m′,′Ty=′+MovementPose[1] ′ .3 ′ + ′ m ′ , ′ T y = ′ + M o v e m e n t P o s e [ 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: Apply the calibration transforms to an already
* acquired disparity image.
* ——-应用
*
* 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)
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)
*
* Display the resulting 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’)
*
* 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’)