Program Listing for File utils.hpp
↰ Return to documentation for file (include/nav2_constrained_smoother/utils.hpp
)
// Copyright (c) 2021 RoboTech Vision
//
// 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 NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
#define NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
#include <limits>
#include "Eigen/Core"
#define EPSILON 0.0001
#if defined(USE_OLD_CERES_API)
#define CERES_ISINF(x) ceres::IsInfinite(x)
#else
#define CERES_ISINF(x) ceres::isinf(x)
#endif
namespace nav2_constrained_smoother
{
template<typename T>
inline Eigen::Matrix<T, 2, 1> arcCenter(
Eigen::Matrix<T, 2, 1> pt_prev,
Eigen::Matrix<T, 2, 1> pt,
Eigen::Matrix<T, 2, 1> pt_next,
bool is_cusp)
{
Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
if (is_cusp) {
d2 = -d2;
pt_next = pt + d2;
}
T det = d1[0] * d2[1] - d1[1] * d2[0];
if (ceres::abs(det) < (T)1e-4) { // straight line
return Eigen::Matrix<T, 2, 1>(
(T)std::numeric_limits<double>::infinity(), (T)std::numeric_limits<double>::infinity());
}
// circle center is at the intersection of mirror axes of the segments:
// http://paulbourke.net/geometry/circlesphere/
// line intersection:
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
Eigen::Matrix<T, 2, 1> mid1 = (pt_prev + pt) / (T)2;
Eigen::Matrix<T, 2, 1> mid2 = (pt + pt_next) / (T)2;
Eigen::Matrix<T, 2, 1> n1(-d1[1], d1[0]);
Eigen::Matrix<T, 2, 1> n2(-d2[1], d2[0]);
T det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0];
T det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0];
Eigen::Matrix<T, 2, 1> center((det1 * n2[0] - det2 * n1[0]) / det,
(det1 * n2[1] - det2 * n1[1]) / det);
return center;
}
template<typename T>
inline Eigen::Matrix<T, 2, 1> tangentDir(
Eigen::Matrix<T, 2, 1> pt_prev,
Eigen::Matrix<T, 2, 1> pt,
Eigen::Matrix<T, 2, 1> pt_next,
bool is_cusp)
{
Eigen::Matrix<T, 2, 1> center = arcCenter(pt_prev, pt, pt_next, is_cusp);
if (CERES_ISINF(center[0])) { // straight line
Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
if (is_cusp) {
d2 = -d2;
pt_next = pt + d2;
}
Eigen::Matrix<T, 2, 1> result(pt_next[0] - pt_prev[0], pt_next[1] - pt_prev[1]);
if (result[0] == 0.0 && result[1] == 0.0) { // a very rare edge situation
return Eigen::Matrix<T, 2, 1>(d1[1], -d1[0]);
}
return result;
}
// tangent is prependicular to (pt - center)
// Note: not determining + or - direction here, this should be handled at the caller side
return Eigen::Matrix<T, 2, 1>(center[1] - pt[1], pt[0] - center[0]);
}
} // namespace nav2_constrained_smoother
#endif // NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_