23 #ifndef __LIBS_PCL_UTILS_TRANSFORMS_H_ 24 #define __LIBS_PCL_UTILS_TRANSFORMS_H_ 27 #include <tf/transformer.h> 28 #include <pcl_utils/utils.h> 30 #include <pcl/point_cloud.h> 31 #include <pcl/common/transforms.h> 46 template <
typename Po
intT>
void 49 const tf::Transform &transform)
59 tf::Quaternion q = transform.getRotation();
60 Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z());
61 tf::Vector3 v = transform.getOrigin();
62 Eigen::Vector3f origin(v.x(), v.y(), v.z());
63 pcl::transformPointCloud(cloud_in, cloud_out, origin, rotation);
72 template <
typename Po
intT>
void 74 const tf::Transform &transform)
77 transform_pointcloud(cloud_inout, tmp, transform);
88 template <
typename Po
intT>
89 void transform_pointcloud(
const std::string &target_frame,
92 const tf::Transformer &transformer)
94 if (cloud_in.header.frame_id == target_frame) {
100 pcl_utils::get_time(cloud_in, source_time);
101 tf::StampedTransform transform;
102 transformer.lookup_transform(target_frame, cloud_in.header.frame_id,
103 source_time, transform);
105 transform_pointcloud(cloud_in, cloud_out, transform);
106 cloud_out.header.frame_id = target_frame;
115 template <
typename Po
intT>
116 void transform_pointcloud(
const std::string &target_frame,
118 const tf::Transformer &transformer)
121 transform_pointcloud(target_frame, cloud_inout, tmp, transformer);
133 template <
typename Po
intT>
134 void transform_pointcloud(
const std::string &target_frame,
const Time &target_time,
135 const std::string &fixed_frame,
138 const tf::Transformer &transformer)
140 if (cloud_in.header.frame_id == target_frame) {
141 cloud_out = cloud_in;
146 pcl_utils::get_time(cloud_in, source_time);
148 tf::StampedTransform transform;
149 transformer.lookup_transform(target_frame, target_time,
150 cloud_in.header.frame_id, source_time,
151 fixed_frame, transform);
153 transform_pointcloud(cloud_in, cloud_out, transform);
154 cloud_out.header.frame_id = target_frame;
156 pcl_utils::set_time(cloud_out, target_time);
165 template <
typename Po
intT>
166 void transform_pointcloud(
const std::string &target_frame,
const Time &target_time,
167 const std::string &fixed_frame,
169 const tf::Transformer &transformer)
172 transform_pointcloud(target_frame, target_time, fixed_frame,
173 cloud_inout, tmp, transformer);
Fawkes library namespace.
A class for handling time.