transforms.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  *
35  */
36 
37 #ifndef pcl_ros_IMPL_TRANSFORMS_H_
38 #define pcl_ros_IMPL_TRANSFORMS_H_
39 
41 #include "pcl_ros/transforms.h"
42 #include <tf2_eigen/tf2_eigen.h>
43 
46 
47 namespace pcl_ros
48 {
50 template <typename PointT> void
51 transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
52  pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
53 {
54  // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
55  // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
56  // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
57  // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
58  // the conversion of the point cloud anyway. Idem for the origin.
59  tf::Quaternion q = transform.getRotation ();
60  Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w)
61  tf::Vector3 v = transform.getOrigin ();
62  Eigen::Vector3f origin (v.x (), v.y (), v.z ());
63  // Eigen::Translation3f translation(v);
64  // Assemble an Eigen Transform
65  //Eigen::Transform3f t;
66  //t = translation * rotation;
67  transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation);
68 }
69 
71 template <typename PointT> void
72 transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
73  pcl::PointCloud <PointT> &cloud_out, const geometry_msgs::Transform &transform)
74 {
75  Eigen::Quaterniond rotation;
76  tf2::fromMsg(transform.rotation, rotation);
77  Eigen::Vector3d origin;
78  tf2::fromMsg(transform.translation, origin);
79 
80  transformPointCloudWithNormals (cloud_in, cloud_out, origin.cast<float>(), rotation.cast<float>());
81 }
82 
84 template <typename PointT> void
85 transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
86  pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
87 {
88  // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
89  // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
90  // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
91  // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
92  // the conversion of the point cloud anyway. Idem for the origin.
93  tf::Quaternion q = transform.getRotation ();
94  Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w)
95  tf::Vector3 v = transform.getOrigin ();
96  Eigen::Vector3f origin (v.x (), v.y (), v.z ());
97  // Eigen::Translation3f translation(v);
98  // Assemble an Eigen Transform
99  //Eigen::Transform3f t;
100  //t = translation * rotation;
101  transformPointCloud (cloud_in, cloud_out, origin, rotation);
102 }
103 
105 template <typename PointT> void
106 transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
107  pcl::PointCloud <PointT> &cloud_out, const geometry_msgs::Transform &transform)
108 {
109  Eigen::Quaterniond rotation;
110  tf2::fromMsg(transform.rotation, rotation);
111  Eigen::Vector3d origin;
112  tf2::fromMsg(transform.translation, origin);
113 
114  transformPointCloud (cloud_in, cloud_out, origin.cast<float>(), rotation.cast<float>());
115 }
116 
118 template <typename PointT> bool
119 transformPointCloudWithNormals (const std::string &target_frame,
120  const pcl::PointCloud <PointT> &cloud_in,
121  pcl::PointCloud <PointT> &cloud_out,
122  const tf::TransformListener &tf_listener)
123 {
124  if (cloud_in.header.frame_id == target_frame)
125  {
126  cloud_out = cloud_in;
127  return (true);
128  }
129 
130  tf::StampedTransform transform;
131  try
132  {
133  tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
134  }
135  catch (const tf::TransformException &e)
136  {
137  ROS_ERROR ("%s", e.what ());
138  return (false);
139  }
140 
141  transformPointCloudWithNormals (cloud_in, cloud_out, transform);
142  cloud_out.header.frame_id = target_frame;
143  return (true);
144 }
145 
147 template <typename PointT> bool
148 transformPointCloudWithNormals (const std::string &target_frame,
149  const pcl::PointCloud <PointT> &cloud_in,
150  pcl::PointCloud <PointT> &cloud_out,
151  const tf2_ros::Buffer &tf_buffer)
152 {
153  if (cloud_in.header.frame_id == target_frame)
154  {
155  cloud_out = cloud_in;
156  return (true);
157  }
158 
159  geometry_msgs::TransformStamped transform;
160  try
161  {
162  transform = tf_buffer.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp);
163  }
164  catch (tf2::LookupException &e)
165  {
166  ROS_ERROR ("%s", e.what ());
167  return (false);
168  }
169  catch (tf2::ExtrapolationException &e)
170  {
171  ROS_ERROR ("%s", e.what ());
172  return (false);
173  }
174 
175  transformPointCloudWithNormals (cloud_in, cloud_out, transform.transform);
176  cloud_out.header.frame_id = target_frame;
177  return (true);
178 }
179 
181 template <typename PointT> bool
182 transformPointCloud (const std::string &target_frame,
183  const pcl::PointCloud <PointT> &cloud_in,
184  pcl::PointCloud <PointT> &cloud_out,
185  const tf::TransformListener &tf_listener)
186 {
187  if (cloud_in.header.frame_id == target_frame)
188  {
189  cloud_out = cloud_in;
190  return (true);
191  }
192 
193  tf::StampedTransform transform;
194  try
195  {
196  tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
197  }
198  catch (const tf::TransformException &e)
199  {
200  ROS_ERROR ("%s", e.what ());
201  return (false);
202  }
203 
204  transformPointCloud (cloud_in, cloud_out, transform);
205  cloud_out.header.frame_id = target_frame;
206  return (true);
207 }
208 
210 template <typename PointT> bool
211 transformPointCloud (const std::string &target_frame,
212  const pcl::PointCloud <PointT> &cloud_in,
213  pcl::PointCloud <PointT> &cloud_out,
214  const tf2_ros::Buffer &tf_buffer)
215 {
216  if (cloud_in.header.frame_id == target_frame)
217  {
218  cloud_out = cloud_in;
219  return (true);
220  }
221 
222  geometry_msgs::TransformStamped transform;
223  try
224  {
225  transform = tf_buffer.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp);
226  }
227  catch (tf2::LookupException &e)
228  {
229  ROS_ERROR ("%s", e.what ());
230  return (false);
231  }
232  catch (tf2::ExtrapolationException &e)
233  {
234  ROS_ERROR ("%s", e.what ());
235  return (false);
236  }
237  transformPointCloud (cloud_in, cloud_out, transform.transform);
238  cloud_out.header.frame_id = target_frame;
239  return (true);
240 }
241 
243 template <typename PointT> bool
244 transformPointCloud (const std::string &target_frame,
245  const ros::Time & target_time,
246  const pcl::PointCloud <PointT> &cloud_in,
247  const std::string &fixed_frame,
248  pcl::PointCloud <PointT> &cloud_out,
249  const tf::TransformListener &tf_listener)
250 {
251  tf::StampedTransform transform;
252  try
253  {
254  tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
255  }
256  catch (const tf::TransformException &e)
257  {
258  ROS_ERROR ("%s", e.what ());
259  return (false);
260  }
261 
262  transformPointCloud (cloud_in, cloud_out, transform);
263  cloud_out.header.frame_id = target_frame;
264  cloud_out.header.seq = cloud_in.header.seq;
265  cloud_out.header.stamp = toPCL(target_time);
266  return (true);
267 }
268 
270 template <typename PointT> bool
271 transformPointCloud (const std::string &target_frame,
272  const ros::Time & target_time,
273  const pcl::PointCloud <PointT> &cloud_in,
274  const std::string &fixed_frame,
275  pcl::PointCloud <PointT> &cloud_out,
276  const tf2_ros::Buffer &tf_buffer)
277 {
278  geometry_msgs::TransformStamped transform;
279  try
280  {
281  transform = tf_buffer.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame);
282  }
283  catch (tf2::LookupException &e)
284  {
285  ROS_ERROR ("%s", e.what ());
286  return (false);
287  }
288  catch (tf2::ExtrapolationException &e)
289  {
290  ROS_ERROR ("%s", e.what ());
291  return (false);
292  }
293 
294  transformPointCloud (cloud_in, cloud_out, transform.transform);
295  cloud_out.header.frame_id = target_frame;
296  cloud_out.header.seq = cloud_in.header.seq;
297  cloud_out.header.stamp = toPCL(target_time);
298  return (true);
299 }
300 
302 template <typename PointT> bool
303 transformPointCloudWithNormals (const std::string &target_frame,
304  const ros::Time & target_time,
305  const pcl::PointCloud <PointT> &cloud_in,
306  const std::string &fixed_frame,
307  pcl::PointCloud <PointT> &cloud_out,
308  const tf::TransformListener &tf_listener)
309 {
310  tf::StampedTransform transform;
311  try
312  {
313  tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
314  }
315  catch (const tf::TransformException &e)
316  {
317  ROS_ERROR ("%s", e.what ());
318  return (false);
319  }
320 
321  transformPointCloudWithNormals (cloud_in, cloud_out, transform);
322  cloud_out.header.frame_id = target_frame;
323  cloud_out.header.seq = cloud_in.header.seq;
324  cloud_out.header.stamp = toPCL(target_time);
325  return (true);
326 }
327 
329 template <typename PointT> bool
330 transformPointCloudWithNormals (const std::string &target_frame,
331  const ros::Time & target_time,
332  const pcl::PointCloud <PointT> &cloud_in,
333  const std::string &fixed_frame,
334  pcl::PointCloud <PointT> &cloud_out,
335  const tf2_ros::Buffer &tf_buffer)
336 {
337  geometry_msgs::TransformStamped transform;
338  try
339  {
340  transform = tf_buffer.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame);
341  }
342  catch (tf2::LookupException &e)
343  {
344  ROS_ERROR ("%s", e.what ());
345  return (false);
346  }
347  catch (tf2::ExtrapolationException &e)
348  {
349  ROS_ERROR ("%s", e.what ());
350  return (false);
351  }
352 
353  transformPointCloudWithNormals (cloud_in, cloud_out, transform.transform);
354  cloud_out.header.frame_id = target_frame;
355  cloud_out.header.seq = cloud_in.header.seq;
356  cloud_out.header.stamp = toPCL(target_time);
357  return (true);
358 }
359 
360 } // namespace pcl_ros
361 #endif
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Transform a point cloud and rotate its normals using an Eigen transform.
Definition: transforms.hpp:51
Quaternion getRotation() const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
void fromMsg(const A &, B &b)
void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Apply a rigid transform defined by a 3D offset and a quaternion.
Definition: transforms.hpp:85
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
#define ROS_ERROR(...)


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu Feb 16 2023 03:08:03