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

本文深入剖析Apollo自动驾驶项目的感知模块,介绍其核心功能包括位置感知、障碍物检测等,并详细解读几何构建方法。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >


/****************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_




In file included from /home/acceler/code/apollo_ros/apollo_ros/src/apollo.ros-1.0.0-master/apollo_common/include/apollo_common/apollo_app.h:46:0, from /home/acceler/code/apollo_ros/apollo_ros/src/apollo.ros-1.0.0-master/apollo_common/src/apollo_app.cc:33: /home/acceler/code/apollo_ros/apollo_ros/src/apollo.ros-1.0.0-master/apollo_common/include/apollo_common/log.h:40:10: fatal error: glog/logging.h: No such file or directory #include <glog/logging.h> ^~~~~~~~~~~~~~~~ compilation terminated. apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/build.make:62: recipe for target 'apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/src/apollo_app.cc.o' failed make[2]: *** [apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/src/apollo_app.cc.o] Error 1 make[2]: *** Waiting for unfinished jobs.... In file included from /home/acceler/code/apollo_ros/apollo_ros/src/apollo.ros-1.0.0-master/apollo_common/include/apollo_common/adapters/adapter_manager.h:48:0, from /home/acceler/code/apollo_ros/apollo_ros/src/apollo.ros-1.0.0-master/apollo_common/src/adapters/adapter_manager.cc:33: /home/acceler/code/apollo_ros/apollo_ros/src/apollo.ros-1.0.0-master/apollo_common/include/apollo_common/adapters/adapter.h:49:10: fatal error: glog/logging.h: No such file or directory #include <glog/logging.h> ^~~~~~~~~~~~~~~~ compilation terminated. apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/build.make:110: recipe for target 'apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/src/adapters/adapter_manager.cc.o' failed make[2]: *** [apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/src/adapters/adapter_manager.cc.o] Error 1 CMakeFiles/Makefile2:3894: recipe for target 'apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/all' failed make[1]: *** [apollo.ros-1.0.0-master/apollo_common/CMakeFiles/apollo_common.dir/all] Error 2 make[1]: *** Waiting for unfinished jobs.... [ 54%] Linking CXX executable /home/acceler/code/apollo_ros/apollo_ros/devel/lib/IntegratedNavigation/IntegratedNavigation_node [ 54%] Built target IntegratedNavigation_node [ 55%] Linking CXX executable /home/acceler/code/apollo_ros/apollo_ros/devel/lib/TimeSynchronierProcess/timeSynchronierProcess_node [ 55%] Built target timeSynchronierProcess_node Makefile:140: recipe for target 'all' failed make: *** [all] Error 2 Invoking "make -j4 -l4" failed
07-23
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值