39 #include <pcl/common/centroid.h> 50 pcl::PointCloud<pcl::PointXYZ> pointCloud;
57 Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0
f, 0.0
f, 0.0
f);
58 pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
59 float noiseLevel=0.00;
60 float minRange = 0.0f;
63 pcl::RangeImage rangeImage;
64 rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,
65 sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
66 ROS_INFO_STREAM(
"input image size " << input->width <<
" x " << input->height <<
"(=" << input->width * input->height <<
")");
67 ROS_INFO_STREAM(
"output image size " << rangeImage.width <<
" x " << rangeImage.height <<
"(=" << rangeImage.width * rangeImage.height <<
")");
70 sensor_msgs::PointCloud2 out;
72 out.header = input->header;
78 ConnectionBasedNodelet::onInit();
80 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
82 pnh_->param<
double>(
"angle_width",
angle_width, 120.0);
84 pnh_->param<
int>(
"min_points",
min_points, 1000);
void publish(const boost::shared_ptr< M > &message) const
double angular_resolution
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void unsubscribe()
virtual void extract(const sensor_msgs::PointCloud2ConstPtr &input)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::OrganizePointCloud, nodelet::Nodelet)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
#define ROS_DEBUG_STREAM(args)
#define ROS_INFO_STREAM(args)