LaserScan.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/LaserScan.h>
31 
32 namespace rtabmap {
33 
34 std::string LaserScan::formatName(const Format & format)
35 {
36  std::string name;
37  switch (format) {
38  case kXY:
39  name = "XY";
40  break;
41  case kXYZ:
42  name = "XYZ";
43  break;
44  case kXYI:
45  name = "XYI";
46  break;
47  case kXYZI:
48  name = "XYZI";
49  break;
50  case kXYZRGB:
51  name = "XYZRGB";
52  break;
53  case kXYNormal:
54  name = "XYNormal";
55  break;
56  case kXYZNormal:
57  name = "XYZNormal";
58  break;
59  case kXYINormal:
60  name = "XYINormal";
61  break;
62  case kXYZINormal:
63  name = "XYZINormal";
64  break;
65  case kXYZRGBNormal:
66  name = "XYZRGBNormal";
67  break;
68  case kXYZIT:
69  name = "XYZIT";
70  break;
71  default:
72  name = "Unknown";
73  break;
74  }
75  return name;
76 }
77 
78 int LaserScan::channels(const Format & format)
79 {
80  int channels=0;
81  switch (format) {
82  case kXY:
83  channels = 2;
84  break;
85  case kXYZ:
86  case kXYI:
87  channels = 3;
88  break;
89  case kXYZI:
90  case kXYZRGB:
91  channels = 4;
92  break;
93  case kXYNormal:
94  case kXYZIT:
95  channels = 5;
96  break;
97  case kXYZNormal:
98  case kXYINormal:
99  channels = 6;
100  break;
101  case kXYZINormal:
102  case kXYZRGBNormal:
103  channels = 7;
104  break;
105  default:
106  UFATAL("Unhandled type %d!", (int)format);
107  break;
108  }
109  return channels;
110 }
111 
112 bool LaserScan::isScan2d(const Format & format)
113 {
114  return format==kXY || format==kXYI || format == kXYNormal || format == kXYINormal;
115 }
117 {
119 }
120 bool LaserScan::isScanHasRGB(const Format & format)
121 {
122  return format==kXYZRGB || format==kXYZRGBNormal;
123 }
125 {
126  return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
127 }
128 bool LaserScan::isScanHasTime(const Format & format)
129 {
130  return format==kXYZIT;
131 }
132 
134  const cv::Mat & oldScanFormat,
135  int maxPoints,
136  int maxRange,
137  const Transform & localTransform)
138 {
139  if(!oldScanFormat.empty())
140  {
141  if(oldScanFormat.channels() == 2)
142  {
143  return LaserScan(oldScanFormat, maxPoints, maxRange, kXY, localTransform);
144  }
145  else if(oldScanFormat.channels() == 3)
146  {
147  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZ, localTransform);
148  }
149  else if(oldScanFormat.channels() == 4)
150  {
151  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGB, localTransform);
152  }
153  else if(oldScanFormat.channels() == 5)
154  {
155  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYNormal, localTransform);
156  }
157  else if(oldScanFormat.channels() == 6)
158  {
159  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZNormal, localTransform);
160  }
161  else if(oldScanFormat.channels() == 7)
162  {
163  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGBNormal, localTransform);
164  }
165  }
166  return LaserScan();
167 }
168 
170  const cv::Mat & oldScanFormat,
171  float minRange,
172  float maxRange,
173  float angleMin,
174  float angleMax,
175  float angleInc,
176  const Transform & localTransform)
177 {
178  if(!oldScanFormat.empty())
179  {
180  if(oldScanFormat.channels() == 2)
181  {
182  return LaserScan(oldScanFormat, kXY, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
183  }
184  else if(oldScanFormat.channels() == 3)
185  {
186  return LaserScan(oldScanFormat, kXYZ, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
187  }
188  else if(oldScanFormat.channels() == 4)
189  {
190  return LaserScan(oldScanFormat, kXYZRGB, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
191  }
192  else if(oldScanFormat.channels() == 5)
193  {
194  return LaserScan(oldScanFormat, kXYNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
195  }
196  else if(oldScanFormat.channels() == 6)
197  {
198  return LaserScan(oldScanFormat, kXYZNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
199  }
200  else if(oldScanFormat.channels() == 7)
201  {
202  return LaserScan(oldScanFormat, kXYZRGBNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
203  }
204  }
205  return LaserScan();
206 }
207 
209  format_(kUnknown),
210  maxPoints_(0),
211  rangeMin_(0),
212  rangeMax_(0),
213  angleMin_(0),
214  angleMax_(0),
215  angleIncrement_(0),
216  localTransform_(Transform::getIdentity())
217 {
218 }
219 
221  const LaserScan & scan,
222  int maxPoints,
223  float maxRange,
224  const Transform & localTransform) :
225  format_(kUnknown),
226  maxPoints_(0),
227  rangeMin_(0),
228  rangeMax_(0),
229  angleMin_(0),
230  angleMax_(0),
231  angleIncrement_(0)
232 {
233  UASSERT(scan.empty() || scan.format() != kUnknown);
234  init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform);
235 }
236 
238  const LaserScan & scan,
239  int maxPoints,
240  float maxRange,
241  Format format,
242  const Transform & localTransform) :
243  format_(kUnknown),
244  maxPoints_(0),
245  rangeMin_(0),
246  rangeMax_(0),
247  angleMin_(0),
248  angleMax_(0),
249  angleIncrement_(0)
250 {
251  init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
252 }
253 
255  const cv::Mat & data,
256  int maxPoints,
257  float maxRange,
258  Format format,
259  const Transform & localTransform) :
260  format_(kUnknown),
261  maxPoints_(0),
262  rangeMin_(0),
263  rangeMax_(0),
264  angleMin_(0),
265  angleMax_(0),
266  angleIncrement_(0)
267 {
268  init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
269 }
270 
272  const LaserScan & scan,
273  float minRange,
274  float maxRange,
275  float angleMin,
276  float angleMax,
277  float angleIncrement,
278  const Transform & localTransform) :
279  format_(kUnknown),
280  maxPoints_(0),
281  rangeMin_(0),
282  rangeMax_(0),
283  angleMin_(0),
284  angleMax_(0),
285  angleIncrement_(0)
286 {
287  UASSERT(scan.empty() || scan.format() != kUnknown);
288  init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
289 }
290 
292  const LaserScan & scan,
293  Format format,
294  float minRange,
295  float maxRange,
296  float angleMin,
297  float angleMax,
298  float angleIncrement,
299  const Transform & localTransform) :
300  format_(kUnknown),
301  maxPoints_(0),
302  rangeMin_(0),
303  rangeMax_(0),
304  angleMin_(0),
305  angleMax_(0),
306  angleIncrement_(0)
307 {
308  init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
309 }
310 
312  const cv::Mat & data,
313  Format format,
314  float minRange,
315  float maxRange,
316  float angleMin,
317  float angleMax,
318  float angleIncrement,
319  const Transform & localTransform) :
320  format_(kUnknown),
321  maxPoints_(0),
322  rangeMin_(0),
323  rangeMax_(0),
324  angleMin_(0),
325  angleMax_(0),
326  angleIncrement_(0)
327 {
328  init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
329 }
330 
332  const cv::Mat & data,
333  Format format,
334  float rangeMin,
335  float rangeMax,
336  float angleMin,
337  float angleMax,
338  float angleIncrement,
339  int maxPoints,
340  const Transform & localTransform)
341 {
342  UASSERT(data.empty() || (data.type() == CV_8UC1 && data.rows == 1) || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
344 
345  bool is2D = false;
346  if(angleIncrement != 0.0f)
347  {
348  // 2D scan
349  is2D = true;
353  }
354  else
355  {
356  // 3D scan
359  if(maxPoints_ == 0 && data.rows>1)
360  {
361  maxPoints_ = data.rows * data.cols;
362  }
363  }
364 
365  data_ = data;
366  format_ = format;
373 
374  if(!data.empty() && !isCompressed())
375  {
376  if(is2D && (int)data_.total() > maxPoints_)
377  {
378  UWARN("The number of points (%ld) in the scan is over the maximum "
379  "points (%d) defined by angle settings (min=%f max=%f inc=%f). "
380  "The scan info may be wrong!",
382  }
383  else if(!is2D && maxPoints_>0 && (int)data_.total() > maxPoints_)
384  {
385  UDEBUG("The number of points (%ld) in the scan is over the maximum "
386  "points (%d) defined by max points setting.",
387  data_.total(), maxPoints_);
388  }
389 
390  if(format == kUnknown)
391  {
392  if(angleIncrement_ != 0)
393  {
395  }
396  else
397  {
399  }
400  }
401  else // verify that format corresponds to expected number of channels
402  {
403  UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
404  UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
405  UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
406  UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
407  UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
408  UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
409  }
410  }
411 }
412 
414 {
415  if(angleIncrement_ > 0.0f)
416  {
418  }
420 }
421 
423 {
424  if(!isOrganized())
425  {
426  return *this;
427  }
428  cv::Mat output(1, data_.total(), data_.type());
429  int oi = 0;
430  for(int i=0; i<data_.rows; ++i)
431  {
432  for(int j=0; j<data_.cols; ++j)
433  {
434  const float * ptr = data_.ptr<float>(i, j);
435  float * outputPtr = output.ptr<float>(0, oi);
436  if(! (std::isnan(ptr[0]) || std::isnan(ptr[1]) || (!is2d() && std::isnan(ptr[2]))))
437  {
438  memcpy(outputPtr, ptr, data_.elemSize());
439  ++oi;
440  }
441  }
442  }
443  return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0,oi)), maxPoints_, rangeMax_, format_, localTransform_.clone());
444 }
445 
446 float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
447 {
448  UASSERT(pointIndex < (unsigned int)data_.total());
449  UASSERT(channelOffset < (unsigned int)data_.channels());
450  unsigned int row = pointIndex / data_.cols;
451  return data_.ptr<float>(row, pointIndex - row * data_.cols)[channelOffset];
452 }
453 
455 {
456  *this = *this+scan;
457  return *this;
458 }
459 
461 {
462  UASSERT(this->empty() || scan.empty() || (this->format() == scan.format() && !this->isOrganized() && !scan.isOrganized()));
463  LaserScan dest;
464  if(!scan.empty())
465  {
466  if(this->empty())
467  {
468  dest = scan.clone();
469  }
470  else
471  {
472  cv::Mat destData(1, data_.cols + scan.data().cols, data_.type());
473  data_.copyTo(destData(cv::Range::all(), cv::Range(0,data_.cols)));
474  scan.data().copyTo(destData(cv::Range::all(), cv::Range(data_.cols, data_.cols+scan.data().cols)));
475  dest = LaserScan(destData, 0, 0, this->format());
476  }
477  }
478  else
479  {
480  dest = this->clone();
481  }
482  return dest;
483 }
484 
485 }
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::LaserScan::LaserScan
LaserScan()
Definition: LaserScan.cpp:208
name
rtabmap::LaserScan::Format
Format
Definition: LaserScan.h:40
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
rtabmap::LaserScan::kXYZ
@ kXYZ
Definition: LaserScan.h:45
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
rtabmap::LaserScan::densify
LaserScan densify() const
Definition: LaserScan.cpp:422
rtabmap::LaserScan::clone
LaserScan clone() const
Definition: LaserScan.cpp:413
rtabmap::LaserScan::angleIncrement_
float angleIncrement_
Definition: LaserScan.h:180
this
this
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::LaserScan::kXYZIT
@ kXYZIT
Definition: LaserScan.h:51
rtabmap::LaserScan::angleMax_
float angleMax_
Definition: LaserScan.h:179
rtabmap::LaserScan::init
void init(const cv::Mat &data, Format format, float minRange, float maxRange, float angleMin, float angleMax, float angleIncrement, int maxPoints, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:331
rtabmap::LaserScan::kXYZI
@ kXYZI
Definition: LaserScan.h:46
rtabmap::LaserScan::kUnknown
@ kUnknown
Definition: LaserScan.h:40
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::isnan
GLM_FUNC_DECL genType::bool_type isnan(genType const &x)
rtabmap::LaserScan::isScanHasTime
static bool isScanHasTime(const Format &format)
Definition: LaserScan.cpp:128
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
rtabmap::LaserScan::kXYI
@ kXYI
Definition: LaserScan.h:42
rtabmap::LaserScan::format_
Format format_
Definition: LaserScan.h:174
rtabmap::LaserScan::field
float & field(unsigned int pointIndex, unsigned int channelOffset)
Definition: LaserScan.cpp:446
rtabmap::LaserScan::kXYZRGB
@ kXYZRGB
Definition: LaserScan.h:47
UFATAL
#define UFATAL(...)
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
rtabmap::LaserScan::rangeMin_
float rangeMin_
Definition: LaserScan.h:176
rtabmap::LaserScan::isScanHasNormals
static bool isScanHasNormals(const Format &format)
Definition: LaserScan.cpp:116
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
all
static const Eigen::internal::all_t all
glm::ceil
GLM_FUNC_DECL genType ceil(genType const &x)
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::LaserScan::kXY
@ kXY
Definition: LaserScan.h:41
rtabmap::LaserScan::rangeMax_
float rangeMax_
Definition: LaserScan.h:177
rtabmap::LaserScan::isScan2d
static bool isScan2d(const Format &format)
Definition: LaserScan.cpp:112
rtabmap::LaserScan::maxPoints_
int maxPoints_
Definition: LaserScan.h:175
UASSERT
#define UASSERT(condition)
rtabmap::LaserScan::angleMin_
float angleMin_
Definition: LaserScan.h:178
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:124
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::LaserScan::isScanHasIntensity
static bool isScanHasIntensity(const Format &format)
Definition: LaserScan.cpp:124
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
LaserScan.h
dest
const char char * dest
Definition: lz4hc.h:181
rtabmap::LaserScan::data_
cv::Mat data_
Definition: LaserScan.h:173
rtabmap::LaserScan::kXYINormal
@ kXYINormal
Definition: LaserScan.h:44
rtabmap::LaserScan::operator+=
LaserScan & operator+=(const LaserScan &)
Definition: LaserScan.cpp:454
c_str
const char * c_str(Args &&...args)
rtabmap::LaserScan::isOrganized
bool isOrganized() const
Definition: LaserScan.h:139
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
UDEBUG
#define UDEBUG(...)
rtabmap::LaserScan::isScanHasRGB
static bool isScanHasRGB(const Format &format)
Definition: LaserScan.cpp:120
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:125
rtabmap::LaserScan::operator+
LaserScan operator+(const LaserScan &)
Definition: LaserScan.cpp:460
rtabmap::LaserScan::localTransform_
Transform localTransform_
Definition: LaserScan.h:181
rtabmap::LaserScan::channels
int channels() const
Definition: LaserScan.h:119
rtabmap::LaserScan::kXYZRGBNormal
@ kXYZRGBNormal
Definition: LaserScan.h:50
rtabmap::LaserScan::formatName
std::string formatName() const
Definition: LaserScan.h:118
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
rtabmap::LaserScan::kXYNormal
@ kXYNormal
Definition: LaserScan.h:43
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:138
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:123
i
int i
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:121
rtabmap::LaserScan::kXYZINormal
@ kXYZINormal
Definition: LaserScan.h:49
rtabmap::LaserScan::kXYZNormal
@ kXYZNormal
Definition: LaserScan.h:48


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11