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