/****************Apollo源码分析****************************
Copyright 2018 The File Authors & zouyu. All Rights Reserved.
Contact with: 1746430162@qq.com 181663309504
源码主要是c++实现的,也有少量python,git下载几百兆,其实代码不太多,主要是地图和数据了大量空间,主要程序
在apollo/modules目录中,
在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)控制汽车.
(识别车辆行人路况标志等),并预测下一步发展;然后把已知信息都传入规划模块(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_