transforms.hpp
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 #ifndef pcl_ros_IMPL_TRANSFORMS_H_
00038 #define pcl_ros_IMPL_TRANSFORMS_H_
00039 
00040 #include <pcl_conversions/pcl_conversions.h>
00041 #include "pcl_ros/transforms.h"
00042 
00043 using pcl_conversions::fromPCL;
00044 using pcl_conversions::toPCL;
00045 
00046 namespace pcl_ros
00047 {
00049 template <typename PointT> void
00050 transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
00051                                 pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
00052 {
00053   // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
00054   // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
00055   // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
00056   // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
00057   // the conversion of the point cloud anyway. Idem for the origin.
00058   tf::Quaternion q = transform.getRotation ();
00059   Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ());       // internally stored as (x,y,z,w)
00060   tf::Vector3 v = transform.getOrigin ();
00061   Eigen::Vector3f origin (v.x (), v.y (), v.z ());
00062   //    Eigen::Translation3f translation(v);
00063   // Assemble an Eigen Transform
00064   //Eigen::Transform3f t;
00065   //t = translation * rotation;
00066   transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation);
00067 }
00068 
00070 template <typename PointT> void
00071 transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
00072                      pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
00073 {
00074   // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
00075   // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
00076   // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
00077   // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
00078   // the conversion of the point cloud anyway. Idem for the origin.
00079   tf::Quaternion q = transform.getRotation ();
00080   Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ());       // internally stored as (x,y,z,w)
00081   tf::Vector3 v = transform.getOrigin ();
00082   Eigen::Vector3f origin (v.x (), v.y (), v.z ());
00083   //    Eigen::Translation3f translation(v);
00084   // Assemble an Eigen Transform
00085   //Eigen::Transform3f t;
00086   //t = translation * rotation;
00087   transformPointCloud (cloud_in, cloud_out, origin, rotation);
00088 }
00089 
00091 template <typename PointT> bool
00092 transformPointCloudWithNormals (const std::string &target_frame,
00093                                 const pcl::PointCloud <PointT> &cloud_in,
00094                                 pcl::PointCloud <PointT> &cloud_out, 
00095                                 const tf::TransformListener &tf_listener)
00096 {
00097   if (cloud_in.header.frame_id == target_frame)
00098   {
00099     cloud_out = cloud_in;
00100     return (true);
00101   }
00102 
00103   tf::StampedTransform transform;
00104   try
00105   {
00106     tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
00107   }
00108   catch (tf::LookupException &e)
00109   {
00110     ROS_ERROR ("%s", e.what ());
00111     return (false);
00112   }
00113   catch (tf::ExtrapolationException &e)
00114   {
00115     ROS_ERROR ("%s", e.what ());
00116     return (false);
00117   }
00118 
00119   transformPointCloudWithNormals (cloud_in, cloud_out, transform);
00120   cloud_out.header.frame_id = target_frame;
00121   return (true);
00122 }
00123 
00125 template <typename PointT> bool
00126 transformPointCloud (const std::string &target_frame,
00127                      const pcl::PointCloud <PointT> &cloud_in,
00128                      pcl::PointCloud <PointT> &cloud_out, 
00129                      const tf::TransformListener &tf_listener)
00130 {
00131   if (cloud_in.header.frame_id == target_frame)
00132   {
00133     cloud_out = cloud_in;
00134     return (true);
00135   }
00136 
00137   tf::StampedTransform transform;
00138   try
00139   {
00140     tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
00141   }
00142   catch (tf::LookupException &e)
00143   {
00144     ROS_ERROR ("%s", e.what ());
00145     return (false);
00146   }
00147   catch (tf::ExtrapolationException &e)
00148   {
00149     ROS_ERROR ("%s", e.what ());
00150     return (false);
00151   }
00152   transformPointCloud (cloud_in, cloud_out, transform);
00153   cloud_out.header.frame_id = target_frame;
00154   return (true);
00155 }
00156 
00158 template <typename PointT> bool
00159 transformPointCloud (const std::string &target_frame,
00160                      const ros::Time & target_time,
00161                      const pcl::PointCloud <PointT> &cloud_in,
00162                      const std::string &fixed_frame,
00163                      pcl::PointCloud <PointT> &cloud_out, 
00164                      const tf::TransformListener &tf_listener)
00165 {
00166   tf::StampedTransform transform;
00167   try
00168   {
00169     tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
00170   }
00171   catch (tf::LookupException &e)
00172   {
00173     ROS_ERROR ("%s", e.what ());
00174     return (false);
00175   }
00176   catch (tf::ExtrapolationException &e)
00177   {
00178     ROS_ERROR ("%s", e.what ());
00179     return (false);
00180   }
00181 
00182   transformPointCloud (cloud_in, cloud_out, transform);
00183   cloud_out.header.frame_id = target_frame;
00184   std_msgs::Header header;
00185   header.stamp = target_time;
00186   cloud_out.header = toPCL(header);
00187   return (true);
00188 }
00189 
00191 template <typename PointT> bool
00192 transformPointCloudWithNormals (const std::string &target_frame,
00193                                 const ros::Time & target_time,
00194                                 const pcl::PointCloud <PointT> &cloud_in,
00195                                 const std::string &fixed_frame,
00196                                 pcl::PointCloud <PointT> &cloud_out, 
00197                                 const tf::TransformListener &tf_listener)
00198 {
00199   tf::StampedTransform transform;
00200   try
00201   {
00202     tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
00203   }
00204   catch (tf::LookupException &e)
00205   {
00206     ROS_ERROR ("%s", e.what ());
00207     return (false);
00208   }
00209   catch (tf::ExtrapolationException &e)
00210   {
00211     ROS_ERROR ("%s", e.what ());
00212     return (false);
00213   }
00214 
00215   transformPointCloudWithNormals (cloud_in, cloud_out, transform);
00216   cloud_out.header.frame_id = target_frame;
00217   std_msgs::Header header;
00218   header.stamp = target_time;
00219   cloud_out.header = toPCL(header);
00220   return (true);
00221 }
00222 
00223 }  // namespace pcl_ros
00224 #endif


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