transforms.cpp
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 #include <sensor_msgs/PointCloud2.h>
38 #include <pcl/common/io.h>
39 #include <pcl/point_types.h>
40 #include <pcl_conversions/pcl_conversions.h>
41 #include "pcl_ros/transforms.h"
42 #include "pcl_ros/impl/transforms.hpp"
43 
44 namespace pcl_ros
45 {
47 bool
48 transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in,
49  sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
50 {
51  if (in.header.frame_id == target_frame)
52  {
53  out = in;
54  return (true);
55  }
56 
57  // Get the TF transform
58  tf::StampedTransform transform;
59  try
60  {
61  tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
62  }
63  catch (tf::LookupException &e)
64  {
65  ROS_ERROR ("%s", e.what ());
66  return (false);
67  }
68  catch (tf::ExtrapolationException &e)
69  {
70  ROS_ERROR ("%s", e.what ());
71  return (false);
72  }
73 
74  // Convert the TF transform to Eigen format
75  Eigen::Matrix4f eigen_transform;
76  transformAsMatrix (transform, eigen_transform);
77 
78  transformPointCloud (eigen_transform, in, out);
79 
80  out.header.frame_id = target_frame;
81  return (true);
82 }
83 
85 void
86 transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
87  const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
88 {
89  if (in.header.frame_id == target_frame)
90  {
91  out = in;
92  return;
93  }
94 
95  // Get the transformation
96  Eigen::Matrix4f transform;
97  transformAsMatrix (net_transform, transform);
98 
99  transformPointCloud (transform, in, out);
100 
101  out.header.frame_id = target_frame;
102 }
103 
105 void
106 transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
107  sensor_msgs::PointCloud2 &out)
108 {
109  // Get X-Y-Z indices
110  int x_idx = pcl::getFieldIndex (in, "x");
111  int y_idx = pcl::getFieldIndex (in, "y");
112  int z_idx = pcl::getFieldIndex (in, "z");
113 
114  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
115  {
116  ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
117  return;
118  }
119 
120  if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
121  in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
122  in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
123  {
124  ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
125  return;
126  }
127 
128  // Check if distance is available
129  int dist_idx = pcl::getFieldIndex (in, "distance");
130 
131  // Copy the other data
132  if (&in != &out)
133  {
134  out.header = in.header;
135  out.height = in.height;
136  out.width = in.width;
137  out.fields = in.fields;
138  out.is_bigendian = in.is_bigendian;
139  out.point_step = in.point_step;
140  out.row_step = in.row_step;
141  out.is_dense = in.is_dense;
142  out.data.resize (in.data.size ());
143  // Copy everything as it's faster than copying individual elements
144  memcpy (&out.data[0], &in.data[0], in.data.size ());
145  }
146 
147  Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
148 
149  for (size_t i = 0; i < in.width * in.height; ++i)
150  {
151  Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
152  Eigen::Vector4f pt_out;
153 
154  bool max_range_point = false;
155  int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
156  float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
157  if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
158  {
159  if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
160  {
161  pt_out = pt;
162  }
163  else // max range point
164  {
165  pt[0] = *distance_ptr; // Replace x with the x value saved in distance
166  pt_out = transform * pt;
167  max_range_point = true;
168  //std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
169  }
170  }
171  else
172  {
173  pt_out = transform * pt;
174  }
175 
176  if (max_range_point)
177  {
178  // Save x value in distance again
179  *(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
180  pt_out[0] = std::numeric_limits<float>::quiet_NaN();
181  }
182 
183  memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
184  memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
185  memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
186 
187 
188  xyz_offset += in.point_step;
189  }
190 
191  // Check if the viewpoint information is present
192  int vp_idx = pcl::getFieldIndex (in, "vp_x");
193  if (vp_idx != -1)
194  {
195  // Transform the viewpoint info too
196  for (size_t i = 0; i < out.width * out.height; ++i)
197  {
198  float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
199  // Assume vp_x, vp_y, vp_z are consecutive
200  Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
201  Eigen::Vector4f vp_out = transform * vp_in;
202 
203  pstep[0] = vp_out[0];
204  pstep[1] = vp_out[1];
205  pstep[2] = vp_out[2];
206  }
207  }
208 }
209 
211 void
212 transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
213 {
214  double mv[12];
215  bt.getBasis ().getOpenGLSubMatrix (mv);
216 
217  tf::Vector3 origin = bt.getOrigin ();
218 
219  out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
220  out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
221  out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
222 
223  out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
224  out_mat (0, 3) = origin.x ();
225  out_mat (1, 3) = origin.y ();
226  out_mat (2, 3) = origin.z ();
227 }
228 
229 } // namespace pcl_ros
230 
232 template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
233 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
234 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
235 
237 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
238 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
239 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
240 
242 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
243 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
244 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
245 
247 template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &);
248 template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &);
249 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &);
250 template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &);
251 template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &);
252 template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
253 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
254 template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
255 template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &);
256 template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &);
257 
259 template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
260 template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
261 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
262 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
263 template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
264 template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
265 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
266 template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
267 template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
268 template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
269 
271 template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZ> &, const std::string &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
272 template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZI> &, const std::string &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
273 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBA> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
274 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGB> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
275 template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::InterestPoint> &, const std::string &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
276 template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointNormal> &, const std::string &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
277 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
278 template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointXYZINormal> &, const std::string &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
279 template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithRange> &, const std::string &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
280 template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const ros::Time &, const pcl::PointCloud <pcl::PointWithViewpoint> &, const std::string &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
281 
tf::Transform::getBasis
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
pcl_ros
Definition: boundary.h:46
pcl_ros::transformAsMatrix
void transformAsMatrix(const tf::Transform &bt, Eigen::Matrix4f &out_mat)
Obtain the transformation matrix from TF into an Eigen form.
Definition: transforms.cpp:212
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
tf::Vector3::z
TFSIMD_FORCE_INLINE const tfScalar & z() const
pcl_conversions.h
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
pcl_ros::transformPointCloud
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:71
ROS_ERROR
#define ROS_ERROR(...)
tf::Matrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(tfScalar *m) const
pcl::getFieldIndex
int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)

pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Jun 5 2019 19:52:52