Apollo  6.0
Open source self driving car software
points_downsampler.h
Go to the documentation of this file.
1 /******************************************************************************
2  * Copyright 2017 The Apollo Authors. All Rights Reserved.
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *****************************************************************************/
16 
21 #pragma once
22 
23 #include <cmath>
24 #include <vector>
25 
26 #include "cyber/common/log.h"
28 
33 namespace apollo {
34 namespace common {
35 namespace util {
36 
45 template <typename Points>
46 double GetPathAngle(const Points &points, const size_t start,
47  const size_t end) {
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.";
52  return 0.0;
53  }
54  if (start >= end) {
55  AERROR << "Second index must be greater than the first index.";
56  return 0.0;
57  }
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);
61 
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);
65 
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));
68 
69  return std::isnan(angle) ? 0.0 : angle;
70 }
71 
79 template <typename Points>
80 std::vector<size_t> DownsampleByAngle(const Points &points,
81  const double angle_threshold) {
82  std::vector<size_t> sampled_indices;
83  if (points.empty()) {
84  return sampled_indices;
85  }
86 
87  if (angle_threshold < 0.0) {
88  AERROR << "Input angle threshold is negative.";
89  return sampled_indices;
90  }
91  sampled_indices.push_back(0);
92  if (points.size() > 1) {
93  size_t start = 0;
94  size_t end = 1;
95  double accum_degree = 0.0;
96  while (end + 1 < static_cast<size_t>(points.size())) {
97  const double angle = GetPathAngle(points, start, end);
98  accum_degree += std::fabs(angle);
99 
100  if (accum_degree > angle_threshold) {
101  sampled_indices.push_back(end);
102  start = end;
103  accum_degree = 0.0;
104  }
105  ++end;
106  }
107  sampled_indices.push_back(end);
108  }
109 
110  ADEBUG << "Point Vector is downsampled from " << points.size() << " to "
111  << sampled_indices.size();
112 
113  return sampled_indices;
114 }
115 
123 template <typename Points>
124 std::vector<size_t> DownsampleByDistance(const Points &points,
125  int downsampleDistance,
126  int steepTurnDownsampleDistance) {
127  std::vector<size_t> sampled_indices;
128  if (points.size() <= 4) {
129  // No need to downsample if there are not too many points.
130  for (size_t i = 0; i < points.size(); ++i) {
131  sampled_indices.push_back(i);
132  }
133  return sampled_indices;
134  }
135 
137  Vec2d v_start =
138  Vec2d(points[1].x() - points[0].x(), points[1].y() - points[0].y());
139  Vec2d v_end =
140  Vec2d(points[points.size() - 1].x() - points[points.size() - 2].x(),
141  points[points.size() - 1].y() - points[points.size() - 2].y());
142  v_start.Normalize();
143  v_end.Normalize();
144  // If the angle exceeds 80 degree, it's a steep turn
145  bool is_steep_turn = v_start.InnerProd(v_end) <= cos(80.0 * M_PI / 180.0);
146  int downsampleRate =
147  is_steep_turn ? steepTurnDownsampleDistance : downsampleDistance;
148 
149  // Make sure the first point is included
150  sampled_indices.push_back(0);
151 
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);
157 
158  if (accum_distance > downsampleRate) {
159  sampled_indices.push_back(pos);
160  accum_distance = 0.0;
161  }
162  }
163 
164  // Make sure the last point is included
165  sampled_indices.push_back(points.size() - 1);
166  return sampled_indices;
167 }
168 
169 } // namespace util
170 } // namespace common
171 } // namespace apollo
Defines the Vec2d class.
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
float cos(Angle16 a)
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