37 #ifndef JSK_PCL_ROS_UTILS_TRANSFORM_POINTCLOUD_IN_BOUNDING_BOX_H_ 38 #define JSK_PCL_ROS_UTILS_TRANSFORM_POINTCLOUD_IN_BOUNDING_BOX_H_ 41 #include <jsk_recognition_msgs/BoundingBox.h> 48 #include <pcl/common/transforms.h> 52 template <
class Po
intT>
54 const jsk_recognition_msgs::BoundingBox& box_msg,
55 const sensor_msgs::PointCloud2& cloud_msg,
56 pcl::PointCloud<PointT>& output,
57 Eigen::Affine3f& offset,
60 geometry_msgs::PoseStamped box_pose;
61 box_pose.header = box_msg.header;
62 box_pose.pose = box_msg.pose;
64 geometry_msgs::PoseStamped box_pose_respected_to_cloud;
67 box_pose_respected_to_cloud);
69 Eigen::Affine3d box_pose_respected_to_cloud_eigend;
71 box_pose_respected_to_cloud_eigend);
72 Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
73 = box_pose_respected_to_cloud_eigend.inverse();
74 Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
75 Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
76 = box_pose_respected_to_cloud_eigend_inversed.matrix();
77 jsk_recognition_utils::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
78 box_pose_respected_to_cloud_eigen_inversed_matrixd,
79 box_pose_respected_to_cloud_eigen_inversed_matrixf);
80 offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
81 pcl::PointCloud<PointT> input;
83 pcl::transformPointCloud(input, output, offset);
91 sensor_msgs::PointCloud2,
98 virtual void transform(
const sensor_msgs::PointCloud2::ConstPtr& msg,
99 const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg);
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
void transformPointcloudInBoundingBox(const jsk_recognition_msgs::BoundingBox &box_msg, const sensor_msgs::PointCloud2 &cloud_msg, pcl::PointCloud< PointT > &output, Eigen::Affine3f &offset, tf::TransformListener &tf_listener)