45 template <
typename Po
ints>
48 if (start >= static_cast<size_t>(points.size() - 1) ||
49 end >= static_cast<size_t>(points.size() - 1)) {
50 AERROR <<
"Input indices are out of the range of the points vector: " 51 <<
"should be less than vector size - 1.";
55 AERROR <<
"Second index must be greater than the first index.";
58 double vec_start_x = points[start + 1].x() - points[start].x();
59 double vec_start_y = points[start + 1].y() - points[start].y();
60 double vec_start_norm = std::hypot(vec_start_x, vec_start_y);
62 double vec_end_x = points[end + 1].x() - points[end].x();
63 double vec_end_y = points[end + 1].y() - points[end].y();
64 double vec_end_norm = std::hypot(vec_end_x, vec_end_y);
66 double dot_product = vec_start_x * vec_end_x + vec_start_y * vec_end_y;
67 double angle = std::acos(dot_product / (vec_start_norm * vec_end_norm));
69 return std::isnan(angle) ? 0.0 : angle;
79 template <
typename Po
ints>
81 const double angle_threshold) {
82 std::vector<size_t> sampled_indices;
84 return sampled_indices;
87 if (angle_threshold < 0.0) {
88 AERROR <<
"Input angle threshold is negative.";
89 return sampled_indices;
91 sampled_indices.push_back(0);
92 if (points.size() > 1) {
95 double accum_degree = 0.0;
96 while (end + 1 < static_cast<size_t>(points.size())) {
98 accum_degree += std::fabs(angle);
100 if (accum_degree > angle_threshold) {
101 sampled_indices.push_back(end);
107 sampled_indices.push_back(end);
110 ADEBUG <<
"Point Vector is downsampled from " << points.size() <<
" to " 111 << sampled_indices.size();
113 return sampled_indices;
123 template <
typename Po
ints>
125 int downsampleDistance,
126 int steepTurnDownsampleDistance) {
127 std::vector<size_t> sampled_indices;
128 if (points.size() <= 4) {
130 for (
size_t i = 0; i < points.size(); ++i) {
131 sampled_indices.push_back(i);
133 return sampled_indices;
138 Vec2d(points[1].x() - points[0].x(), points[1].y() - points[0].y());
140 Vec2d(points[points.size() - 1].x() - points[points.size() - 2].x(),
141 points[points.size() - 1].y() - points[points.size() - 2].y());
145 bool is_steep_turn = v_start.InnerProd(v_end) <=
cos(80.0 * M_PI / 180.0);
147 is_steep_turn ? steepTurnDownsampleDistance : downsampleDistance;
150 sampled_indices.push_back(0);
152 double accum_distance = 0.0;
153 for (
size_t pos = 1; pos < points.size() - 1; ++pos) {
154 Vec2d point_start = Vec2d(points[pos - 1].x(), points[pos - 1].y());
155 Vec2d point_end = Vec2d(points[pos].x(), points[pos].y());
156 accum_distance += point_start.DistanceTo(point_end);
158 if (accum_distance > downsampleRate) {
159 sampled_indices.push_back(pos);
160 accum_distance = 0.0;
165 sampled_indices.push_back(points.size() - 1);
166 return sampled_indices;
PlanningContext is the runtime context in planning. It is persistent across multiple frames...
Definition: atomic_hash_map.h:25
std::vector< size_t > DownsampleByAngle(const Points &points, const double angle_threshold)
Downsample the points on the path according to the angle.
Definition: points_downsampler.h:80
#define ADEBUG
Definition: log.h:41
Implements a class of 2-dimensional vectors.
Definition: vec2d.h:42
double GetPathAngle(const Points &points, const size_t start, const size_t end)
Calculate the angle between the directions of two points on the path.
Definition: points_downsampler.h:46
std::vector< size_t > DownsampleByDistance(const Points &points, int downsampleDistance, int steepTurnDownsampleDistance)
Downsample the points on the path based on distance.
Definition: points_downsampler.h:124
#define AERROR
Definition: log.h:44