angle_histogram.h
Go to the documentation of this file.
1 #ifndef SLAM_CTOR_FEATURES_ANGLE_HISTOGRAM_H
2 #define SLAM_CTOR_FEATURES_ANGLE_HISTOGRAM_H
3 
4 #include <vector>
5 #include <algorithm>
6 
7 #include "../states/sensor_data.h"
8 
10 private:
11  using Storage = std::vector<unsigned>;
12 public:
13  AngleHistogram(Storage::size_type resolution = 20)
14  : _n{resolution}, _hist(_n, 0) /* NB: () - ctor, not {} - init list */
15  , _ang_sum(_n, 0.0) {}
16 
17  auto reset(const LaserScan2D &scan) {
18  std::fill(std::begin(_hist), std::end(_hist), 0);
19  std::fill(std::begin(_ang_sum), std::end(_ang_sum), 0.0);
20 
21  const auto &pts = scan.points();
22  _drift_dirs.resize(pts.size());
23 
24  using IndType = LaserScan2D::Points::size_type;
25  for (IndType i = 1; i < pts.size(); ++i) {
26  auto angle = estimate_ox_based_angle(pts[i-1], pts[i]);
27  auto hist_i = hist_index(angle);
28  _hist[hist_i]++;
29  _ang_sum[hist_i] += angle;
30  _drift_dirs[i] = angle;
31  }
32  return *this;
33  }
34 
35  Storage::value_type operator[](std::size_t i) const { return _hist[i]; }
36 
37  Storage::value_type value(const LaserScan2D::Points &pts,
38  LaserScan2D::Points::size_type pt_i) const {
39  if (pt_i == 0) {
40  return pts.size(); // ~ignore the first point
41  }
42  return (*this)[hist_index(_drift_dirs[pt_i])];
43  }
44 
45  Storage::size_type max_i() const {
46  assert(0 < _hist.size());
47  auto max_e = std::max_element(_hist.begin(), _hist.end());
48  // assume random access iterator.
49  return std::distance(_hist.begin(), max_e);
50  }
51 
52  Storage::size_type min_i() const {
53  assert(0 < _hist.size());
54  auto max_e = std::min_element(_hist.begin(), _hist.end());
55  // assume random access iterator.
56  return std::distance(_hist.begin(), max_e);
57  }
58 
59  auto least_freq_angle() const {
60  auto i = min_i();
61  return _ang_sum[i] / _hist[i];
62  }
63 
64  double angle_step() const {
65  return deg2rad(180) / _n;
66  }
67 
68  // TODO: better naming
69  static double estimate_ox_based_angle(
70  const LaserScan2D::Points::value_type &base,
71  const LaserScan2D::Points::value_type &sp) {
72 
73  auto d_x = sp.x() - base.x();
74  auto d_y = sp.y() - base.y();
75  if (d_y == 0) { // TODO: math utils
76  return 0; // 180 is equivalent to 0
77  }
78  auto d_d = std::sqrt(d_x*d_x + d_y*d_y);
79  auto angle = std::acos(d_x / d_d);
80 
81  if (d_y < 0 && d_x != 0) { // TODO: math utils
82  angle = M_PI - angle;
83  }
84 
85  return angle;
86  }
87 
88 protected:
89  std::size_t hist_index(double angle) const {
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());
93  return hist_i;
94  }
95 
96 private:
97  Storage::size_type _n;
99  std::vector<double> _ang_sum, _drift_dirs;
100 };
101 
102 #endif
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
Definition: sensor_data.h:148
Storage::size_type _n
std::vector< unsigned > Storage
std::vector< double > _drift_dirs
AngleHistogram(Storage::size_type resolution=20)
constexpr double deg2rad(double angle_deg)
Definition: math_utils.h:56
std::vector< ScanPoint2D > Points
Definition: sensor_data.h:146
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)


slam_constructor
Author(s): JetBrains Research, OSLL team
autogenerated on Mon Jun 10 2019 15:08:25