23 #ifndef _LIBS_PCL_UTILS_TRANSFORMS_H_
24 #define _LIBS_PCL_UTILS_TRANSFORMS_H_
26 #include <pcl/common/transforms.h>
27 #include <pcl/point_cloud.h>
28 #include <pcl_utils/utils.h>
29 #include <tf/transformer.h>
41 template <
typename Po
intT>
45 const tf::Transform & transform)
55 tf::Quaternion q = transform.getRotation();
56 Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z());
57 tf::Vector3 v = transform.getOrigin();
58 Eigen::Vector3f origin(v.x(), v.y(), v.z());
59 pcl::transformPointCloud(cloud_in, cloud_out, origin, rotation);
67 template <
typename Po
intT>
72 transform_pointcloud(cloud_inout, tmp, transform);
83 template <
typename Po
intT>
85 transform_pointcloud(
const std::string & target_frame,
88 const tf::Transformer & transformer)
90 if (cloud_in.header.frame_id == target_frame) {
96 pcl_utils::get_time(cloud_in, source_time);
97 tf::StampedTransform transform;
98 transformer.lookup_transform(target_frame, cloud_in.header.frame_id, source_time, transform);
100 transform_pointcloud(cloud_in, cloud_out, transform);
101 cloud_out.header.frame_id = target_frame;
110 template <
typename Po
intT>
112 transform_pointcloud(
const std::string & target_frame,
114 const tf::Transformer & transformer)
117 transform_pointcloud(target_frame, cloud_inout, tmp, transformer);
128 template <
typename Po
intT>
130 transform_pointcloud(
const std::string & target_frame,
131 const Time & target_time,
132 const std::string & fixed_frame,
135 const tf::Transformer & transformer)
137 if (cloud_in.header.frame_id == target_frame) {
138 cloud_out = cloud_in;
143 pcl_utils::get_time(cloud_in, source_time);
145 tf::StampedTransform transform;
146 transformer.lookup_transform(
147 target_frame, target_time, cloud_in.header.frame_id, source_time, fixed_frame, transform);
149 transform_pointcloud(cloud_in, cloud_out, transform);
150 cloud_out.header.frame_id = target_frame;
152 pcl_utils::set_time(cloud_out, target_time);
161 template <
typename Po
intT>
163 transform_pointcloud(
const std::string & target_frame,
164 const Time & target_time,
165 const std::string & fixed_frame,
167 const tf::Transformer & transformer)
170 transform_pointcloud(target_frame, target_time, fixed_frame, cloud_inout, tmp, transformer);
A class for handling time.
Fawkes library namespace.