Apollo 10.0
自动驾驶开放平台
apollo::common::math::PathMatcher类 参考

#include <path_matcher.h>

apollo::common::math::PathMatcher 的协作图:

Public 成员函数

 PathMatcher ()=delete
 

静态 Public 成员函数

static PathPoint MatchToPath (const std::vector< PathPoint > &reference_line, const double x, const double y)
 
static std::pair< double, double > GetPathFrenetCoordinate (const std::vector< PathPoint > &reference_line, const double x, const double y)
 
static PathPoint MatchToPath (const std::vector< PathPoint > &reference_line, const double s)
 

详细描述

在文件 path_matcher.h32 行定义.

构造及析构函数说明

◆ PathMatcher()

apollo::common::math::PathMatcher::PathMatcher ( )
delete

成员函数说明

◆ GetPathFrenetCoordinate()

std::pair< double, double > apollo::common::math::PathMatcher::GetPathFrenetCoordinate ( const std::vector< PathPoint > &  reference_line,
const double  x,
const double  y 
)
static

在文件 path_matcher.cc69 行定义.

71 {
72 auto matched_path_point = MatchToPath(reference_line, x, y);
73 double rtheta = matched_path_point.theta();
74 double rx = matched_path_point.x();
75 double ry = matched_path_point.y();
76 double delta_x = x - rx;
77 double delta_y = y - ry;
78 double side = std::cos(rtheta) * delta_y - std::sin(rtheta) * delta_x;
79 std::pair<double, double> relative_coordinate;
80 relative_coordinate.first = matched_path_point.s();
81 relative_coordinate.second =
82 std::copysign(std::hypot(delta_x, delta_y), side);
83 return relative_coordinate;
84}
static PathPoint MatchToPath(const std::vector< PathPoint > &reference_line, const double x, const double y)

◆ MatchToPath() [1/2]

PathPoint apollo::common::math::PathMatcher::MatchToPath ( const std::vector< PathPoint > &  reference_line,
const double  s 
)
static

在文件 path_matcher.cc86 行定义.

87 {
88 auto comp = [](const PathPoint& point, const double s) {
89 return point.s() < s;
90 };
91
92 auto it_lower =
93 std::lower_bound(reference_line.begin(), reference_line.end(), s, comp);
94 if (it_lower == reference_line.begin()) {
95 return reference_line.front();
96 } else if (it_lower == reference_line.end()) {
97 return reference_line.back();
98 }
99
100 // interpolate between it_lower - 1 and it_lower
101 // return interpolate(*(it_lower - 1), *it_lower, s);
102 return InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower, s);
103}
SLPoint InterpolateUsingLinearApproximation(const SLPoint &p0, const SLPoint &p1, const double w)

◆ MatchToPath() [2/2]

PathPoint apollo::common::math::PathMatcher::MatchToPath ( const std::vector< PathPoint > &  reference_line,
const double  x,
const double  y 
)
static

在文件 path_matcher.cc35 行定义.

36 {
37 CHECK_GT(reference_line.size(), 0U);
38
39 auto func_distance_square = [](const PathPoint& point, const double x,
40 const double y) {
41 double dx = point.x() - x;
42 double dy = point.y() - y;
43 return dx * dx + dy * dy;
44 };
45
46 double distance_min = func_distance_square(reference_line.front(), x, y);
47 std::size_t index_min = 0;
48
49 for (std::size_t i = 1; i < reference_line.size(); ++i) {
50 double distance_temp = func_distance_square(reference_line[i], x, y);
51 if (distance_temp < distance_min) {
52 distance_min = distance_temp;
53 index_min = i;
54 }
55 }
56
57 std::size_t index_start = (index_min == 0) ? index_min : index_min - 1;
58 std::size_t index_end =
59 (index_min + 1 == reference_line.size()) ? index_min : index_min + 1;
60
61 if (index_start == index_end) {
62 return reference_line[index_start];
63 }
64
65 return FindProjectionPoint(reference_line[index_start],
66 reference_line[index_end], x, y);
67}

该类的文档由以下文件生成: