transforms.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  */
00036 
00037 #include <sensor_msgs/PointCloud2.h>
00038 #include <pcl/common/io.h>
00039 #include <pcl/point_types.h>
00040 #include <pcl_conversions/pcl_conversions.h>
00041 #include "pcl_ros/transforms.h"
00042 #include "pcl_ros/impl/transforms.hpp"
00043 
00044 namespace pcl_ros
00045 {
00047 bool
00048 transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, 
00049                      sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
00050 {
00051   if (in.header.frame_id == target_frame)
00052   {
00053     out = in;
00054     return (true);
00055   }
00056 
00057   // Get the TF transform
00058   tf::StampedTransform transform;
00059   try
00060   {
00061     tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
00062   }
00063   catch (tf::LookupException &e)
00064   {
00065     ROS_ERROR ("%s", e.what ());
00066     return (false);
00067   }
00068   catch (tf::ExtrapolationException &e)
00069   {
00070     ROS_ERROR ("%s", e.what ());
00071     return (false);
00072   }
00073 
00074   // Convert the TF transform to Eigen format
00075   Eigen::Matrix4f eigen_transform;
00076   transformAsMatrix (transform, eigen_transform);
00077 
00078   transformPointCloud (eigen_transform, in, out);
00079 
00080   out.header.frame_id = target_frame;
00081   return (true);
00082 }
00083 
00085 void 
00086 transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
00087                      const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
00088 {
00089   if (in.header.frame_id == target_frame)
00090   {
00091     out = in;
00092     return;
00093   }
00094 
00095   // Get the transformation
00096   Eigen::Matrix4f transform;
00097   transformAsMatrix (net_transform, transform);
00098 
00099   transformPointCloud (transform, in, out);
00100 
00101   out.header.frame_id = target_frame;
00102 }
00103 
00105 void 
00106 transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
00107                      sensor_msgs::PointCloud2 &out)
00108 {
00109   // Get X-Y-Z indices
00110   int x_idx = pcl::getFieldIndex (in, "x");
00111   int y_idx = pcl::getFieldIndex (in, "y");
00112   int z_idx = pcl::getFieldIndex (in, "z");
00113 
00114   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00115   {
00116     ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
00117     return;
00118   }
00119 
00120   if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
00121       in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
00122       in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
00123   {
00124     ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
00125     return;
00126   }
00127 
00128   // Check if distance is available
00129   int dist_idx = pcl::getFieldIndex (in, "distance");
00130 
00131   // Copy the other data
00132   if (&in != &out)
00133   {
00134     out.header = in.header;
00135     out.height = in.height;
00136     out.width  = in.width;
00137     out.fields = in.fields;
00138     out.is_bigendian = in.is_bigendian;
00139     out.point_step   = in.point_step;
00140     out.row_step     = in.row_step;
00141     out.is_dense     = in.is_dense;
00142     out.data.resize (in.data.size ());
00143     // Copy everything as it's faster than copying individual elements
00144     memcpy (&out.data[0], &in.data[0], in.data.size ());
00145   }
00146 
00147   Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
00148 
00149   for (size_t i = 0; i < in.width * in.height; ++i)
00150   {
00151     Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
00152     Eigen::Vector4f pt_out;
00153     
00154     bool max_range_point = false;
00155     int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
00156     float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
00157     if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
00158     {
00159       if (distance_ptr==NULL || !std::isfinite(*distance_ptr))  // Invalid point
00160       {
00161         pt_out = pt;
00162       }
00163       else  // max range point
00164       {
00165         pt[0] = *distance_ptr;  // Replace x with the x value saved in distance
00166         pt_out = transform * pt;
00167         max_range_point = true;
00168         //std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
00169       }
00170     }
00171     else
00172     {
00173       pt_out = transform * pt;
00174     }
00175 
00176     if (max_range_point)
00177     {
00178       // Save x value in distance again
00179       *(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
00180       pt_out[0] = std::numeric_limits<float>::quiet_NaN();
00181     }
00182 
00183     memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
00184     memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
00185     memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
00186   
00187     
00188     xyz_offset += in.point_step;
00189   }
00190 
00191   // Check if the viewpoint information is present
00192   int vp_idx = pcl::getFieldIndex (in, "vp_x");
00193   if (vp_idx != -1)
00194   {
00195     // Transform the viewpoint info too
00196     for (size_t i = 0; i < out.width * out.height; ++i)
00197     {
00198       float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
00199       // Assume vp_x, vp_y, vp_z are consecutive
00200       Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
00201       Eigen::Vector4f vp_out = transform * vp_in;
00202 
00203       pstep[0] = vp_out[0];
00204       pstep[1] = vp_out[1];
00205       pstep[2] = vp_out[2];
00206     }
00207   }
00208 }
00209 
00211 void
00212 transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
00213 {
00214   double mv[12];
00215   bt.getBasis ().getOpenGLSubMatrix (mv);
00216 
00217   tf::Vector3 origin = bt.getOrigin ();
00218 
00219   out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
00220   out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
00221   out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
00222                                                                      
00223   out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
00224   out_mat (0, 3) = origin.x ();
00225   out_mat (1, 3) = origin.y ();
00226   out_mat (2, 3) = origin.z ();
00227 }
00228 
00229 } // namespace pcl_ros
00230 
00232 template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
00233 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
00234 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
00235 
00237 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &);
00238 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
00239 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &);
00240 
00242 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 &);
00243 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 &);
00244 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 &);
00245 
00247 template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &);
00248 template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &);
00249 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &);
00250 template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &);
00251 template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &);
00252 template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &);
00253 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &);
00254 template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &);
00255 template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &);
00256 template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &);
00257 
00259 template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &);
00260 template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &);
00261 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &);
00262 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &);
00263 template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &);
00264 template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &);
00265 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &);
00266 template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &);
00267 template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &);
00268 template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &);
00269 
00271 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 &);
00272 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 &);
00273 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 &);
00274 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 &);
00275 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 &);
00276 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 &);
00277 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 &);
00278 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 &);
00279 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 &);
00280 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 &);
00281 


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu May 5 2016 04:16:43