pick_and_place_with_2d_matching_moving_cam.hdev *眼在手上 2D匹配，3D抓取【案例解析】

发布时间：2026/10/9 1:54:16

pick_and_place_with_2d_matching_moving_cam.hdev *眼在手上 2D匹配，3D抓取【案例解析】
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匹配平面在模型坐标系中的定义

相关新闻

Red Mad Robot（red_mad_robot）Робопрактика 前端测试任务：用 React 构建员工社交网络用时统计表格

Red Mad Robot（red_mad_robot）Робопрактика 前端测试任务：用 React 构建员工社交网络用时统计表格

教程 【免费下载链接】ru-test-assignments Тестовые задания для самостоятельного выполнения от разных it компаний 项目地址： https://gitcode.com/gh_mirrors/ru/ru-test-assignments 点击查看 …

2026/10/9 1:53:16 阅读更多 →
纯血鸿蒙适配实战：工业IDE ETest的驱动重构与性能验证

纯血鸿蒙适配实战：工业IDE ETest的驱动重构与性能验证

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/9 1:53:16 阅读更多 →
Agent Skills实战：从概念到落地，打造模块化AI智能体技能库

Agent Skills实战：从概念到落地，打造模块化AI智能体技能库

“skills”这个词单看很抽象，但在2025年下半年AI Agent的开发语境里，它已经成了和“MCP”并列的热门关键词。如果你关注过Claude最近的更新，一定见过Agent Skills这个新概念——它本质上是把一组针对特定任务的指令、脚本和上下文打包成一个可…

2026/10/9 1:53:16 阅读更多 →

最新新闻

rrdtool-1.4.7源码编译与监控实践：从依赖到出图避坑指南

rrdtool-1.4.7源码编译与监控实践：从依赖到出图避坑指南

简介：RRDTool 1.4.7 是一套面向时序数据存储与绘图的成熟开源工具，常作为 Smokeping、Cacti、MRTG 等监控系统的底层组件，解决网络流量、CPU 使用率、内存占用等性能数据的采集、归档与可视化问题，适合运维工程师、监控平台开发者…

2026/10/9 2:27:35 阅读更多 →
GitPuk 集成企业微信统一认证登录实战指南

GitPuk 集成企业微信统一认证登录实战指南

最近帮几位朋友和内部团队搭过几次 GitPuk 的企业微信登录，前后踩了不少坑，也把这些配置反复梳理了几遍。刚好有同事问到“统一认证登录到底怎么接”，索性把完整的实战过程整理出来，给准备做同样事情的人一个可以参考的路线。说实…

2026/10/9 2:27:35 阅读更多 →
Midway 拦截器（AOP 切面）机制全解析：@Aspect 装饰器、切面生命周期与执行顺序

Midway 拦截器（AOP 切面）机制全解析：@Aspect 装饰器、切面生命周期与执行顺序

后端微服务云原生 【免费下载链接】midway 🍔 A Node.js Serverless Framework for front-end/full-stack developers. Build the application for next decade. Works on AWS, Alibaba Cloud, Tencent Cloud and traditional VM/Container. Super easy integrate w…

2026/10/9 2:27:35 阅读更多 →
Arduino ESP32 故障排除快速指南：6 类常见问题的修复清单

Arduino ESP32 故障排除快速指南：6 类常见问题的修复清单

Arduino ESP32 故障排除快速指南：6 类常见问题的修复清单 【免费下载链接】arduino-esp32 Arduino core for the ESP32 family of SoCs 项目地址: https://gitcode.com/GitHub_Trending/ar/arduino-esp32 Arduino ESP32（arduino-esp32&#xff09…

2026/10/9 2:27:35 阅读更多 →
GameDevMind 客户端开发工具实战指南：从 IDE 选型、日志诊断到图集打包与热更新资源工具链

GameDevMind 客户端开发工具实战指南：从 IDE 选型、日志诊断到图集打包与热更新资源工具链

文档知识库教程游戏开发 【免费下载链接】GameDevMind 最全面的游戏开发技术图谱(Game Development Map)。帮助游戏开发者们在已知问题上节省时间，省出更多的精力投入到更有创造性的工作中去。 项目地址： https://gitcode.com/gonglei007/GameDevMind 点…

2026/10/9 2:27:34 阅读更多 →
VSCode Python开发环境配置：解释器、虚拟环境与调试

VSCode Python开发环境配置：解释器、虚拟环境与调试

简介：面向2024年初学Python的开发者，这份资料包以VSCode为落脚点，系统整理配置Python开发环境所需的各类文件与说明，帮助使用者跳过摸索环节，快速完成解释器选择、调试器启用与代码提示等基础设置。包体共152个文件&am…

2026/10/9 2:26:34 阅读更多 →

日新闻

Java时间API实战：LocalDate、Date与ZonedDateTime的转换与避坑指南

Java时间API实战：LocalDate、Date与ZonedDateTime的转换与避坑指南

Java时间API这个话题，隔三差五就会在群里被翻出来讨论一次。上周还有个同事线上处理一个订单超时问题，排查到最后发现是ZonedDateTime序列化后时区丢了，用户在下单当天晚上看到的时间整整差了8个小时。这类问题几乎每个做Java开发的人都遇到过…

2026/10/9 0:00:49 阅读更多 →
EasyTier实践：从NAT穿透到子网代理的异地组网部署与排错

EasyTier实践：从NAT穿透到子网代理的异地组网部署与排错

前几个月我手头有好几台机器需要互相访问：办公室台式机、家里 NAS、还有一台云主机。如果只是偶尔传个文件倒还好，问题是工作场景经常要在几处环境之间来回切换，每次都先登录跳板机再层层代理，实在折腾。我先后试过端口映射、自建…

2026/10/9 0:00:49 阅读更多 →
AI Agent工程实战：从七要素到七个决策点的系统设计指南

AI Agent工程实战：从七要素到七个决策点的系统设计指南

AI Agent 这个词在过去一年里被反复提及，但真正动手搭过一套能跑起来的 Agent 系统的人都知道，从"知道它是什么"到"让它稳定干活"之间隔着一整套工程决策。我前后参与过几个 Agent 项目的落地，从最初用现成框架拼装&…

2026/10/9 0:01:50 阅读更多 →

周新闻

KT148A语音芯片外挂8002D功放的工程实践指南

KT148A语音芯片外挂8002D功放的工程实践指南

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/8 15:26:32 阅读更多 →
LLC谐振变换器增益公式推导：从FHA等效到完整归一化表达式

LLC谐振变换器增益公式推导：从FHA等效到完整归一化表达式

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/8 15:26:40 阅读更多 →
ARM架构深度解析：从RISC设计理念到交叉编译实战

ARM架构深度解析：从RISC设计理念到交叉编译实战

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/8 10:10:36 阅读更多 →

月新闻

我发现了一个新思路：用 Remotion + Claude Code 像写代码一样自动化生成短视频

我发现了一个新思路：用 Remotion + Claude Code 像写代码一样自动化生成短视频

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/8 21:13:17 阅读更多 →
Windows下 Codex 中 Chrome 和 Computer Use 插件不可用问题排查及解决参考方式：TaoToken 统一 Key 配置与验证

Windows下 Codex 中 Chrome 和 Computer Use 插件不可用问题排查及解决参考方式：TaoToken 统一 Key 配置与验证

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/8 15:26:17 阅读更多 →
黑夜航拍船只数据集训练YOLOV5模型全流程解析

黑夜航拍船只数据集训练YOLOV5模型全流程解析

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */#content_views .toc,/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */#content_views.markdown_views > p:empty:has(+ .toc),#content_views.markdown_views …

2026/10/7 13:34:55 阅读更多 →