ScanClustering.cpp
Go to the documentation of this file.
1 #include "ScanClustering.h"
2 
3 #include <cmath>
4 #include <vector>
5 
6 #include <RobotAPI/interface/units/LaserScannerUnit.h>
7 
9 {
10  ScanClustering::ScanClustering(const Params& params) : params(params)
11  {
12  }
13 
14  bool
15  ScanClustering::add(const LaserScanStep& scanStep)
16  {
17  if (scan.empty())
18  {
19  scan.push_back(scanStep);
20  return true;
21  }
22 
23  if (not supports(scanStep))
24  {
25  return false;
26  }
27 
28  scan.push_back(scanStep);
29  return true;
30  }
31 
32  std::vector<LaserScan>
33  ScanClustering::detectClusters(const LaserScan& scan)
34  {
35  const auto isInvalid = [&](const LaserScanStep& step) -> bool
36  { return step.distance > params.maxDistance; };
37 
38  std::vector<LaserScan> clusters;
39  for (const auto& laserScanStep : scan)
40  {
41  // cluster finished
42  if (isInvalid(laserScanStep) or (not add(laserScanStep)))
43  {
44  if (not cluster().empty())
45  {
46  clusters.push_back(cluster());
47  clear();
48 
49  // work on a new cluster
50  add(laserScanStep);
51  }
52  }
53  }
54 
55  return clusters;
56  }
57 
58  const LaserScan&
60  {
61  return scan;
62  }
63 
64  void
66  {
67  scan.clear();
68  }
69 
70  bool
71  ScanClustering::supports(const LaserScanStep& scanStep)
72  {
73  // OK to create a new cluster if it's empty
74  if (scan.empty())
75  {
76  return true;
77  }
78 
79  const float distanceDiff = std::fabs(scanStep.distance - scan.back().distance);
80  const bool isWithinDistanceRange = distanceDiff < params.distanceThreshold;
81 
82  const float angleDiff = std::fabs(scanStep.angle - scan.back().angle);
83  const bool isWithinAngleRange = angleDiff < params.angleThreshold;
84 
85  return (isWithinDistanceRange and isWithinAngleRange);
86  }
87 } // namespace armarx::navigation::components::laser_scanner_feature_extraction
armarx::navigation::components::laser_scanner_feature_extraction::detail::ScanClusteringParams
Definition: ScanClustering.h:32
armarx::navigation::components::laser_scanner_feature_extraction::ScanClustering::ScanClustering
ScanClustering(const Params &params)
Definition: ScanClustering.cpp:10
armarx::navigation::components::laser_scanner_feature_extraction::ScanClustering::detectClusters
Clusters detectClusters(const LaserScan &scan)
Performs cluster detection on a full laser scan.
Definition: ScanClustering.cpp:33
armarx::navigation::components::laser_scanner_feature_extraction::ScanClustering::cluster
const LaserScan & cluster() const
Definition: ScanClustering.cpp:59
armarx::navigation::components::laser_scanner_feature_extraction::ScanClustering::clear
void clear()
Definition: ScanClustering.cpp:65
armarx::navigation::components::laser_scanner_feature_extraction::ScanClustering::supports
bool supports(const LaserScanStep &scanStep)
Definition: ScanClustering.cpp:71
ScanClustering.h
armarx::navigation::components::laser_scanner_feature_extraction::detail::ScanClusteringParams::angleThreshold
float angleThreshold
Definition: ScanClustering.h:36
armarx::navigation::components::laser_scanner_feature_extraction::detail::ScanClusteringParams::distanceThreshold
float distanceThreshold
Definition: ScanClustering.h:35
armarx::navigation::components::laser_scanner_feature_extraction::ScanClustering::add
bool add(const LaserScanStep &scanStep)
Definition: ScanClustering.cpp:15
armarx::navigation::components::laser_scanner_feature_extraction
Definition: ArVizDrawer.cpp:28
armarx::navigation::components::laser_scanner_feature_extraction::detail::ScanClusteringParams::maxDistance
float maxDistance
Range filter: only consider points that are closer than maxDistance.
Definition: ScanClustering.h:39