util3d.hpp
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 #ifndef UTIL3D_HPP_
29 #define UTIL3D_HPP_
30 
31 #include <rtabmap/core/util3d_transforms.h>
32 
33 namespace rtabmap{
34 namespace util3d{
35 
36 template<typename PointCloud2T>
37 LaserScan 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[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
45 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
46  std::uint32_t fieldOffsets[8] = {0};
47 #else
48  pcl::uint32_t fieldOffsets[8] = {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
106  {
107  UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
108  }
109  }
110  if(fieldStates[0]==0 || fieldStates[1]==0)
111  {
112  //should have at least x and y set
113  UERROR("Cloud has not corresponding fields to laser scan!");
114  return LaserScan();
115  }
116 
117  bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
118  bool hasIntensity = fieldStates[7];
119  bool hasRGB = !hasIntensity&&fieldStates[6];
120  bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
121 
122  LaserScan::Format format;
123  int outputNormalOffset = 0;
124  if(is3D)
125  {
126  if(hasNormals && hasIntensity)
127  {
128  format = LaserScan::kXYZINormal;
129  outputNormalOffset = 4;
130  }
131  else if(hasNormals && !hasIntensity && !hasRGB)
132  {
133  format = LaserScan::kXYZNormal;
134  outputNormalOffset = 3;
135  }
136  else if(hasNormals && hasRGB)
137  {
138  format = LaserScan::kXYZRGBNormal;
139  outputNormalOffset = 4;
140  }
141  else if(!hasNormals && hasIntensity)
142  {
143  format = LaserScan::kXYZI;
144  }
145  else if(!hasNormals && hasRGB)
146  {
147  format = LaserScan::kXYZRGB;
148  }
149  else
150  {
151  format = LaserScan::kXYZ;
152  }
153  }
154  else
155  {
156  if(hasNormals && hasIntensity)
157  {
158  format = LaserScan::kXYINormal;
159  outputNormalOffset = 3;
160  }
161  else if(hasNormals && !hasIntensity)
162  {
163  format = LaserScan::kXYNormal;
164  outputNormalOffset = 2;
165  }
166  else if(!hasNormals && hasIntensity)
167  {
168  format = LaserScan::kXYI;
169  }
170  else
171  {
172  format = LaserScan::kXY;
173  }
174  }
175 
176  UASSERT(cloud.data.size()/cloud.point_step == (uint32_t)cloud.height*cloud.width);
177  cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
178 
179  bool transformValid = !transform.isNull() && !transform.isIdentity();
180  Transform transformRot;
181  if(transformValid)
182  {
183  transformRot = transform.rotation();
184  }
185  int oi=0;
186  for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
187  {
188  const uint8_t* row_data = &cloud.data[row * cloud.row_step];
189  for (uint32_t col = 0; col < (uint32_t)cloud.width; ++col)
190  {
191  const uint8_t* msg_data = row_data + col * cloud.point_step;
192 
193  float * ptr = laserScan.ptr<float>(0, oi);
194 
195  bool valid = true;
196  if(laserScan.channels() == 2)
197  {
198  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
199  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
200  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]);
201  }
202  else if(laserScan.channels() == 3)
203  {
204  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
205  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
206  if(format == LaserScan::kXYI)
207  {
208  ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
209  }
210  else // XYZ
211  {
212  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
213  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
214  }
215  }
216  else if(laserScan.channels() == 4)
217  {
218  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
219  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
220  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
221  if(format == LaserScan::kXYZI)
222  {
223  ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
224  }
225  else // XYZRGB
226  {
227 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
228  std::uint8_t b=*(msg_data + fieldOffsets[6]);
229  std::uint8_t g=*(msg_data + fieldOffsets[6]+1);
230  std::uint8_t r=*(msg_data + fieldOffsets[6]+2);
231 #else
232  pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
233  pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
234  pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
235 #endif
236  int * ptrInt = (int*)ptr;
237  ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
238  }
239  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
240  }
241  else if(laserScan.channels() == 5)
242  {
243  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
244  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
245  ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
246  ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
247  ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
248  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
249  }
250  else if(laserScan.channels() == 6)
251  {
252  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
253  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
254  if(format == LaserScan::kXYINormal)
255  {
256  ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
257  }
258  else // XYZNormal
259  {
260  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
261  }
262  ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
263  ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
264  ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
265  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
266  }
267  else if(laserScan.channels() == 7)
268  {
269  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
270  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
271  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
272  if(format == LaserScan::kXYZINormal)
273  {
274  ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
275  }
276  else // XYZRGBNormal
277  {
278 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
279  std::uint8_t b=*(msg_data + fieldOffsets[6]);
280  std::uint8_t g=*(msg_data + fieldOffsets[6]+1);
281  std::uint8_t r=*(msg_data + fieldOffsets[6]+2);
282 #else
283  pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
284  pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
285  pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
286 #endif
287  int * ptrInt = (int*)ptr;
288  ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
289  }
290  ptr[4] = *(float*)(msg_data + fieldOffsets[3]);
291  ptr[5] = *(float*)(msg_data + fieldOffsets[4]);
292  ptr[6] = *(float*)(msg_data + fieldOffsets[5]);
293  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]) && uIsFinite(ptr[6]);
294  }
295  else
296  {
297  UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
298  }
299 
300  if(!filterNaNs || valid)
301  {
302  if(valid && transformValid)
303  {
304  cv::Point3f pt = util3d::transformPoint(cv::Point3f(ptr[0], ptr[1], is3D?ptr[3]:0), transform);
305  ptr[0] = pt.x;
306  ptr[1] = pt.y;
307  if(is3D)
308  {
309  ptr[2] = pt.z;
310  }
311  if(hasNormals)
312  {
313  pt = util3d::transformPoint(cv::Point3f(ptr[outputNormalOffset], ptr[outputNormalOffset+1], ptr[outputNormalOffset+2]), transformRot);
314  ptr[outputNormalOffset] = pt.x;
315  ptr[outputNormalOffset+1] = pt.y;
316  ptr[outputNormalOffset+2] = pt.z;
317  }
318  }
319 
320  ++oi;
321  }
322  }
323  }
324  if(oi == 0)
325  {
326  return LaserScan();
327  }
328  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
329 }
330 
331 }
332 }
333 
334 #endif /* UTIL3D_HPP_ */
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::LaserScan::channels
int channels() const
Definition: LaserScan.h:115
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
UFATAL
#define UFATAL(...)
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UASSERT
#define UASSERT(condition)
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
util3d_transforms.h
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
UWARN
#define UWARN(...)
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58