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 <tf/types.h>
27 #include <tf/transformer.h>
28 #include <pcl_utils/utils.h>
29 
30 #include <pcl/point_cloud.h>
31 #include <pcl/common/transforms.h>
32 
33 namespace fawkes {
34  namespace pcl_utils {
35 #if 0 /* just to make Emacs auto-indent happy */
36  }
37 }
38 #endif
39 
40 /** Apply a rigid transform.
41  * @param cloud_in the input point cloud
42  * @param cloud_out the input point cloud
43  * @param transform a rigid transformation from tf
44  * @note calls the Eigen version
45  */
46 template <typename PointT> void
47 transform_pointcloud(const pcl::PointCloud<PointT> &cloud_in,
48  pcl::PointCloud<PointT> &cloud_out,
49  const tf::Transform &transform)
50 {
51  // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w
52  // order, despite the ordering of arguments in Eigen's
53  // constructor. We could use an Eigen Map to convert without copy,
54  // but this only works if Bullet uses floats, that is if
55  // BT_USE_DOUBLE_PRECISION is not defined. Rather that risking a
56  // mistake, we copy the quaternion, which is a small cost compared
57  // to the conversion of the point cloud anyway. Idem for the origin.
58 
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  pcl::transformPointCloud(cloud_in, cloud_out, origin, rotation);
64 }
65 
66 
67 /** Apply a rigid transform.
68  * @param cloud_inout input and output point cloud
69  * @param transform a rigid transformation from tf
70  * @note calls the Eigen version
71  */
72 template <typename PointT> void
73 transform_pointcloud(pcl::PointCloud<PointT> &cloud_inout,
74  const tf::Transform &transform)
75 {
77  transform_pointcloud(cloud_inout, tmp, transform);
78  cloud_inout = tmp;
79 }
80 
81 /** Transform a point cloud in a given target TF frame using the given transfomer.
82  * @param target_frame the target TF frame the point cloud should be transformed to
83  * @param cloud_in input point cloud
84  * @param cloud_out output point cloud
85  * @param transformer TF transformer
86  * @exception tf::TransformException if transform retrieval fails
87  */
88 template <typename PointT>
89 void transform_pointcloud(const std::string &target_frame,
90  const pcl::PointCloud<PointT> &cloud_in,
91  pcl::PointCloud<PointT> &cloud_out,
92  const tf::Transformer &transformer)
93 {
94  if (cloud_in.header.frame_id == target_frame) {
95  cloud_out = cloud_in;
96  return;
97  }
98 
99  fawkes::Time source_time;
100  pcl_utils::get_time(cloud_in, source_time);
101  tf::StampedTransform transform;
102  transformer.lookup_transform(target_frame, cloud_in.header.frame_id,
103  source_time, transform);
104 
105  transform_pointcloud(cloud_in, cloud_out, transform);
106  cloud_out.header.frame_id = target_frame;
107 }
108 
109 /** Transform a point cloud in a given target TF frame using the given transfomer.
110  * @param target_frame the target TF frame the point cloud should be transformed to
111  * @param cloud_inout input and output point cloud
112  * @param transformer TF transformer
113  * @exception tf::TransformException if transform retrieval fails
114  */
115 template <typename PointT>
116 void transform_pointcloud(const std::string &target_frame,
117  pcl::PointCloud<PointT> &cloud_inout,
118  const tf::Transformer &transformer)
119 {
121  transform_pointcloud(target_frame, cloud_inout, tmp, transformer);
122  cloud_inout = tmp;
123 }
124 
125 
126 /** Transform a point cloud in a given target TF frame using the given transfomer.
127  * @param target_frame the target TF frame the point cloud should be transformed to
128  * @param cloud_in input point cloud
129  * @param cloud_out output point cloud
130  * @param transformer TF transformer
131  * @exception tf::TransformException if transform retrieval fails
132  */
133 template <typename PointT>
134 void transform_pointcloud(const std::string &target_frame, const Time &target_time,
135  const std::string &fixed_frame,
136  const pcl::PointCloud<PointT> &cloud_in,
137  pcl::PointCloud<PointT> &cloud_out,
138  const tf::Transformer &transformer)
139 {
140  if (cloud_in.header.frame_id == target_frame) {
141  cloud_out = cloud_in;
142  return;
143  }
144 
145  fawkes::Time source_time;
146  pcl_utils::get_time(cloud_in, source_time);
147 
148  tf::StampedTransform transform;
149  transformer.lookup_transform(target_frame, target_time,
150  cloud_in.header.frame_id, source_time,
151  fixed_frame, transform);
152 
153  transform_pointcloud(cloud_in, cloud_out, transform);
154  cloud_out.header.frame_id = target_frame;
155 
156  pcl_utils::set_time(cloud_out, target_time);
157 }
158 
159 /** Transform a point cloud in a given target TF frame using the given transfomer.
160  * @param target_frame the target TF frame the point cloud should be transformed to
161  * @param cloud_inout input and output point cloud
162  * @param transformer TF transformer
163  * @exception tf::TransformException if transform retrieval fails
164  */
165 template <typename PointT>
166 void transform_pointcloud(const std::string &target_frame, const Time &target_time,
167  const std::string &fixed_frame,
168  pcl::PointCloud<PointT> &cloud_inout,
169  const tf::Transformer &transformer)
170 {
172  transform_pointcloud(target_frame, target_time, fixed_frame,
173  cloud_inout, tmp, transformer);
174  cloud_inout = tmp;
175 }
176 
177 } // end namespace pcl_utils
178 } // end namespace fawkes
179 
180 #endif
Fawkes library namespace.
A class for handling time.
Definition: time.h:91