雷达坐标系下的点转换为图像坐标系的思路

SEU-侯亮平

已于 2023-12-05 15:10:09 修改

阅读量206

点赞数

文章标签：计算机视觉 opencv 人工智能

于 2023-12-05 15:08:04 首次发布

本文链接：https://blog.csdn.net/qq_42373896/article/details/134808047

版权

cv::Point2i ProjectPoint(const cv::Point3f &in_point)
{
  auto u = int(in_point.x * fx_ / in_point.z + cx_);
  auto v = int(in_point.y * fy_ / in_point.z + cy_);

  return cv::Point2i(u, v);
}


cv::Rect ProjectDetectionToRect(const autoware_msgs::DetectedObject &in_detection)
{
  cv::Rect projected_box;

  Eigen::Vector3f pos;      //三维检测框的位置
  pos << in_detection.pose.position.x,
    in_detection.pose.position.y,
    in_detection.pose.position.z;

  Eigen::Quaternionf rot(in_detection.pose.orientation.w,   //四元数矩阵
                         in_detection.pose.orientation.x,
                         in_detection.pose.orientation.y,
                         in_detection.pose.orientation.z);

  std::vector<double> dims = {
    in_detection.dimensions.x,
    in_detection.dimensions.y,
    in_detection.dimensions.z
  };

  jsk_recognition_utils::Cube cube(pos, rot, dims);

  Eigen::Affine3f range_vision_tf;
  // tf::transformTFToEigen(camera_lidar_tf_, range_vision_tf);
  tf::transformTFToEigen(cameraTolidar_tf, range_vision_tf);
  jsk_recognition_utils::Vertices vertices = cube.transformVertices(range_vision_tf);

  std::vector<cv::Point> polygon;     //二维检测框点
  for (auto &vertex : vertices)
  {
    cv::Point p = ProjectPoint(cv::Point3f(vertex.x(), vertex.y(), vertex.z()));    //转换到图像坐标系
    polygon.push_back(p);
  }

  projected_box = cv::boundingRect(polygon);

  return projected_box;
}