LdmrsScanpointCoordinateApp.cpp
Go to the documentation of this file.
1 //
2 // LdmrsScanpointCoordinateApp.cpp
3 //
4 // Demo application. Receives scans, and searches each layer for the point at a certain horizontal scan angle.
5 // Adds up some scans and then prints the distance and coordinates of these points.
6 // This demo shows the scanpoint coordinates of scanner layers, e.g. the z coordinates. It can be used to verify the
7 // calculation of the coordinates.
8 // Also shows how to work with the mirror sides and 4- and 8-layer-scanners.
9 //
10 // In the constructor, set the angle and number of scans to the desired values.
11 // Note that scans must be enabled in the MRS device in order for this application to work.
12 //
13 
15 #include "../tools/errorhandler.hpp" // for printInfoMessage()
16 #include "../tools/toolbox.hpp" // for toString()
17 #include "../datatypes/Scan.hpp"
18 #include "../datatypes/Object.hpp"
19 #include "../datatypes/Msg.hpp"
20 #include "../datatypes/Measurement.hpp"
21 #include "../datatypes/Fields.hpp"
22 #include "../datatypes/EvalCases.hpp"
23 #include "../datatypes/EvalCaseResults.hpp"
24 
25 namespace application
26 {
27 
28 //
29 // Constructor
30 //
32 {
33  // Enable this flag for *very* verbose debug output
34  m_beVerbose = false; // true;
35 
36  // Number of scans to build the statistics with. Note that for 8-layer scanners, the number of points in each layer
37  // will be half of this value as the scanner needs two scans for a complete 8-layer-scan.
38  m_numScansToCount = 20;
39 
40  // Horizontal scan angle at which the sampling is done.
41  // +degrees: "to the left",
42  // 0.0 : "straight forward",
43  // -degrees: "to the right"
44  m_wantedHorzAngle = 0.0 * deg2rad;
45 
46  // Some other stuff
47  initCycle();
48  m_isFourLayerScanner = true;
49 
50  printInfoMessage("LdmrsScanpointCoordinateApp constructor done.", m_beVerbose);
51 }
52 
53 
54 // Destructor
55 // Clean up all dynamic data structures
57 {
58  printInfoMessage("LdmrsScanpointCoordinateApp says Goodbye!", m_beVerbose);
59 }
60 
61 //
62 // Initialize all data for a new collection cycle.
63 //
65 {
66  m_scanCounter = 0;
67  for (UINT8 i = 0; i<8; i++)
68  {
69  m_numPoints[i] = 0;
70  m_meanDist[i] = 0.0;
71  m_meanXyz[i] = Point3D(0.0, 0.0, 0.0);
72  }
73 }
74 
75 
76 //
77 // Receiver for new data from the manager.
78 //
80 {
81  //
82  // Do something with it.
83  //
84  // Here, we sort out scan data and hand it over to the processing.
85  //
86  std::string datatypeStr;
87  std::string sourceIdStr;
88 
89  if (data.getDatatype() == Datatype_Scan)
90  {
91  Scan* scan = dynamic_cast<Scan*>(&data);
92 
93  // Search the points and sum them up
94  processScan(scan);
95  m_scanCounter++;
96 
98  {
99  // Calculate and print the statistics
101 
102  // Begin new cycle, reset all variables
103  initCycle();
104  }
105  }
106 
107  printInfoMessage("LdmrsScanpointCoordinateApp::setData(): Done.", m_beVerbose);
108 }
109 
110 //
111 // Calculate the mean values of the coordinates, and print the values.
112 //
114 {
115  // Calc statistics
116  for (UINT8 layer = 0; layer < 8; layer++)
117  {
118  if (m_numPoints[layer] > 1)
119  {
120  m_meanXyz[layer] /= static_cast<double>(m_numPoints[layer]);
121  m_meanDist[layer] /= static_cast<double>(m_numPoints[layer]);
122  }
123  }
124 
125 
126  // Show statistics
127  printInfoMessage(" ", true);
128  printInfoMessage("Statistics for " + toString(m_numScansToCount) + " scans at angle " +
129  toString(m_wantedHorzAngle * rad2deg, 1) + " degrees.", true);
130 
131  // 4 or 8 layers?
132  INT16 startLayer = 3;
133  if (m_isFourLayerScanner == false)
134  {
135  // 8 layers
136  startLayer = 7;
137  }
138 
139  for (INT16 layer = startLayer; layer >= 0; layer -= 1)
140  {
141  printInfoMessage("Layer " + toString(layer + 1) + ": " + toString(m_numPoints[layer]) + " pts;" +
142  " x=" + toString(m_meanXyz[layer].getX(), 2) + " m," +
143  " y=" + toString(m_meanXyz[layer].getY(), 2) + " m," +
144  " z=" + toString(m_meanXyz[layer].getZ(), 2) + " m," +
145  "; dist=" + toString(m_meanDist[layer], 2) + " m.", true);
146  }
147  printInfoMessage(" ", true);
148 }
149 
150 //
151 // Returns true if the scan originates from a 4-layer-scanner.
152 //
154 {
155  printInfoMessage("LdmrsScanpointCoordinateApp::isFourLayerScanner: Beam tilt=" + toString(fabs(scan->getScannerInfos().at(0).getBeamTilt() * rad2deg), 1) + " degrees.", m_beVerbose);
156 
157  // Check beam tilt
158  if (fabs(scan->getScannerInfos().at(0).getBeamTilt()) < (0.2 * deg2rad))
159  {
160  // Beam tilt is near 0, so it is a 4-layer-scanner
161  return true;
162  }
163 
164  return false;
165 }
166 
167 //
168 // Decode and print scan point coordinates.
169 //
171 {
172  ScanPoint point;
173  bool success;
174  UINT8 layerOffset = 0;
175 
176  // 4-layer or 8-layer scanner?
177  if (isFourLayerScanner(scan) == true)
178  {
179  // No beam tilt, it is a 4-layer scanner
180  m_isFourLayerScanner = true;
181  layerOffset = 0;
182  }
183  else
184  {
185  // 8-layer
186  m_isFourLayerScanner = false;
187 
188  // What layers are we in? Upper or lower 4?
189  if (scan->getScannerInfos().at(0).isRearMirrorSide() == true)
190  {
191  // Rear mirror side is facing upward in 8-layer scanners -> Upper 4 layers
192  layerOffset = 4;
193  }
194  else
195  {
196  // Lower 4 layers
197  layerOffset = 0;
198  }
199  }
200 
201 
202  // Find the points at the desired horizontal angle
203  for (UINT8 layer = 0; layer < 4; layer++)
204  {
205  success = getClosestScanpoint(layer, m_wantedHorzAngle, scan, &point);
206  if (success == true)
207  {
208 // printInfoMessage("LdmrsScanpointCoordinateApp::processScan: Point found, hAngle is " + toString(point.getHAngle() * rad2deg, 1) +
209 // " degrees in layer " + toString(layer) + ".", true);
210  }
211  else
212  {
213  printInfoMessage("LdmrsScanpointCoordinateApp::processScan: No point found for hAngle " + toString(m_wantedHorzAngle * rad2deg, 1) +
214  " degrees in layer " + toString(layer) + ".", true);
215  }
216 
217  // Sum up the point coordinates
218  m_numPoints[layer + layerOffset]++;
219  m_meanXyz[layer + layerOffset] += point.toPoint3D();
220  m_meanDist[layer + layerOffset] += point.getDist();
221  }
222 }
223 
224 //
225 // Searches the scan for the best matching scanpoint in the given layer and close to the given horizontal scan angle.
226 // If no point is found within 2 degrees around the wanted angle, no point is returned.
227 // Returns true if a point was found, false if not.
228 //
229 bool LdmrsScanpointCoordinateApp::getClosestScanpoint(const UINT8 layer, const double horzAngle, const Scan* scan, ScanPoint* point)
230 {
231  UINT32 i;
232  ScanPoint bestPoint;
233  ScanPoint currentPoint;
234  bool success;
235 
236  // Set coordinates to a point far, far away
237  bestPoint.setPolar(1000, 90 * deg2rad, 90 * deg2rad);
238 
239  // Search for best match to search parameters
240  for (i=0; i<scan->getNumPoints(); i++)
241  {
242  currentPoint = scan->at(i);
243  if (currentPoint.getLayer() == layer)
244  {
245 // printInfoMessage("Checking angle " + toString(currentPoint.getHAngle() * rad2deg, 1) + " against wanted " +
246 // toString(horzAngle * rad2deg, 1) + ".", true);
247  if (fabs(currentPoint.getHAngle() - horzAngle) < fabs(bestPoint.getHAngle() - horzAngle))
248  {
249  bestPoint = currentPoint;
250 // printInfoMessage("Best point yet!", true);
251  }
252  }
253  }
254 
255  // Is the match good enough?
256  if (fabs(bestPoint.getHAngle() - horzAngle) < (2.0 * deg2rad))
257  {
258  // Good enough
259  *point = bestPoint;
260  success = true;
261  }
262  else
263  {
264  // No match found
265  success = false;
266  }
267 
268  return success;
269 }
270 
271 } // namespace application
double getDist() const
Definition: ScanPoint.hpp:93
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
reference at(size_type n)
Definition: Scan.hpp:239
double getHAngle() const
Definition: ScanPoint.hpp:94
#define rad2deg
bool getClosestScanpoint(const UINT8 layer, const double horzAngle, const Scan *scan, ScanPoint *point)
#define printInfoMessage(a, b)
uint32_t UINT32
void setPolar(double dist, double hAngle, double vAngle)
Definition: ScanPoint.cpp:106
#define deg2rad
UINT16 getNumPoints() const
Definition: Scan.hpp:107
const ScannerInfoVector & getScannerInfos() const
Definition: Scan.hpp:261
Point3D toPoint3D() const
Returns the x,y,z coordinates as a Point3D structure.
Definition: ScanPoint.cpp:116
UINT8 getLayer() const
Definition: ScanPoint.hpp:100
int16_t INT16
uint8_t UINT8


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33