Apollo 10.0
自动驾驶开放平台
math_utils.h
浏览该文件的文档.
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
22#pragma once
23
24#include <cmath>
25#include <limits>
26#include <type_traits>
27#include <utility>
28
29#include "Eigen/Dense"
30
32
37namespace apollo {
38namespace common {
39namespace math {
40
41double Sqr(const double x);
42
52double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1,
53 const Vec2d &end_point_2);
54
64double InnerProd(const Vec2d &start_point, const Vec2d &end_point_1,
65 const Vec2d &end_point_2);
66
78double CrossProd(const double x0, const double y0, const double x1,
79 const double y1);
80
92double InnerProd(const double x0, const double y0, const double x1,
93 const double y1);
94
100double WrapAngle(const double angle);
101
107double NormalizeAngle(const double angle);
108
115double AngleDiff(const double from, const double to);
116
124int RandomInt(const int s, const int t, unsigned int rand_seed = 1);
125
133double RandomDouble(const double s, const double t, unsigned int rand_seed = 1);
134
140template <typename T>
141inline T Square(const T value) {
142 return value * value;
143}
144
154template <typename T>
155T Clamp(const T value, T bound1, T bound2) {
156 if (bound1 > bound2) {
157 std::swap(bound1, bound2);
158 }
159
160 if (value < bound1) {
161 return bound1;
162 } else if (value > bound2) {
163 return bound2;
164 }
165 return value;
166}
167
168// Gaussian
169double Gaussian(const double u, const double std, const double x);
170
171inline double Sigmoid(const double x) { return 1.0 / (1.0 + std::exp(-x)); }
172
173// Rotate a 2d vector counter-clockwise by theta
174Eigen::Vector2d RotateVector2d(const Eigen::Vector2d &v_in, const double theta);
175
176inline std::pair<double, double> RFUToFLU(const double x, const double y) {
177 return std::make_pair(y, -x);
178}
179
180inline std::pair<double, double> FLUToRFU(const double x, const double y) {
181 return std::make_pair(-y, x);
182}
183
184inline void L2Norm(int feat_dim, float *feat_data) {
185 if (feat_dim == 0) {
186 return;
187 }
188 // feature normalization
189 float l2norm = 0.0f;
190 for (int i = 0; i < feat_dim; ++i) {
191 l2norm += feat_data[i] * feat_data[i];
192 }
193 if (l2norm == 0) {
194 float val = 1.f / std::sqrt(static_cast<float>(feat_dim));
195 for (int i = 0; i < feat_dim; ++i) {
196 feat_data[i] = val;
197 }
198 } else {
199 l2norm = std::sqrt(l2norm);
200 for (int i = 0; i < feat_dim; ++i) {
201 feat_data[i] /= l2norm;
202 }
203 }
204}
205
206// Cartesian coordinates to Polar coordinates
207std::pair<double, double> Cartesian2Polar(double x, double y);
208
209template <class T>
210typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
211almost_equal(T x, T y, int ulp) {
212 // the machine epsilon has to be scaled to the magnitude of the values used
213 // and multiplied by the desired precision in ULPs (units in the last place)
214 // unless the result is subnormal
215 return std::fabs(x - y) <=
216 std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp ||
217 std::fabs(x - y) < std::numeric_limits<T>::min();
218}
219
220double check_negative(double input_data);
221
222} // namespace math
223} // namespace common
224} // namespace apollo
double Sigmoid(const double x)
Definition math_utils.h:171
double Sqr(const double x)
Definition math_utils.cc:26
int RandomInt(const int s, const int t, unsigned int rand_seed)
Get a random integer between two integer values by a random seed.
Definition math_utils.cc:65
std::pair< double, double > RFUToFLU(const double x, const double y)
Definition math_utils.h:176
std::enable_if<!std::numeric_limits< T >::is_integer, bool >::type almost_equal(T x, T y, int ulp)
Definition math_utils.h:211
double Gaussian(const double u, const double std, const double x)
Definition math_utils.cc:77
double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1, const Vec2d &end_point_2)
Cross product between two 2-D vectors from the common start point, and end at two other points.
Definition math_utils.cc:28
double RandomDouble(const double s, const double t, unsigned int rand_seed)
Get a random double between two integer values by a random seed.
Definition math_utils.cc:72
T Clamp(const T value, T bound1, T bound2)
Clamp a value between two bounds.
Definition math_utils.h:155
double WrapAngle(const double angle)
Wrap angle to [0, 2 * PI).
Definition math_utils.cc:48
double InnerProd(const Vec2d &start_point, const Vec2d &end_point_1, const Vec2d &end_point_2)
Inner product between two 2-D vectors from the common start point, and end at two other points.
Definition math_utils.cc:33
std::pair< double, double > Cartesian2Polar(double x, double y)
Definition math_utils.cc:93
T Square(const T value)
Compute squared value.
Definition math_utils.h:141
double AngleDiff(const double from, const double to)
Calculate the difference between angle from and to
Definition math_utils.cc:61
void L2Norm(int feat_dim, float *feat_data)
Definition math_utils.h:184
double check_negative(double input_data)
Definition math_utils.cc:99
std::pair< double, double > FLUToRFU(const double x, const double y)
Definition math_utils.h:180
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Definition math_utils.cc:53
Eigen::Vector2d RotateVector2d(const Eigen::Vector2d &v_in, const double theta)
Definition math_utils.cc:82
class register implement
Definition arena_queue.h:37
Definition future.h:29
Defines the Vec2d class.