$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 #include <sensor_msgs/PointCloud2.h> 00038 #include <pcl/io/io.h> 00039 #include <pcl/point_types.h> 00040 #include "pcl_ros/transforms.h" 00041 #include "pcl_ros/impl/transforms.hpp" 00042 00043 namespace pcl_ros 00044 { 00046 bool 00047 transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, 00048 sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener) 00049 { 00050 if (in.header.frame_id == target_frame) 00051 { 00052 out = in; 00053 return (true); 00054 } 00055 00056 // Get the TF transform 00057 tf::StampedTransform transform; 00058 try 00059 { 00060 tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform); 00061 } 00062 catch (tf::LookupException &e) 00063 { 00064 ROS_ERROR ("%s", e.what ()); 00065 return (false); 00066 } 00067 catch (tf::ExtrapolationException &e) 00068 { 00069 ROS_ERROR ("%s", e.what ()); 00070 return (false); 00071 } 00072 00073 // Convert the TF transform to Eigen format 00074 Eigen::Matrix4f eigen_transform; 00075 transformAsMatrix (transform, eigen_transform); 00076 00077 transformPointCloud (eigen_transform, in, out); 00078 00079 out.header.frame_id = target_frame; 00080 return (true); 00081 } 00082 00084 void 00085 transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, 00086 const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) 00087 { 00088 if (in.header.frame_id == target_frame) 00089 { 00090 out = in; 00091 return; 00092 } 00093 00094 // Get the transformation 00095 Eigen::Matrix4f transform; 00096 transformAsMatrix (net_transform, transform); 00097 00098 transformPointCloud (transform, in, out); 00099 00100 out.header.frame_id = target_frame; 00101 } 00102 00104 void 00105 transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, 00106 sensor_msgs::PointCloud2 &out) 00107 { 00108 // Get X-Y-Z indices 00109 int x_idx = pcl::getFieldIndex (in, "x"); 00110 int y_idx = pcl::getFieldIndex (in, "y"); 00111 int z_idx = pcl::getFieldIndex (in, "z"); 00112 00113 if (x_idx == -1 || y_idx == -1 || z_idx == -1) 00114 { 00115 ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); 00116 return; 00117 } 00118 00119 if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || 00120 in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || 00121 in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) 00122 { 00123 ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported."); 00124 return; 00125 } 00126 00127 // Check if distance is available 00128 int dist_idx = pcl::getFieldIndex (in, "distance"); 00129 00130 // Copy the other data 00131 if (&in != &out) 00132 { 00133 out.header = in.header; 00134 out.height = in.height; 00135 out.width = in.width; 00136 out.fields = in.fields; 00137 out.is_bigendian = in.is_bigendian; 00138 out.point_step = in.point_step; 00139 out.row_step = in.row_step; 00140 out.is_dense = in.is_dense; 00141 out.data.resize (in.data.size ()); 00142 // Copy everything as it's faster than copying individual elements 00143 memcpy (&out.data[0], &in.data[0], in.data.size ()); 00144 } 00145 00146 Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0); 00147 00148 for (size_t i = 0; i < in.width * in.height; ++i) 00149 { 00150 Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1); 00151 Eigen::Vector4f pt_out; 00152 00153 bool max_range_point = false; 00154 int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset; 00155 float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset])); 00156 if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) 00157 { 00158 if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point 00159 { 00160 pt_out = pt; 00161 } 00162 else // max range point 00163 { 00164 pt[0] = *distance_ptr; // Replace x with the x value saved in distance 00165 pt_out = transform * pt; 00166 max_range_point = true; 00167 //std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n"; 00168 } 00169 } 00170 else 00171 { 00172 pt_out = transform * pt; 00173 } 00174 00175 if (max_range_point) 00176 { 00177 // Save x value in distance again 00178 *(float*)(&out.data[distance_ptr_offset]) = pt_out[0]; 00179 pt_out[0] = std::numeric_limits<float>::quiet_NaN(); 00180 } 00181 00182 memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float)); 00183 memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float)); 00184 memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float)); 00185 00186 00187 xyz_offset += in.point_step; 00188 } 00189 00190 // Check if the viewpoint information is present 00191 int vp_idx = pcl::getFieldIndex (in, "vp_x"); 00192 if (vp_idx != -1) 00193 { 00194 // Transform the viewpoint info too 00195 for (size_t i = 0; i < out.width * out.height; ++i) 00196 { 00197 float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset]; 00198 // Assume vp_x, vp_y, vp_z are consecutive 00199 Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1); 00200 Eigen::Vector4f vp_out = transform * vp_in; 00201 00202 pstep[0] = vp_out[0]; 00203 pstep[1] = vp_out[1]; 00204 pstep[2] = vp_out[2]; 00205 } 00206 } 00207 } 00208 00210 void 00211 transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) 00212 { 00213 double mv[12]; 00214 bt.getBasis ().getOpenGLSubMatrix (mv); 00215 00216 tf::Vector3 origin = bt.getOrigin (); 00217 00218 out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; 00219 out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; 00220 out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; 00221 00222 out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; 00223 out_mat (0, 3) = origin.x (); 00224 out_mat (1, 3) = origin.y (); 00225 out_mat (2, 3) = origin.z (); 00226 } 00227 00228 } // namespace pcl_ros 00229 00231 template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &); 00232 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &); 00233 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &); 00234 00236 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (const std::string &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &); 00237 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &); 00238 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &); 00239 00241 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 &); 00242 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 &); 00243 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 &); 00244 00246 template void pcl_ros::transformPointCloud<pcl::PointXYZ> (const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::Transform &); 00247 template void pcl_ros::transformPointCloud<pcl::PointXYZI> (const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::Transform &); 00248 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::Transform &); 00249 template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::Transform &); 00250 template void pcl_ros::transformPointCloud<pcl::InterestPoint> (const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::Transform &); 00251 template void pcl_ros::transformPointCloud<pcl::PointNormal> (const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::Transform &); 00252 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::Transform &); 00253 template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::Transform &); 00254 template void pcl_ros::transformPointCloud<pcl::PointWithRange> (const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::Transform &); 00255 template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::Transform &); 00256 00258 template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (const std::string &, const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &, const tf::TransformListener &); 00259 template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (const std::string &, const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &, const tf::TransformListener &); 00260 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &, const tf::TransformListener &); 00261 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &, const tf::TransformListener &); 00262 template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (const std::string &, const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &, const tf::TransformListener &); 00263 template bool pcl_ros::transformPointCloud<pcl::PointNormal> (const std::string &, const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &, const tf::TransformListener &); 00264 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &, const tf::TransformListener &); 00265 template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (const std::string &, const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &, const tf::TransformListener &); 00266 template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (const std::string &, const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &, const tf::TransformListener &); 00267 template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (const std::string &, const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &, const tf::TransformListener &); 00268 00270 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 &); 00271 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 &); 00272 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 &); 00273 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 &); 00274 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 &); 00275 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 &); 00276 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 &); 00277 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 &); 00278 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 &); 00279 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 &); 00280