37 #ifndef pcl_ros_IMPL_TRANSFORMS_H_ 38 #define pcl_ros_IMPL_TRANSFORMS_H_ 49 template <
typename Po
intT>
void 51 pcl::PointCloud <PointT> &cloud_out,
const tf::Transform &transform)
59 Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ());
61 Eigen::Vector3f origin (v.x (), v.y (), v.z ());
70 template <
typename Po
intT>
void 72 pcl::PointCloud <PointT> &cloud_out,
const tf::Transform &transform)
80 Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ());
82 Eigen::Vector3f origin (v.x (), v.y (), v.z ());
91 template <
typename Po
intT>
bool 93 const pcl::PointCloud <PointT> &cloud_in,
94 pcl::PointCloud <PointT> &cloud_out,
97 if (cloud_in.header.frame_id == target_frame)
106 tf_listener.
lookupTransform (target_frame, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, transform);
120 cloud_out.header.frame_id = target_frame;
125 template <
typename Po
intT>
bool 127 const pcl::PointCloud <PointT> &cloud_in,
128 pcl::PointCloud <PointT> &cloud_out,
131 if (cloud_in.header.frame_id == target_frame)
133 cloud_out = cloud_in;
140 tf_listener.
lookupTransform (target_frame, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, transform);
153 cloud_out.header.frame_id = target_frame;
158 template <
typename Po
intT>
bool 161 const pcl::PointCloud <PointT> &cloud_in,
162 const std::string &fixed_frame,
163 pcl::PointCloud <PointT> &cloud_out,
169 tf_listener.
lookupTransform (target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, fixed_frame, transform);
183 cloud_out.header.frame_id = target_frame;
185 header.stamp = target_time;
186 cloud_out.header =
toPCL(header);
191 template <
typename Po
intT>
bool 194 const pcl::PointCloud <PointT> &cloud_in,
195 const std::string &fixed_frame,
196 pcl::PointCloud <PointT> &cloud_out,
202 tf_listener.
lookupTransform (target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, fixed_frame, transform);
216 cloud_out.header.frame_id = target_frame;
218 header.stamp = target_time;
219 cloud_out.header =
toPCL(header);
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Transform a point cloud and rotate its normals using an Eigen transform.
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
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 toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)