Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "jsk_pcl_ros/normal_estimation_integral_image.h"
00037 #include <pluginlib/class_list_macros.h>
00038
00039 #include <pcl/features/integral_image_normal.h>
00040
00041
00042 namespace jsk_pcl_ros
00043 {
00044 void NormalEstimationIntegralImage::onInit()
00045 {
00046 ConnectionBasedNodelet::onInit();
00047
00048 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00049 dynamic_reconfigure::Server<Config>::CallbackType f =
00050 boost::bind (&NormalEstimationIntegralImage::configCallback, this, _1, _2);
00051 srv_->setCallback (f);
00052
00053 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00054 pub_with_xyz_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_with_xyz", 1);
00055 }
00056
00057 void NormalEstimationIntegralImage::subscribe()
00058 {
00059 sub_input_ = pnh_->subscribe("input", 1, &NormalEstimationIntegralImage::compute, this);
00060 }
00061
00062 void NormalEstimationIntegralImage::unsubscribe()
00063 {
00064 sub_input_.shutdown();
00065 }
00066
00067 void NormalEstimationIntegralImage::configCallback(Config& config, uint32_t level)
00068 {
00069 boost::mutex::scoped_lock lock(mutex_);
00070 max_depth_change_factor_ = config.max_depth_change_factor;
00071 normal_smoothing_size_ = config.normal_smoothing_size;
00072 depth_dependent_smoothing_ = config.depth_dependent_smoothing;
00073 estimation_method_ = config.estimation_method;
00074 border_policy_ignore_ = config.border_policy_ignore;
00075 }
00076
00077 void NormalEstimationIntegralImage::compute(const sensor_msgs::PointCloud2::ConstPtr& msg)
00078 {
00079 boost::mutex::scoped_lock lock(mutex_);
00080
00081 pcl::PointCloud<pcl::PointXYZRGB>::Ptr input(new pcl::PointCloud<pcl::PointXYZRGB>());
00082 pcl::fromROSMsg(*msg, *input);
00083 pcl::PointCloud<pcl::Normal> output;
00084 pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
00085 if (estimation_method_ == 0) {
00086 ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
00087 }
00088 else if (estimation_method_ == 1) {
00089 ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
00090 }
00091 else if (estimation_method_ == 2) {
00092 ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
00093 }
00094 else {
00095 JSK_NODELET_FATAL("unknown estimation method: %d", estimation_method_);
00096 return;
00097 }
00098
00099 if (border_policy_ignore_) {
00100 ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal>::BORDER_POLICY_IGNORE);
00101 }
00102 else {
00103 ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal>::BORDER_POLICY_MIRROR);
00104 }
00105
00106 ne.setMaxDepthChangeFactor(max_depth_change_factor_);
00107 ne.setNormalSmoothingSize(normal_smoothing_size_);
00108 ne.setDepthDependentSmoothing(depth_dependent_smoothing_);
00109 ne.setInputCloud(input);
00110 ne.compute(output);
00111 sensor_msgs::PointCloud2 ros_output;
00112 pcl::toROSMsg(output, ros_output);
00113 pub_.publish(ros_output);
00114
00115
00116 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr concat(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
00117 concat->points.resize(output.points.size());
00118 for (size_t i = 0; i < output.points.size(); i++) {
00119 pcl::PointXYZRGBNormal point;
00120 point.x = input->points[i].x;
00121 point.y = input->points[i].y;
00122 point.z = input->points[i].z;
00123 point.rgb = input->points[i].rgb;
00124 point.normal_x = output.points[i].normal_x;
00125 point.normal_y = output.points[i].normal_y;
00126 point.normal_z = output.points[i].normal_z;
00127 point.curvature = output.points[i].curvature;
00128 concat->points[i] = point;
00129 }
00130 concat->header = input->header;
00131 sensor_msgs::PointCloud2 ros_output_with_xyz;
00132 pcl::toROSMsg(*concat, ros_output_with_xyz);
00133 pub_with_xyz_.publish(ros_output_with_xyz);
00134 }
00135
00136 }
00137
00138
00139 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::NormalEstimationIntegralImage,
00140 nodelet::Nodelet);