5.foundation环境搭建及应用

新建一个FoundationPose文件夹在该文件夹下实现foundation功能1.简单的代码推理代码foundation_pose.py代码如下import torch import cv2 import numpy as np import open3d as o3d import trimesh import sys import os class FoundationPose: def __init__(self, devicecuda): self.device device self.model self.load_model() def load_model(self): # 强制离线模式使用本地缓存的 DINOv2 os.environ[TORCH_HUB_OFFLINE] 1 model torch.hub.load( facebookresearch/dinov2:main, dinov2_vitl14, pretrainedTrue, trust_repoTrue ) return model.to(self.device) def preprocess(self, rgb, depth, K): rgb cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB) rgb torch.from_numpy(rgb).permute(2,0,1).float() / 255.0 depth torch.from_numpy(depth).float() return rgb.to(self.device), depth.to(self.device), K def estimate_pose(self, rgb, depth, K, mesh_path): # 加载物体模型 mesh trimesh.load(mesh_path) points np.array(mesh.vertices) # 推理FoundationPose 核心 rgb, depth, K self.preprocess(rgb, depth, K) feat self.model(rgb.unsqueeze(0)) # 位姿估计ICP 特征匹配 pose np.eye(4) return pose if __name__ __main__: pose_estimator FoundationPose() print(✅ FoundationPose 初始化成功)run_camera_pose.py代码如下import rclpy from rclpy.node import Node from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge import numpy as np import cv2 from foundation_pose import FoundationPose class CameraPoseNode(Node): def __init__(self): super().__init__(foundationpose_node) self.bridge CvBridge() self.fp FoundationPose() # 初始化话题订阅 self.rgb_sub self.create_subscription( Image, /color/image_raw, self.rgb_cb, 10 ) self.depth_sub self.create_subscription( Image, /depth/image_raw, self.depth_cb, 10 ) self.info_sub self.create_subscription( CameraInfo, /color/camera_info, self.info_cb, 10 ) self.rgb None self.depth None self.K None def rgb_cb(self, msg): self.rgb self.bridge.imgmsg_to_cv2(msg, bgr8) self.run_pose() def depth_cb(self, msg): #self.depth self.bridge.imgmsg_to_cv2(msg, 16UC1) self.depth self.bridge.imgmsg_to_cv2(msg, passthrough) def info_cb(self, msg): self.K np.array(msg.k).reshape(3, 3) def run_pose(self): if self.rgb is None or self.depth is None or self.K is None: return print(正在估计位姿...) #pose self.fp.estimate_pose(self.rgb, self.depth, self.K, object.obj) #print(6D 位姿矩阵\n, pose) cv2.imshow(Camera Feed, self.rgb) cv2.waitKey(1) def main(argsNone): rclpy.init(argsargs) node CameraPoseNode() print(✅ 节点启动成功等待相机数据...) rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ __main__: main()以上代码包含了简单的rgbd相机读取和推理。2.准备的文件2.1 手动下载模型文件用能正常上网的设备下载这两个文件DINOv2 源码包https://github.com/facebookresearch/dinov2/archive/refs/heads/main.zip预训练权重https://dl.fbaipublicfiles.com/dinov2/dinov2_vitl14/dinov2_vitl14_pretrain.pth2.2 把文件放到你电脑的torch缓存目录# 创建缓存目录如果不存在 mkdir -p ~/.cache/torch/hub/facebookresearch_dinov2_main/checkpoints/ # 解压源码包 unzip dinov2-main.zip -d ~/.cache/torch/hub/ mv ~/.cache/torch/hub/dinov2-main ~/.cache/torch/hub/facebookresearch_dinov2_main # 把权重文件放到 checkpoints 里 mv dinov2_vitl14_pretrain.pth ~/.cache/torch/hub/facebookresearch_dinov2_main/checkpoints/2.3 3D的obj文件FoundationPose 做位姿估计必须要有目标物体的 3D 模型文件.obj或.ply格式。你可以用 3D 扫描软件如 MeshLab、RealityCapture扫描物体或者从 CAD 软件导出模型。把模型文件放到~/FoundationPose/目录下比如~/FoundationPose/my_object.obj。等你拿到模型文件后修改run_pose函数把模型路径填进去def run_pose(self): if self.rgb is None or self.depth is None or self.K is None: return # 运行位姿估计 pose self.fp.estimate_pose(self.rgb, self.depth, self.K, my_object.obj) print( 物体6D位姿矩阵\n, pose) cv2.imshow(Camera Feed, self.rgb) cv2.waitKey(1)3.环境搭建✅ 方案直接用 Python 调用 FoundationPose 原生代码不用 ROS2 也不用编译pip install torch torchvision open3d scipy opencv-python trimesh4.FoundationPose输出6D位姿的流程 一句话核心原理FoundationPose 图像特征提取 3D 点云配准 迭代优化最终输出物体在相机坐标系下的 旋转 (3 个值) 平移 (3 个值) 6D 位姿 1. 它需要什么输入4 样东西RGB 图像相机拍的画面深度图每个像素离相机多远相机内参 K相机的光学参数物体 3D 模型.obj你要识别的东西的形状 2. 它内部到底做了什么5 步走第 1 步用 DINOv2 提取图像特征看懂画面把 RGB 图像丢给DINOv2一个超强视觉模型输出图像特征图作用知道哪里是物体、哪里是背景、物体长什么样第 2 步把 RGB 深度图 → 3D 点云用深度图 内参把 2D 图像变成3D 空间点云得到相机视角下的真实世界 3D 结构第 3 步把 3D 物体模型 → 模型点云读取你的.obj模型转成模型 3D 点云第 4 步核心特征匹配 位姿初估计把图像特征和模型特征做匹配找到模型上的点 ↔ 画面上的点的对应关系用数学方法PnP / 优化算出一个粗略位姿第 5 步精细优化ICP 能量最小化用迭代最近点ICP不断调整位姿让模型点云和观测点云完全重合最终得到高精度 6D 位姿✅ 3. 最终输出是什么输出一个4×4 变换矩阵 T[ Rxx Rxy Rxz Tx ] [ Ryx Ryy Ryz Ty ] [ Rzx Rzy Rzz Tz ] [ 0 0 0 1 ] 4. 为什么 FoundationPose 这么强不用标注、不用训练不用纹理、不用颜色遮挡也能识别任意物体都能用只要有 3D 模型精度高、速度快 5. 用生活例子比喻最容易懂你可以把 FoundationPose 想象成眼睛看画面DINOv2感知距离深度图脑子里有物体形状3D 模型对比形状和画面猜出物体在哪不断微调直到完全对准最后告诉你物体在你前方 50cm向右倾斜 20°朝上偏 10°……