rviz是如何获取图像里选择的点云的3D坐标的

以前以为rviz是用OpenGL渲染绘图,那么获取图像里像素点对应的真实3D坐标是采用的OpenGL里提供的API实现的,结果一看代码还真不是这样,rviz也就渲染用了OpenGL,其他都是自己实现的,图像界面的实现完全是遵循MVC设计模式自己实现的透视投影和坐标转换等所有相关类。获取点云图像里所选择的点云点的3D坐标相关的代码是这里:

复制代码
src/rviz/selection/selection_manager.cpp:

bool SelectionManager::getPatchDepthImage(Ogre::Viewport* viewport,
                                          int x,
                                          int y,
                                          unsigned width,
                                          unsigned height,
                                          std::vector<float>& depth_vector)
{
  unsigned int num_pixels = width * height;
  depth_vector.reserve(num_pixels);

  setDepthTextureSize(width, height);


  M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
  M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();

  for (; handler_it != handler_end; ++handler_it)
  {
    handler_it->second->preRenderPass(0);
  }

  if (render(viewport, depth_render_texture_, x, y, x + width, y + height, depth_pixel_box_, "Depth",
             depth_texture_width_, depth_texture_height_))
  {
    uint8_t* data_ptr = (uint8_t*)depth_pixel_box_.data;

    for (uint32_t pixel = 0; pixel < num_pixels; ++pixel)
    {
      uint8_t a = data_ptr[4 * pixel];
      uint8_t b = data_ptr[4 * pixel + 1];
      uint8_t c = data_ptr[4 * pixel + 2];

      int int_depth = (c << 16) | (b << 8) | a;
      float normalized_depth = ((float)int_depth) / (float)0xffffff;
      depth_vector.push_back(normalized_depth * camera_->getFarClipDistance());
    }
  }
  else
  {
    ROS_WARN("Failed to render depth patch\n");
    return false;
  }

  handler_it = objects_.begin();
  handler_end = objects_.end();
  for (; handler_it != handler_end; ++handler_it)
  {
    handler_it->second->postRenderPass(0);
  }

  return true;
}

bool SelectionManager::get3DPatch(Ogre::Viewport* viewport,
                                  int x,
                                  int y,
                                  unsigned width,
                                  unsigned height,
                                  bool skip_missing,
                                  std::vector<Ogre::Vector3>& result_points)
{
  boost::recursive_mutex::scoped_lock lock(global_mutex_);
  ROS_DEBUG("SelectionManager.get3DPatch()");

  std::vector<float> depth_vector;


  if (!getPatchDepthImage(viewport, x, y, width, height, depth_vector))
    return false;


  unsigned int pixel_counter = 0;
  Ogre::Matrix4 projection = camera_->getProjectionMatrix();
  float depth;

  for (unsigned y_iter = 0; y_iter < height; ++y_iter)
    for (unsigned x_iter = 0; x_iter < width; ++x_iter)
    {
      depth = depth_vector[pixel_counter];

      // Deal with missing or invalid points
      if ((depth > camera_->getFarClipDistance()) || (depth == 0))
      {
        ++pixel_counter;
        if (!skip_missing)
        {
          result_points.push_back(Ogre::Vector3(NAN, NAN, NAN));
        }
        continue;
      }


      Ogre::Vector3 result_point;
      // We want to shoot rays through the center of pixels, not the corners,
      // so add .5 pixels to the x and y coordinate to get to the center
      // instead of the top left of the pixel.
      Ogre::Real screenx = float(x_iter + .5) / float(width);
      Ogre::Real screeny = float(y_iter + .5) / float(height);
      if (projection[3][3] == 0.0) // If this is a perspective projection
      {
        // get world-space ray from camera & mouse coord
        Ogre::Ray vp_ray = camera_->getCameraToViewportRay(screenx, screeny);
        // transform ray direction back into camera coords
        Ogre::Vector3 dir_cam = camera_->getDerivedOrientation().Inverse() * vp_ray.getDirection();

        // normalize, so dir_cam.z == -depth
        dir_cam = dir_cam / dir_cam.z * depth * -1;

        // compute 3d point from camera origin and direction*/
        result_point = camera_->getDerivedPosition() + camera_->getDerivedOrientation() * dir_cam;
      }
      else // else this must be an orthographic projection.
      {
        // For orthographic projection, getCameraToViewportRay() does
        // the right thing for us, and the above math does not work.
        Ogre::Ray ray;
        camera_->getCameraToViewportRay(screenx, screeny, &ray);

        result_point = ray.getPoint(depth);
      }

      result_points.push_back(result_point);
      ++pixel_counter;
    }

  return !result_points.empty();
}

bool SelectionManager::get3DPoint(Ogre::Viewport* viewport, int x, int y, Ogre::Vector3& result_point)
{
  ROS_DEBUG("SelectionManager.get3DPoint()");

  std::vector<Ogre::Vector3> result_points_temp;
  bool success = get3DPatch(viewport, x, y, 1, 1, true, result_points_temp);
  if (result_points_temp.empty())
  {
    // return result_point unmodified if get point fails.
    return false;
  }
  result_point = result_points_temp[0];

  return success;
}

世界3D坐标是用的射线法计算出来。SelectionManager::get3DPoint()被rviz里多个地方调用,凡是UI界面上需要查看点的坐标地方都是调用它。

相关推荐
陈小峰_iefreer4 小时前
stone 3d v3.3.0版本发布,含时间线和连接器等新功能
3d·webgl·metaverse·cadcg
子燕若水21 小时前
“Daz to Unreal”将 G8 角色(包括表情)从 daz3d 导入到 UE5。在 UE5 中,我发现使用某个表情并与闭眼混合后,上眼睑出现了问题
3d·ue5
zhu_zhu_xia1 天前
JS通过GetCapabilities获取wms服务元数据信息并在SuperMap iClient3D for WebGL进行叠加显示
javascript·3d·webgl
星空寻流年1 天前
css3新特性第七章(3D变换)
前端·css·3d
在下胡三汉1 天前
Google Store 如何利用 glTF 3D 模型改变产品教育
3d
Hali_Botebie1 天前
【激光雷达3D(6)】3D点云目标检测方法;CenterPoint、PV-RCNN和M3DETR的骨干网络选择存在差异
网络·目标检测·3d
whuzhang162 天前
3DGS之齐次坐标
人工智能·3d·自动驾驶
90后小陈老师2 天前
WebXR教学 05 项目3 太空飞船小游戏
windows·3d·web·js
艾恩小灰灰3 天前
CSS中的`transform-style`属性:3D变换的秘密武器
前端·css·3d·css3·html5·web开发·transform-style
工业3D_大熊3 天前
HOOPS Exchange 与HOOPS Communicator集成:打造工业3D可视化新标杆!
3d·hoops·3d数据格式转换·3d模型可视化·bim技术数字解决方案·3d模型桌面可视化