1 #ifndef SLAM_CTOR_FEATURES_ANGLE_HISTOGRAM_H 2 #define SLAM_CTOR_FEATURES_ANGLE_HISTOGRAM_H 7 #include "../states/sensor_data.h" 18 std::fill(std::begin(
_hist), std::end(
_hist), 0);
21 const auto &pts = scan.
points();
24 using IndType = LaserScan2D::Points::size_type;
25 for (IndType i = 1; i < pts.size(); ++i) {
38 LaserScan2D::Points::size_type pt_i)
const {
45 Storage::size_type
max_i()
const {
46 assert(0 <
_hist.size());
47 auto max_e = std::max_element(
_hist.begin(),
_hist.end());
49 return std::distance(
_hist.begin(), max_e);
52 Storage::size_type
min_i()
const {
53 assert(0 <
_hist.size());
54 auto max_e = std::min_element(
_hist.begin(),
_hist.end());
56 return std::distance(
_hist.begin(), max_e);
70 const LaserScan2D::Points::value_type &base,
71 const LaserScan2D::Points::value_type &sp) {
73 auto d_x = sp.x() - base.x();
74 auto d_y = sp.y() - base.y();
78 auto d_d = std::sqrt(d_x*d_x + d_y*d_y);
79 auto angle = std::acos(d_x / d_d);
81 if (d_y < 0 && d_x != 0) {
90 assert(0 <= angle && angle < M_PI);
91 auto hist_i = std::size_t(std::floor(angle /
angle_step()));
92 assert(hist_i <
_hist.size());
97 Storage::size_type
_n;
static double estimate_ox_based_angle(const LaserScan2D::Points::value_type &base, const LaserScan2D::Points::value_type &sp)
double angle_step() const
auto least_freq_angle() const
std::size_t hist_index(double angle) const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
std::vector< double > _ang_sum
Storage::value_type value(const LaserScan2D::Points &pts, LaserScan2D::Points::size_type pt_i) const
const Points & points() const
std::vector< unsigned > Storage
std::vector< double > _drift_dirs
AngleHistogram(Storage::size_type resolution=20)
constexpr double deg2rad(double angle_deg)
std::vector< ScanPoint2D > Points
Storage::value_type operator[](std::size_t i) const
Storage::size_type max_i() const
Storage::size_type min_i() const
auto reset(const LaserScan2D &scan)