Halcon学习：calibrate_hand_eye_scara_stationary_cam_approx

*此示例说明了如何执行近似的手眼校准
*对于没有校准板的SCARA机器人。机器人相对于世界坐标系的近似校准或“对齐”假定工具中心点位于工具坐标系中的（未知）位置（0、0，Z），即抓取器或真空杯相对于工具坐标系的z轴居中。此外，假设在抓取物体时不需要考虑抓取器或真空杯的方向（围绕Z轴的旋转）。
*它至少基于四个所谓的校准点，这些点是已知图像坐标和机器人坐标的点，即它们必须在图像中清晰可辨，并且必须有可能准确地接近它们用机器人工具。可以通过以下方式提供这些点：例如，将要抓取的对象放置在相机的视野中，获取图像，手动指示图像中的抓取位置，然后手动将工具中心点移动到抓取位置，然后记录相应的机器人姿势。在此示例中使用了此类校准点。除了使用物体上的抓取位置，还可以简单地在纸上绘制十字并将这些点用作校准点。

*在此示例中，我们使用五个校准点，即比必要点多一个点，以使校准结果稍微更准确。

*请注意，此示例仅校准手眼系统。它使用预定义的图像坐标以及机器人姿势作为校准点。为了使该示例适合实际应用，必须确定图像坐标，然后由机器人首先摆姿势。在此示例中，假定摄像机相对于机器人是静止的，并且观察机器人的工作空间。

*最后，得出校准结果以及确定要抓住的物体的位置所需的数据写入文件。然后，示例程序pick_and_place_scara_stationary_cam.hdev使用此信息。注意，这种“校准”仅提供“真实”校准信息的近似值。精度取决于图像坐标的精度以及校准点各自的机器人姿势

dev_update_off ()
dev_close_window ()
read_image (Image, '3d_machine_vision/handeye/scara_stationary_cam_setup_01_calib_approx_01')
dev_clear_window ()
dev_open_window_fit_image (Image, 0, 0, -1, -1, WindowHandle)
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
Message := 'This example program shows how to perform an approximate hand-eye calibration for a SCARA robot without a calibration plate. In a first step, the input data - the row and column coordinates and the respective robot poses for the calibration points - are shown. Then, the approximate calibration is carried out. The example does not display any results. Instead, the results are written to file and can be used with the HDevelop example program pick_and_place_scara_stationary_cam.hdev'
MessageWrapped := regexp_replace(Message + ' ',['(.{0,50})\\s','replace_all'],'$1\n')

*该示例程序显示了如何对没有校准板的SCARA机器人执行近似的手眼校准。 
*第一步，显示输入数据-行和列坐标以及校准点的相应机器人姿势。 
*然后，进行近似校准。 
*该示例不显示结果，而是将结果写入文件，并可与HDevelop示例程序pick_and_place_scara_stationary_cam.hdev一起使用。
disp_message (WindowHandle, MessageWrapped, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* 

*提供先前校准过的摄像机的摄像机参数
CameraParam := [0.0165251,-642.277,4.65521e-006,4.65e-006,595.817,521.75,1280,1024]
* 
*提供行/列坐标和校准点的相应机器人姿势。在实际应用中，必须如上所述确定这些值。
CalibrationPointsRow := [139.5,111.5,485.5,809.5,915.5] //五个校准点行坐标
CalibrationPointsColumn := [252.0,1069.5,629.5,217.0,1042.0] //五个校准点列坐标
CalibrationPointsRobotPose := {[0.309454,0.147978,0.122474,0.0,0.0,379.475,0],[0.204182,0.150825,0.123036,0.0,0.0,405.695,0],[0.257784,0.191762,0.123514,0.0,0.0,394.668,0],[0.315887,0.231502,0.124396,0.0,0.0,379.325,0],[0.206101,0.238083,0.124235,0.0,0.0,408.274,0]} //五个点对应的机器人pose
NumCalibrationPoints := |CalibrationPointsRow|
* 
*可视化输入数据
read_image (Image, '3d_machine_vision/handeye/scara_stationary_cam_setup_01_calib_approx_01')
dev_set_line_width (2)
for Index := 0 to NumCalibrationPoints - 1 by 1
    dev_clear_window ()
    dev_display (Image)
    dev_set_color ('yellow')
    gen_cross_contour_xld (Cross, CalibrationPointsRow[Index], CalibrationPointsColumn[Index], 25, 0.785398) //在五个校准点处生成十字叉
    dev_display (Cross)
    disp_message (WindowHandle, 'Calibration point ' + (Index + 1), 'window', 12, 12, 'black', 'true')
    Message := 'Image coordinates'
    Message[1] := '  Row    = ' + CalibrationPointsRow[Index]
    Message[2] := '  Column = ' + CalibrationPointsColumn[Index]
    Message[3] := 'Robot pose:'
    Message[4] := '  Tx    = ' + CalibrationPointsRobotPose.at(Index)[0]$'.3f'
    Message[5] := '  Ty    = ' + CalibrationPointsRobotPose.at(Index)[1]$'.3f'
    Message[6] := '  Tz    = ' + CalibrationPointsRobotPose.at(Index)[2]$'.3f'
    Message[7] := '  Alpha = ' + CalibrationPointsRobotPose.at(Index)[3]$'.1f'
    Message[8] := '  Beta  = ' + CalibrationPointsRobotPose.at(Index)[4]$'.1f'
    Message[9] := '  Gamma =  ' + CalibrationPointsRobotPose.at(Index)[5]$'.1f'
    disp_message (WindowHandle, Message, 'image', min([CalibrationPointsRow[Index],500]), min([CalibrationPointsColumn[Index],900]), 'white', 'false')
    disp_continue_message (WindowHandle, 'black', 'true')
    stop ()
endfor
dev_clear_window ()
* 
*进行近似的手眼校准以确定测量平面
* 
*使用上面定义（或测量）的校准点，我们可以很容易地计算出相机在机器人基本坐标系中的姿态（实际上这是手眼校准的主要结果）。我们只需要基本坐标系中的校准点坐标（直到z中的未知平移）以及相应的图像坐标即可通过操作符vector_to_pose确定姿态。
*为了能够确定机器人要抓住的物体的位置，我们还必须确定测量平面的姿态。为此，上面定义（或测量）的校准点与HALCON校准板的校准标记类似地使用。第一步，我们必须确定校准点的X，Y和Z坐标。该信息可以从机器人姿势中得出。我们定义一个坐标系，在该坐标系中给出校准点的对象坐标，使其原点位于第一个校准点。然后，可以通过相应的相对机器人姿态的平移部分确定其余点的坐标。然后，使用模拟的校准板再次通过操作符vector_to_pose.

calibrate_hand_eye_scara_stationary_cam_approx（CameraParam，CalibrationPointsRow，CalibrationPointsColumn，CalibrationPointsRobotPose，CamInBasePose，CPInCamPose） //执行近似手眼校准，或使用SCARA机器人确定测量平面。

*确定测量平面。如果校准点不位于抓握点所在的平面上，例如，因为校准点仅标记在一张纸上（->CalibrationObjectThickness：= 0.000），并且要抓握的对象厚度为1 mm（-> ObjectThickness：= 0.001），所以校准平面的姿态必须为校正以描述测量平面。相反，如果将抓取点用作校准点，则必须将CalibrationObjectThickness设置为ObjectThickness的值。
CalibrationObjectThickness := 0.001
ObjectThickness := 0.001
set_origin_pose (CPInCamPose, 0, 0, CalibrationObjectThickness - ObjectThickness, MPInCamPose) //转换3D姿态原点。
* 
* 写下结果—手眼标定结果
write_pose (CamInBasePose, 'cam_in_base_pose.dat')
* 从原始图像估计要抓取物体的姿态所需的数据
write_cam_par (CameraParam, 'camera_parameters.dat')
write_pose (MPInCamPose, 'measurement_plane_in_cam_pose.dat')
disp_end_of_program_message (WindowHandle, 'black', 'true')

---

CameraParam := [0.0165251,-642.277,4.65521e-006,4.65e-006,595.817,521.75,1280,1024]

焦点（input_control）实→（整数/实）
主要距离。
默认值：0.008
Kappa（input_control）实数→（整数/实数）
变形系数可以模拟径向透镜的变形。
默认值：0.0
Sx（input_control）实数→（整数/实数）
传感器上两个相邻单元之间的水平距离。
默认值：5.2e-6
SY（输入控制）实数→（整数/实数）
传感器上两个相邻单元格之间的垂直距离。
默认值：5.2e-6
Cx（input_control）实数→（整数/实数）
图像中主要点的列坐标。
默认值：640
Cy（input_control）实数→（整数/实数）
图像中主要点的行坐标。
默认值：512
ImageWidth（input_control）整数→（整数）
图像宽度。
默认值：1280
ImageHeight（input_control）整数→（整数）
图像高度。
默认值：1024
CameraParam（output_control）Campar数组→（整数/实数/字符串）
输出相机参数元组。

---

calibrate_hand_eye_scara_stationary_cam_approx（CameraParam，CalibrationPointsRow，CalibrationPointsColumn，CalibrationPointsRobotPose，CamInBasePose，CPInCamPose） //执行近似手眼标定或SCARA机器人，并确定测量平面。

CamInBasePose（output_control）→（整数/实数）相机在机器人基础坐标系中的姿态。

CPInCamPose（output_control）→（整数/实数）由相机坐标系中的校准点定义的平面姿态。

---

总结：

整理下思路，还不太明白其中各个参数的含义，以及标定参数CamInBasePose、CPInCamPose的使用。

首先，  输入四个参数：CameraParam 、CalibrationPointsRow、CalibrationPointsColumn、CalibrationPointsRobotPose，CameraParam先前校准过的摄像机的摄像机参数，CalibrationPointsRow、CalibrationPointsColumn 五个标准点行、列坐标，CalibrationPointsRobotPose五个标准点对应的机器人pose

处理：calibrate_hand_eye_scara_stationary_cam_approx() //输出CamInBasePose、CPInCamPose

CamInBasePose：相机在机器人基础坐标系中的姿态。

CPInCamPose：由相机坐标系中的校准点定义的平面姿态。

最后，输出：CamInBasePose、CameraParam、MPInCamPose的.dat文件

* 写下结果—手眼标定结果
write_pose (CamInBasePose, 'cam_in_base_pose.dat')
* 从原始图像估计要抓取物体的姿态所需的数据
write_cam_par (CameraParam, 'camera_parameters.dat')
write_pose (MPInCamPose, 'measurement_plane_in_cam_pose.dat')