37 #include <sensor_msgs/PointCloud2.h> 38 #include <pcl/common/io.h> 39 #include <pcl/point_types.h> 51 if (in.header.frame_id == target_frame)
61 tf_listener.
lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
75 Eigen::Matrix4f eigen_transform;
80 out.header.frame_id = target_frame;
87 const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
89 if (in.header.frame_id == target_frame)
96 Eigen::Matrix4f transform;
101 out.header.frame_id = target_frame;
107 sensor_msgs::PointCloud2 &out)
114 if (x_idx == -1 || y_idx == -1 || z_idx == -1)
116 ROS_ERROR (
"Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
120 if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
121 in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
122 in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
124 ROS_ERROR (
"X-Y-Z coordinates not floats. Currently only floats are supported.");
134 out.header = in.header;
135 out.height = in.height;
136 out.width = in.width;
137 out.fields = in.fields;
138 out.is_bigendian = in.is_bigendian;
139 out.point_step = in.point_step;
140 out.row_step = in.row_step;
141 out.is_dense = in.is_dense;
142 out.data.resize (in.data.size ());
144 memcpy (&out.data[0], &in.data[0], in.data.size ());
147 Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
149 for (
size_t i = 0; i < in.width * in.height; ++i)
151 Eigen::Vector4f pt (*(
float*)&in.data[xyz_offset[0]], *(
float*)&in.data[xyz_offset[1]], *(
float*)&in.data[xyz_offset[2]], 1);
152 Eigen::Vector4f pt_out;
154 bool max_range_point =
false;
155 int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
156 float* distance_ptr = (dist_idx < 0 ? NULL : (
float*)(&in.data[distance_ptr_offset]));
157 if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
159 if (distance_ptr==NULL || !std::isfinite(*distance_ptr))
165 pt[0] = *distance_ptr;
166 pt_out = transform * pt;
167 max_range_point =
true;
173 pt_out = transform * pt;
179 *(
float*)(&out.data[distance_ptr_offset]) = pt_out[0];
180 pt_out[0] = std::numeric_limits<float>::quiet_NaN();
183 memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (
float));
184 memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (
float));
185 memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (
float));
188 xyz_offset += in.point_step;
196 for (
size_t i = 0; i < out.width * out.height; ++i)
198 float *pstep = (
float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
200 Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
201 Eigen::Vector4f vp_out = transform * vp_in;
203 pstep[0] = vp_out[0];
204 pstep[1] = vp_out[1];
205 pstep[2] = vp_out[2];
219 out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
220 out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
221 out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
223 out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
224 out_mat (0, 3) = origin.
x ();
225 out_mat (1, 3) = origin.
y ();
226 out_mat (2, 3) = origin.
z ();
232 template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (
const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &,
const tf::Transform &);
233 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (
const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &,
const tf::Transform &);
234 template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (
const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &,
const tf::Transform &);
237 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal> (
const std::string &,
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::TransformListener &);
238 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal> (
const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::TransformListener &);
239 template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal> (
const std::string &,
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::TransformListener &);
242 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 &);
243 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 &);
244 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 &);
247 template void pcl_ros::transformPointCloud<pcl::PointXYZ> (
const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &,
const tf::Transform &);
248 template void pcl_ros::transformPointCloud<pcl::PointXYZI> (
const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &,
const tf::Transform &);
249 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (
const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &,
const tf::Transform &);
250 template void pcl_ros::transformPointCloud<pcl::PointXYZRGB> (
const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &,
const tf::Transform &);
251 template void pcl_ros::transformPointCloud<pcl::InterestPoint> (
const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &,
const tf::Transform &);
252 template void pcl_ros::transformPointCloud<pcl::PointNormal> (
const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &,
const tf::Transform &);
253 template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (
const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &,
const tf::Transform &);
254 template void pcl_ros::transformPointCloud<pcl::PointXYZINormal> (
const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &,
const tf::Transform &);
255 template void pcl_ros::transformPointCloud<pcl::PointWithRange> (
const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &,
const tf::Transform &);
256 template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (
const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &,
const tf::Transform &);
259 template bool pcl_ros::transformPointCloud<pcl::PointXYZ> (
const std::string &,
const pcl::PointCloud <pcl::PointXYZ> &, pcl::PointCloud <pcl::PointXYZ> &,
const tf::TransformListener &);
260 template bool pcl_ros::transformPointCloud<pcl::PointXYZI> (
const std::string &,
const pcl::PointCloud <pcl::PointXYZI> &, pcl::PointCloud <pcl::PointXYZI> &,
const tf::TransformListener &);
261 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA> (
const std::string &,
const pcl::PointCloud <pcl::PointXYZRGBA> &, pcl::PointCloud <pcl::PointXYZRGBA> &,
const tf::TransformListener &);
262 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB> (
const std::string &,
const pcl::PointCloud <pcl::PointXYZRGB> &, pcl::PointCloud <pcl::PointXYZRGB> &,
const tf::TransformListener &);
263 template bool pcl_ros::transformPointCloud<pcl::InterestPoint> (
const std::string &,
const pcl::PointCloud <pcl::InterestPoint> &, pcl::PointCloud <pcl::InterestPoint> &,
const tf::TransformListener &);
264 template bool pcl_ros::transformPointCloud<pcl::PointNormal> (
const std::string &,
const pcl::PointCloud <pcl::PointNormal> &, pcl::PointCloud <pcl::PointNormal> &,
const tf::TransformListener &);
265 template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal> (
const std::string &,
const pcl::PointCloud <pcl::PointXYZRGBNormal> &, pcl::PointCloud <pcl::PointXYZRGBNormal> &,
const tf::TransformListener &);
266 template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal> (
const std::string &,
const pcl::PointCloud <pcl::PointXYZINormal> &, pcl::PointCloud <pcl::PointXYZINormal> &,
const tf::TransformListener &);
267 template bool pcl_ros::transformPointCloud<pcl::PointWithRange> (
const std::string &,
const pcl::PointCloud <pcl::PointWithRange> &, pcl::PointCloud <pcl::PointWithRange> &,
const tf::TransformListener &);
268 template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint> (
const std::string &,
const pcl::PointCloud <pcl::PointWithViewpoint> &, pcl::PointCloud <pcl::PointWithViewpoint> &,
const tf::TransformListener &);
271 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 &);
272 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 &);
273 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 &);
274 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 &);
275 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 &);
276 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 &);
277 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 &);
278 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 &);
279 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 &);
280 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 &);
void transformAsMatrix(const tf::Transform &bt, Eigen::Matrix4f &out_mat)
Obtain the transformation matrix from TF into an Eigen form.
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Apply a rigid transform defined by a 3D offset and a quaternion.
void getOpenGLSubMatrix(tfScalar *m) const
int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)