prepare_poses_and_rectification_data_moving_cam函数解析这段代码的核心价值功能说明位姿准备构建与图像对齐的匹配平面并考虑物体高度Z轴方向校正确保平面法向量朝向合理避免背面自适应分辨率通过投影点拟合自动确定校正尺度高效校正预生成RectificationMap避免重复计算透视变换格式兼容输入/输出保持机器人原有位姿格式模块化输出通过字典传递数据便于后续流程使用这段 HALCON/HDevelop 脚本代码用于为机器人抓取任务准备位姿poses和图像校正映射rectification map核心目标是在已知物体高度的前提下构建一个与相机图像对齐的“匹配平面”并生成可用于快速重投影的校正映射map以便后续进行模板匹配或抓取规划。下面逐部分详细解析。一、输入读取与校验read_dict_tuple (HandEyeCalibData, CamParam, CamParam) read_dict_tuple (HandEyeCalibData, ToolInCamPose, ToolInCamPose) read_dict_tuple (HandEyeCalibData, PlaneInBasePose0, PlaneInBasePose0)从手眼标定数据中读取相机参数CamParam工具末端在相机坐标系中的位姿ToolInCamPose初始参考平面在机器人基坐标系中的位姿PlaneInBasePose0if (ObjectHeight 0.0) throw(...) if (CamParam[0] line_scan) throw(...)校验物体高度不能为负不支持线扫相机。二、统一所有位姿表示格式get_pose_type (ToolInBasePose, OrderOfTransform, ...) convert_pose_type (..., RpT, gba, point, ...)获取当前机器人使用的位姿格式旋转顺序、平移/旋转顺序等将所有位姿转换为内部统一格式RpT旋转平移、gbaZYX欧拉角、point最后会再转回原始格式输出保证兼容性三、构建“对齐图像”的参考平面PlaneInCamPose1. 计算参考平面在相机坐标系中的初始位姿pose_invert (ToolInBasePose, BaseInToolPose) pose_compose (ToolInCamPose, BaseInToolPose, BaseInCamPose) pose_compose (BaseInCamPose, PlaneInBasePose0, PlaneInCamPose0)得到PlaneInCamPose0参考平面在相机坐标系中的位姿2. 确保平面 Z 轴指向远离相机方向affine_trans_point_3d (..., 0,0,1, ..., CosAngleBetweenZAxis) if (CosAngleBetweenZAxis 0) // 绕X轴旋转180°翻转Z方向 pose_compose (PlaneInCamPose0, SwitchZDirection, PlaneInCamPose1) endif避免平面“背面朝向”相机影响后续投影3. 对齐图像坐标系消除绕Z轴旋转PlaneInCamPose参考平面在相机坐标系中的位姿PlaneInCamPose : PlaneInCamPose0 PlaneInCamPose[5] : 0.0 // 第6个元素是绕Z轴的旋转角gamma设为0使得平面在图像中“正对”X/Y 与图像行列方向一致无偏航4. 更新该平面对应的基坐标系位姿pose_invert (BaseInCamPose, CamInBasePose) pose_compose (CamInBasePose, PlaneInCamPose, PlaneInBasePose)✅此时PlaneInCamPose是一个与当前图像对齐、Z轴朝外的标准参考平面四、创建“匹配平面”考虑物体高度create_pose (0, 0, -ObjectHeight, 0, 0, 0, ..., MatchingPlaneInPlanePose) pose_compose (PlaneInBasePose, ..., MatchingPlaneInBasePose) pose_compose (PlaneInCamPose, ..., MatchingPlaneInCamPose)在参考平面上向下偏移ObjectHeight因为物体表面高于参考平面或反之取决于约定得到实际用于匹配的平面位姿MatchingPlaneInCamPose和MatchingPlaneInBasePose 假设PlaneInBasePose0是工作台平面而物体有一定高度则匹配应在物体顶面进行。五、根据 RectifyImage 模式处理校正情况 Ano_rectification直接使用MatchingPlaneInCamPoseScaleRectification留空情况 Bonly_rectify或align_and_rectify步骤 1自动计算校正尺度分辨率gen_grid_region (RegionGrid, 20, 20, points, Width, Height) contour_to_world_plane_xld (..., MatchingPlaneInCamPose, ...) fit_ellipse_contour_xld (...) ScaleRectification : min(Radius2)在原始图像上撒点20x20网格将这些1*1的像素点投影到匹配平面拟合椭圆取短轴半径min(Radius2)作为物理尺寸对应1像素的尺度单位米/像素目的保证校正后图像分辨率不低于原图 这是一种自适应分辨率策略避免过度缩放导致信息丢失或计算浪费。步骤 2确定校正图像应覆盖的物理范围gen_rectangle1 (ImageArea, 0, 0, Height-1, Width-1) boundary → get_region_points → image_points_to_world_plane将原始图像四边边界点反投影到匹配平面得到(BorderX, BorderY)计算 X/Y 方向的物理范围[min, max]步骤 3构建校正用的位姿和图像尺寸create_pose (min(BorderX), min(BorderY), 0, ...) // 新原点在左下角 pose_compose → MatchingPlaneRectifiedPartInCamPose WidthRect (ΔX) / Scale 0.5 HeightRect (ΔY) / Scale 0.5步骤 4生成校正映射Map而非直接校正图像gen_image_to_world_plane_map (RectificationMap, CamParam, MatchingPlaneInCamPose, Width, Height, WidthRect, HeightRect, ScaleRectification, bilinear)✅关键优化不每次都调用image_to_world_plane而是预计算一个映射表map后续只需map_image(Image, RectificationMap, ImageRectified)即可快速校正适用于需要多次校正同一视角图像的场景如实时抓取六、恢复位姿格式并输出结果convert_pose_type (..., original_format, ...)将所有内部计算的位姿转回机器人原始使用的格式create_pose (0, 0, ObjectHeight, 180, 0, 0, ..., ModelInPlanePose) pose_invert (ModelInPlanePose, PlaneInModelPose)定义模型坐标系相对于匹配平面的变换用于后续将匹配结果转为抓取位姿七、打包输出到字典1.Poses字典供匹配和抓取使用set_dict_tuple (Poses, PlaneInCamPose, ...) set_dict_tuple (Poses, MatchingPlaneInBasePose, ...) set_dict_tuple (Poses, PlaneInModelPose, ...) // 关键模型到平面的变换2.RectificationData字典供图像校正使用set_dict_tuple (RectificationData, RectifyImage, ...) if (need rectify) set_dict_tuple (..., ScaleRectification, ...) set_dict_object (RectificationMap, ...) // 存储映射对象 endif set_dict_tuple (..., MatchingPlaneRectifiedPartInCamPose, ...)obtain_3d_pose_of_match_moving_cam函数解析核心功能是根据 2D 形状匹配结果Row, Column, Angle和系统标定数据计算物体模型在机器人基坐标系中的 3D 位姿ModelInBasePose。这是Eye-in-Hand相机装在机械臂上场景下实现6D 物体定位的关键步骤。下面逐段详细解析。一、输入数据读取read_dict_tuple (HandEyeCalibData, CamParam, CamParam) read_dict_tuple (HandEyeCalibData, ToolInCamPose, ToolInCamPose) read_dict_tuple (Poses, PlaneInModelPose, PlaneInModelPose) read_dict_tuple (RectificationData, RectifyImage, RectifyImage) if (RectifyImage ! no_rectification) read_dict_tuple (RectificationData, ScaleRectification, ScaleRectification) endif read_dict_tuple (RectificationData, MatchingPlaneRectifiedPartInCamPose, MatchingPlaneRectifiedPartInCamPose)关键输入说明变量含义CamParam相机内参含畸变ToolInCamPose工具末端在相机坐标系中的位姿来自手眼标定PlaneInModelPose匹配平面在物体模型坐标系中的位姿通常为已知如模型底面RectifyImage是否启用图像校正no_rectification, only_rectify, align_and_rectifyScaleRectification校正图像的物理尺度米/像素MatchingPlaneRectifiedPartInCamPose用于校正的匹配平面在相机坐标系中的位姿由前序流程生成 注意MatchingPlaneRectifiedPartInCamPose是一个“对齐且可能平移过原点”的平面位姿专用于校正。二、统一所有位姿表示格式get_pose_type (ToolInBasePose, OrderOfTransform, OrderOfRotation, ViewOfTransform) convert_pose_type (..., RpT, gba, point, ...)获取当前机器人使用的位姿格式用于最后输出时还原将所有内部位姿转换为统一格式RpT旋转 平移gbaZYX 欧拉角gamma-beta-alphapoint位姿描述的是一个点非方向或约定三、核心逻辑从 2D 匹配 → 3D 位姿前提仅处理单个匹配结果if (|Row| 1) ... else throw (Exactly one match should be given as input) endif步骤 1构建 2D 刚性变换矩阵vector_angle_to_rigid (0, 0, 0, Row, Column, Angle, HomMat2DObject)将匹配结果(Row, Column, Angle)转换为 2D 齐次变换矩阵HomMat2DObject表示从模型坐标系原点在模型中心到图像像素坐标的变换注意 HALCON 中Col x, Row y四、分两种情况处理是否图像校正✅ 情况 ARectifyImage no_rectification无校正affine_trans_pixel (HomMat2DObject, 0, 0, RowObject, ColObject) image_points_to_world_plane (CamParam, MatchingPlaneRectifiedPartInCamPose, RowObject, ColObject, m, PXM, PYM)将模型原点(0,0)变换到图像像素(ColObject, RowObject)再通过image_points_to_world_plane将该像素点反投影到匹配平面得到物理坐标(PXM, PYM)HomMat3DObject : [ ... ] // 构造 3x4 齐次矩阵 hom_mat3d_to_pose (HomMat3DObject, ModelToMatchInPlanePose)手动构造 3D 变换矩阵Z0因为假设物体在平面上转为位姿ModelToMatchInPlanePose模型原点在匹配平面坐标系中的位姿pose_compose (ModelToMatchInPlanePose, PlaneInModelPose, ModelInPlanePose) pose_compose (MatchingPlaneRectifiedPartInCamPose, ModelInPlanePose, ModelInCamPose)PlaneInModelPose是平面在模型坐标系中的位姿其逆即为模型在平面坐标系中的位姿但此处直接compose(ModelToMatchInPlanePose, PlaneInModelPose)实际等价于ModelInPlane ModelToMatchInPlane ∘ PlaneInModelModelInPlane ModelToMatchInPlane ∘ PlaneInModel⚠️ 这里逻辑需注意通常应为inv(PlaneInModelPose)但若PlaneInModelPose定义为“从模型到平面”则正确。最终得到ModelInCamPose模型在相机坐标系中的位姿✅ 情况 BRectifyImage only_rectify or align_and_rectify有校正HomMat3DObject : [ HomMat2DObject[4], HomMat2DObject[3], 0, HomMat2DObject[5] * ScaleRectification, HomMat2DObject[1], HomMat2DObject[0], 0, HomMat2DObject[2] * ScaleRectification, 0, 0, 1, 0 ]关键区别在校正图像中像素已直接对应物理坐标乘以 ScaleRectificationHomMat2DObject[5]和[2]是平移项x, y乘以ScaleRectification米/像素得到物理平移米旋转部分直接填入因校正后无透视2D 旋转 ≈ 3D 绕Z旋转hom_mat3d_to_pose (..., ModelToMatchInPlanePartRectPose) pose_compose (..., PlaneInModelPose, ModelInMatchingPlaneRectifiedPartPose) pose_compose (MatchingPlaneRectifiedPartInCamPose, ..., ModelInCamPose)同样组合得到ModelInCamPose✅ 优势避免调用image_points_to_world_plane计算更快因校正图已是度量空间五、将模型位姿转换到机器人基坐标系pose_invert (ToolInBasePose, BaseInToolPose) pose_compose (ToolInCamPose, BaseInToolPose, BaseInCamPose) pose_invert (BaseInCamPose, CamInBasePose) pose_compose (CamInBasePose, ModelInCamPose, ModelInBasePose)推导链ToolInBasePose工具在基坐标系BaseInToolPose inv(ToolInBasePose)BaseInCamPose ToolInCamPose ∘ BaseInToolPose→ 基坐标系在相机中的位姿CamInBasePose inv(BaseInCamPose)→ 相机在基坐标系中的位姿ModelInBasePose CamInBasePose ∘ ModelInCamPose→最终输出模型在基坐标系中的位姿 这就是机器人控制器需要的抓取目标位姿 六、恢复原始位姿格式并返回convert_pose_type (ModelInBasePose, OrderOfTransform, OrderOfRotation, ViewOfTransform, ModelInBasePose)将内部计算的RpT, gba格式转回机器人实际使用的格式如RpT, xyz或TRp等确保与机器人通信兼容总结函数输入/输出输入说明Row, Column, Angle2D 形状匹配结果单个ToolInBasePose拍摄图像时工具在基坐标系中的位姿HandEyeCalibData手眼标定结果ToolInCamPoseCamParamRectificationData图像校正参数Poses.PlaneInModelPose匹配平面在模型坐标系中的定义