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 #define BOOST_PARAMETER_MAX_ARITY 7
00037
00038 #include "jsk_pcl_ros/plane_supported_cuboid_estimator.h"
00039 #include "jsk_pcl_ros/pcl_conversion_util.h"
00040 #include "jsk_pcl_ros/geo_util.h"
00041 #include "jsk_pcl_ros/pcl_util.h"
00042 #include <ctime>
00043 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00044 #include <pcl/filters/extract_indices.h>
00045 #include <pcl/filters/crop_box.h>
00046 #include <algorithm>
00047
00048 namespace jsk_pcl_ros
00049 {
00050 void PlaneSupportedCuboidEstimator::onInit()
00051 {
00052 DiagnosticNodelet::onInit();
00053 tf_ = TfListenerSingleton::getInstance();
00054 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00055 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00056 boost::bind (&PlaneSupportedCuboidEstimator::configCallback, this, _1, _2);
00057 srv_->setCallback (f);
00058 pnh_->param("sensor_frame", sensor_frame_, std::string("odom"));
00059 pub_result_ = pnh_->advertise<jsk_recognition_msgs::BoundingBoxArray>("output/result", 1);
00060 pub_particles_ = pnh_->advertise<sensor_msgs::PointCloud2>("output/particles", 1);
00061 pub_candidate_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>("output/candidate_cloud", 1);
00062 pub_histogram_global_x_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/global/x", 1);
00063 pub_histogram_global_y_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/global/y", 1);
00064 pub_histogram_global_z_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/global/z", 1);
00065 pub_histogram_global_roll_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/global/roll", 1);
00066 pub_histogram_global_pitch_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/global/pitch", 1);
00067 pub_histogram_global_yaw_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/global/yaw", 1);
00068 pub_histogram_dx_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/dx", 1);
00069 pub_histogram_dy_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/dy", 1);
00070 pub_histogram_dz_ = pnh_->advertise<jsk_recognition_msgs::HistogramWithRange>("output/histogram/dz", 1);
00071 random_generator_ = boost::mt19937(static_cast<unsigned long>(time(0)));
00072
00073 sub_polygon_.subscribe(*pnh_, "input/polygon", 10);
00074 sub_coefficients_.subscribe(*pnh_, "input/coefficients", 10);
00075 sync_polygon_ = boost::make_shared<message_filters::Synchronizer<PolygonSyncPolicy> >(100);
00076 sync_polygon_->connectInput(sub_polygon_, sub_coefficients_);
00077 sync_polygon_->registerCallback(boost::bind(&PlaneSupportedCuboidEstimator::polygonCallback, this, _1, _2));
00078 sub_cloud_ = pnh_->subscribe("input", 1, &PlaneSupportedCuboidEstimator::cloudCallback, this);
00079 srv_reset_ = pnh_->advertiseService("reset", &PlaneSupportedCuboidEstimator::resetCallback, this);
00080 }
00081
00082 void PlaneSupportedCuboidEstimator::subscribe()
00083 {
00084
00085 }
00086
00087 void PlaneSupportedCuboidEstimator::unsubscribe()
00088 {
00089
00090 }
00091
00092 size_t PlaneSupportedCuboidEstimator::getNearestPolygon(
00093 const Particle& p,
00094 const std::vector<ConvexPolygon::Ptr>& polygons)
00095 {
00096 size_t min_index = 0;
00097 double min_distance = DBL_MAX;
00098 Eigen::Vector3f inp = p.getVector3fMap();
00099 for (size_t i = 0; i < polygons.size(); i++) {
00100 ConvexPolygon::Ptr polygon = polygons[i];
00101 Eigen::Vector3f p;
00102 polygon->project(inp, p);
00103 double d = (p - inp).norm();
00104 if (d < min_distance) {
00105 min_distance = d;
00106 min_index = i;
00107 }
00108 }
00109 return min_index;
00110 }
00111
00112 void PlaneSupportedCuboidEstimator::updateParticlePolygonRelationship(
00113 ParticleCloud::Ptr particles)
00114 {
00115 if (latest_polygon_msg_->polygons.size() == 0) {
00116 NODELET_ERROR("no valid polygons, skip update relationship");
00117 return;
00118 }
00119
00120
00121
00122 std::vector<ConvexPolygon::Ptr> convexes;
00123 for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
00124 ConvexPolygon::Ptr polygon = ConvexPolygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
00125 convexes.push_back(polygon);
00126 }
00127
00128
00129
00130 for (size_t i = 0; i < particles->points.size(); i++) {
00131 size_t nearest_index = getNearestPolygon(particles->points[i], convexes);
00132
00133 particles->points[i].plane_index = (int)nearest_index;
00134 }
00135 }
00136
00137 void PlaneSupportedCuboidEstimator::cloudCallback(
00138 const sensor_msgs::PointCloud2::ConstPtr& msg)
00139 {
00140 boost::mutex::scoped_lock lock(mutex_);
00141 NODELET_INFO("cloudCallback");
00142 if (!latest_polygon_msg_ || !latest_coefficients_msg_) {
00143 JSK_NODELET_WARN("Not yet polygon is available");
00144 return;
00145 }
00146
00147
00148 polygons_.clear();
00149 for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
00150 Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
00151 polygons_.push_back(polygon);
00152 }
00153
00154
00155 tf::StampedTransform transform
00156 = lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
00157 ros::Time(0.0),
00158 ros::Duration(0.0));
00159 tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
00160
00161 if (!tracker_) {
00162 NODELET_INFO("initTracker");
00163 pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr particles = initParticles();
00164 tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
00165 tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
00166 tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
00167 tracker_->setParticles(particles);
00168 tracker_->setParticleNum(particle_num_);
00169 }
00170 else {
00171 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00172 pcl::fromROSMsg(*msg, *cloud);
00173 tracker_->setInputCloud(cloud);
00174
00175
00176 candidate_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00177 std::set<int> all_indices;
00178 for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
00179 Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
00180 pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism_extract;
00181 pcl::PointCloud<pcl::PointXYZ>::Ptr
00182 boundaries (new pcl::PointCloud<pcl::PointXYZ>);
00183 polygon->boundariesToPointCloud<pcl::PointXYZ>(*boundaries);
00184 boundaries->points.push_back(boundaries->points[0]);
00185 prism_extract.setInputCloud(cloud);
00186 prism_extract.setViewPoint(viewpoint_[0], viewpoint_[1], viewpoint_[2]);
00187 prism_extract.setHeightLimits(init_local_position_z_min_, init_local_position_z_max_);
00188 prism_extract.setInputPlanarHull(boundaries);
00189 pcl::PointIndices output_indices;
00190 prism_extract.segment(output_indices);
00191 for (size_t i = 0; i < output_indices.indices.size(); i++) {
00192 all_indices.insert(output_indices.indices[i]);
00193 }
00194 }
00195 pcl::ExtractIndices<pcl::PointXYZ> ex;
00196 ex.setInputCloud(cloud);
00197 pcl::PointIndices::Ptr indices (new pcl::PointIndices);
00198 indices->indices = std::vector<int>(all_indices.begin(), all_indices.end());
00199 ex.setIndices(indices);
00200 ex.filter(*candidate_cloud_);
00201 sensor_msgs::PointCloud2 ros_candidate_cloud;
00202 pcl::toROSMsg(*candidate_cloud_, ros_candidate_cloud);
00203 ros_candidate_cloud.header = msg->header;
00204 pub_candidate_cloud_.publish(ros_candidate_cloud);
00205 tree_.setInputCloud(candidate_cloud_);
00206 if (support_plane_updated_) {
00207
00208 NODELET_INFO("polygon updated");
00209 updateParticlePolygonRelationship(tracker_->getParticles());
00210 }
00211 tracker_->compute();
00212 Particle result = tracker_->getResult();
00213 jsk_recognition_msgs::BoundingBoxArray box_array;
00214 box_array.header = msg->header;
00215 box_array.boxes.push_back(result.toBoundingBox());
00216 box_array.boxes[0].header = msg->header;
00217 pub_result_.publish(box_array);
00218 }
00219 support_plane_updated_ = false;
00220 ParticleCloud::Ptr particles = tracker_->getParticles();
00221
00222 publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
00223 publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
00224 publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
00225 publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
00226 publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
00227 publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
00228 publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
00229 publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
00230 publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
00231
00232 sensor_msgs::PointCloud2 ros_particles;
00233 pcl::toROSMsg(*particles, ros_particles);
00234 ros_particles.header = msg->header;
00235 pub_particles_.publish(ros_particles);
00236
00237 }
00238
00239 void PlaneSupportedCuboidEstimator::publishHistogram(
00240 ParticleCloud::Ptr particles, int index, ros::Publisher& pub, const std_msgs::Header& header)
00241 {
00242 const double step = 0.001;
00243
00244 float max_value = -FLT_MAX;
00245 float min_value = FLT_MAX;
00246 for (size_t i = 0; i < particles->points.size(); i++) {
00247 max_value = std::max(max_value, particles->points[i][index]);
00248 min_value = std::min(min_value, particles->points[i][index]);
00249 }
00250 int num = (max_value - min_value) / step + 1;
00251 std::vector<unsigned int> bins(num, 0);
00252 for (size_t i = 0; i < particles->points.size(); i++) {
00253 float value = particles->points[i][index];
00254 const int bin_index = (value - min_value) / step;
00255 const int min_confirmed_bin_index = std::min(bin_index, num - 1);
00256 bins[min_confirmed_bin_index] = bins[min_confirmed_bin_index] + 1;
00257 }
00258
00259 jsk_recognition_msgs::HistogramWithRange histogram;
00260 histogram.header = header;
00261 for (size_t i = 0; i < bins.size(); i++) {
00262 jsk_recognition_msgs::HistogramWithRangeBin bin;
00263 bin.min_value = i * step + min_value;
00264 bin.max_value = (i + 1) * step + min_value;
00265 bin.count = bins[i];
00266 histogram.bins.push_back(bin);
00267 }
00268 pub.publish(histogram);
00269 }
00270
00271 pcl::tracking::ParticleCuboid PlaneSupportedCuboidEstimator::sample(const pcl::tracking::ParticleCuboid& p)
00272 {
00273 pcl::tracking::ParticleCuboid sampled_particle;
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283 sampled_particle.x = randomGaussian(p.x, step_x_variance_, random_generator_);
00284 sampled_particle.y = randomGaussian(p.y, step_y_variance_, random_generator_);
00285 sampled_particle.z = randomGaussian(p.z, step_z_variance_, random_generator_);
00286 sampled_particle.roll = randomGaussian(p.roll, step_roll_variance_, random_generator_);
00287 sampled_particle.pitch = randomGaussian(p.pitch, step_pitch_variance_, random_generator_);
00288 sampled_particle.yaw = randomGaussian(p.yaw, step_yaw_variance_, random_generator_);
00289 sampled_particle.dx = std::max(randomGaussian(p.dx, step_dx_variance_, random_generator_),
00290 min_dx_);
00291 sampled_particle.dy = std::max(randomGaussian(p.dy, step_dy_variance_, random_generator_),
00292 min_dy_);
00293 sampled_particle.dz = std::max(randomGaussian(p.dz, step_dz_variance_, random_generator_),
00294 min_dz_);
00295 sampled_particle.plane_index = p.plane_index;
00296 sampled_particle.weight = p.weight;
00297 return sampled_particle;
00298 }
00299
00300 void PlaneSupportedCuboidEstimator::likelihood(pcl::PointCloud<pcl::PointXYZ>::ConstPtr input,
00301 pcl::tracking::ParticleCuboid& p)
00302 {
00303 p.weight = computeLikelihood(p, candidate_cloud_, tree_, viewpoint_,
00304 polygons_, latest_polygon_msg_->likelihood,
00305 config_);
00306 }
00307
00308 void PlaneSupportedCuboidEstimator::polygonCallback(
00309 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00310 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coef_msg)
00311 {
00312 boost::mutex::scoped_lock lock(mutex_);
00313 latest_polygon_msg_ = polygon_msg;
00314 latest_coefficients_msg_ = coef_msg;
00315 support_plane_updated_ = true;
00316 }
00317
00318 size_t PlaneSupportedCuboidEstimator::chooseUniformRandomPlaneIndex(const std::vector<Polygon::Ptr>& polygons)
00319 {
00320
00321 std::vector<double> area_table(polygons.size());
00322 double sum = 0;
00323 for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
00324 area_table[i] = polygons[i]->area();
00325 if (use_init_polygon_likelihood_) {
00326 area_table[i] = area_table[i] * latest_polygon_msg_->likelihood[i];
00327 }
00328 sum += area_table[i];
00329 }
00330 double val = randomUniform(0, sum, random_generator_);
00331 double bin_start = 0.0;
00332 for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
00333 if (val >= bin_start && val < bin_start + area_table[i]) {
00334 return i;
00335 }
00336 bin_start += area_table[i];
00337 }
00338
00339 NODELET_ERROR("should not reach here, failed to select plane randomly");
00340 return 0;
00341 }
00342
00343 pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr PlaneSupportedCuboidEstimator::initParticles()
00344 {
00345 pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr particles (new pcl::PointCloud<pcl::tracking::ParticleCuboid>);
00346 particles->points.resize(particle_num_);
00347 std::vector<Polygon::Ptr> polygons(latest_polygon_msg_->polygons.size());
00348 for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
00349 Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
00350 polygons[i] = polygon;
00351 }
00352 for (size_t i = 0; i < particle_num_; i++) {
00353 while (true) {
00354 pcl::tracking::ParticleCuboid p_local;
00355 size_t plane_i = chooseUniformRandomPlaneIndex(polygons);
00356 Polygon::Ptr polygon = polygons[plane_i];
00357 Eigen::Vector3f v = polygon->randomSampleLocalPoint(random_generator_);
00358 v[2] = randomUniform(init_local_position_z_min_, init_local_position_z_max_,
00359 random_generator_);
00360 p_local.getVector3fMap() = v;
00361 p_local.roll = randomGaussian(0, init_local_orientation_roll_variance_, random_generator_);
00362 p_local.pitch = randomGaussian(0, init_local_orientation_pitch_variance_, random_generator_);
00363 p_local.yaw = randomGaussian(init_local_orientation_yaw_mean_,
00364 init_local_orientation_yaw_variance_,
00365 random_generator_);
00366 p_local.dx = randomGaussian(init_dx_mean_, init_dx_variance_, random_generator_);
00367 p_local.dy = randomGaussian(init_dy_mean_, init_dy_variance_, random_generator_);
00368 p_local.dz = randomGaussian(init_dz_mean_, init_dz_variance_, random_generator_);
00369 pcl::tracking::ParticleCuboid p_global = p_local * polygon->coordinates();
00370 if (use_init_world_position_z_model_) {
00371 if (p_global.z < init_world_position_z_min_ ||
00372 p_global.z > init_world_position_z_max_) {
00373 continue;
00374 }
00375 }
00376 p_global.plane_index = plane_i;
00377 particles->points[i] = p_global;
00378 break;
00379 }
00380 }
00381 return particles;
00382 }
00383
00384 void PlaneSupportedCuboidEstimator::configCallback(Config& config, uint32_t level)
00385 {
00386 boost::mutex::scoped_lock lock(mutex_);
00387 config_ = config;
00388 init_local_position_z_min_ = config.init_local_position_z_min;
00389 init_local_position_z_max_ = config.init_local_position_z_max;
00390 use_init_world_position_z_model_ = config.use_init_world_position_z_model;
00391 init_world_position_z_min_ = config.init_world_position_z_min;
00392 init_world_position_z_max_ = config.init_world_position_z_max;
00393 init_local_orientation_roll_variance_ = config.init_local_orientation_roll_variance;
00394 init_local_orientation_pitch_variance_ = config.init_local_orientation_pitch_variance;
00395 init_local_orientation_yaw_mean_ = config.init_local_orientation_yaw_mean;
00396 init_local_orientation_yaw_variance_ = config.init_local_orientation_yaw_variance;
00397 init_dx_mean_ = config.init_dx_mean;
00398 init_dx_variance_ = config.init_dx_variance;
00399 init_dy_mean_ = config.init_dy_mean;
00400 init_dy_variance_ = config.init_dy_variance;
00401 init_dz_mean_ = config.init_dz_mean;
00402 init_dz_variance_ = config.init_dz_variance;
00403 particle_num_ = config.particle_num;
00404 step_x_variance_ = config.step_x_variance;
00405 step_y_variance_ = config.step_y_variance;
00406 step_z_variance_ = config.step_z_variance;
00407 step_roll_variance_ = config.step_roll_variance;
00408 step_pitch_variance_ = config.step_pitch_variance;
00409 step_yaw_variance_ = config.step_yaw_variance;
00410 step_dx_variance_ = config.step_dx_variance;
00411 step_dy_variance_ = config.step_dy_variance;
00412 step_dz_variance_ = config.step_dz_variance;
00413 min_dx_ = config.min_dx;
00414 min_dy_ = config.min_dy;
00415 min_dz_ = config.min_dz;
00416 use_init_polygon_likelihood_ = config.use_init_polygon_likelihood;
00417 }
00418
00419 bool PlaneSupportedCuboidEstimator::resetCallback(std_srvs::EmptyRequest& req,
00420 std_srvs::EmptyResponse& res)
00421 {
00422 boost::mutex::scoped_lock lock(mutex_);
00423 latest_polygon_msg_ = jsk_recognition_msgs::PolygonArray::ConstPtr();
00424 latest_coefficients_msg_ = jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr();
00425 tracker_.reset();
00426 return true;
00427 }
00428 }
00429
00430 #include <pluginlib/class_list_macros.h>
00431 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PlaneSupportedCuboidEstimator, nodelet::Nodelet);