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
00042
00043 #include <ros/ros.h>
00044
00045 #include <sensor_msgs/PointCloud.h>
00046 #include <geometry_msgs/Polygon.h>
00047 #include <mapping_msgs/PolygonalMap.h>
00048
00049
00050 #include <point_cloud_mapping/sample_consensus/sac.h>
00051 #include <point_cloud_mapping/sample_consensus/msac.h>
00052 #include <point_cloud_mapping/sample_consensus/sac_model_plane.h>
00053
00054 #include <tf/transform_listener.h>
00055 #include <angles/angles.h>
00056
00057
00058 #include <point_cloud_mapping/kdtree/kdtree_ann.h>
00059
00060
00061 #include <point_cloud_mapping/geometry/areas.h>
00062 #include <point_cloud_mapping/geometry/point.h>
00063 #include <point_cloud_mapping/geometry/distances.h>
00064 #include <point_cloud_mapping/geometry/nearest.h>
00065 #include <point_cloud_mapping/geometry/statistics.h>
00066
00067 #include <sys/time.h>
00068
00069 using namespace std;
00070 using namespace mapping_msgs;
00071
00072 struct Region
00073 {
00074 char region_type;
00075 vector<int> indices;
00076 };
00077
00078 class SemanticPointAnnotator
00079 {
00080 protected:
00081 ros::NodeHandle& node_;
00082
00083 public:
00084
00085
00086 sensor_msgs::PointCloud cloud_, cloud_annotated_;
00087 geometry_msgs::Point32 z_axis_;
00088 PolygonalMap pmap_;
00089
00090 tf::TransformListener tf_;
00091
00092
00093 int sac_min_points_per_model_, sac_min_points_left_;
00094 double sac_distance_threshold_, eps_angle_, region_angle_threshold_, boundary_angle_threshold_;
00095
00096 double rule_floor_, rule_ceiling_, rule_wall_;
00097 double rule_table_min_, rule_table_max_;
00098
00099 double region_growing_tolerance_;
00100
00101 bool polygonal_map_, concave_;
00102 int min_cluster_pts_;
00103
00104 ros::Publisher polygonal_map_publisher_, cloud_publisher_;
00105 ros::Subscriber cloud_subscriber_;
00106
00108 SemanticPointAnnotator (ros::NodeHandle& anode) : node_ (anode)
00109 {
00110 node_.param ("rule_floor", rule_floor_, 0.3);
00111 node_.param ("rule_ceiling", rule_ceiling_, 2.0);
00112 node_.param ("rule_table_min", rule_table_min_, 0.5);
00113 node_.param ("rule_table_max", rule_table_max_, 1.5);
00114 node_.param ("rule_wall", rule_wall_, 2.0);
00115
00116 node_.param ("min_cluster_pts", min_cluster_pts_, 10);
00117
00118 node_.param ("region_growing_tolerance", region_growing_tolerance_, 0.25);
00119
00120 node_.param ("region_angle_threshold", region_angle_threshold_, 30.0);
00121 region_angle_threshold_ = angles::from_degrees (region_angle_threshold_);
00122
00123 node_.param ("p_sac_min_points_left", sac_min_points_left_, 10);
00124 node_.param ("p_sac_min_points_per_model", sac_min_points_per_model_, 10);
00125
00126
00127 node_.param ("p_sac_distance_threshold", sac_distance_threshold_, 0.05);
00128
00129 node_.param ("p_eps_angle", eps_angle_, 15.0);
00130
00131 node_.param ("create_polygonal_map", polygonal_map_, true);
00132 node_.param ("concave", concave_, false);
00133 node_.param ("boundary_angle_threshold", boundary_angle_threshold_, 120.0);
00134
00135 if (polygonal_map_)
00136 polygonal_map_publisher_ = node_.advertise<PolygonalMap> ("semantic_polygonal_map", 1);
00137
00138 eps_angle_ = angles::from_degrees (eps_angle_);
00139
00140 if (concave_)
00141 ROS_INFO ("Concave hulls enabled. Angle threshold set to %g.", boundary_angle_threshold_);
00142
00143 boundary_angle_threshold_ = angles::from_degrees (boundary_angle_threshold_);
00144
00145 string cloud_topic ("cloud_normals");
00146 std::vector<ros::master::TopicInfo> t_list;
00147 bool topic_found = false;
00148 ros::master::getTopics (t_list);
00149 for (vector<ros::master::TopicInfo>::iterator it = t_list.begin (); it != t_list.end (); it++)
00150 {
00151 if (it->name == cloud_topic)
00152 {
00153 topic_found = true;
00154 break;
00155 }
00156 }
00157 if (!topic_found)
00158 ROS_WARN ("Trying to subscribe to %s, but the topic doesn't exist!", cloud_topic.c_str ());
00159
00160 cloud_subscriber_ = node_.subscribe(cloud_topic, 1, &SemanticPointAnnotator::cloud_cb, this);
00161 cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud> ("cloud_annotated", 1);
00162
00163 z_axis_.x = 0; z_axis_.y = 0; z_axis_.z = 1;
00164
00165 cloud_annotated_.channels.resize (1);
00166 cloud_annotated_.channels[0].name = "rgb";
00167 }
00168
00170
00180 void
00181 findClusters (const sensor_msgs::PointCloud &points, const vector<int> &indices, double tolerance, vector<Region> &clusters,
00182 int nx_idx, int ny_idx, int nz_idx,
00183 unsigned int min_pts_per_cluster = 1)
00184 {
00185
00186 cloud_kdtree::KdTree* tree = new cloud_kdtree::KdTreeANN (points, indices);
00187
00188
00189 vector<bool> processed;
00190 processed.resize (indices.size (), false);
00191
00192 vector<int> nn_indices;
00193 vector<float> nn_distances;
00194
00195 for (unsigned int i = 0; i < indices.size (); i++)
00196 {
00197 if (processed[i])
00198 continue;
00199
00200 vector<int> seed_queue;
00201 int sq_idx = 0;
00202 seed_queue.push_back (i);
00203
00204 double norm_a = sqrt (points.channels[nx_idx].values[indices.at (i)] * points.channels[nx_idx].values[indices.at (i)] +
00205 points.channels[ny_idx].values[indices.at (i)] * points.channels[ny_idx].values[indices.at (i)] +
00206 points.channels[nz_idx].values[indices.at (i)] * points.channels[nz_idx].values[indices.at (i)]);
00207
00208 processed[i] = true;
00209
00210 while (sq_idx < (int)seed_queue.size ())
00211 {
00212 tree->radiusSearch (seed_queue.at (sq_idx), tolerance, nn_indices, nn_distances);
00213
00214 for (unsigned int j = 1; j < nn_indices.size (); j++)
00215 {
00216 if (!processed.at (nn_indices[j]))
00217 {
00218 double norm_b = sqrt (points.channels[nx_idx].values[indices.at (nn_indices[j])] * points.channels[nx_idx].values[indices.at (nn_indices[j])] +
00219 points.channels[ny_idx].values[indices.at (nn_indices[j])] * points.channels[ny_idx].values[indices.at (nn_indices[j])] +
00220 points.channels[nz_idx].values[indices.at (nn_indices[j])] * points.channels[nz_idx].values[indices.at (nn_indices[j])]);
00221
00222 double dot_p = points.channels[nx_idx].values[indices.at (i)] * points.channels[nx_idx].values[indices.at (nn_indices[j])] +
00223 points.channels[ny_idx].values[indices.at (i)] * points.channels[ny_idx].values[indices.at (nn_indices[j])] +
00224 points.channels[nz_idx].values[indices.at (i)] * points.channels[nz_idx].values[indices.at (nn_indices[j])];
00225 if ( acos (dot_p / (norm_a * norm_b)) < region_angle_threshold_)
00226 {
00227 processed[nn_indices[j]] = true;
00228 seed_queue.push_back (nn_indices[j]);
00229 }
00230 }
00231 }
00232
00233 sq_idx++;
00234 }
00235
00236
00237 if (seed_queue.size () >= min_pts_per_cluster)
00238 {
00239 Region r;
00240
00241 r.indices.resize (seed_queue.size ());
00242 for (unsigned int j = 0; j < r.indices.size (); j++)
00243 r.indices[j] = indices.at (seed_queue[j]);
00244 clusters.push_back (r);
00245 }
00246 }
00247
00248
00249 delete tree;
00250 }
00251
00253 void
00254 sortConcaveHull2D (const sensor_msgs::PointCloud &points, const vector<int> &indices, geometry_msgs::Polygon &poly)
00255 {
00256
00257 cloud_kdtree::KdTree* tree = new cloud_kdtree::KdTreeANN (points, indices);
00258
00259 vector<int> seed_queue;
00260 seed_queue.push_back (0);
00261
00262
00263 vector<bool> processed;
00264 processed.resize (indices.size (), false);
00265 vector<int> nn_indices;
00266 vector<float> nn_distances;
00267
00268
00269 int i = 0;
00270 processed[i] = true;
00271 while (seed_queue.size () < indices.size ())
00272 {
00273 tree->nearestKSearch (seed_queue[i], indices.size (), nn_indices, nn_distances);
00274
00275 for (unsigned int j = 0; j < nn_indices.size (); j++)
00276 {
00277 if (!processed.at (nn_indices[j]))
00278 {
00279 processed[nn_indices[j]] = true;
00280 seed_queue.push_back (nn_indices[j]);
00281 i++;
00282 break;
00283 }
00284 }
00285 }
00286
00287 poly.points.resize (seed_queue.size ());
00288 for (unsigned int i = 0; i < seed_queue.size (); i++)
00289 {
00290 poly.points[i].x = points.points.at (indices.at (seed_queue[i])).x;
00291 poly.points[i].y = points.points.at (indices.at (seed_queue[i])).y;
00292 poly.points[i].z = points.points.at (indices.at (seed_queue[i])).z;
00293 }
00294
00295 delete tree;
00296 }
00297
00299 int
00300 fitSACPlane (sensor_msgs::PointCloud *points, vector<int> *indices, vector<vector<int> > &inliers, vector<vector<double> > &coeff)
00301 {
00302 vector<int> empty_inliers;
00303 vector<double> empty_coeffs;
00304 if ((int)indices->size () < sac_min_points_per_model_)
00305 {
00306
00307 inliers.push_back (empty_inliers);
00308 coeff.push_back (empty_coeffs);
00309 return (-1);
00310 }
00311
00312
00313 sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
00314 sample_consensus::SAC *sac = new sample_consensus::MSAC (model, sac_distance_threshold_);
00315 sac->setMaxIterations (120);
00316 sac->setProbability (0.99);
00317 model->setDataSet (points, *indices);
00318
00319 sensor_msgs::PointCloud pts (*points);
00320 int nr_points_left = indices->size ();
00321 int nr_models = 0;
00322 while (nr_points_left > sac_min_points_left_)
00323 {
00324
00325 if (sac->computeModel ())
00326 {
00327
00328 if ((int)sac->getInliers ().size () < sac_min_points_per_model_)
00329 {
00330
00331 inliers.push_back (empty_inliers);
00332 coeff.push_back (empty_coeffs);
00333
00334 break;
00335 }
00336 inliers.push_back (sac->getInliers ());
00337 vector<double> model_coeff;
00338 sac->computeCoefficients (model_coeff);
00339 sac->refineCoefficients (model_coeff);
00340 coeff.push_back (model_coeff);
00341
00342
00343
00344
00345
00346 model->projectPointsInPlace (sac->getInliers (), coeff[coeff.size () - 1]);
00347
00348
00349 nr_points_left = sac->removeInliers ();
00350 nr_models++;
00351 }
00352 }
00353 return (0);
00354 }
00355
00357 void
00358 getObjectClassForParallel (sensor_msgs::PointCloud *points, vector<int> *indices, vector<double> *coeff,
00359 geometry_msgs::PointStamped map_origin, double &rgb)
00360 {
00361 double r = 0, g = 0, b = 0;
00362
00363 geometry_msgs::Point32 robot_origin;
00364 robot_origin.x = map_origin.point.x;
00365 robot_origin.y = map_origin.point.y;
00366 robot_origin.z = map_origin.point.z;
00367
00368
00369
00370 robot_origin.x = robot_origin.y = 0;
00371 for (unsigned int i = 0; i < indices->size (); i++)
00372 {
00373 robot_origin.x += points->points.at (indices->at (i)).x;
00374 robot_origin.y += points->points.at (indices->at (i)).y;
00375 }
00376 robot_origin.x /= indices->size ();
00377 robot_origin.y /= indices->size ();
00378
00379
00380 double distance = cloud_geometry::distances::pointToPlaneDistance (robot_origin, *coeff);
00381
00382
00383 if (distance < rule_floor_)
00384 {
00385 r = 81; g = 239; b = 168;
00386 }
00387
00388 else if (distance > rule_ceiling_)
00389 {
00390 r = 196; g = 197; b = 102;
00391 }
00392
00393 else if (distance >= rule_table_min_ && distance <= rule_table_max_)
00394 {
00395 r = 229; g = 86; b = 35;
00396 }
00397 int res = (int(r) << 16) | (int(g) << 8) | int(b);
00398 rgb = *(float*)(&res);
00399 }
00400
00402 void
00403 getObjectClassForPerpendicular (sensor_msgs::PointCloud *points, vector<int> *indices,
00404 double &rgb)
00405 {
00406 double r, g, b;
00407
00408 geometry_msgs::Point32 minP, maxP;
00409 cloud_geometry::statistics::getMinMax (*points, *indices, minP, maxP);
00410
00411 if (maxP.z > rule_wall_)
00412 {
00413 r = rand () / (RAND_MAX + 1.0);
00414 g = rand () / (RAND_MAX + 1.0);
00415 b = rand () / (RAND_MAX + 1.0);
00416 r = r * .3;
00417 b = b * .3 + .7;
00418 g = g * .3;
00419
00420 int res = (int(r * 255) << 16) | (int(g*255) << 8) | int(b*255);
00421 rgb = *(float*)(&res);
00422 }
00423 }
00424
00425
00427 void
00428 computeConcaveHull (const sensor_msgs::PointCloud &points, const vector<int> &indices, const vector<double> &coeff,
00429 const vector<vector<int> > &neighbors, geometry_msgs::Polygon &poly)
00430 {
00431 Eigen::Vector3d u, v;
00432 cloud_geometry::getCoordinateSystemOnPlane (coeff, u, v);
00433
00434 vector<int> inliers (indices.size ());
00435 int nr_p = 0;
00436 for (unsigned int i = 0; i < indices.size (); i++)
00437 {
00438 if (cloud_geometry::nearest::isBoundaryPoint (points, indices.at (i), neighbors.at (i), u, v, boundary_angle_threshold_))
00439 {
00440 inliers[nr_p] = indices.at (i);
00441 nr_p++;
00442 }
00443 }
00444 inliers.resize (nr_p);
00445
00446 sortConcaveHull2D (points, inliers, poly);
00447 }
00448
00449
00451
00452 void cloud_cb (const sensor_msgs::PointCloudConstPtr &cloud_in )
00453 {
00454 cloud_ = *cloud_in;
00455 geometry_msgs::PointStamped base_link_origin, map_origin;
00456 base_link_origin.point.x = base_link_origin.point.y = base_link_origin.point.z = 0.0;
00457 base_link_origin.header.frame_id = "base_link";
00458 base_link_origin.header.stamp = ros::Time();
00459
00460 tf_.transformPoint ("base_link", base_link_origin, map_origin);
00461
00462 ROS_INFO ("Received %d data points. Current robot pose is %g, %g, %g", (int)cloud_.points.size (), map_origin.point.x, map_origin.point.y, map_origin.point.z);
00463
00464 cloud_annotated_.header = cloud_.header;
00465
00466 int nx = cloud_geometry::getChannelIndex (cloud_, "nx");
00467 int ny = cloud_geometry::getChannelIndex (cloud_, "ny");
00468 int nz = cloud_geometry::getChannelIndex (cloud_, "nz");
00469
00470 if ( (cloud_.channels.size () < 3) || (nx == -1) || (ny == -1) || (nz == -1) )
00471 {
00472 ROS_ERROR ("This PointCloud message does not contain normal information!");
00473 return;
00474 }
00475
00476 timeval t1, t2;
00477 double time_spent;
00478 gettimeofday (&t1, NULL);
00479
00480
00481 vector<int> indices_z;
00482 cloud_geometry::getPointIndicesAxisParallelNormals (cloud_, nx, ny, nz, eps_angle_, z_axis_, indices_z);
00483
00484
00485 vector<int> indices_xy;
00486 cloud_geometry::getPointIndicesAxisPerpendicularNormals (cloud_, nx, ny, nz, eps_angle_, z_axis_, indices_xy);
00487
00488 vector<Region> clusters;
00489
00490 findClusters (cloud_, indices_z, region_growing_tolerance_, clusters, nx, ny, nz, min_cluster_pts_);
00491 int z_c = clusters.size ();
00492 for (int i = 0; i < z_c; i++)
00493 clusters[i].region_type = 0;
00494
00495
00496 findClusters (cloud_, indices_xy, region_growing_tolerance_, clusters, nx, ny, nz, min_cluster_pts_);
00497 for (unsigned int i = z_c; i < clusters.size (); i++)
00498 clusters[i].region_type = 1;
00499
00500
00501 int total_p = 0;
00502 for (int cc = 0; cc < (int)clusters.size (); cc++)
00503 total_p += clusters[cc].indices.size ();
00504
00505 gettimeofday (&t2, NULL);
00506 time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
00507 ROS_INFO ("Found %d clusters with %d points in %g seconds.", (int)clusters.size (), total_p, time_spent);
00508 gettimeofday (&t1, NULL);
00509
00510 vector<vector<vector<int> > > all_cluster_inliers (clusters.size ());
00511 vector<vector<vector<double> > > all_cluster_coeff (clusters.size ());
00512
00513
00514 cloud_annotated_.points.resize (total_p);
00515 cloud_annotated_.channels[0].values.resize (total_p);
00516
00517 int nr_p = 0;
00518
00519 if (polygonal_map_)
00520 {
00521 pmap_.header = cloud_.header;
00522 pmap_.polygons.resize (clusters.size ());
00523 }
00524
00525
00526 #pragma omp parallel for schedule(dynamic)
00527 for (int cc = 0; cc < (int)clusters.size (); cc++)
00528 {
00529
00530 int ret = fitSACPlane (&cloud_, &clusters[cc].indices, all_cluster_inliers[cc], all_cluster_coeff[cc]);
00531 if (ret != 0 || all_cluster_inliers[cc].size () == 0)
00532 continue;
00533 }
00534
00535
00536
00537 vector<float> nn_distances;
00538 vector<vector<vector<int> > > neighbors (clusters.size ());
00539 if (concave_)
00540 {
00541 for (int cc = 0; cc < (int)clusters.size (); cc++)
00542 {
00543 if (all_cluster_inliers[cc].size () == 0 || all_cluster_coeff[cc].size () == 0)
00544 continue;
00545
00546 vector<vector<int> > *cluster_neighbors = &neighbors[cc];
00547
00548 if (all_cluster_inliers[cc][0].size () == 0)
00549 continue;
00550 cluster_neighbors->resize (all_cluster_inliers[cc][0].size ());
00551
00552
00553 cloud_kdtree::KdTree* tree = new cloud_kdtree::KdTreeANN (cloud_, all_cluster_inliers[cc][0]);
00554 for (unsigned int i = 0; i < all_cluster_inliers[cc][0].size (); i++)
00555 {
00556 tree->radiusSearch (i, 0.3, cluster_neighbors->at (i), nn_distances);
00557
00558
00559
00560 for (unsigned int j = 0; j < cluster_neighbors->at (i).size (); j++)
00561 cluster_neighbors->at(i).at(j) = all_cluster_inliers[cc][0].at (cluster_neighbors->at(i).at(j));
00562 }
00563
00564 delete tree;
00565 }
00566 gettimeofday (&t2, NULL);
00567 time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
00568 ROS_INFO ("Nearest neighbors (concave hull) estimated in %g seconds.", time_spent);
00569 gettimeofday (&t1, NULL);
00570 }
00571
00572 if (polygonal_map_)
00573 {
00574
00575
00576
00577 for (int cc = 0; cc < (int)clusters.size (); cc++)
00578 {
00579 if (all_cluster_inliers[cc].size () == 0 || all_cluster_coeff[cc].size () == 0)
00580 continue;
00581
00582 if (all_cluster_inliers[cc][0].size () == 0 || all_cluster_coeff[cc][0].size () == 0)
00583 continue;
00584
00585 if (concave_)
00586 computeConcaveHull (cloud_, all_cluster_inliers[cc][0], all_cluster_coeff[cc][0], neighbors[cc], pmap_.polygons[cc]);
00587 else
00588 cloud_geometry::areas::convexHull2D (cloud_, all_cluster_inliers[cc][0], all_cluster_coeff[cc][0], pmap_.polygons[cc]);
00589 }
00590 }
00591
00592
00593 for (int cc = 0; cc < (int)clusters.size (); cc++)
00594 {
00595 double r, g, b, rgb;
00596 r = g = b = 1.0;
00597 int res = (int(r * 255) << 16) | (int(g*255) << 8) | int(b*255);
00598 rgb = *(float*)(&res);
00599
00600
00601 vector<vector<int> > *planes_inliers = &all_cluster_inliers[cc];
00602 vector<vector<double> > *planes_coeffs = &all_cluster_coeff[cc];
00603
00604 if (planes_inliers->size () == 0 || planes_coeffs->size () == 0 || planes_inliers->size () != planes_coeffs->size ())
00605 continue;
00606
00607
00608 for (unsigned int j = 0; j < planes_inliers->size (); j++)
00609 {
00610 vector<int> *plane_inliers = &planes_inliers->at (j);
00611 vector<double> *plane_coeffs = &planes_coeffs->at (j);
00612
00613 if (plane_inliers->size () == 0 || plane_coeffs->size () == 0)
00614 continue;
00615
00616
00617 switch (clusters[cc].region_type)
00618 {
00619 case 0:
00620 {
00621 getObjectClassForParallel (&cloud_, plane_inliers, plane_coeffs, map_origin, rgb);
00622 break;
00623 }
00624 case 1:
00625 {
00626 getObjectClassForPerpendicular (&cloud_, plane_inliers, rgb);
00627 break;
00628 }
00629 }
00630 for (unsigned int k = 0; k < plane_inliers->size (); k++)
00631 {
00632 cloud_annotated_.points[nr_p].x = cloud_.points.at (plane_inliers->at (k)).x;
00633 cloud_annotated_.points[nr_p].y = cloud_.points.at (plane_inliers->at (k)).y;
00634 cloud_annotated_.points[nr_p].z = cloud_.points.at (plane_inliers->at (k)).z;
00635 cloud_annotated_.channels[0].values[nr_p] = rgb;
00636 nr_p++;
00637 }
00638 }
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650 }
00651
00652 gettimeofday (&t2, NULL);
00653 time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
00654 ROS_INFO ("Cloud annotated in: %g seconds.", time_spent);
00655 gettimeofday (&t1, NULL);
00656 cloud_annotated_.points.resize (nr_p);
00657 cloud_annotated_.channels[0].values.resize (nr_p);
00658
00659 cloud_publisher_.publish(cloud_annotated_);
00660
00661 if (polygonal_map_)
00662 polygonal_map_publisher_.publish (pmap_);
00663 return;
00664
00665
00687 }
00688 };
00689
00690
00691 int
00692 main (int argc, char** argv)
00693 {
00694 ros::init (argc, argv,"semantic_point_annotator");
00695 ros::NodeHandle ros_node("~");
00696
00697 SemanticPointAnnotator p (ros_node);
00698 ros::spin ();
00699
00700 return (0);
00701 }
00702
00703