Fawkes API  Fawkes Development Version
transforms.h
1 
2 /***************************************************************************
3  * transforms.h - PCL utilities: apply transforms to point clouds
4  *
5  * Created: Fri Nov 30 13:33:40 2012
6  * Copyright 2012 Tim Niemueller [www.niemueller.de]
7  * 2010 Willow Garage, Inc.
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef _LIBS_PCL_UTILS_TRANSFORMS_H_
24 #define _LIBS_PCL_UTILS_TRANSFORMS_H_
25 
26 #include <pcl/common/transforms.h>
27 #include <pcl/point_cloud.h>
28 #include <pcl_utils/utils.h>
29 #include <tf/transformer.h>
30 #include <tf/types.h>
31 
32 namespace fawkes {
33 namespace pcl_utils {
34 
35 /** Apply a rigid transform.
36  * @param cloud_in the input point cloud
37  * @param cloud_out the input point cloud
38  * @param transform a rigid transformation from tf
39  * @note calls the Eigen version
40  */
41 template <typename PointT>
42 void
43 transform_pointcloud(const pcl::PointCloud<PointT> &cloud_in,
44  pcl::PointCloud<PointT> & cloud_out,
45  const tf::Transform & transform)
46 {
47  // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w
48  // order, despite the ordering of arguments in Eigen's
49  // constructor. We could use an Eigen Map to convert without copy,
50  // but this only works if Bullet uses floats, that is if
51  // BT_USE_DOUBLE_PRECISION is not defined. Rather that risking a
52  // mistake, we copy the quaternion, which is a small cost compared
53  // to the conversion of the point cloud anyway. Idem for the origin.
54 
55  tf::Quaternion q = transform.getRotation();
56  Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
57  tf::Vector3 v = transform.getOrigin();
58  Eigen::Vector3f origin(v.x(), v.y(), v.z());
59  pcl::transformPointCloud(cloud_in, cloud_out, origin, rotation);
60 }
61 
62 /** Apply a rigid transform.
63  * @param cloud_inout input and output point cloud
64  * @param transform a rigid transformation from tf
65  * @note calls the Eigen version
66  */
67 template <typename PointT>
68 void
69 transform_pointcloud(pcl::PointCloud<PointT> &cloud_inout, const tf::Transform &transform)
70 {
72  transform_pointcloud(cloud_inout, tmp, transform);
73  cloud_inout = tmp;
74 }
75 
76 /** Transform a point cloud in a given target TF frame using the given transfomer.
77  * @param target_frame the target TF frame the point cloud should be transformed to
78  * @param cloud_in input point cloud
79  * @param cloud_out output point cloud
80  * @param transformer TF transformer
81  * @exception tf::TransformException if transform retrieval fails
82  */
83 template <typename PointT>
84 void
85 transform_pointcloud(const std::string & target_frame,
86  const pcl::PointCloud<PointT> &cloud_in,
87  pcl::PointCloud<PointT> & cloud_out,
88  const tf::Transformer & transformer)
89 {
90  if (cloud_in.header.frame_id == target_frame) {
91  cloud_out = cloud_in;
92  return;
93  }
94 
95  fawkes::Time source_time;
96  pcl_utils::get_time(cloud_in, source_time);
97  tf::StampedTransform transform;
98  transformer.lookup_transform(target_frame, cloud_in.header.frame_id, source_time, transform);
99 
100  transform_pointcloud(cloud_in, cloud_out, transform);
101  cloud_out.header.frame_id = target_frame;
102 }
103 
104 /** Transform a point cloud in a given target TF frame using the given transfomer.
105  * @param target_frame the target TF frame the point cloud should be transformed to
106  * @param cloud_inout input and output point cloud
107  * @param transformer TF transformer
108  * @exception tf::TransformException if transform retrieval fails
109  */
110 template <typename PointT>
111 void
112 transform_pointcloud(const std::string & target_frame,
113  pcl::PointCloud<PointT> &cloud_inout,
114  const tf::Transformer & transformer)
115 {
117  transform_pointcloud(target_frame, cloud_inout, tmp, transformer);
118  cloud_inout = tmp;
119 }
120 
121 /** Transform a point cloud in a given target TF frame using the given transfomer.
122  * @param target_frame the target TF frame the point cloud should be transformed to
123  * @param cloud_in input point cloud
124  * @param cloud_out output point cloud
125  * @param transformer TF transformer
126  * @exception tf::TransformException if transform retrieval fails
127  */
128 template <typename PointT>
129 void
130 transform_pointcloud(const std::string & target_frame,
131  const Time & target_time,
132  const std::string & fixed_frame,
133  const pcl::PointCloud<PointT> &cloud_in,
134  pcl::PointCloud<PointT> & cloud_out,
135  const tf::Transformer & transformer)
136 {
137  if (cloud_in.header.frame_id == target_frame) {
138  cloud_out = cloud_in;
139  return;
140  }
141 
142  fawkes::Time source_time;
143  pcl_utils::get_time(cloud_in, source_time);
144 
145  tf::StampedTransform transform;
146  transformer.lookup_transform(
147  target_frame, target_time, cloud_in.header.frame_id, source_time, fixed_frame, transform);
148 
149  transform_pointcloud(cloud_in, cloud_out, transform);
150  cloud_out.header.frame_id = target_frame;
151 
152  pcl_utils::set_time(cloud_out, target_time);
153 }
154 
155 /** Transform a point cloud in a given target TF frame using the given transfomer.
156  * @param target_frame the target TF frame the point cloud should be transformed to
157  * @param cloud_inout input and output point cloud
158  * @param transformer TF transformer
159  * @exception tf::TransformException if transform retrieval fails
160  */
161 template <typename PointT>
162 void
163 transform_pointcloud(const std::string & target_frame,
164  const Time & target_time,
165  const std::string & fixed_frame,
166  pcl::PointCloud<PointT> &cloud_inout,
167  const tf::Transformer & transformer)
168 {
170  transform_pointcloud(target_frame, target_time, fixed_frame, cloud_inout, tmp, transformer);
171  cloud_inout = tmp;
172 }
173 
174 } // end namespace pcl_utils
175 } // end namespace fawkes
176 
177 #endif
A class for handling time.
Definition: time.h:93
Fawkes library namespace.