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>
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UConversion.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  default:
69  name = "Unknown";
70  break;
71  }
72  return name;
73 }
74 
75 int LaserScan::channels(const Format & format)
76 {
77  int channels=0;
78  switch (format) {
79  case kXY:
80  channels = 2;
81  break;
82  case kXYZ:
83  case kXYI:
84  channels = 3;
85  break;
86  case kXYZI:
87  case kXYZRGB:
88  channels = 4;
89  break;
90  case kXYNormal:
91  channels = 5;
92  break;
93  case kXYZNormal:
94  case kXYINormal:
95  channels = 6;
96  break;
97  case kXYZINormal:
98  case kXYZRGBNormal:
99  channels = 7;
100  break;
101  default:
102  UFATAL("Unhandled type %d!", (int)format);
103  break;
104  }
105  return channels;
106 }
107 
108 bool LaserScan::isScan2d(const Format & format)
109 {
110  return format==kXY || format==kXYI || format == kXYNormal || format == kXYINormal;
111 }
112 bool LaserScan::isScanHasNormals(const Format & format)
113 {
114  return format==kXYZNormal || format==kXYZINormal || format==kXYZRGBNormal || format == kXYNormal || format == kXYINormal;
115 }
116 bool LaserScan::isScanHasRGB(const Format & format)
117 {
118  return format==kXYZRGB || format==kXYZRGBNormal;
119 }
120 bool LaserScan::isScanHasIntensity(const Format & format)
121 {
122  return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
123 }
124 
125 LaserScan LaserScan::backwardCompatibility(
126  const cv::Mat & oldScanFormat,
127  int maxPoints,
128  int maxRange,
129  const Transform & localTransform)
130 {
131  if(!oldScanFormat.empty())
132  {
133  if(oldScanFormat.channels() == 2)
134  {
135  return LaserScan(oldScanFormat, maxPoints, maxRange, kXY, localTransform);
136  }
137  else if(oldScanFormat.channels() == 3)
138  {
139  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZ, localTransform);
140  }
141  else if(oldScanFormat.channels() == 4)
142  {
143  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGB, localTransform);
144  }
145  else if(oldScanFormat.channels() == 5)
146  {
147  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYNormal, localTransform);
148  }
149  else if(oldScanFormat.channels() == 6)
150  {
151  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZNormal, localTransform);
152  }
153  else if(oldScanFormat.channels() == 7)
154  {
155  return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGBNormal, localTransform);
156  }
157  }
158  return LaserScan();
159 }
160 
161 LaserScan LaserScan::backwardCompatibility(
162  const cv::Mat & oldScanFormat,
163  float minRange,
164  float maxRange,
165  float angleMin,
166  float angleMax,
167  float angleInc,
168  const Transform & localTransform)
169 {
170  if(!oldScanFormat.empty())
171  {
172  if(oldScanFormat.channels() == 2)
173  {
174  return LaserScan(oldScanFormat, kXY, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
175  }
176  else if(oldScanFormat.channels() == 3)
177  {
178  return LaserScan(oldScanFormat, kXYZ, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
179  }
180  else if(oldScanFormat.channels() == 4)
181  {
182  return LaserScan(oldScanFormat, kXYZRGB, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
183  }
184  else if(oldScanFormat.channels() == 5)
185  {
186  return LaserScan(oldScanFormat, kXYNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
187  }
188  else if(oldScanFormat.channels() == 6)
189  {
190  return LaserScan(oldScanFormat, kXYZNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
191  }
192  else if(oldScanFormat.channels() == 7)
193  {
194  return LaserScan(oldScanFormat, kXYZRGBNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
195  }
196  }
197  return LaserScan();
198 }
199 
200 LaserScan::LaserScan() :
201  format_(kUnknown),
202  maxPoints_(0),
203  rangeMin_(0),
204  rangeMax_(0),
205  angleMin_(0),
206  angleMax_(0),
207  angleIncrement_(0),
208  localTransform_(Transform::getIdentity())
209 {
210 }
211 
212 LaserScan::LaserScan(
213  const cv::Mat & data,
214  int maxPoints,
215  float maxRange,
216  Format format,
217  const Transform & localTransform) :
218  data_(data),
219  format_(format),
220  maxPoints_(maxPoints),
221  rangeMin_(0),
222  rangeMax_(maxRange),
223  angleMin_(0),
224  angleMax_(0),
225  angleIncrement_(0),
226  localTransform_(localTransform)
227 {
228  UASSERT(data.empty() || data.rows == 1);
229  UASSERT(data.empty() || data.type() == CV_8UC1 || 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));
230  UASSERT(!localTransform.isNull());
231 
232  if(!data.empty() && !isCompressed())
233  {
234  if(format == kUnknown)
235  {
236  *this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
237  }
238  else // verify that format corresponds to expected number of channels
239  {
240  UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
241  UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
242  UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
243  UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
244  UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
245  UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
246  }
247  }
248 }
249 
250 LaserScan::LaserScan(
251  const cv::Mat & data,
252  Format format,
253  float minRange,
254  float maxRange,
255  float angleMin,
256  float angleMax,
257  float angleIncrement,
258  const Transform & localTransform) :
259  data_(data),
260  format_(format),
261  rangeMin_(minRange),
262  rangeMax_(maxRange),
263  angleMin_(angleMin),
264  angleMax_(angleMax),
265  angleIncrement_(angleIncrement),
266  localTransform_(localTransform)
267 {
268  UASSERT(maxRange>minRange);
269  UASSERT(angleMax>angleMin);
270  UASSERT(angleIncrement != 0.0f);
271  maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement)+1;
272 
273  UASSERT(data.empty() || data.rows == 1);
274  UASSERT(data.empty() || data.type() == CV_8UC1 || 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));
275  UASSERT(!localTransform.isNull());
276 
277  if(!data.empty() && !isCompressed())
278  {
279  if(data_.cols > maxPoints_)
280  {
281  UWARN("The number of points (%d) in the scan is over the maximum "
282  "points (%d) defined by angle settings (min=%f max=%f inc=%f). "
283  "The scan info may be wrong!",
284  data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
285  }
286  if(format == kUnknown)
287  {
288  *this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
289  }
290  else // verify that format corresponds to expected number of channels
291  {
292  UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
293  UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
294  UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
295  UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
296  UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
297  UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
298  }
299  }
300 }
301 
302 LaserScan LaserScan::clone() const
303 {
304  if(angleIncrement_ > 0.0f)
305  {
306  return LaserScan(data_.clone(), format_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_.clone());
307  }
308  return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
309 }
310 
311 }
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:88
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:89
f
f
rtabmap::LaserScan::channels
int channels() const
Definition: LaserScan.h:91
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:92
UConversion.h
Some conversion functions.
rtabmap::LaserScan::clone
LaserScan clone() const
Definition: LaserScan.cpp:302
UFATAL
#define UFATAL(...)
rtabmap::LaserScan::isScanHasNormals
static bool isScanHasNormals(const Format &format)
Definition: LaserScan.cpp:112
UASSERT
#define UASSERT(condition)
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:95
rtabmap::LaserScan::isScanHasRGB
static bool isScanHasRGB(const Format &format)
Definition: LaserScan.cpp:116
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:108
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:96
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:98
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:97
rtabmap::LaserScan::formatName
std::string formatName() const
Definition: LaserScan.h:90
ULogger.h
ULogger class and convenient macros.
rtabmap::LaserScan::isScan2d
static bool isScan2d(const Format &format)
Definition: LaserScan.cpp:108
UWARN
#define UWARN(...)
rtabmap::LaserScan::localTransform_
Transform localTransform_
Definition: LaserScan.h:126
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
rtabmap::LaserScan::isScanHasIntensity
static bool isScanHasIntensity(const Format &format)
Definition: LaserScan.cpp:120
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:125
glm::ceil
GLM_FUNC_DECL genType ceil(genType const &x)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59