81 lines
2.0 KiB
C++
81 lines
2.0 KiB
C++
/*
|
|
* Copyright 2016 The Cartographer Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#ifndef CARTOGRAPHER_COMMON_MATH_H_
|
|
#define CARTOGRAPHER_COMMON_MATH_H_
|
|
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
#include "Eigen/Core"
|
|
#include "cartographer/common/port.h"
|
|
#include "ceres/ceres.h"
|
|
|
|
namespace cartographer {
|
|
namespace common {
|
|
|
|
// Clamps 'value' to be in the range ['min', 'max'].
|
|
template <typename T>
|
|
T Clamp(const T value, const T min, const T max) {
|
|
if (value > max) {
|
|
return max;
|
|
}
|
|
if (value < min) {
|
|
return min;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
// Calculates 'base'^'exponent'.
|
|
template <typename T>
|
|
constexpr T Power(T base, int exponent) {
|
|
return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
|
|
}
|
|
|
|
// Calculates a^2.
|
|
template <typename T>
|
|
constexpr T Pow2(T a) {
|
|
return Power(a, 2);
|
|
}
|
|
|
|
// Converts from degrees to radians.
|
|
constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
|
|
|
|
// Converts form radians to degrees.
|
|
constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
|
|
|
|
// Bring the 'difference' between two angles into [-pi; pi].
|
|
template <typename T>
|
|
T NormalizeAngleDifference(T difference) {
|
|
while (difference > M_PI) {
|
|
difference -= T(2. * M_PI);
|
|
}
|
|
while (difference < -M_PI) {
|
|
difference += T(2. * M_PI);
|
|
}
|
|
return difference;
|
|
}
|
|
|
|
template <typename T>
|
|
T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
|
|
return ceres::atan2(vector.y(), vector.x());
|
|
}
|
|
|
|
} // namespace common
|
|
} // namespace cartographer
|
|
|
|
#endif // CARTOGRAPHER_COMMON_MATH_H_
|