RTAB-Map 0.23.10
Real-Time Appearance-Based Mapping
Loading...
Searching...
No Matches
util3d.hpp
1/*
2Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without
6modification, 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
16THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23ON 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
25SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26*/
27
28#ifndef UTIL3D_HPP_
29#define UTIL3D_HPP_
30
31#include <rtabmap/core/util3d_transforms.h>
32
33namespace rtabmap{
34namespace util3d{
35
36template<typename PointCloud2T>
37LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, bool is2D, const Transform & transform)
38{
39 if(cloud.data.empty())
40 {
41 return LaserScan();
42 }
43 //determine the output type
44 int fieldStates[10] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity,time,ring
45#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
46 std::uint32_t fieldOffsets[10] = {0};
47#else
48 pcl::uint32_t fieldOffsets[10] = {0};
49#endif
50 for(unsigned int i=0; i<cloud.fields.size(); ++i)
51 {
52 if(cloud.fields[i].name.compare("x") == 0)
53 {
54 fieldStates[0] = 1;
55 fieldOffsets[0] = cloud.fields[i].offset;
56 }
57 else if(cloud.fields[i].name.compare("y") == 0)
58 {
59 fieldStates[1] = 1;
60 fieldOffsets[1] = cloud.fields[i].offset;
61 }
62 else if(cloud.fields[i].name.compare("z") == 0 && !is2D)
63 {
64 fieldStates[2] = 1;
65 fieldOffsets[2] = cloud.fields[i].offset;
66 }
67 else if(cloud.fields[i].name.compare("normal_x") == 0)
68 {
69 fieldStates[3] = 1;
70 fieldOffsets[3] = cloud.fields[i].offset;
71 }
72 else if(cloud.fields[i].name.compare("normal_y") == 0)
73 {
74 fieldStates[4] = 1;
75 fieldOffsets[4] = cloud.fields[i].offset;
76 }
77 else if(cloud.fields[i].name.compare("normal_z") == 0)
78 {
79 fieldStates[5] = 1;
80 fieldOffsets[5] = cloud.fields[i].offset;
81 }
82 else if(cloud.fields[i].name.compare("rgb") == 0 || cloud.fields[i].name.compare("rgba") == 0)
83 {
84 fieldStates[6] = 1;
85 fieldOffsets[6] = cloud.fields[i].offset;
86 }
87 else if(cloud.fields[i].name.compare("intensity") == 0)
88 {
89 if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
90 {
91 static bool warningShown = false;
92 if(!warningShown)
93 {
94 UWARN("The input scan cloud has an \"intensity\" field "
95 "but the datatype (%d) is not supported. Intensity will be ignored. "
96 "This message is only shown once.", cloud.fields[i].datatype);
97 warningShown = true;
98 }
99 continue;
100 }
101
102 fieldStates[7] = 1;
103 fieldOffsets[7] = cloud.fields[i].offset;
104 }
105 else if(cloud.fields[i].name.compare("time") == 0)
106 {
107 if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
108 {
109 static bool warningShown = false;
110 if(!warningShown)
111 {
112 UWARN("The input scan cloud has an \"time\" field "
113 "but the datatype (%d) is not supported. Time will be ignored. "
114 "This message is only shown once.", cloud.fields[i].datatype);
115 warningShown = true;
116 }
117 continue;
118 }
119
120 fieldStates[8] = 1;
121 fieldOffsets[8] = cloud.fields[i].offset;
122 }
123 else if(cloud.fields[i].name.compare("ring") == 0)
124 {
125 if(cloud.fields[i].datatype != pcl::PCLPointField::UINT16)
126 {
127 static bool warningShown = false;
128 if(!warningShown)
129 {
130 UWARN("The input scan cloud has an \"ring\" field "
131 "but the datatype (%d) is not supported. Ring will be ignored. "
132 "This message is only shown once.", cloud.fields[i].datatype);
133 warningShown = true;
134 }
135 continue;
136 }
137
138 fieldStates[9] = 1;
139 fieldOffsets[9] = cloud.fields[i].offset;
140 }
141 else
142 {
143 UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
144 }
145 }
146 if(fieldStates[0]==0 || fieldStates[1]==0)
147 {
148 //should have at least x and y set
149 UERROR("Cloud has not corresponding fields to laser scan!");
150 return LaserScan();
151 }
152
153 bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
154 bool hasIntensity = fieldStates[7];
155 bool hasRGB = !hasIntensity&&fieldStates[6];
156 bool hasTime = hasIntensity&&fieldStates[8];
157 bool hasRing = hasIntensity&&fieldStates[9];
158 bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
159
160 LaserScan::Format format;
161 int outputNormalOffset = 0;
162 if(is3D)
163 {
164 if(hasNormals && hasIntensity)
165 {
166 format = LaserScan::kXYZINormal;
167 outputNormalOffset = 4;
168 }
169 else if(hasNormals && !hasIntensity && !hasRGB)
170 {
171 format = LaserScan::kXYZNormal;
172 outputNormalOffset = 3;
173 }
174 else if(hasNormals && hasRGB)
175 {
176 format = LaserScan::kXYZRGBNormal;
177 outputNormalOffset = 4;
178 }
179 else if(!hasNormals && hasIntensity)
180 {
181 if(hasTime && hasRing)
182 {
183 format = LaserScan::kXYZIRT;
184 }
185 else if(hasTime)
186 {
187 format = LaserScan::kXYZIT;
188 }
189 else
190 {
191 format = LaserScan::kXYZI;
192 }
193 }
194 else if(!hasNormals && hasRGB)
195 {
196 format = LaserScan::kXYZRGB;
197 }
198 else
199 {
200 format = LaserScan::kXYZ;
201 }
202 }
203 else
204 {
205 if(hasNormals && hasIntensity)
206 {
207 format = LaserScan::kXYINormal;
208 outputNormalOffset = 3;
209 }
210 else if(hasNormals && !hasIntensity)
211 {
212 format = LaserScan::kXYNormal;
213 outputNormalOffset = 2;
214 }
215 else if(!hasNormals && hasIntensity)
216 {
217 format = LaserScan::kXYI;
218 }
219 else
220 {
221 format = LaserScan::kXY;
222 }
223 }
224
225 UASSERT(cloud.data.size()/cloud.point_step == (uint32_t)cloud.height*cloud.width);
226 cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
227
228 bool transformValid = !transform.isNull() && !transform.isIdentity();
229 Transform transformRot;
230 if(transformValid)
231 {
232 transformRot = transform.rotation();
233 }
234 int oi=0;
235 UASSERT(cloud.height == 1 || cloud.row_step != 0);
236 for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
237 {
238 const uint8_t* row_data = &cloud.data[row * cloud.row_step];
239 for (uint32_t col = 0; col < (uint32_t)cloud.width; ++col)
240 {
241 const uint8_t* msg_data = row_data + col * cloud.point_step;
242
243 float * ptr = laserScan.ptr<float>(0, oi);
244
245 bool valid = true;
246 if(laserScan.channels() == 2)
247 {
248 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
249 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
250 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]);
251 }
252 else if(laserScan.channels() == 3)
253 {
254 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
255 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
256 if(format == LaserScan::kXYI)
257 {
258 ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
259 }
260 else // XYZ
261 {
262 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
263 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
264 }
265 }
266 else if(laserScan.channels() == 4)
267 {
268 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
269 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
270 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
271 if(format == LaserScan::kXYZI)
272 {
273 ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
274 }
275 else // XYZRGB
276 {
277#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
278 std::uint8_t b=*(msg_data + fieldOffsets[6]);
279 std::uint8_t g=*(msg_data + fieldOffsets[6]+1);
280 std::uint8_t r=*(msg_data + fieldOffsets[6]+2);
281#else
282 pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
283 pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
284 pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
285#endif
286 int * ptrInt = (int*)ptr;
287 ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
288 }
289 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
290 }
291 else if(laserScan.channels() == 5)
292 {
293 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
294 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
295 if(format == LaserScan::kXYZIT)
296 {
297 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
298 ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
299 ptr[4] = *(float*)(msg_data + fieldOffsets[8]);
300 }
301 else // kXYNormal
302 {
303 ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
304 ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
305 ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
306 }
307 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
308 }
309 else if(laserScan.channels() == 6)
310 {
311 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
312 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
313 if(format == LaserScan::kXYZIRT)
314 {
315 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
316 ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
317 ptr[4] = float(*(unsigned short*)(msg_data + fieldOffsets[9])); // Convert 16U to float
318 ptr[5] = *(float*)(msg_data + fieldOffsets[8]);
319 }
320 else // with normal
321 {
322 if(format == LaserScan::kXYINormal)
323 {
324 ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
325 }
326 else // XYZNormal
327 {
328 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
329 }
330 ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
331 ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
332 ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
333 }
334 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
335 }
336 else if(laserScan.channels() == 7)
337 {
338 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
339 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
340 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
341 if(format == LaserScan::kXYZINormal)
342 {
343 ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
344 }
345 else // XYZRGBNormal
346 {
347#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
348 std::uint8_t b=*(msg_data + fieldOffsets[6]);
349 std::uint8_t g=*(msg_data + fieldOffsets[6]+1);
350 std::uint8_t r=*(msg_data + fieldOffsets[6]+2);
351#else
352 pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
353 pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
354 pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
355#endif
356 int * ptrInt = (int*)ptr;
357 ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
358 }
359 ptr[4] = *(float*)(msg_data + fieldOffsets[3]);
360 ptr[5] = *(float*)(msg_data + fieldOffsets[4]);
361 ptr[6] = *(float*)(msg_data + fieldOffsets[5]);
362 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]) && uIsFinite(ptr[6]);
363 }
364 else
365 {
366 UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
367 }
368
369 if(!filterNaNs || valid)
370 {
371 if(valid && transformValid)
372 {
373 cv::Point3f pt = util3d::transformPoint(cv::Point3f(ptr[0], ptr[1], is3D?ptr[3]:0), transform);
374 ptr[0] = pt.x;
375 ptr[1] = pt.y;
376 if(is3D)
377 {
378 ptr[2] = pt.z;
379 }
380 if(hasNormals)
381 {
382 pt = util3d::transformPoint(cv::Point3f(ptr[outputNormalOffset], ptr[outputNormalOffset+1], ptr[outputNormalOffset+2]), transformRot);
383 ptr[outputNormalOffset] = pt.x;
384 ptr[outputNormalOffset+1] = pt.y;
385 ptr[outputNormalOffset+2] = pt.z;
386 }
387 }
388
389 ++oi;
390 }
391 }
392 }
393 if(oi == 0)
394 {
395 return LaserScan();
396 }
397 return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
398}
399
400}
401}
402
403#endif /* UTIL3D_HPP_ */
UERROR
#define UERROR(...)
Definition ULogger.h:62
UDEBUG
#define UDEBUG(...)
Definition ULogger.h:59
UFATAL
#define UFATAL(...)
Definition ULogger.h:63
UASSERT
#define UASSERT(condition)
Definition ULogger.h:66
UWARN
#define UWARN(...)
Definition ULogger.h:61
uIsFinite
bool uIsFinite(const T &value)
Definition UMath.h:53
rtabmap::LaserScan
Represents 2D or 3D laser scan data with support for multiple point data formats.
Definition LaserScan.h:46
rtabmap::LaserScan::Format
Format
Enumeration of possible formats for laser scan data.
Definition LaserScan.h:55
rtabmap::Transform
Represents a 3D rigid body transformation (rotation + translation).
Definition Transform.h:53
rtabmap::Transform::isNull
bool isNull() const
Checks whether the transform is null (all zeros).
rtabmap::Transform::isIdentity
bool isIdentity() const
Checks whether the transform is identity.
rtabmap::Transform::rotation
Transform rotation() const
Returns only the rotation component.
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Convert pcl::PCLPointCloud2 to rtabmap::LaserScan with all supported fields (see rtabmap::LaserScan::...
Definition util3d.hpp:37
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Transforms cv::Point3f point type.