Halcon 单目相机标定测量

双面相机标定链接

一维测量

gen_cam_par_area_scan_division -为区域扫描相机生成一个相机参数元组，该相机的变形由分割模型建模。(相机自带参数)

gen_cam_par_area_scan_division( : : Focus, Kappa, Sx, Sy, Cx, Cy, ImageWidth, ImageHeight : CameraParam)
Focus (输入控制)  实数 → (整数 / 实数)
    主距离。
    默认值：0.008
Kappa (输入控制)  实数 → (整数 / 实数)
    扭曲系数，用于建模径向镜头畸变。
    默认值：0.0
Sx (输入控制)  实数 → (整数 / 实数)
    传感器上相邻单元之间的水平距离。
    默认值：5.2e-6
Sy (输入控制)  实数 → (整数 / 实数)
    传感器上相邻单元之间的垂直距离。
    默认值：5.2e-6
Cx (输入控制)  实数 → (整数 / 实数)
    图像中主点的列坐标。
    默认值：640
Cy (输入控制)  实数 → (整数 / 实数)
    图像中主点的行坐标。
    默认值：512
ImageWidth (输入控制)  整数 → (整数)
    图像宽度。
    默认值：1280
ImageHeight (输入控制)  整数 → (整数)
    图像高度。
    默认值：1024
CameraParam (输出控制)  campar-array → (整数 / 实数 / 字符串)
    输出相机参数元组。

create_calib_data -创建Halcon 数据标定模型

create_calib_data( : : CalibSetup, NumCameras, NumCalibObjects : CalibDataID)
CalibSetup (输入控制)  字符串 → (字符串)
    校准设置的类型。

    默认值：'calibration_object'
    可选值列表：'calibration_object', 'hand_eye_moving_cam', 'hand_eye_scara_moving_cam', 'hand_eye_scara_stationary_cam', 'hand_eye_stationary_cam'

NumCameras (输入控制)  数字 → (整数)
    校准设置中相机的数量。
    默认值：1
    限制条件：NumCameras >= 0

NumCalibObjects (输入控制)  数字 → (整数)
    校准对象的数量。
    默认值：1
    限制条件：NumCalibObjects >= 0

CalibDataID (输出控制)  校准数据 → (句柄)
    创建的校准数据模型的句柄。
     'calibration_object' 的模型用于基于从校准物体观测中提取的度量信息，来校准一个或多个相机的内部参数（如焦距、畸变系数等）以及相机位姿（位置和方向）。
     类型为 'hand_eye_moving_cam'、'hand_eye_stationary_cam'、'hand_eye_scara_moving_cam' 或 'hand_eye_scara_stationary_cam' 的模型则用于执行手眼标定。这种标定基于对校准物体的观测数据，以及机器人工具在机器人基坐标系中的对应位姿数据。

上述四种模型类型的区分有两个方面：
    是由机器人移动相机还是移动校准物体：
        moving_cam：表示校准物体固定不动，而相机由机器人移动。
        stationary_cam：表示相机固定不动，而校准物体由机器人移动。

    使用的机器人类型是六自由度的关节型机器人还是四自由度的 SCARA 机器人：
        关节型机器人（Articulated robot）：具有三个旋转关节，通常可以覆盖6个自由度（3个平移 + 3个旋转）。
        SCARA 机器人：具有两个平行的旋转关节和一个直线运动的滑动关节，仅能覆盖4个自由度（3个平移 + 1个旋转）。

通俗地讲：
    关节型机器人可以倾斜其末端执行器（例如机械手）；
    SCARA 机器人只能进行平面内移动和旋转，不能倾斜末端执行器。

set_calib_data_cam_param -设置校准数据模型中摄像机的类型和初始参数。

set_calib_data_cam_param( : : CalibDataID, CameraIdx, CameraType, CameraParam : )
CalibDataID (输入控制，状态被修改)  校准数据 → (句柄)
    一个校准数据模型的句柄。该句柄指向已创建或正在被修改的相机校准数据模型。
    注意：此参数在调用过程中会被修改，即更新校准数据模型的内容。

CameraIdx (输入控制)  数组（数字类型） → (整数 / 字符串)
    相机的索引号，用于指定当前操作的是哪一个相机。
    默认值：0
    推荐值：'all', 0, 1, 2 等（表示单个相机或所有相机）

CameraType (输入控制)  字符串（数组可选） → (字符串)
    指定相机的类型。
    默认值：[]（空）
    可选值列表：[]（根据具体系统实现，可能支持如 'area_scan', 'line_scan' 等）

CameraParam (输入控制)  相机参数 → (实数 / 整数 / 字符串)
    初始的相机内部参数，通常是一个包含焦距、畸变系数、主点坐标等的元组或数组。
    示例值（可能结构）：
    [Focus, Kappa, Sx, Sy, Cx, Cy, ImageWidth, ImageHeight]

set_calib_data_calib_object -设置标定板

set_calib_data_calib_object( : : CalibDataID, CalibObjIdx, CalibObjDescr : )
CalibDataID (输入控制，状态被修改)  校准数据 → (句柄)
    校准数据模型的句柄。
    说明：该句柄指向一个已创建或正在被修改的校准数据模型。
    注意：此参数在调用过程中会被修改，表示对当前校准模型进行更新。

CalibObjIdx (输入控制)  数字 → (整数)
    校准物体的索引号，用于标识当前操作的是哪一个校准物体。

    默认值：0
    推荐值：0, 1, 2（根据实际存在的校准物体数量）

CalibObjDescr (输入控制)  数组（可选）→ (实数 / 整数 / 字符串)
    校准物体的描述信息，可以是：
    - 一个包含校准物体3D点坐标的数组；
    - 或者是一个描述文件的文件名（字符串），用于指定校准板的几何结构和尺寸。

    可选值列表（常见校准板描述文件）：
    'calplate.cpd',
    'calplate_10mm.cpd',
    'calplate_1200mm.cpd',
    'calplate_160mm.cpd',
    'calplate_20mm.cpd',
    'calplate_20mm_dark_on_light.cpd',
    'calplate_320mm.cpd',
    'calplate_40mm.cpd',
    'calplate_40mm_dark_on_light.cpd',
    'calplate_5mm.cpd',
    'calplate_640mm.cpd',
    'calplate_80mm.cpd',
    'calplate_80mm_dark_on_light.cpd',
    'caltab.descr',
    'caltab_100mm.descr',
    'caltab_10mm.descr',
    'caltab_200mm.descr',
    'caltab_2500um.descr',
    'caltab_30mm.descr',
    'caltab_650um.descr',
    'caltab_6mm.descr',
    'caltab_800mm.descr',
    'caltab_big.descr',
    'caltab_small.descr'

find_calib_object -查找 HALCON 校准板，并将提取的点和等高线设置到校准数据模型中。

find_calib_object(Image : : CalibDataID, CameraIdx, CalibObjIdx, CalibObjPoseIdx, GenParamName, GenParamValue : )

Image (输入对象)  单通道图像 → 对象 (byte / uint2)
    输入图像，通常为灰度图像或二值图像。
    用于检测校准物体（如标定板）的图像数据。

CalibDataID (输入控制，状态被修改)  校准数据 → (句柄)
    校准数据模型的句柄。
    说明：该句柄指向一个已创建或正在被修改的校准数据模型。
    注意：此参数在调用过程中会被修改，表示对当前校准模型进行更新。

CameraIdx (输入控制)  数字 → (整数)
    观测图像所对应的相机索引号。

    默认值：0
    推荐值：0, 1, 2（根据实际使用的相机数量）

CalibObjIdx (输入控制)  数字 → (整数)
    当前操作的校准物体索引号。

    默认值：0
    推荐值：0, 1, 2（根据实际存在的校准物体数量）

CalibObjPoseIdx (输入控制)  数字 → (整数)
    已观测到的校准物体位姿索引号，用于区分不同位姿下的观测。

    默认值：0
    推荐值：0, 1, 2
    限制条件：CalibObjPoseIdx >= 0

GenParamName (输入控制)  属性名数组 → (字符串)
    要设置的通用参数名称列表。

    默认值：[]
    可选参数名列表：
        'alpha'                  - 图像缩放因子
        'gap_tolerance'          - 检测标记点之间的间隙容忍度
        'max_diam_marks'         - 最大标记直径（像素）
        'sigma'                  - 高斯滤波标准差
        'skip_find_caltab'       - 是否跳过查找标定板步骤（true/false）

GenParamValue (输入控制)  属性值数组 → (字符串 / 实数 / 整数)
    与 GenParamName 对应的参数值。

    默认值：[]
    推荐值示例：0.5, 0.9, 1.0, 1.2, 1.5, 2.0, 'true', 'false'

get_calib_data_observ_contours -从校准数据模型中获取基于等高线的观测数据。

get_calib_data_observ_contours( : Contours : CalibDataID, ContourName, CameraIdx, CalibObjIdx, CalibObjPoseIdx : )
Contours (输出对象)  xld_cont(-数组) → 对象
    基于轮廓的结果。此参数返回检测到的轮廓信息，例如标定板上的标记点轮廓或其他特征轮廓。

CalibDataID (输入控制)  校准数据 → (句柄)
    校准数据模型的句柄。
    说明：该句柄指向一个已创建的校准数据模型，从中可以获取校准相关的数据。

ContourName (输入控制)  字符串 → (字符串)
    要返回的轮廓对象名称。

    默认值：'marks'
    可选值列表：
        'caltab'            - 返回整个标定板的轮廓。
        'last_caltab'       - 返回最后一个检测到的标定板的轮廓。
        'marks'             - 返回标定板上所有标记点的轮廓。
        'marks_with_hole'   - 返回带有孔洞的标记点轮廓（如果有）。

CameraIdx (输入控制)  数字 → (整数)
    观测相机的索引号。

    默认值：0

CalibObjIdx (输入控制)  数字 → (整数)
    观测到的校准板的索引号。

    默认值：0

CalibObjPoseIdx (输入控制)  数字 → (整数)
    观测到的校准物体姿态的索引号。

    默认值：0

calibrate_cameras - 标定相机

calibrate_cameras( : : CalibDataID : Error)

get_calib_data -查询校准数据模型中存储或计算的数据。

get_calib_data( : : CalibDataID, ItemType, ItemIdx, DataName : DataValue)
CalibDataID (输入控制)  校准数据 → (句柄)
    校准数据模型的句柄。
    说明：该句柄指向一个已创建的校准数据模型，从中可以获取或设置校准相关的数据。

ItemType (输入控制)  字符串 → (字符串)
    校准数据项的类型。

    默认值：'camera'
    可选值列表：
        'calib_obj'          - 校准物体
        'calib_obj_pose'     - 校准物体姿态
        'camera'             - 相机
        'model'              - 模型
        'tool'               - 工具

ItemIdx (输入控制)  整数(-数组) → (整数 / 字符串)
    影响项的索引（根据所选的 ItemType）。

    默认值：0
    推荐值：0, 1, 2, 'general'

DataName (输入控制)  属性名(-数组) → (字符串)
    要检查的数据名称。

    默认值：'params'
    可选值列表：
        'base_in_cam_pose', 'base_in_cam_pose_covariances', 'base_in_cam_pose_deviations', ...
        （更多选项包括相机校准误差、手眼校准误差、初始参数、初始姿态等）

DataValue (输出控制)  属性值(-数组) → (实数 / 整数 / 字符串)
    请求的数据。这是从校准数据模型中提取出的具体数值或信息。

write_cam_par -写入相机参数

write_cam_par( : : CameraParam, CamParFile : )

read_cam_par -读取相机参数

read_cam_par( : : CamParFile : CameraParam)

get_calib_data_observ_points -从校准数据模型中获取基于点的观测数据。

get_calib_data_observ_points( : : CalibDataID, CameraIdx, CalibObjIdx, CalibObjPoseIdx : Row, Column, Index, Pose)
CalibDataID (输入控制)  校准数据 → (句柄)
    校准数据模型的句柄。
    说明：该句柄指向一个已创建并配置好的校准数据模型。

CameraIdx (输入控制)  数字 → (整数)
    观测图像所对应的相机索引号。

    默认值：0
    推荐值：0, 1, 2（根据实际使用的相机数量）

CalibObjIdx (输入控制)  数字 → (整数)
    被观测的校准物体索引号。

    默认值：0
    推荐值：0, 1, 2（根据实际存在的校准物体数量）

CalibObjPoseIdx (输入控制)  数字 → (整数)
    已观测到的校准物体姿态索引号，用于区分不同位姿下的观测。

    默认值：0
    推荐值：0, 1, 2
    限制条件：CalibObjPoseIdx >= 0

Row (输出控制)  数组（数字类型）→ (实数 / 整数)
    检测到的特征点在图像中的行坐标（像素单位）。

Column (输出控制)  数组（数字类型）→ (实数 / 整数)
    检测到的特征点在图像中的列坐标（像素单位）。

Index (输出控制)  数组（数字类型）→ (整数 / 实数)
    表示检测到的图像点与校准物体上对应点之间的映射关系。
    每个元素表示该校正点在校准物体上的编号索引。

Pose (输出控制)  数组（数字类型）→ (实数 / 整数)
    校准物体相对于观测相机的粗略估计位姿（姿态 + 位置）。
    输出包含7个元素：
        - 前3个是旋转角度（Rx, Ry, Rz），单位通常是弧度或度；
        - 后4个是平移向量（Tx, Ty, Tz）和一个比例因子或额外参数；
    具体格式可能依赖于所用的库或系统实现。

disp_caltab -在图像中投影并显示校准板的 3D 模型。

disp_caltab( : : WindowHandle, CalPlateDescr, CameraParam, CalPlatePose, ScaleFac : )
WindowHandle (输入控制)  窗口 → (句柄)
    显示校准板图形的窗口句柄。
    说明：用于在指定的图像窗口中可视化校准板模型。

CalPlateDescr (输入控制)  文件名（读取） → (字符串)
    校准板描述文件的文件名，包含校准点的3D坐标信息。

    默认值：'calplate_320mm.cpd'
    可选值列表：
        'calplate_10mm.cpd', 
        'calplate_1200mm.cpd', 
        'calplate_160mm.cpd',
        'calplate_20mm.cpd',
        'calplate_20mm_dark_on_light.cpd',
        'calplate_320mm.cpd',
        'calplate_40mm.cpd',
        'calplate_40mm_dark_on_light.cpd',
        'calplate_5mm.cpd',
        'calplate_640mm.cpd',
        'calplate_80mm.cpd',
        'calplate_80mm_dark_on_light.cpd',
        'caltab_100mm.descr',
        'caltab_10mm.descr',
        'caltab_200mm.descr',
        'caltab_2500um.descr',
        'caltab_30mm.descr',
        'caltab_650um.descr',
        'caltab_6mm.descr',
        'caltab_800mm.descr',
        'caltab_big.descr',
        'caltab_small.descr'
    支持的文件扩展名：.cpd（校准板文件）、.descr（描述文件）

CameraParam (输入控制)  相机参数 → (实数 / 整数 / 字符串)
    相机的内部参数（内参），通常包括焦距、主点、像素尺寸和畸变系数等。
    示例结构（顺序可能不同）：
    [Focus, Kappa, Sx, Sy, Cx, Cy, ImageWidth, ImageHeight]

CalPlatePose (输入控制)  位姿 → (实数 / 整数)
    校准板相对于相机坐标系的外部位姿（即校准板在空间中的位置和方向）。

    元素个数：7
    前3个元素为旋转角度（Rx, Ry, Rz），后4个为平移向量（Tx, Ty, Tz）及一个附加参数（如比例因子或四元数的一部分），具体格式依赖于所用系统。

ScaleFac (输入控制)  实数 → (实数)
    可视化缩放因子，用于调整校准板在窗口中的显示大小。

    默认值：1.0
    推荐值：0.5, 1.0, 2.0, 3.0
    推荐增量步长：0.05
    限制条件：ScaleFac > 0.0

set_origin_pose -平移三维姿势的原点

set_origin_pose( : : PoseIn, DX, DY, DZ : PoseNewOrigin)
PoseIn (输入控制)  姿态 → (实数 / 整数)
    原始的三维姿态信息。
    
    元素数量：7

DX (输入控制)  实数 → (实数)
    在X轴方向上的原点平移量。

    默认值：0

DY (输入控制)  实数 → (实数)
    在Y轴方向上的原点平移量。

    默认值：0

DZ (输入控制)  实数 → (实数)
    在Z轴方向上的原点平移量。

    默认值：0

PoseNewOrigin (输出控制)  姿态 → (实数 / 整数)
    应用平移后的新的三维姿态。
    
    元素数量：7

clear_calib_data -清除标定板

clear_calib_data( : : CalibDataID : )

image_points_to_world_plane -将图像点转换为世界坐标系的 z=0 平面。

image_points_to_world_plane( : : CameraParam, WorldPose, Rows, Cols, Scale : X, Y)
# 参数说明：相机内部参数
CameraParam: 输入控制参数 (real / integer / string)
# 描述：相机的内部参数，例如焦距、光学中心等信息

# 参数说明：世界坐标系在相机坐标中的3D姿态
WorldPose: 输入控制参数 (real / integer)
# 描述：在相机坐标系中世界坐标系的3D姿态（位置和方向）
# 元素数量：7 （通常包含位置x, y, z和旋转表示）

# 参数说明：待转换点的行坐标数组
Rows: 输入控制参数 (real / integer) 
# 默认值：100.0
# 描述：待转换点的Y轴坐标

# 参数说明：待转换点的列坐标数组
Cols: 输入控制参数 (real / integer)
# 默认值：100.0
# 描述：待转换点的X轴坐标

# 参数说明：比例或维度
Scale: 输入控制参数 (string / integer / real)
# 默认值：'m'（米）
# 建议值：'m', 'cm', 'mm', 'microns', 'um', 1.0, 0.01, 0.001, 1.0e-6, 0.0254, 0.3048, 0.9144
# 限制条件：必须大于0
# 描述：用于指定尺度或尺寸，确保不同单位下的测量值能够正确转换

# 参数说明：转换后点的世界坐标系下的X坐标
X: 输出控制参数 (real)
# 描述：输出点在世界坐标系下的X坐标

# 参数说明：转换后点的世界坐标系下的Y坐标
Y: 输出控制参数 (real)
# 描述：输出点在世界坐标系下的Y坐标

单目相机标定

ImgPath := '3d_machine_vision/calib/'
dev_close_window ()
dev_open_window (0, 0, 652, 494, 'black', WindowHandle)
dev_update_off ()
dev_set_draw ('margin')
dev_set_line_width (3)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
* 
* Calibrate the camera.
* 
gen_cam_par_area_scan_division (0.016, 0, 0.0000074, 0.0000074, 326, 247, 652, 494, StartCamPar)
* 创建标定板
create_calib_data ('calibration_object', 1, 1, CalibDataID)
* 设置相机参数
set_calib_data_cam_param (CalibDataID, 0, [], StartCamPar)
* 设置标定板
set_calib_data_calib_object (CalibDataID, 0, 'caltab_30mm.descr')
NumImages := 10
* Note, we do not use the image from which the pose of the measurement plane can be derived
for I := 1 to NumImages by 1
    read_image (Image, ImgPath + 'calib_' + I$'02d')
    dev_display (Image)
    * 发现标定板
    find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
    * 获取标定轮廓
    get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
    dev_set_color ('green')
    dev_display (Caltab)
endfor
*标定相机
calibrate_cameras (CalibDataID, Error)
*获取标定参数
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* Write the internal camera parameters to a file 写入
write_cam_par (CamParam, 'camera_parameters.dat')
Message := 'Interior camera parameters have'
Message[1] := 'been written to file'
disp_message (WindowHandle, Message, 'window', 12, 12, 'red', 'false')
clear_calib_data (CalibDataID)

相机标定完成后进行测量

* Attention:
* 注意：
* This program reads the internal camera parameters from the file
* 本程序从文件中读取相机内部参数
* 'camera_parameters.dat', which, e.g., could be generated by the program
* 文件'camera_parameters.dat'可由程序
* 'camera_calibration_internal.hdev' generate
* 'camera_calibration_internal.hdev'生成

* 定义图像路径
ImgPath := '3d_machine_vision/calib/'

* 关闭当前窗口并打开新窗口
dev_close_window ()
dev_open_window (0, 0, 652, 494, 'black', WindowHandle)

* 关闭自动更新显示
dev_update_off ()

* 设置绘图模式为边缘
dev_set_draw ('margin')

* 设置线宽为1
dev_set_line_width (1)

* 设置显示字体
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')

* 尝试从文件读取相机参数
try
    * 读取相机标定参数
    read_cam_par ('camera_parameters.dat', CamParam)
catch (Exception)
    * 如果失败提示先运行标定程序
    * run 'camera_calibration_internal.hdev' first to generate camera
    * parameter file 'camera_parameters.dat'
    stop ()
endtry

* 从图像点确定外部相机参数和世界坐标
* 外部相机参数可以通过标定板直接放置在测量平面上的图像确定

* 读取图像
read_image (Image, ImgPath + 'calib_11')
dev_display (Image)

* 定义标定板描述文件
CaltabName := 'caltab_30mm.descr' //标定板

* 创建标定数据
create_calib_data ('calibration_object', 1, 1, CalibDataID)

* 这里最终相机参数已知，可以用来代替初始值
* 设置相机参数
set_calib_data_cam_param (CalibDataID, 0, [], CamParam)

* 设置标定对象
set_calib_data_calib_object (CalibDataID, 0, CaltabName)

* 查找标定板
find_calib_object (Image, CalibDataID, 0, 0, 1, [], [])

* 获取标定板轮廓
get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, 1)

* 获取标定板点坐标
get_calib_data_observ_points (CalibDataID, 0, 0, 1, RCoord, CCoord, Index, PoseForCalibrationPlate)

* 设置颜色为绿色并显示标定板轮廓
dev_set_color ('green')
dev_display (Caltab)

* 设置颜色为红色
dev_set_color ('red')

* 显示标定板
disp_caltab (WindowHandle, CaltabName, CamParam, PoseForCalibrationPlate, 1)

* 设置线宽为3
dev_set_line_width (3)

* 显示标定板上的圆点
disp_circle (WindowHandle, RCoord, CCoord, gen_tuple_const(|RCoord|,1.5))

* 考虑标定板厚度，调整Z值
* 如果不想添加校正，请禁用下面一行
set_origin_pose (PoseForCalibrationPlate, 0, 0, 0.00075, PoseForCalibrationPlate)

* 清除标定数据
clear_calib_data (CalibDataID)

* 显示继续消息
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

* 或者，可以通过至少三个点对应关系确定外部相机参数
* 这些点在世界坐标系和像素坐标系中有对应关系

* 读取新图像
read_image (Image, ImgPath + 'caliper_01')
dev_display (Image)

* 设置规则上三个点的世界坐标
X := [0, 50, 100, 80]
Y := [5, 0, 5, 0]
Z := [0, 0, 0, 0]

* 设置三个点对应的图像平面坐标
RCoord := [414, 227, 85, 128]
CCoord := [119, 318, 550, 448]

* 显示十字标记
disp_cross (WindowHandle, RCoord, CCoord, 6, 0)

* 通过点对应关系计算位姿
vector_to_pose (X, Y, Z, RCoord, CCoord, CamParam, 'iterative', 'error', FinalPose, Errors)

* 将位姿写入文件
write_pose (FinalPose, 'pose_from_three_points.dat')

* 现在将交互测量的点转换到世界坐标系
dev_update_window ('on')
dev_display (Image)
while (1)
    * 显示测量提示消息
    disp_message (WindowHandle, 'Measure one point: left mouse button', 'window', 12, 12, 'red', 'false')
    disp_message (WindowHandle, 'Exit measure mode: right mouse button', 'window', 36, 12, 'red', 'false')
    
    * 获取鼠标点击
    get_mbutton (WindowHandle, Row, Column, Button)
    if (Button == 4)
        break
    endif
    
    * 显示图像和测量点
    dev_display (Image)
    dev_set_color ('green')
    disp_cross (WindowHandle, Row, Column, 6, 0)
    
    * 将图像点转换到世界平面
    image_points_to_world_plane (CamParam, FinalPose, Row, Column, 1, X1, Y1)
    
    * 显示世界坐标
    disp_message (WindowHandle, 'X = ' + X1, 'window', 320, 400, 'red', 'false')
    disp_message (WindowHandle, 'Y = ' + Y1, 'window', 340, 400, 'red', 'false')
endwhile

* 应用测量工具并将结果点坐标转换到世界坐标系
dev_set_color ('red')
dev_display (Image)

* 设置定义测量工具ROI的四个点的世界坐标 ，卡尺坐标点
ROI_X_WCS := [-2, -2, 112, 112]
ROI_Y_WCS := [0, 0.5, 0.5, 0]
ROI_Z_WCS := [0, 0, 0, 0]

* 确定从世界坐标系到相机坐标系的变换矩阵
pose_to_hom_mat3d (FinalPose, CCS_HomMat_WCS)

* 将点坐标转换到图像坐标系
affine_trans_point_3d (CCS_HomMat_WCS, ROI_X_WCS, ROI_Y_WCS, ROI_Z_WCS, CCS_RectangleX, CCS_RectangleY, CCS_RectangleZ)
project_3d_point (CCS_RectangleX, CCS_RectangleY, CCS_RectangleZ, CamParam, RectangleRow, RectangleCol)

* 创建ROI区域
gen_region_polygon_filled (ROI, RectangleRow, RectangleCol)
smallest_rectangle2 (ROI, RowCenterROI, ColCenterROI, PhiROI, Length1ROI, Length2ROI)

* 创建测量对象
gen_measure_rectangle2 (RowCenterROI, ColCenterROI, PhiROI, Length1ROI, Length2ROI, 652, 494, 'bilinear', MeasureHandle)

* 执行边缘对测量
measure_pairs (Image, MeasureHandle, 0.4, 5, 'all_strongest', 'all', RowEdgeFirst, ColumnEdgeFirst, AmplitudeFirst, RowEdgeSecond, ColumnEdgeSecond, AmplitudeSecond, IntraDistance, InterDistance)

* 关闭测量对象
close_measure (MeasureHandle)
dev_display (Image)

* 显示测量消息
disp_message (WindowHandle, 'Measuring the position of the pitch lines', 'window', 450, 25, 'red', 'false')
dev_set_color ('green')

* 计算并显示中线
RowPitchLine := (RowEdgeFirst + RowEdgeSecond) / 2.0
ColPitchLine := (ColumnEdgeFirst + ColumnEdgeSecond) / 2.0
disp_cross (WindowHandle, RowPitchLine, ColPitchLine, 6, 0)

* 将中线点转换到世界平面
image_points_to_world_plane (CamParam, FinalPose, RowPitchLine, ColPitchLine, 1, X1, Y1)

* 显示测量结果
for I := 1 to |X1| by 1
    set_tposition (WindowHandle, RowEdgeFirst[I - 1] + 5, ColumnEdgeFirst[I - 1] - 20)
    if (I == |X1|)
        set_tposition (WindowHandle, RowEdgeFirst[I - 1], ColumnEdgeFirst[I - 2])
    endif
    write_string (WindowHandle, X1[I - 1]$'.3f' + 'mm')
endfor

* 显示继续消息
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

* 应用线提取并将结果XLD轮廓转换到世界坐标系
dev_display (Image)

* 设置定义ROI的四个点的世界坐标
ROI_X_WCS := [11, 11, 13, 13]
ROI_Y_WCS := [4, 6, 6, 4]
ROI_Z_WCS := [0, 0, 0, 0]

* 将点坐标转换到图像坐标系
affine_trans_point_3d (CCS_HomMat_WCS, ROI_X_WCS, ROI_Y_WCS, ROI_Z_WCS, CCS_RectangleX, CCS_RectangleY, CCS_RectangleZ)
project_3d_point (CCS_RectangleX, CCS_RectangleY, CCS_RectangleZ, CamParam, RectangleRow, RectangleCol)

* 在原始图像中可视化矩形
disp_polygon (WindowHandle, [RectangleRow,RectangleRow[0]], [RectangleCol,RectangleCol[0]])
dev_display (Image)

* 创建ROI区域
gen_region_polygon_filled (ROI, RectangleRow, RectangleCol)
reduce_domain (Image, ROI, ImageReduced)

* 提取线条
lines_gauss (ImageReduced, Lines, 1, 3, 8, 'dark', 'true', 'bar-shaped', 'true')

* 调整测量平面的位姿以适应游标尺的倾斜平面
RelPose := [0, 3.2, 0, -14, 0, 0, 0]
pose_to_hom_mat3d (FinalPose, HomMat3D)
pose_to_hom_mat3d (RelPose, HomMat3DRel)
hom_mat3d_compose (HomMat3D, HomMat3DRel, HomMat3DAdapted)

* 也可以使用hom_mat3d_translate_local和hom_mat3d_rotate_local操作符进行调整
hom_mat3d_translate_local (HomMat3D, 0, 3.2, 0, HomMat3DTranslate)
hom_mat3d_rotate_local (HomMat3DTranslate, rad(-14), 'x', HomMat3DAdapted)
hom_mat3d_to_pose (HomMat3DAdapted, PoseAdapted)

* 使用调整后的位姿将XLD轮廓转换到世界坐标系
contour_to_world_plane_xld (Lines, ContoursTrans, CamParam, PoseAdapted, 1)
get_contour_xld (ContoursTrans, YOfContour, XOfContour)
tuple_mean (XOfContour, MeterReading)

* 显示线条和读数
dev_display (Lines)
disp_message (WindowHandle, 'Meter reading: ' + MeterReading$'.3f' + 'mm', 'window', 400, 180, 'green', 'false')

* 显示继续消息
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

* 关闭检查控件
dev_close_inspect_ctrl (YOfContour)
dev_close_inspect_ctrl (XOfContour)

* 现在转换整个图像
WidthMappedImage := 652
HeightMappedImage := 494
dev_display (Image)

* 首先确定映射的比例
* (这里确定比例使得在点P0和P1周围，
* 映射图像的图像比例与原始图像的图像比例相似)
distance_pp (X[0], Y[0], X[1], Y[1], DistP0P1WCS)
distance_pp (RCoord[0], CCoord[0], RCoord[1], CCoord[1], DistP0P1PCS)
Scale := DistP0P1WCS / DistP0P1PCS

* 然后确定set_origin_pose的参数设置，
* 使得通过get_mbutton定义的点位于映射图像的中心
dev_display (Image)
disp_message (WindowHandle, 'Define the center of the mapped image', 'window', 12, 12, 'red', 'false')
get_mbutton (WindowHandle, CenterRow, CenterColumn, Button1)
image_points_to_world_plane (CamParam, FinalPose, CenterRow, CenterColumn, 1, CenterX, CenterY)
set_origin_pose (FinalPose, CenterX - Scale * WidthMappedImage / 2.0, CenterY - Scale * HeightMappedImage / 2.0, 0, PoseNewOrigin)

* 生成图像到世界平面的映射
gen_image_to_world_plane_map (Map, CamParam, PoseNewOrigin, 652, 494, WidthMappedImage, HeightMappedImage, Scale, 'bilinear')
map_image (Image, Map, ImageMapped)

* 显示映射后的图像
dev_clear_window ()
dev_display (ImageMapped)

* 如果只需要映射一个图像，可以使用image_to_world_plane操作符
* 代替gen_image_to_world_plane_map和map_image操作符
image_to_world_plane (Image, ImageMapped, CamParam, PoseNewOrigin, WidthMappedImage, HeightMappedImage, Scale, 'bilinear')

标定+测量

ImgPath := '3d_machine_vision/calib/'
dev_close_window ()
dev_open_window (0, 0, 640, 480, 'black', WindowHandle)
dev_update_off ()
dev_set_draw ('margin')
dev_set_line_width (3)
set_display_font (WindowHandle, 22, 'mono', 'true', 'false')
* 
* Calibrate the camera.相机标定
* 
gen_cam_par_area_scan_division (0.012, 0, 0.00000375, 0.00000375, 640, 480, 1280, 960, StartCamPar)
create_calib_data ('calibration_object', 1, 1, CalibDataID)
set_calib_data_cam_param (CalibDataID, 0, [], StartCamPar)
set_calib_data_calib_object (CalibDataID, 0, 'calplate_80mm.cpd')
NumImages := 7
for I := 1 to NumImages by 1
    read_image (Image, ImgPath + 'calib_image_' + I$'02d')
    dev_display (Image)
    find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
    get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
    get_calib_data_observ_points (CalibDataID, 0, 0, I, Row, Column, Index, StartPose)
    dev_set_color ('green')
    dev_display (Caltab)
    dev_set_color ('red')
    disp_circle (WindowHandle, Row, Column, gen_tuple_const(|Row|,1.5))
endfor
calibrate_cameras (CalibDataID, Errors)
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* The reference image, i.e., the image in which the calibration
* plate is located on the ruler is the first image
get_calib_data (CalibDataID, 'calib_obj_pose', [0, 1], 'pose', Pose)
* To take the thickness of the calibration plate into account, the z-value
* of the origin given by the camera pose has to be translated by the
* thickness of the calibration plate.
* Deactivate the following line if you do not want to add the correction.
set_origin_pose (Pose, 0, 0, 0.002, Pose)
* measure the distance between the pitch lines
read_image (Image, ImgPath + 'ruler')
dev_display (Image)
*产生一个测量矩形
gen_measure_rectangle2 (690, 680, rad(-0.25), 480, 8, 1280, 960, 'bilinear', MeasureHandle)

measure_pairs (Image, MeasureHandle, 0.5, 5, 'all', 'all', RowEdgeFirst, ColumnEdgeFirst, AmplitudeFirst, RowEdgeSecond, ColumnEdgeSecond, AmplitudeSecond, IntraDistance, InterDistance)
Row := (RowEdgeFirst + RowEdgeSecond) / 2.0
Col := (ColumnEdgeFirst + ColumnEdgeSecond) / 2.0
disp_cross (WindowHandle, Row, Col, 20, rad(45))
*坐标系转换
image_points_to_world_plane (CamParam, Pose, Row, Col, 'mm', X1, Y1)
*计算两个点的距离
distance_pp (X1[0:11], Y1[0:11], X1[1:12], Y1[1:12], Distance)
tuple_mean (Distance, MeanDistance)
tuple_deviation (Distance, DeviationDistance)
disp_message (WindowHandle, 'Mean distance: ' + MeanDistance$'.3f' + 'mm +/- ' + DeviationDistance$'.3f' + 'mm', 'window', 30, 60, 'yellow', 'false')