Apollo2.0自动驾驶之apollo/modules/prediction/common/geometry_util.h

/****************Apollo源码分析****************************

Copyright 2018 The File Authors & zouyu. All Rights Reserved.

Contact with: 1746430162@qq.com 181663309504

源码主要是c++实现的，也有少量python，git下载几百兆，其实代码不太多，主要是地图和数据了大量空间，主要程序
在apollo/modules目录中，

我们把它分成以下几部分（具体说明见各目录下的modules）：

感知：感知当前位置，速度，障碍物等等

Apollo/modules/perception

预测：对场景下一步的变化做出预测

Apollo/modules/prediction

规划：

(1) 全局路径规划：通过起点终点计算行驶路径

Apollo/modules/routing

(2) 规划当前轨道：通过感知，预测，路径规划等信息计算轨道

Apollo/modules/planning

(3) 规划转换成命令：将轨道转换成控制汽车的命令（加速，制动，转向等）

Apollo/modules/control

其它

(1) 输入输出

i. Apollo/modules/drivers 设备驱动

ii. Apollo/modules/localization 位置信息

iii. Apollo/modules/monitor 监控模块

iv. Apollo/modules/canbus 与汽车硬件交互

v. Apollo/modules/map 地图数据

vi. Apollo/modules/third_party_perception 三方感知器支持

(2) 交互

i. Apollo/modules/dreamview 可视化模块

ii. Apollo/modules/hmi 把汽车当前状态显示给用户

(3) 工具

i. Apollo/modules/calibration 标注工具

ii. Apollo/modules/common 支持其它模块的公共工具

iii. Apollo/modules/data 数据工具

iv. Apollo/modules/tools 一些Python工具

(4) 其它

i. Apollo/modules/elo 高精度定位系统，无源码，但有文档

ii. Apollo/modules/e2e 收集传感器数据给PX2,ROS

  自动驾驶系统先通过起点终点规划出整体路径（routing）；然后在行驶过程中感知（perception）当前环境
（识别车辆行人路况标志等），并预测下一步发展；然后把已知信息都传入规划模块（planning），规划出之后的轨道；
控制模块（control）将轨道数据转换成对车辆的控制信号，通过汽车交互模块（canbus）控制汽车．

我觉得这里面算法技术含量最高的是感知perception和规划planning，具体请见本博客中各模块的分析代码。

/****************************************************************************************

感知模块的几何构建：

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#ifndef MODULES_PERCEPTION_OBSTACLE_COMMON_GEOMETRY_UTIL_H_
#define MODULES_PERCEPTION_OBSTACLE_COMMON_GEOMETRY_UTIL_H_

#include <cfloat>

#include <algorithm>
#include <vector>

#include "Eigen/Core"

#include "modules/common/log.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"

namespace apollo {
namespace perception {

template <typename PointT>
void TransformPointCloud(const Eigen::Matrix4d& trans_mat,
                         pcl::PointCloud<PointT>* cloud_in_out) {
  for (std::size_t i = 0; i < cloud_in_out->size(); ++i) {
    PointT& p = cloud_in_out->at(i);
    Eigen::Vector4d v(p.x, p.y, p.z, 1);
    v = trans_mat * v;
    p.x = v.x();
    p.y = v.y();
    p.z = v.z();
  }
}

template <typename PointT>
void TransformPointCloud(const Eigen::Matrix4d& trans_mat,
                         typename pcl::PointCloud<PointT>::Ptr cloud_in_out) {
  CHECK_NOTNULL(cloud_in_out.get());
  return TransformPointCloud(trans_mat, cloud_in_out.get());
}

template <typename PointType>
void TransformPoint(const PointType& point_in, const Eigen::Matrix4d& trans_mat,
                    PointType* point_out) {
  Eigen::Vector4d v(point_in.x, point_in.y, point_in.z, 1);
  v = trans_mat * v;
  *point_out = point_in;
  point_out->x = v.x();
  point_out->y = v.y();
  point_out->z = v.z();
}

template <typename PointType>
void TransformPointCloud(const pcl::PointCloud<PointType>& cloud_in,
                         const Eigen::Matrix4d& trans_mat,
                         pcl::PointCloud<PointType>* cloud_out) {
  if (cloud_out->points.size() < cloud_in.points.size()) {
    cloud_out->points.resize(cloud_in.points.size());
  }
  for (std::size_t i = 0; i < cloud_in.size(); ++i) {
    const PointType& p = cloud_in.at(i);
    Eigen::Vector4d v(p.x, p.y, p.z, 1);
    v = trans_mat * v;
    PointType& pd = cloud_out->points[i];
    pd.x = v.x();
    pd.y = v.y();
    pd.z = v.z();
  }
}

void TransformPointCloud(pcl_util::PointCloudPtr cloud,
                         const std::vector<int>& indices,
                         pcl_util::PointDCloud* trans_cloud);

void TransformPointCloud(pcl_util::PointCloudPtr cloud,
                         const Eigen::Matrix4d& pose_velodyne,
                         typename pcl_util::PointDCloudPtr trans_cloud);
/*
 * Other point cloud related methods
 * */
template <typename PointT>
void GetCloudMinMax3D(typename pcl::PointCloud<PointT>::Ptr cloud,
                      Eigen::Vector4f* min_point, Eigen::Vector4f* max_point) {
  Eigen::Vector4f& min_pt = *min_point;
  Eigen::Vector4f& max_pt = *max_point;
  min_pt[0] = min_pt[1] = min_pt[2] = FLT_MAX;
  max_pt[0] = max_pt[1] = max_pt[2] = -FLT_MAX;
  if (cloud->is_dense) {
    for (size_t i = 0; i < cloud->points.size(); ++i) {
      min_pt[0] = std::min(min_pt[0], cloud->points[i].x);
      max_pt[0] = std::max(max_pt[0], cloud->points[i].x);
      min_pt[1] = std::min(min_pt[1], cloud->points[i].y);
      max_pt[1] = std::max(max_pt[1], cloud->points[i].y);
      min_pt[2] = std::min(min_pt[2], cloud->points[i].z);
      max_pt[2] = std::max(max_pt[2], cloud->points[i].z);
    }
  } else {
    for (size_t i = 0; i < cloud->points.size(); ++i) {
      if (!pcl_isfinite(cloud->points[i].x) ||
          !pcl_isfinite(cloud->points[i].y) ||
          !pcl_isfinite(cloud->points[i].z)) {
        continue;
      }
      min_pt[0] = std::min(min_pt[0], cloud->points[i].x);
      max_pt[0] = std::max(max_pt[0], cloud->points[i].x);
      min_pt[1] = std::min(min_pt[1], cloud->points[i].y);
      max_pt[1] = std::max(max_pt[1], cloud->points[i].y);
      min_pt[2] = std::min(min_pt[2], cloud->points[i].z);
      max_pt[2] = std::max(max_pt[2], cloud->points[i].z);
    }
  }
}

template <typename PointT>
void ComputeBboxSizeCenter(typename pcl::PointCloud<PointT>::Ptr cloud,
                           const Eigen::Vector3d& direction,
                           Eigen::Vector3d* size, Eigen::Vector3d* center) {
  Eigen::Vector3d dir(direction[0], direction[1], 0);
  dir.normalize();
  Eigen::Vector3d ortho_dir(-dir[1], dir[0], 0.0);

  Eigen::Vector3d z_dir(dir.cross(ortho_dir));
  Eigen::Vector3d min_pt(DBL_MAX, DBL_MAX, DBL_MAX);
  Eigen::Vector3d max_pt(-DBL_MAX, -DBL_MAX, -DBL_MAX);

  Eigen::Vector3d loc_pt;
  for (std::size_t i = 0; i < cloud->size(); i++) {
    Eigen::Vector3d pt = Eigen::Vector3d(cloud->points[i].x, cloud->points[i].y,
                                         cloud->points[i].z);
    loc_pt[0] = pt.dot(dir);
    loc_pt[1] = pt.dot(ortho_dir);
    loc_pt[2] = pt.dot(z_dir);
    for (int j = 0; j < 3; j++) {
      min_pt[j] = std::min(min_pt[j], loc_pt[j]);
      max_pt[j] = std::max(max_pt[j], loc_pt[j]);
    }
  }
  *size = max_pt - min_pt;
  *center = dir * ((max_pt[0] + min_pt[0]) * 0.5) +
            ortho_dir * ((max_pt[1] + min_pt[1]) * 0.5) + z_dir * min_pt[2];
}

template <typename PointT>
Eigen::Vector3d GetCloudBarycenter(
    typename pcl::PointCloud<PointT>::Ptr cloud) {
  int point_num = cloud->points.size();
  Eigen::Vector3d barycenter(0, 0, 0);

  for (int i = 0; i < point_num; i++) {
    const PointT& pt = cloud->points[i];
    barycenter[0] += pt.x;
    barycenter[1] += pt.y;
    barycenter[2] += pt.z;
  }

  if (point_num > 0) {
    barycenter[0] /= point_num;
    barycenter[1] /= point_num;
    barycenter[2] /= point_num;
  }
  return barycenter;
}

void TransAffineToMatrix4(const Eigen::Vector3d& translation,
                          const Eigen::Vector4d& rotation,
                          Eigen::Matrix4d* trans_matrix);

void ComputeMostConsistentBboxDirection(const Eigen::Vector3f& previous_dir,
                                        Eigen::Vector3f* current_dir);

double VectorCosTheta2dXy(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2);

double VectorTheta2dXy(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2);

}  // namespace perception
}  // namespace apollo

#endif  // MODULES_PERCEPTION_OBSTACLE_COMMON_GEOMETRY_UTIL_H_

/******************************************************************************

* Copyright 2017 The Apollo Authors. All Rights Reserved.

*

* Licensed under the Apache License, Version 2.0 (the "License");

* you may not use this file except in compliance with the License.

* You may obtain a copy of the License at

*

* http://www.apache.org/licenses/LICENSE-2.0

*

* Unless required by applicable law or agreed to in writing, software

* distributed under the License is distributed on an "AS IS" BASIS,

* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

* See the License for the specific language governing permissions and

* limitations under the License.

*****************************************************************************/

#ifndef MODULES_PERCEPTION_OBSTACLE_COMMON_GEOMETRY_UTIL_H_

#define MODULES_PERCEPTION_OBSTACLE_COMMON_GEOMETRY_UTIL_H_

#include <cfloat>

#include <algorithm>

#include <vector>

#include "Eigen/Core"

#include "modules/common/log.h"

#include "modules/perception/lib/pcl_util/pcl_types.h" //点云构建头文件 请自行搜索点云PCL库

namespace apollo {

namespace perception {

template < typename PointT >

void TransformPointCloud ( const Eigen::Matrix4d & trans_mat, //点云转换

pcl::PointCloud < PointT >* cloud_in_out) {

for (std:: size_t i = 0 ; i < cloud_in_out-> size (); ++ i) { //以输入的点云尺寸大小为SIZE大小

PointT & p = cloud_in_out-> at (i);

Eigen::Vector4d v (p. x , p. y , p. z , 1 ); //这里是一个四维向量

v = trans_mat * v; //trans_mat是一个转换矩阵，这是一个形参，当你输入相应的转换矩阵值，对应

//的点就会被转换了。

p. x = v. x ();

p. y = v. y ();

p. z = v. z ();

}

}

template < typename PointT >

void TransformPointCloud ( const Eigen::Matrix4d & trans_mat,

typename pcl::PointCloud < PointT > :: Ptr cloud_in_out) {

CHECK_NOTNULL (cloud_in_out. get ());

return TransformPointCloud (trans_mat, cloud_in_out. get ());

}

template < typename PointType >

void TransformPoint ( const PointType & point_in, const Eigen::Matrix4d & trans_mat,

PointType * point_out) {

Eigen::Vector4d v (point_in. x , point_in. y , point_in. z , 1 );

v = trans_mat * v;

* point_out = point_in;

point_out-> x = v. x ();

point_out-> y = v. y ();

point_out-> z = v. z ();

}

template < typename PointType >

void TransformPointCloud ( const pcl::PointCloud < PointType >& cloud_in,

const Eigen::Matrix4d & trans_mat,

pcl::PointCloud < PointType >* cloud_out) {

if (cloud_out-> points . size () < cloud_in. points . size ()) {

cloud_out-> points . resize (cloud_in. points . size ());

}

for (std:: size_t i = 0 ; i < cloud_in. size (); ++ i) {

const PointType & p = cloud_in. at (i);

Eigen::Vector4d v (p. x , p. y , p. z , 1 );

v = trans_mat * v;

PointType & pd = cloud_out-> points [i];

pd. x = v. x ();

pd. y = v. y ();

pd. z = v. z ();

}

}

void TransformPointCloud (pcl_util::PointCloudPtr cloud,

const std::vector < int >& indices,

pcl_util::PointDCloud * trans_cloud);

void TransformPointCloud (pcl_util::PointCloudPtr cloud,

const Eigen::Matrix4d & pose_velodyne,

typename pcl_util::PointDCloudPtr trans_cloud);

/*

* Other point cloud related methods

* */

template < typename PointT >

void GetCloudMinMax3D ( typename pcl::PointCloud < PointT > :: Ptr cloud,

Eigen::Vector4f * min_point, Eigen::Vector4f * max_point) {

Eigen::Vector4f & min_pt = * min_point;

Eigen::Vector4f & max_pt = * max_point;

min_pt[ 0 ] = min_pt[ 1 ] = min_pt[ 2 ] = FLT_MAX;

max_pt[ 0 ] = max_pt[ 1 ] = max_pt[ 2 ] = - FLT_MAX;

if (cloud-> is_dense ) {

for ( size_t i = 0 ; i < cloud-> points . size (); ++ i) {

min_pt[ 0 ] = std::min (min_pt[ 0 ], cloud-> points [i]. x );

max_pt[ 0 ] = std::max (max_pt[ 0 ], cloud-> points [i]. x );

min_pt[ 1 ] = std::min (min_pt[ 1 ], cloud-> points [i]. y );

max_pt[ 1 ] = std::max (max_pt[ 1 ], cloud-> points [i]. y );

min_pt[ 2 ] = std::min (min_pt[ 2 ], cloud-> points [i]. z );

max_pt[ 2 ] = std::max (max_pt[ 2 ], cloud-> points [i]. z );

}

} else {

for ( size_t i = 0 ; i < cloud-> points . size (); ++ i) {

if ( ! pcl_isfinite (cloud-> points [i]. x ) ||

! pcl_isfinite (cloud-> points [i]. y ) ||

! pcl_isfinite (cloud-> points [i]. z )) {

continue ;

}

min_pt[ 0 ] = std::min (min_pt[ 0 ], cloud-> points [i]. x );

max_pt[ 0 ] = std::max (max_pt[ 0 ], cloud-> points [i]. x );

min_pt[ 1 ] = std::min (min_pt[ 1 ], cloud-> points [i]. y );

max_pt[ 1 ] = std::max (max_pt[ 1 ], cloud-> points [i]. y );

min_pt[ 2 ] = std::min (min_pt[ 2 ], cloud-> points [i]. z );

max_pt[ 2 ] = std::max (max_pt[ 2 ], cloud-> points [i]. z );

}

}

}

template < typename PointT > //这个地方是计算ROI的中以坐标

void ComputeBboxSizeCenter ( typename pcl::PointCloud < PointT > :: Ptr cloud,

const Eigen::Vector3d & direction,

Eigen::Vector3d * size, Eigen::Vector3d * center) {

Eigen::Vector3d dir (direction[ 0 ], direction[ 1 ], 0 );

dir. normalize ();

Eigen::Vector3d ortho_dir ( - dir[ 1 ], dir[ 0 ], 0.0 );

Eigen::Vector3d z_dir (dir. cross (ortho_dir));

Eigen::Vector3d min_pt (DBL_MAX, DBL_MAX, DBL_MAX);

Eigen::Vector3d max_pt ( - DBL_MAX, - DBL_MAX, - DBL_MAX);

Eigen::Vector3d loc_pt;

for (std:: size_t i = 0 ; i < cloud-> size (); i ++ ) {

Eigen::Vector3d pt = Eigen::Vector3d (cloud-> points [i]. x , cloud-> points [i]. y ,

cloud-> points [i]. z );

loc_pt[ 0 ] = pt. dot (dir);

loc_pt[ 1 ] = pt. dot (ortho_dir);

loc_pt[ 2 ] = pt. dot (z_dir);

for ( int j = 0 ; j < 3 ; j ++ ) {

min_pt[j] = std::min (min_pt[j], loc_pt[j]);

max_pt[j] = std::max (max_pt[j], loc_pt[j]);

}

}

* size = max_pt - min_pt;

* center = dir * ((max_pt[ 0 ] + min_pt[ 0 ]) * 0.5 ) +

ortho_dir * ((max_pt[ 1 ] + min_pt[ 1 ]) * 0.5 ) + z_dir * min_pt[ 2 ];

}

template < typename PointT >

Eigen::Vector3d GetCloudBarycenter (

typename pcl::PointCloud < PointT > :: Ptr cloud) {

int point_num = cloud-> points . size ();

Eigen::Vector3d barycenter ( 0 , 0 , 0 );

for ( int i = 0 ; i < point_num; i ++ ) {

const PointT & pt = cloud-> points [i];

barycenter[ 0 ] += pt. x ;

barycenter[ 1 ] += pt. y ;

barycenter[ 2 ] += pt. z ;

}

if (point_num > 0 ) {

barycenter[ 0 ] /= point_num;

barycenter[ 1 ] /= point_num;

barycenter[ 2 ] /= point_num;

}

return barycenter;

}

void TransAffineToMatrix4 ( const Eigen::Vector3d & translation,

const Eigen::Vector4d & rotation,

Eigen::Matrix4d * trans_matrix);

void ComputeMostConsistentBboxDirection ( const Eigen::Vector3f & previous_dir,

Eigen::Vector3f * current_dir);

double VectorCosTheta2dXy ( const Eigen::Vector3f & v1, const Eigen::Vector3f & v2);

double VectorTheta2dXy ( const Eigen::Vector3f & v1, const Eigen::Vector3f & v2);

} // namespace perception

} // namespace apollo

#endif // MODULES_PERCEPTION_OBSTACLE_COMMON_GEOMETRY_UTIL_H_