autoware.universe源码略读(3.21)--perception:shape_estimation

什么都不会的小澎友2024-12-01 14:36

autoware.universe源码略读3.21--perception:shape_estimation

Overview

刚看完一个简单的模块，立马又来一个如此复杂的模块。。这个模块的目的总的来说就是就是细化形状的 ，里面涉及到了几个部分，一个是边界框生成bounding box ，这部分用的是L-shape fitting ，算法来自文章Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners， 然后第二部分是圆柱 cylinder ，这部分用的是OpenCV的cv::minEnclosingCircle，也就是最小外接圆 ；第三部分则是凸包了，不过这里用的是OpenCV里的凸包类cv::convexHull。

这里再简单看下这个模块的代码构造，里面是分成了corrector、filter以及model 三个文件夹，看起来model 模块应该是L-shape拟合这种方法的位置，然后corrector 应该是利用算法结果进行修正？filter应该是进行过滤？接下来还是每个部分来单独看一下

model

（Class）ShapeEstimationModelInterface

这个是整体模型接口的一个基类，可以看到里面会被不同模型进行重写的虚函数只有estimate，里面的参数包括

cpp 复制代码 
const pcl::PointCloud<pcl::PointXYZ> & cluster				// 经过聚类后的点云
autoware_auto_perception_msgs::msg::Shape & shape_output	// 输出的形状
geometry_msgs::msg::Pose & pose_output) = 0;				// 输出的位置

（Class）BoundingBoxShapeModel

根据L-shape方法来进行形状拟合（针对boundingbox）

（Class Constructor）BoundingBoxShapeModel::BoundingBoxShapeModel

构造函数非常简单，第一种是没有输入参数，这个时候就默认成员变量；不然的话就对对应的成员变量赋值

（mFunc）BoundingBoxShapeModel::estimate

这个相当于也是个对外的接口，这个里面先对min_angle和max_angle进行了设定，这个主要是为了后边指定搜索范围的

cpp 复制代码 
if (ref_yaw_info_) {
  min_angle = ref_yaw_info_.get().yaw - ref_yaw_info_.get().search_angle_range;
  max_angle = ref_yaw_info_.get().yaw + ref_yaw_info_.get().search_angle_range;
} else {
  min_angle = 0.0;
  max_angle = M_PI * 0.5;
}

之后直接调用fitLShape

cpp 复制代码 
return fitLShape(cluster, min_angle, max_angle, shape_output, pose_output);

（mFunc）BoundingBoxShapeModel::fitLShape

进行形状拟合的主要函数，这里面用到了之前说的那篇文章的方法

cpp 复制代码 
/*
 * Paper : IV2017, Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners
 * Authors : Xio Zhang, Wenda Xu, Chiyu Dong and John M. Dolan
 */
// Paper : Algo.2 Search-Based Rectangle Fitting

可以看到里面的优化是分成了两种，一种是用到了boost库进行优化，另一种是没有用到

cpp 复制代码 
if (use_boost_bbox_optimizer_) {
  theta_star = boostOptimize(cluster, min_angle, max_angle);
} else {
  theta_star = optimize(cluster, min_angle, max_angle);
}

接下来就是算法的主要步骤了，这个地方的流程可以见原论文

这里具体计算的步骤就略过了，直接看下输出吧

cpp 复制代码 
// output
shape_output.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
shape_output.dimensions.x = std::fabs(e_x.dot(diagonal_vec));
shape_output.dimensions.y = std::fabs(e_y.dot(diagonal_vec));
shape_output.dimensions.z = std::max((max_z - min_z), epsilon);
pose_output.position.x = (intersection_x_1 + intersection_x_2) * 0.5;
pose_output.position.y = (intersection_y_1 + intersection_y_2) * 0.5;
pose_output.position.z = min_z + shape_output.dimensions.z * 0.5;
pose_output.orientation = tf2::toMsg(quat);
// check wrong output
shape_output.dimensions.x = std::max(static_cast<float>(shape_output.dimensions.x), epsilon);
shape_output.dimensions.y = std::max(static_cast<float>(shape_output.dimensions.y), epsilon);

（mFunc）BoundingBoxShapeModel::calcClosenessCriterion

这里指的应该是点与边的接近度最大化 ，对应在文章中的Algo.4 Closeness Criterion ，这里具体的原理先没仔细看，代码就是对应下面这个流程一步一步执行下来了，大致的流程可以参考这位大佬的文章论文解读--Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners，能对这个有个大概的了解

（mFunc）BoundingBoxShapeModel::optimize

在不使用boost 库的情况下的优化过程，这里的优化过程相当于执行Algo. 2中的循环过程，也就是里面的步骤3到步骤8，当然里面还有一个步骤10，就是把最大的值找到并提取出来

cpp 复制代码 
std::vector<std::pair<float /*theta*/, float /*q*/>> Q;
constexpr float angle_resolution = M_PI / 180.0;
for (float theta = min_angle; theta <= max_angle + epsilon; theta += angle_resolution) {
  Eigen::Vector2f e_1;
  e_1 << std::cos(theta), std::sin(theta);  // col.3, Algo.2
  Eigen::Vector2f e_2;
  e_2 << -std::sin(theta), std::cos(theta);  // col.4, Algo.2
  std::vector<float> C_1;                    // col.5, Algo.2
  std::vector<float> C_2;                    // col.6, Algo.2
  for (const auto & point : cluster) {
    C_1.push_back(point.x * e_1.x() + point.y * e_1.y());
    C_2.push_back(point.x * e_2.x() + point.y * e_2.y());
  }
  float q = calcClosenessCriterion(C_1, C_2);  // col.7, Algo.2
  Q.push_back(std::make_pair(theta, q));       // col.8, Algo.2
}
float theta_star{0.0};  // col.10, Algo.2
float max_q = 0.0;
for (size_t i = 0; i < Q.size(); ++i) {
  if (max_q < Q.at(i).second || i == 0) {
    max_q = Q.at(i).second;
    theta_star = Q.at(i).first;
  }
}
return theta_star;

（mFunc）BoundingBoxShapeModel::boostOptimize

这里是使用boost库的优化过程，其实原理是一样的，不过我们可以看到因为在里面使用到了boost 库的函数boost::math::tools::brent_find_minima，也就是找到最小值，所以这里在上边存值的时候都是取负值的

cpp 复制代码 
float q = calcClosenessCriterion(C_1, C_2);
return -q;

然后直接用boost库的函数找到最小值，也就是取负前的最大值了

cpp 复制代码 
std::pair<float, float> min = boost::math::tools::brent_find_minima(
  closeness_func, min_angle, max_angle + epsilon, bits, max_iter);

convex_hull

这个应该是涉及到计算目标对象的凸包的，其实之前在common里是有写过自己实现的凸包类的，不过这里看起来依旧是没有用到

（mFunc）ConvexHullShapeModel::estimate

这里只重写了estimate这一个函数，首先是对所有的点进行了一个去中心化的过程，中心点就是单纯对各个方向取平均值算的

cpp 复制代码 
for (size_t i = 0; i < cluster.size(); ++i) {
  v_pointcloud.push_back(
    cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.0));
}

然后直接调用cv::convexHull

cpp 复制代码 
cv::convexHull(v_pointcloud, v_polygon_points);

然后再把点返处理回去

cpp 复制代码 
pcl::PointXYZ polygon_centroid;
polygon_centroid.x = 0;
polygon_centroid.y = 0;
for (size_t i = 0; i < v_polygon_points.size(); ++i) {
  polygon_centroid.x += static_cast<double>(v_polygon_points.at(i).x) / 1000.0;
  polygon_centroid.y += static_cast<double>(v_polygon_points.at(i).y) / 1000.0;
}
polygon_centroid.x = polygon_centroid.x / static_cast<double>(v_polygon_points.size());
polygon_centroid.y = polygon_centroid.y / static_cast<double>(v_polygon_points.size());
for (size_t i = 0; i < v_polygon_points.size(); ++i) {
  geometry_msgs::msg::Point32 point;
  point.x = static_cast<double>(v_polygon_points.at(i).x) / 1000.0 - polygon_centroid.x;
  point.y = static_cast<double>(v_polygon_points.at(i).y) / 1000.0 - polygon_centroid.y;
  point.z = 0.0;
  shape_output.footprint.points.push_back(point);
}

最后也是直接输出

cpp 复制代码 
constexpr float ep = 0.001;
shape_output.type = autoware_auto_perception_msgs::msg::Shape::POLYGON;
shape_output.dimensions.x = 0.0;
shape_output.dimensions.y = 0.0;
shape_output.dimensions.z = std::max((max_z - min_z), ep);
pose_output.position.x = centroid.x + polygon_centroid.x;
pose_output.position.y = centroid.y + polygon_centroid.y;
pose_output.position.z = min_z + shape_output.dimensions.z * 0.5;
pose_output.orientation.x = 0;
pose_output.orientation.y = 0;
pose_output.orientation.z = 0;
pose_output.orientation.w = 1;

cylinder

这里说是圆柱体，但实际的逻辑是找到一个最小圆，然后给一个Z轴就可以了

（mFunc）CylinderShapeModel::estimate

这里也是只重写了estimate一个函数，这里相当于也是直接调用了OpenCV的函数

cpp 复制代码 
cv::minEnclosingCircle(cv::Mat(cv_points).reshape(2), center, radius);

然后也是输出设置就好了

cpp 复制代码 
shape_output.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER
shape_output.dimensions.x = static_cast<float>(radius) * 2.0;
shape_output.dimensions.y = static_cast<float>(radius) * 2.0;
shape_output.dimensions.z = std::max((max_z - min_z), ep);
pose_output.position.x = center.x;
pose_output.position.y = center.y;
pose_output.position.z = min_z + shape_output.dimensions.z * 0.5;
pose_output.orientation.x = 0;
pose_output.orientation.y = 0;
pose_output.orientation.z = 0;
pose_output.orientation.w = 1;

filter

这里的滤波，也是针对不同的载体模型进行了区分的，里面分成了car、bus、truck以及不进行滤波的no_filter

（Func）filterVehicleBoundingBox

在utils 文件中单独定义了这样一个辅助函数，可以看到输入的有形状信息shape以及设置的阈值，其实这里可以看到，因为正常的一个BOUNDING_BOX 是没有指定长宽方向的，所以一边指定了最大值，所以相当于是四选三了（min_width、max_width和max_length），然后还对面积进行了一下限制

（Class）ShapeEstimationFilterInterface

和刚才的形状拟合部分相同，这里也是单纯的定义了一个基类，之后不同的形状拟合就可以继承自这个基类，里面需要重写的函数也是只有一个

cpp 复制代码 
virtual bool filter(
  const autoware_auto_perception_msgs::msg::Shape & shape,
  const geometry_msgs::msg::Pose & pose) = 0;

（Class）NoFilter

如果不需要执行滤波操作的话，就直接在filter函数返回true就好了

（Class）BusFilter、CarFilter和TruckFilter

所以这里其实可以看到，不同的对象的滤波函数，只是为调用utils::filterVehicleBoundingBox函数的时候输入了不同的阈值。。所以其实也没什么好说的了

corrector

看起来会对形状进行一个修正？里面似乎也是针对不同的车辆定义了不同的子类

utils

（Struct）CorrectionBBParameters

这个结构体是进行形状修正的时候需要设置的参数，里面其实就是长宽的各种值（最大最小和默认）

cpp 复制代码 
double min_width;       // minimum width of bounding box
double max_width;
double default_width;
double min_length;      // minimum length of bounding box
double max_length;
double default_length;  // default length of bounding box

（Func）correctWithDefaultValue

这个看起来是利用默认值对形状进行的修正，那么这里的修正具体指的是什么呢。

首先，是得到了一个变换矩阵，这里我的理解是形状的局部坐标系到世界坐标系的变换矩阵。

cpp 复制代码 
Eigen::Translation<double, 2> trans =
  Eigen::Translation<double, 2>(pose.position.x, pose.position.y);
Eigen::Rotation2Dd rotate(tf2::getYaw(pose.orientation));
Eigen::Affine2d affine_mat;
affine_mat = trans * rotate.toRotationMatrix();

接下来是找到了形状上的四个点，可以看注释标的那张图

cpp 复制代码 
/*
 *         ^ x
 *         |
 *        (0)
 * y       |
 * <--(1)--|--(3)--
 *         |
 *        (2)
 *         |
 */
std::vector<Eigen::Vector2d> v_point;
v_point.push_back(Eigen::Vector2d(shape.dimensions.x / 2.0, 0.0));
v_point.push_back(Eigen::Vector2d(0.0, shape.dimensions.y / 2.0));
v_point.push_back(Eigen::Vector2d(-shape.dimensions.x / 2.0, 0.0));
v_point.push_back(Eigen::Vector2d(0.0, -shape.dimensions.y / 2.0));

然后依次找到了距离原点最远、第二远和第三远的点（一共四个点。。。为啥不直接对距离排序呢）

cpp 复制代码 
// most distant index from base link
size_t first_most_distant_index = 0;
{
  double distance = 0.0;
  for (size_t i = 0; i < v_point.size(); ++i) {
    if (distance < (affine_mat * v_point.at(i)).norm()) {
      distance = (affine_mat * v_point.at(i)).norm();
      first_most_distant_index = i;
    }
  }
}
// second distant index from base link
size_t second_most_distant_index = 0;
{
  double distance = 0.0;
  for (size_t i = 0; i < v_point.size(); ++i) {
    if (distance < (affine_mat * v_point.at(i)).norm() && i != first_most_distant_index) {
      distance = (affine_mat * v_point.at(i)).norm();
      second_most_distant_index = i;
    }
  }
}
// third distant index from base link
size_t third_most_distant_index = 0;
{
  double distance = 0.0;
  for (size_t i = 0; i < v_point.size(); ++i) {
    if (
      (distance < (affine_mat * v_point.at(i)).norm()) && i != first_most_distant_index &&
      i != second_most_distant_index) {
      distance = (affine_mat * v_point.at(i)).norm();
      third_most_distant_index = i;
    }
  }
}

接下来就是执行矫正了，这里关于矫正的具体步骤很复杂，又挺难懂 ，看了半天下来，感觉就是先确定一个主方向，然后另一边直接找第三远的点，最终是使得shape的主要方向（长边）与预期的一致，代码其实就是对几个点的各种情况进行处理。。这里就不做记录，但根据最后的输出，可以看到确实是确保长边设置成x了

cpp 复制代码 
shape.dimensions.x += std::abs(correction_vector.x()) * 2.0;
shape.dimensions.y += std::abs(correction_vector.y()) * 2.0;
pose.position.x += (rotate.toRotationMatrix() * correction_vector).x();
pose.position.y += (rotate.toRotationMatrix() * correction_vector).y();
// correct to set long length is x, short length is y
if (shape.dimensions.x < shape.dimensions.y) {
  geometry_msgs::msg::Vector3 rpy = tier4_autoware_utils::getRPY(pose.orientation);
  rpy.z = rpy.z + M_PI_2;
  pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(rpy.x, rpy.y, rpy.z);
  double temp = shape.dimensions.x;
  shape.dimensions.x = shape.dimensions.y;
  shape.dimensions.y = temp;
}

（mFunc）correctWithReferenceYaw

关于这种修正方法，可以参考代码里的注释图

cpp 复制代码 
/*
  c1 is nearest point and other points are arranged like below
  c is center of bounding box
  width
  4---2
  |   |
  | c |length
  |   |
  3---1
 */

不过这里说是WithReferenceYaw ，但代码里又没看出来怎么个WithReferenceYaw ，这里感觉是先找到基向量，然后再根据长和宽生成四个顶点，这四个顶点就是修正完的了，所以WithReferenceYaw可能是体现在这个基向量的生成过程吧

cpp 复制代码 
Eigen::Vector3d c = Eigen::Vector3d::Zero();
Eigen::Vector3d local_c1 = base2obj_transform.inverse() * c1;
Eigen::Vector3d radiation_vec = c - local_c1;
double ex = radiation_vec.x();
double ey = radiation_vec.y();
Eigen::Vector3d e1 = (Eigen::Vector3d(0, -ey, 0) - local_c1).normalized();
Eigen::Vector3d e2 = (Eigen::Vector3d(-ex, 0, 0) - local_c1).normalized();

（mFunc）correctWithReferenceYawAndShapeSize

这个和上边其实是一样的，不过相比上边的形式还会用到输入的形状大小信息ReferenceShapeSizeInfo & ref_shape_size_info

cpp 复制代码 
if (
  ref_shape_size_info.mode == ReferenceShapeSizeInfo::Mode::Min &&
  ref_shape_size_info.shape.dimensions.x < shape.dimensions.x) {
  length = shape.dimensions.x;
} else {
  length = ref_shape_size_info.shape.dimensions.x;
}
double width;
if (
  ref_shape_size_info.mode == ReferenceShapeSizeInfo::Mode::Min &&
  ref_shape_size_info.shape.dimensions.y < shape.dimensions.y) {
  width = shape.dimensions.y;
} else {
  width = ref_shape_size_info.shape.dimensions.y;
}

（Class）corrector_interface

和之前的形状拟合以及滤波一样，这里也是先简单定义了一个基类，这样不同的模型继承生成不同的子类就好了，这里需要继承的虚函数是矫正函数correct

cpp 复制代码 
virtual bool correct(
  autoware_auto_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) = 0;

（Class）VehicleCorrector

这里有所不同的是，针对几种车辆模型，定义了一个中间等级的VehicleCorrector 类，这里重写了correct函数，其实就是根据设置直接调用在utils里定义的几种不同的修正函数

cpp 复制代码 
if (!params_) return false;
if (use_reference_yaw_)
  return corrector_utils::correctWithReferenceYaw(params_.get(), shape, pose);
else
  return correctWithDefaultValue(params_.get(), shape, pose);

（Class）BusCorrector、CarCorrector和TruckCorrector

针对不同的车辆模型，也是单纯在参数设置的时候不太一样，别的也没什么不同的

（Class）NoCorrector

不进行修正，直接返回

（Class）ReferenceShapeBasedVehicleCorrector

前面可以看到，车辆的修正模型只调用了两种形式，这里就是针对有形状大小信息的情况，单独定义了一个子类，专门调用第三种修正函数的形式

（Class）ShapeEstimator

（Class Constructor）ShapeEstimator::ShapeEstimator

设置一下相关的参数到成员变量

Name Type Default Value Description
use_corrector bool true The flag to apply rule-based corrector
use_filter bool true The flag to apply rule-based filter
use_vehicle_reference_yaw bool true The flag to use vehicle reference yaw for corrector

（mFunc）ShapeEstimator::estimateShapeAndPose

这个是一个总接口，里面会调用类中一些其他的成员函数，来完成整个estimateShapeAndPose 的流程

首先就是估计原始的结果

cpp 复制代码 
if (!estimateOriginalShapeAndPose(label, cluster, ref_yaw_info, shape, pose)) {
  return false;
}

然后分别执行滤波和修正

cpp 复制代码 
// rule based filter
if (use_filter_) {
  if (!applyFilter(label, shape, pose)) {
    return false;
  }
}
// rule based corrector
if (use_corrector_) {
  bool use_reference_yaw = ref_yaw_info ? true : false;
  if (!applyCorrector(label, use_reference_yaw, ref_shape_size_info, shape, pose)) {
    return false;
  }
}

（mFunc）ShapeEstimator::estimateOriginalShapeAndPose

这里就是根据输入的标签类型，调用对应的模型进行形状拟合了，简单来说分成了车、行人和位置物体三种

cpp 复制代码 
if (
  label == Label::CAR || label == Label::TRUCK || label == Label::BUS ||
  label == Label::TRAILER || label == Label::MOTORCYCLE || label == Label::BICYCLE) {
  model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_));
} else if (label == Label::PEDESTRIAN) {
  model_ptr.reset(new CylinderShapeModel());
} else {
  model_ptr.reset(new ConvexHullShapeModel());
}
return model_ptr->estimate(cluster, shape_output, pose_output);

（mFunc）ShapeEstimator::applyFilter

执行滤波，只针对CAR\BUS和TRUCK

（mFunc）ShapeEstimator::applyCorrector

执行修正，也是分成几种形式分别实例化对应的类就行

node

这个节点类还是挺简单的，构造函数也是加载了一些参数，然后这里实例化了ShapeEstimation类

cpp 复制代码 
estimator_ =
  std::make_unique<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);

至于回调函数里，就调用在ShapeEstimation类中定义的接口函数就可以了

cpp 复制代码 
const bool estimated_success = estimator_->estimateShapeAndPose(
  label, *cluster, ref_yaw_info, ref_shape_size_info, shape, pose);
相关推荐
就叫飞六吧43 分钟前
51 单片机和 STM32 引脚命名对照表与解析
c++·stm32·单片机·嵌入式硬件·51单片机
霜雪殇璃1 小时前
c++对结构体的扩充以及类的介绍
开发语言·c++·笔记·学习
冉佳驹1 小时前
C++ ——— 匿名对象
c++·学习·类和对象·匿名对象
誓约酱2 小时前
git的基本使用
linux·运维·服务器·c++·git·后端
范纹杉想快点毕业2 小时前
XML通过HTTP POST 请求发送到指定的 API 地址，进行数据回传
xml·c语言·开发语言·数据结构·c++·python·c#
滴滴哒哒答答2 小时前
《自动驾驶与机器人中的SLAM技术》ch2：基础数学知识
机器学习·机器人·自动驾驶
float_六七2 小时前
C/C++头文件locale
c语言·开发语言·c++
誓约酱2 小时前
Linux下字符设备驱动编写(RK3568)
linux·运维·服务器·c语言·c++·嵌入式硬件·物联网
月亮有痕迹诶3 小时前
找到一个或多个多重定义的符号的问题
c++
Growthofnotes3 小时前
C++—9、如何在Microsoft Visual Studio中调试C++
c++·microsoft·visual studio
热门推荐
01图像分割：Pytorch实现UNet++进行医学细胞分割02半导体应用系统一些小知识收集(strip&wafer mapping,EAP&scada)03HCIA-datacom数通题库和录播视频资料04ESP-IDF学习记录（4） ESP-IDF examples目录05渗透测试之SQLMAP工具详解 kali自带SQLmap解释 重点sqlmap --tamper 使用方式详解 搞完你就很nice了06优化手机性能，解决卡顿问题：关闭这3个微信开关，释放内存空间07如何在 Ubuntu 22.04 上安装并开始使用 RabbitMQ08新手学习VR全景需要知道的几个问题09校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站10密码学原理技术-第六章-introduction to pulibc-key cryptography