00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
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
00129 int dist_idx = pcl::getFieldIndex (in, "distance");
00130
00131
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
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))
00160 {
00161 pt_out = pt;
00162 }
00163 else
00164 {
00165 pt[0] = *distance_ptr;
00166 pt_out = transform * pt;
00167 max_range_point = true;
00168
00169 }
00170 }
00171 else
00172 {
00173 pt_out = transform * pt;
00174 }
00175
00176 if (max_range_point)
00177 {
00178
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
00192 int vp_idx = pcl::getFieldIndex (in, "vp_x");
00193 if (vp_idx != -1)
00194 {
00195
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
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 }
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