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
32namespace fawkes {
33namespace 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 */
41template <typename PointT>
42void
43transform_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 */
67template <typename PointT>
68void
69transform_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 */
83template <typename PointT>
84void
85transform_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 */
110template <typename PointT>
111void
112transform_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 */
128template <typename PointT>
129void
130transform_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 */
161template <typename PointT>
162void
163transform_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.