math.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
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
17 #ifndef CARTOGRAPHER_COMMON_MATH_H_
18 #define CARTOGRAPHER_COMMON_MATH_H_
19
20 #include <cmath>
21 #include <vector>
22
23 #include "Eigen/Core"
25 #include "ceres/ceres.h"
26
27 namespace cartographer {
28 namespace common {
29
30 // Clamps 'value' to be in the range ['min', 'max'].
31 template <typename T>
32 T Clamp(const T value, const T min, const T max) {
33  if (value > max) {
34  return max;
35  }
36  if (value < min) {
37  return min;
38  }
39  return value;
40 }
41
42 // Calculates 'base'^'exponent'.
43 template <typename T>
44 constexpr T Power(T base, int exponent) {
45  return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
46 }
47
48 // Calculates a^2.
49 template <typename T>
50 constexpr T Pow2(T a) {
51  return Power(a, 2);
52 }
53
54 // Converts from degrees to radians.
55 constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
56
57 // Converts form radians to degrees.
59
60 // Bring the 'difference' between two angles into [-pi; pi].
61 template <typename T>
62 T NormalizeAngleDifference(T difference) {
63  while (difference > M_PI) {
64  difference -= T(2. * M_PI);
65  }
66  while (difference < -M_PI) {
67  difference += T(2. * M_PI);
68  }
69  return difference;
70 }
71
72 template <typename T>
73 T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
74  return ceres::atan2(vector.y(), vector.x());
75 }
76
77 } // namespace common
78 } // namespace cartographer
79
80 #endif // CARTOGRAPHER_COMMON_MATH_H_
constexpr T Pow2(T a)
Definition: math.h:50
T atan2(const Eigen::Matrix< T, 2, 1 > &vector)
Definition: math.h:73
Definition: math.h:58
constexpr T Power(T base, int exponent)
Definition: math.h:44