util3d_transforms.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/util3d_transforms.h"
29 
30 #include <pcl/common/transforms.h>
31 #include <rtabmap/utilite/ULogger.h>
32 
33 namespace rtabmap
34 {
35 
36 namespace util3d
37 {
38 
39 LaserScan transformLaserScan(const LaserScan & laserScan, const Transform & transform)
40 {
41  cv::Mat output = laserScan.data().clone();
42 
43  if(!transform.isNull() && !transform.isIdentity())
44  {
45  Eigen::Affine3f transform3f = transform.toEigen3f();
46  int nx = laserScan.getNormalsOffset();
47  int ny = nx+1;
48  int nz = ny+1;
49  for(int i=0; i<laserScan.size(); ++i)
50  {
51  const float * ptr = laserScan.data().ptr<float>(0, i);
52  float * out = output.ptr<float>(0, i);
53  if(nx == -1)
54  {
55  pcl::PointXYZ pt(ptr[0], ptr[1], laserScan.is2d()?0:ptr[2]);
56  pt = pcl::transformPoint(pt, transform3f);
57  out[0] = pt.x;
58  out[1] = pt.y;
59  if(!laserScan.is2d())
60  {
61  out[2] = pt.z;
62  }
63  }
64  else
65  {
66  pcl::PointNormal pt;
67  pt.x=ptr[0];
68  pt.y=ptr[1];
69  pt.z=laserScan.is2d()?0:ptr[2];
70  pt.normal_x=ptr[nx];
71  pt.normal_y=ptr[ny];
72  pt.normal_z=ptr[nz];
73  pt = util3d::transformPoint(pt, transform);
74  out[0] = pt.x;
75  out[1] = pt.y;
76  if(!laserScan.is2d())
77  {
78  out[2] = pt.z;
79  }
80  out[nx] = pt.normal_x;
81  out[ny] = pt.normal_y;
82  out[nz] = pt.normal_z;
83  }
84  }
85  }
86  if(laserScan.angleIncrement() > 0.0f)
87  {
88  return LaserScan(output,
89  laserScan.format(),
90  laserScan.rangeMin(),
91  laserScan.rangeMax(),
92  laserScan.angleMin(),
93  laserScan.angleMax(),
94  laserScan.angleIncrement(),
95  laserScan.localTransform());
96  }
97  else
98  {
99  return LaserScan(output,
100  laserScan.maxPoints(),
101  laserScan.rangeMax(),
102  laserScan.format(),
103  laserScan.localTransform());
104  }
105 }
106 
107 pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
108  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
109  const Transform & transform)
110 {
111  pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
112  pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
113  return output;
114 }
115 pcl::PointCloud<pcl::PointXYZI>::Ptr transformPointCloud(
116  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
117  const Transform & transform)
118 {
119  pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
120  pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
121  return output;
122 }
123 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
124  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
125  const Transform & transform)
126 {
127  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
128  pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
129  return output;
130 }
131 pcl::PointCloud<pcl::PointNormal>::Ptr transformPointCloud(
132  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
133  const Transform & transform)
134 {
135  pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
136  pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
137  return output;
138 }
139 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
140  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
141  const Transform & transform)
142 {
143  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
144  pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
145  return output;
146 }
147 pcl::PointCloud<pcl::PointXYZINormal>::Ptr transformPointCloud(
148  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
149  const Transform & transform)
150 {
151  pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
152  pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
153  return output;
154 }
155 
156 pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
157  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
158  const pcl::IndicesPtr & indices,
159  const Transform & transform)
160 {
161  pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
162  pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
163  return output;
164 }
165 pcl::PointCloud<pcl::PointXYZI>::Ptr transformPointCloud(
166  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
167  const pcl::IndicesPtr & indices,
168  const Transform & transform)
169 {
170  pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
171  pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
172  return output;
173 }
174 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
175  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
176  const pcl::IndicesPtr & indices,
177  const Transform & transform)
178 {
179  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
180  pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
181  return output;
182 }
183 pcl::PointCloud<pcl::PointNormal>::Ptr transformPointCloud(
184  const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
185  const pcl::IndicesPtr & indices,
186  const Transform & transform)
187 {
188  pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
189  pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
190  return output;
191 }
192 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
193  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
194  const pcl::IndicesPtr & indices,
195  const Transform & transform)
196 {
197  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
198  pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
199  return output;
200 }
201 pcl::PointCloud<pcl::PointXYZINormal>::Ptr transformPointCloud(
202  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
203  const pcl::IndicesPtr & indices,
204  const Transform & transform)
205 {
206  pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
207  pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
208  return output;
209 }
210 
211 cv::Point3f transformPoint(
212  const cv::Point3f & point,
213  const Transform & transform)
214 {
215  cv::Point3f ret = point;
216  ret.x = transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3);
217  ret.y = transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3);
218  ret.z = transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3);
219  return ret;
220 }
221 cv::Point3d transformPoint(
222  const cv::Point3d & point,
223  const Transform & transform)
224 {
225  cv::Point3d ret = point;
226  ret.x = transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3);
227  ret.y = transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3);
228  ret.z = transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3);
229  return ret;
230 }
231 pcl::PointXYZ transformPoint(
232  const pcl::PointXYZ & pt,
233  const Transform & transform)
234 {
235  return pcl::transformPoint(pt, transform.toEigen3f());
236 }
237 pcl::PointXYZI transformPoint(
238  const pcl::PointXYZI & pt,
239  const Transform & transform)
240 {
241  return pcl::transformPoint(pt, transform.toEigen3f());
242 }
243 pcl::PointXYZRGB transformPoint(
244  const pcl::PointXYZRGB & pt,
245  const Transform & transform)
246 {
247  pcl::PointXYZRGB ptRGB = pcl::transformPoint(pt, transform.toEigen3f());
248  ptRGB.rgb = pt.rgb;
249  return ptRGB;
250 }
251 pcl::PointNormal transformPoint(
252  const pcl::PointNormal & point,
253  const Transform & transform)
254 {
255  pcl::PointNormal ret;
256  Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
257  ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
258  ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
259  ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
260 
261  // Rotate normals
262  Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
263  ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
264  ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
265  ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
266  return ret;
267 }
268 pcl::PointXYZRGBNormal transformPoint(
269  const pcl::PointXYZRGBNormal & point,
270  const Transform & transform)
271 {
272  pcl::PointXYZRGBNormal ret;
273  Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
274  ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
275  ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
276  ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
277 
278  // Rotate normals
279  Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
280  ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
281  ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
282  ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
283 
284  ret.rgb = point.rgb;
285  return ret;
286 }
287 pcl::PointXYZINormal transformPoint(
288  const pcl::PointXYZINormal & point,
289  const Transform & transform)
290 {
291  pcl::PointXYZINormal ret;
292  Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
293  ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
294  ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
295  ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
296 
297  // Rotate normals
298  Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
299  ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
300  ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
301  ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
302 
303  ret.intensity = point.intensity;
304  return ret;
305 }
306 
307 }
308 
309 }
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:119
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:137
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
rtabmap::util3d::transformLaserScan
LaserScan RTABMAP_EXP transformLaserScan(const LaserScan &laserScan, const Transform &transform)
Definition: util3d_transforms.cpp:39
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:117
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:381
util3d_transforms.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:120
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
Definition: Transform.cpp:362
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:121
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136

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