Scan.cpp
Go to the documentation of this file.
1 //
2 // Scan.cpp
3 //
4 // Scan data container.
5 //
6 // Version history
7 // 17.11.2011, willhvo: First version
8 //
9 
10 #include "Scan.hpp"
11 #include <algorithm>
12 #include <cmath>
13 #include <limits>
14 #include "ScannerInfo.hpp"
15 #include "../tools/errorhandler.hpp"
16 #include <set>
17 #include "../tools/toolbox.hpp"
18 
19 namespace datatypes
20 {
21 
22 //
23 // maxPoints The maximum number of scan points that are allocated by the
24 // constructor. This can be increased later by calling reserve().
25 //
26 Scan::Scan (size_type maxPoints)
27  : m_flags(0)
28  , m_scanNumber(0x0000)
29  , m_points()
30 {
32  m_points.reserve(maxPoints); // allocate memory for the point array
33 
34  m_beVerbose = false;
35 }
36 
37 Scan::Scan (const Scan& other)
38  : m_points()
39 {
40  copy(other);
41 }
42 
43 // Estimate the memory usage of this object
45 {
46  return (sizeof(*this) +
47  (getNumPoints() * sizeof(ScanPoint)) +
48  (m_scannerInfos.size() * sizeof(ScannerInfo)));
49 }
50 
51 
52 Scan& Scan::copy (const Scan& other)
53 {
54  // These are the trivial by-value assignments
55 // m_scanStartTime = other.m_scanStartTime;
56 // m_scanEndTime = other.m_scanEndTime;
57 // m_scanMidTime = other.m_scanMidTime;
58 // m_syncTimestamp = other.m_syncTimestamp;
59 // m_startAngle = other.m_startAngle;
60 // m_endAngle = other.m_endAngle;
61 // m_scannerStatus = other.m_scannerStatus;
62  m_flags = other.m_flags;
63  m_scanNumber = other.m_scanNumber;
64 // m_deviceID = other.m_deviceID;
65 // m_scannerType = other.m_scannerType;
66  m_sourceId = other.m_sourceId;
67  m_datatype = other.m_datatype;
68 
69  // The PointList vector itself is copied element-by-element.
70  m_points = other.m_points;
71  m_points.reserve (other.capacity());
72 
73  // This vector is copied element-by-element, by value.
75 
76  // Verbose-Flag
77  m_beVerbose = other.m_beVerbose;
78 
79  return *this;
80 }
81 
82 //
83 // Tries its best to estimate the current memory usage of this object.
84 // This function is required to estimate the required storage space in buffers.
85 //
87 {
88  UINT32 size;
89 
90  size = sizeof(*this); // Dieses Objekt selber
91  size += getNumPoints() * sizeof(ScanPoint); // Seine Scanpunkte
92  size += m_scannerInfos.size() * sizeof(ScannerInfo); // Die Scanner-Infos
93 
94  return size;
95 }
96 
97 Scan& Scan::operator= (const Scan& other)
98 {
99  return copy(other);
100 }
101 
102 
103 /*
104 bool Scan::operator==(const Scan& other) const
105 {
106  return
107  m_scanNumber == other.m_scanNumber
108  && m_flags == other.m_flags
109  && m_scanStartTime == other.m_scanStartTime
110  && m_scanEndTime == other.m_scanEndTime
111  && m_scanMidTime == other.m_scanMidTime
112  && m_syncTimestamp == other.m_syncTimestamp
113  && m_points == other.m_points
114  && m_scannerInfos == other.m_scannerInfos
115  // Either scannerInfos is not empty (in which case the
116  // deprecated variables are ignored anyway), or the old
117  // variables must be equal
118  && (!m_scannerInfos.empty()
119  || ((m_startAngle == other.m_startAngle
120  || (isNaN(m_startAngle) && isNaN(other.m_startAngle)))
121  && (m_endAngle == other.m_endAngle
122  || (isNaN(m_endAngle) && isNaN(other.m_endAngle)))
123  && m_scannerStatus == other.m_scannerStatus
124  && m_deviceID == other.m_deviceID
125  && m_scannerType == other.m_scannerType
126  ));
127 }
128 */
129 
130 //
131 // After clear(), the scan will not contain any scanpoints at all.
132 //
133 void Scan::clear ()
134 {
135  // Clear member variables
136  m_flags = 0;
137  m_scanNumber = 0x0000;
138  m_sourceId = 0;
139 
140  m_points.clear();
141  m_scannerInfos.clear();
142 }
143 
144 // Default destructor
146 {
147 }
148 
149 void Scan::resize(size_type new_size, const ScanPoint& default_point)
150 {
151  if (size_type((UINT16)new_size) != new_size)
152  {
153  throw std::out_of_range ("Scan::resize was called with new size larger than what fits into UINT16: " + toString(new_size) + " but only 65536 is allowed!");
154  }
155 
156  m_points.resize(new_size, default_point);
157 }
158 
159 void Scan::reserve(size_type new_capacity)
160 {
161  if (size_type((UINT16)new_capacity) != new_capacity)
162  {
163  throw std::out_of_range ("Scan::reserve was called with new capacity larger than what fits into UINT16: " + toString(new_capacity) + " but only 65536 is allowed!");
164  }
165 
166  m_points.reserve(new_capacity);
167 }
168 
174 {
175  m_points.push_back(ScanPoint());
176  return m_points.back();
177 }
178 
179 void Scan::setVehicleCoordinates(bool inVehicleCoordinates)
180 {
181  if (inVehicleCoordinates)
183  else
185 }
186 
187 
188 // Sort criterion. This method is passed to std::sort() to sort scan
189 // points by descending azimuth angles.
190 static bool isDescendingAngle (const ScanPoint& P1, const ScanPoint& P2)
191 {
192  return (P1.getHAngle() > P2.getHAngle());
193 }
194 
196 {
198 }
199 
200 void Scan::addCartesianOffset(double offsetX, double offsetY, double offsetZ)
201 {
202  for (PointList::iterator point = getPointListBegin(); point != getPointListEnd(); point++)
203  point->addCartesianOffset(offsetX, offsetY, offsetZ);
204 }
205 
206 void Scan::addPolarOffset(double distOffset, double hAngleOffset, double vAngleOffset)
207 {
208  for (PointList::iterator point = getPointListBegin(); point != getPointListEnd(); point++)
209  point->addPolarOffset(distOffset, hAngleOffset, vAngleOffset);
210 }
211 
213 {
214  // Sanity check: Only one ScannerInfo per deviceID allowed, except
215  // for 8-Layer scanners.
216  std::set<UINT8> availableDevicesOnce;
217  std::set<UINT8> availableDevicesTwice;
218  for (ScannerInfoVector::const_iterator it = v.begin();
219  it != v.end(); it++)
220  {
221  const ScannerInfo& sinfo = *it;
222  const UINT8 deviceID = sinfo.getDeviceID();
223 
224  // Did we already see this deviceID previously?
225  if (availableDevicesOnce.count(deviceID) == 0)
226  {
227  // No, so record this deviceID for later.
228  availableDevicesOnce.insert(deviceID);
229  }
230  else
231  {
232  // Error: 4L-deviceID is here a second time!
233  throw std::logic_error("Scan::setScannerInfos called with a list (size=" +
234  ::toString(v.size()) + ") that contains two ScannerInfos with device ID " +
235  ::toString(int(deviceID)) + ", type " + ScannerInfo::scannerTypeToString(sinfo.getScannerType()) +
236  ". This is not allowed - for each scanner at most one ScannerInfo may be stored in the ScannerInfoVector.");
237  }
238  }
239 
240  m_scannerInfos = v;
241 }
242 
243 
244 
245 // Flags
246 /*
247 void Scan::setGroundLabeled(bool isGroundLabeled)
248 {
249  if (isGroundLabeled)
250  m_flags |= FlagGroundLabeled;
251  else
252  m_flags &= ~FlagGroundLabeled;
253 }
254 
255 void Scan::setRainLabeled(bool isRainLabeled)
256 {
257  if (isRainLabeled)
258  m_flags |= FlagRainLabeled;
259  else
260  m_flags &= ~FlagRainLabeled;
261 }
262 
263 void Scan::setDirtLabeled(bool isDirtLabeled)
264 {
265  if (isDirtLabeled)
266  m_flags |= FlagDirtLabeled;
267  else
268  m_flags &= ~FlagDirtLabeled;
269 }
270 
271 void Scan::setCoverageLabeled(bool isCoverageLabeled)
272 {
273  if (isCoverageLabeled)
274  m_flags |= FlagCoverageLabeled;
275  else
276  m_flags &= ~FlagCoverageLabeled;
277 }
278 
279 void Scan::setBackgroundLabeled(bool isBackgroundLabeled)
280 {
281  if (isBackgroundLabeled)
282  m_flags |= FlagBackgroundLabeled;
283  else
284  m_flags &= ~FlagBackgroundLabeled;
285 }
286 
287 void Scan::setReflectorLabeled(bool isReflectorLabeled)
288 {
289  if (isReflectorLabeled)
290  m_flags |= FlagReflectorLabeled;
291  else
292  m_flags &= ~FlagReflectorLabeled;
293 }
294 */
295 
296 //
297 // Transform the scan to vehicle coordinates, but do not sort the resulting scanpoints by angles.
298 //
300 {
301  // Check if Scan might already be converted
302  if ((getFlags() & FlagVehicleCoordinates) != 0)
303  {
304  // Ist schon in Fahrzeugkoordinaten.
305  // traceDebug(SCAN_VERSION) << "convertToVehicleCoordinates: Scan (#" << getScanNumber() << ") already in vehicle coordinates. Nothing to do." << std::endl;
306  return true; // it's ok, job is already done
307  }
308 
309  // If there are no scanner informations available -> no transformation
310  if (getScannerInfos().empty())
311  {
312  printWarning("Scan::convertToVehicleCoordinates: Scan (#" + ::toString(getScanNumber()) +
313  ") has empty ScannerInfos - no conversion possible, aborting!");
314  return false;
315  }
316 
317  // Hole die Scanner-Position
318  Position3D mountPos = m_scannerInfos.at(0).getMountingPosition();
319 // printInfoMessage("Scan::transformToVehicleCoordinatesUnsorted: Mounting position is " + mountPos.toString() + ".", true);
320 
321  // Transformiere alle Punkte
322  for (Scan::iterator sptIt = begin(); sptIt != end(); sptIt++)
323  {
324  Point3D pt = sptIt->toPoint3D();
325  mountPos.transformToVehicle(&pt);
326  sptIt->setPoint3D(pt);
327  }
328 
329 
330  // Set transformation flag (if everything went well)
332 
333  // Also set the transformation flag in all the ScannerInfo entries
334  for (ScannerInfoVector::iterator siIt = getScannerInfos().begin(); siIt != getScannerInfos().end(); siIt++)
335  {
336  ScannerInfo& sinfo = *siIt;
338  }
339 
340  return true;
341 }
342 
344 {
345  const bool r = transformToVehicleCoordinatesUnsorted();
346 
347  // We also sort this because this got
348  // forgotten all too often.
349  sort();
350 
351  return r;
352 }
353 
354 
356 {
357  return m_scannerInfos;
358 }
359 
361 {
362  for (ScannerInfoVector::const_iterator it = m_scannerInfos.begin();
363  it != m_scannerInfos.end();
364  it++)
365  {
366  if ((*it).getDeviceID() == id)
367  return &(*it);
368  }
369 
370  return NULL;
371 }
372 
374 {
375  UINT32 test = ~(scanFlag);
376  m_flags &= test;
377 }
378 
379 } // namespace datatypes
void clear()
Resets all members of this object.
Definition: Scan.cpp:133
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
UINT32 getFlags() const
Definition: Scan.hpp:168
const_iterator end() const
Definition: Scan.hpp:227
ScannerInfoVector m_scannerInfos
The ScannerInfo collection.
Definition: Scan.hpp:73
virtual const UINT32 getUsedMemory() const
Definition: Scan.cpp:44
bool m_beVerbose
Definition: Scan.hpp:321
void setVehicleCoordinates(bool inVehicleCoordinates)
Set whether the scanpoints are given in vehicle coordinates.
Definition: Scan.cpp:179
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
double getHAngle() const
Definition: ScanPoint.hpp:94
size_type size() const
Definition: Scan.hpp:110
PointList::size_type size_type
Definition: Scan.hpp:40
uint16_t UINT16
bool empty() const
Definition: Scan.hpp:113
A Position with orientation.
Definition: Position3D.hpp:149
UINT8 getDeviceID() const
Returns the ID of the device that has recorded this scan.
Definition: ScannerInfo.hpp:42
PointList::const_iterator getPointListEnd() const
Definition: Scan.hpp:215
Scan(size_type maxPoints=5280)
Definition: Scan.cpp:26
bool transformToVehicleCoordinates()
Transforms this scan (i.e. the scan points) to the vehicle coordinates.
Definition: Scan.cpp:343
size_type capacity() const
Definition: Scan.hpp:137
uint32_t UINT32
void setScannerInfos(const ScannerInfoVector &v)
Definition: Scan.cpp:212
UINT8 getScannerType() const
Definition: ScannerInfo.hpp:46
void reserve(size_type new_capacity)
Allocates memory for a total of new_capacity points.
Definition: Scan.cpp:159
UINT32 m_flags
Definition: Scan.hpp:60
UINT16 getNumPoints() const
Definition: Scan.hpp:107
static std::string scannerTypeToString(UINT8 st)
Definition: ScannerInfo.cpp:39
UINT16 getScanNumber() const
Definition: Scan.hpp:153
UINT16 m_scanNumber
Definition: Scan.hpp:63
ScanPoint & addNewPoint()
Definition: Scan.cpp:173
const ScannerInfoVector & getScannerInfos() const
Definition: Scan.hpp:261
static bool isDescendingAngle(const ScanPoint &P1, const ScanPoint &P2)
Definition: Scan.cpp:190
PointList::const_iterator getPointListBegin() const
Definition: Scan.hpp:210
void addPolarOffset(double distOffset, double hAngleOffset, double vAngleOffset)
Definition: Scan.cpp:206
UINT32 getScanFlags() const
bool transformToVehicle(Point3D *pt)
Definition: Position3D.cpp:113
PointList::iterator iterator
Definition: Scan.hpp:46
const ScannerInfo * getScannerInfoByDeviceId(UINT8 id) const
Definition: Scan.cpp:360
const_iterator begin() const
Definition: Scan.hpp:221
Scan & copy(const Scan &)
Definition: Scan.cpp:52
void resize(size_type new_size, const ScanPoint &default_point=ScanPoint())
Resizes the scan to the specified number of points.
Definition: Scan.cpp:149
std::vector< ScannerInfo > ScannerInfoVector
Definition: Scan.hpp:31
void setScanFlags(UINT32 flags)
void addCartesianOffset(double offsetX, double offsetY, double offsetZ)
Definition: Scan.cpp:200
void clearLabelFlag(Scan::ScanFlags scanFlag)
Definition: Scan.cpp:373
UINT32 getTotalObjectSize()
Definition: Scan.cpp:86
PointList m_points
Definition: Scan.hpp:70
~Scan()
Default destructor.
Definition: Scan.cpp:145
void setFlags(UINT32 val)
Definition: Scan.hpp:183
Bit 11: Scanpoint coordinate system; 0 = scanner coordinates, 1 = vehicle / reference coordinates...
Definition: Scan.hpp:51
void sort()
Definition: Scan.cpp:195
void printWarning(std::string message)
Scan & operator=(const Scan &)
Definition: Scan.cpp:97
bool transformToVehicleCoordinatesUnsorted()
Transforms this scan (i.e. the scan points) to the vehicle coordinates.
Definition: Scan.cpp:299
uint8_t UINT8


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