37 lines
1004 B
C++
37 lines
1004 B
C++
#pragma once
|
|
|
|
#include "common/types.h"
|
|
|
|
namespace oh_loam {
|
|
|
|
template <typename PointT>
|
|
inline double Distance(const PointT& pt) {
|
|
return std::hypot(pt.x, pt.y, pt.z);
|
|
}
|
|
|
|
template <typename PointT>
|
|
inline double DistanceSqure(const PointT& pt) {
|
|
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
|
|
}
|
|
|
|
template <typename PointT>
|
|
inline double IsFinite(const PointT& pt) {
|
|
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
|
|
}
|
|
|
|
// normalize an angle to [-pi, pi)
|
|
inline double NormalizeAngle(double ang) {
|
|
const double& two_pi = 2 * M_PI;
|
|
return ang - two_pi * std::floor((ang + M_PI) / two_pi);
|
|
}
|
|
|
|
std::pair<double, double> GetYawRange(const PointCloud& cloud) {
|
|
const auto& pts = cloud.points;
|
|
int pt_num = pts.size();
|
|
double yaw_start = -atan2(pts[0].y, pt[0].x);
|
|
double yaw_end = -atan2(pts[pt_num - 1].y, pts[pt_num - 1].x) + 2 * M_PI;
|
|
double yaw_diff = NormalizeAngle(yaw_end - yaw_start);
|
|
return {yaw_start, yaw_start + yaw_diff + 2 * M_PI};
|
|
}
|
|
|
|
} // oh_loam
|