object_builder

用来创建object。

目录

头文件功能解释

Options定义

object_builder.cc

Init

Build

private:

ComputePolygon2D

ComputePolygonSizeCenter

ComputeOtherObjectInformation​​​​​​​

SetDefaultValue

LinePerturbation

GetMinMax3D

---

头文件功能解释

#include "modules/perception/base/object.h"
#include "modules/perception/base/point.h"
#include "modules/perception/base/point_cloud.h"
#include "modules/perception/lib/registerer/registerer.h"
#include "modules/perception/lidar/common/lidar_frame.h"

Options定义

struct ObjectBuilderOptions {
  Eigen::Vector3d ref_center = Eigen::Vector3d(0, 0, 0);
};

object_builder.cc

文件开头定义3个常变量

static const float kEpsilon = 1e-6f;
static const float kEpsilonForSize = 1e-2f;
static const float kEpsilonForLine = 1e-3f;

Init

// @brief: initialization. Get orientation estimator instance.

Build

// @brief: calculate and fill object size, center, directions.

bool ObjectBuilder::Build(const ObjectBuilderOptions& options,
                          LidarFrame* frame) 

从frame中获取检测到的objects，遍历所有object，然后给object配置id，计算2D polygon，计算中点等。

for (size_t i = 0; i < objects->size(); ++i) {
    if (objects->at(i)) {
      objects->at(i)->id = static_cast<int>(i);
      ComputePolygon2D(objects->at(i));
      ComputePolygonSizeCenter(objects->at(i));
      ComputeOtherObjectInformation(objects->at(i));

private:

ComputePolygon2D

void ObjectBuilder::ComputePolygon2D(ObjectPtr object)

从object拿取属于这个object的点云，前面segment会有label分类。

首先计算点云坐标的最大最小值，GetMinMax3D;

如果点云数量少于4u，就会SetDefaultValue;

然后通过线扰动函数 LinePerturbation(&cloud);

最后GetConvexHull，计算凸包（百度百科）。

ComputePolygonSizeCenter

// @brief: calculate the size, center of polygon.

如果点太少，就不用管。然后计算bounding box Size 和 Center

  // @brief calculate the size and center of the bounding-box of a point cloud
  // old name: compute_bbox_size_center_xy
  template <typename PointCloudT>
  void CalculateBBoxSizeCenter2DXY(
      const PointCloudT &cloud, const Eigen::Vector3f &dir, Eigen::Vector3f *size,
      Eigen::Vector3d *center,
      float minimum_edge_length = std::numeric_limits<float>::epsilon()) 

ComputeOtherObjectInformation

// @brief: calculate and fill timestamp and anchor_point.

for (size_t i = 0; i < num_point; ++i) {
    timestamp += object->lidar_supplement.cloud.points_timestamp(i);
  }
  if (num_point > 0) {
    timestamp /= static_cast<double>(num_point);
  }
  object->latest_tracked_time = timestamp;

求解障碍物的latest tracked time，这个时间是所有点云时间相加后的平均值。

障碍物的anchor_point就是障碍物的center。

object->anchor_point = object->center;

SetDefaultValue

// @brief: calculate and fill default polygon value.

这样来计算center，感觉噪点造成的影响会很大，如果有一个点特别远，那这个center救会有较大偏移。

Eigen::Vector3f center((min_pt[0] + max_pt[0]) / 2,
                         (min_pt[1] + max_pt[1]) / 2,
                         (min_pt[2] + max_pt[2]) / 2);

构建ploygon，2D的四个顶点object->polygon[0][1][2][3],分别更新x，y，z

// polygon
  if (object->lidar_supplement.cloud.size() < 4) {
    object->polygon.resize(4);
    object->polygon[0].x = static_cast<double>(min_pt[0]);
    object->polygon[0].y = static_cast<double>(min_pt[1]);
    object->polygon[0].z = static_cast<double>(min_pt[2]);

    object->polygon[1].x = static_cast<double>(max_pt[0]);
    object->polygon[1].y = static_cast<double>(min_pt[1]);
    object->polygon[1].z = static_cast<double>(min_pt[2]);

    object->polygon[2].x = static_cast<double>(max_pt[0]);
    object->polygon[2].y = static_cast<double>(max_pt[1]);
    object->polygon[2].z = static_cast<double>(min_pt[2]);

    object->polygon[3].x = static_cast<double>(min_pt[0]);
    object->polygon[3].y = static_cast<double>(max_pt[1]);
    object->polygon[3].z = static_cast<double>(min_pt[2]);
  }

LinePerturbation

// @brief: decide whether input cloud is on the same line. if ture, add perturbation.

随机选择的两个点，然后求出两个点的x,y值之差。

获取第三个点和第i 个点做同样的计算，得到tdiff_x 和 tdiff_y, 然后遍历所有点

 for (idx = 2; idx < cloud->size(); ++idx) 

如果说所有点计算结束，可以判断这些点大致处于一条直线上，然后通过摄动法求解渐进解。

 原理以后补充:

        摄动法又称小参数展开法。利用摄动法求解方程的渐进解。百度百科

GetMinMax3D

// @brief: calculate 3D min max point

for (size_t i = 0; i < cloud.size(); ++i) {
    if (std::isnan(cloud[i].x) || std::isnan(cloud[i].y) ||
        std::isnan(cloud[i].z)) {
      continue;
    }
    (*min_pt)[0] = std::min((*min_pt)[0], cloud[i].x);
    (*max_pt)[0] = std::max((*max_pt)[0], cloud[i].x);
    (*min_pt)[1] = std::min((*min_pt)[1], cloud[i].y);
    (*max_pt)[1] = std::max((*max_pt)[1], cloud[i].y);
    (*min_pt)[2] = std::min((*min_pt)[2], cloud[i].z);
    (*max_pt)[2] = std::max((*max_pt)[2], cloud[i].z);
  }

遍历所有点云，求解出点云 x, y, z的最大最小值，然后存入max_pt, min_pt。