从 (x,y) 像素坐标获取 3D 世界坐标

Question

我对 ROS/Gazebo 世界完全陌生；这可能是一个简单的问题，但我找不到好的答案。

我在 Gazebo 场景中有一个模拟深度相机 (Kinect)。经过一些阐述，我在像素坐标中的 RGB 图像中得到了一个兴趣点，我想在世界框架中检索它的 3D 坐标。 我不明白该怎么做。

我已尝试补偿 CameraInfo 消息给出的失真。我曾尝试将 PointCloud 与 pcl 库一起使用，将点检索为 cloud.at(x,y)。

在这两种情况下，坐标都不正确（我在程序给出的坐标中放了一个小球，所以检查它是否正确）。

每一个帮助将不胜感激。非常感谢。

编辑： 从 PointCloud 开始，我尝试找到点的坐标，如下所示：

point = cloud.at(xInPixel, yInPixel);
point.x = point.x + cameraPos.x;
point.y = point.y + cameraPos.y;
point.z = point.z - cameraPos.z;

但是我得到的 x,y,z 坐标 point.x 似乎不正确。 相机的俯仰角为 pi/2，因此对于地面上的点。 我显然错过了一些东西。

Answer 1

我假设您已经看过 kinect 的凉亭示例（brief、full）。作为主题，您可以获得原始图像、原始深度和计算的点云（通过在配置中设置它们）：

<imageTopicName>/camera/color/image_raw</imageTopicName>
<cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/camera/dept/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>

除非您需要使用 image_raw 为 rgb 和深度帧做自己的事情（例如 ML over rgb 帧并通过 camera_infos 找到相应的深度点），否则点云主题应该足够了 - 它与 c++ 中的 pcl 点云相同，如果您包含正确的标题。

编辑（作为回应）： ros中有一个神奇的东西叫做tf/tf2。如果您查看msg.header.frame_id，您的点云会显示类似"camera" 的内容，表明它在相机框架中。 tf，就像 ros 中的消息传递系统一样，发生在后台，但它会查找/侦听从一个参考帧到另一帧的转换。然后，您可以转换/查询另一帧中的数据。例如，如果摄像头安装在机器人的旋转位置，您可以在启动文件中指定静态转换。看起来您正在尝试自己进行转换，但您可以让tf 为您做；这使您可以轻松找出点在世界/地图框架中的位置，与机器人/base_link 框架或执行器/相机/等框架中的位置。

我还会查看这些 ros wiki 问题，这些问题演示了几种不同的方法，具体取决于您想要什么：ex1、ex2、ex3