Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * tabletop_objects_thread.cpp - Thread to detect tabletop objects 00004 * 00005 * Created: Sat Nov 05 00:22:41 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #include "tabletop_objects_thread.h" 00023 #include "cluster_colors.h" 00024 #ifdef HAVE_VISUAL_DEBUGGING 00025 # include "visualization_thread_base.h" 00026 #endif 00027 00028 #include <pcl_utils/utils.h> 00029 #include <pcl_utils/comparisons.h> 00030 #include <utils/time/wait.h> 00031 #include <utils/math/angle.h> 00032 #ifdef USE_TIMETRACKER 00033 # include <utils/time/tracker.h> 00034 #endif 00035 00036 #include <pcl/point_cloud.h> 00037 #include <pcl/point_types.h> 00038 #include <pcl/sample_consensus/method_types.h> 00039 #include <pcl/sample_consensus/model_types.h> 00040 #include <pcl/segmentation/extract_clusters.h> 00041 #include <pcl/filters/extract_indices.h> 00042 #include <pcl/surface/convex_hull.h> 00043 #include <pcl/kdtree/kdtree.h> 00044 #include <pcl/kdtree/kdtree_flann.h> 00045 #include <pcl/filters/project_inliers.h> 00046 #include <pcl/filters/conditional_removal.h> 00047 #include <pcl/common/centroid.h> 00048 #include <pcl/common/transforms.h> 00049 #include <pcl/common/distances.h> 00050 00051 #include <interfaces/Position3DInterface.h> 00052 #include <interfaces/SwitchInterface.h> 00053 00054 #include <iostream> 00055 using namespace std; 00056 00057 #define CFG_PREFIX "/perception/tabletop-objects/" 00058 00059 /** @class TabletopObjectsThread "tabletop_objects_thread.h" 00060 * Main thread of tabletop objects plugin. 00061 * @author Tim Niemueller 00062 */ 00063 00064 using namespace fawkes; 00065 00066 00067 #ifdef USE_TIMETRACKER 00068 #define TIMETRACK_START(c) \ 00069 tt_->ping_start(c); \ 00070 00071 #define TIMETRACK_INTER(c1, c2) \ 00072 tt_->ping_end(c1); \ 00073 tt_->ping_start(c2); 00074 00075 #define TIMETRACK_END(c) \ 00076 tt_->ping_end(c); 00077 00078 #define TIMETRACK_ABORT(c) \ 00079 tt_->ping_abort(c); 00080 00081 #else 00082 00083 #define TIMETRACK_START(c) 00084 #define TIMETRACK_INTER(c1, c2) 00085 #define TIMETRACK_END(c) 00086 #define TIMETRACK_ABORT(c) 00087 #endif 00088 00089 /** Constructor. */ 00090 TabletopObjectsThread::TabletopObjectsThread() 00091 : Thread("TabletopObjectsThread", Thread::OPMODE_CONTINUOUS), 00092 TransformAspect(TransformAspect::ONLY_LISTENER) 00093 { 00094 #ifdef HAVE_VISUAL_DEBUGGING 00095 visthread_ = NULL; 00096 #endif 00097 } 00098 00099 00100 /** Destructor. */ 00101 TabletopObjectsThread::~TabletopObjectsThread() 00102 { 00103 } 00104 00105 00106 void 00107 TabletopObjectsThread::init() 00108 { 00109 cfg_depth_filter_min_x_ = config->get_float(CFG_PREFIX"depth_filter_min_x"); 00110 cfg_depth_filter_max_x_ = config->get_float(CFG_PREFIX"depth_filter_max_x"); 00111 cfg_voxel_leaf_size_ = config->get_float(CFG_PREFIX"voxel_leaf_size"); 00112 cfg_segm_max_iterations_ = 00113 config->get_uint(CFG_PREFIX"table_segmentation_max_iterations"); 00114 cfg_segm_distance_threshold_ = 00115 config->get_float(CFG_PREFIX"table_segmentation_distance_threshold"); 00116 cfg_segm_inlier_quota_ = 00117 config->get_float(CFG_PREFIX"table_segmentation_inlier_quota"); 00118 cfg_max_z_angle_deviation_ = config->get_float(CFG_PREFIX"max_z_angle_deviation"); 00119 cfg_table_min_height_ = config->get_float(CFG_PREFIX"table_min_height"); 00120 cfg_table_max_height_ = config->get_float(CFG_PREFIX"table_max_height"); 00121 cfg_table_model_length_ = config->get_float(CFG_PREFIX"table_model_length"); 00122 cfg_table_model_width_ = config->get_float(CFG_PREFIX"table_model_width"); 00123 cfg_table_model_step_ = config->get_float(CFG_PREFIX"table_model_step"); 00124 cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX"horizontal_viewing_angle")); 00125 cfg_vertical_va_ = deg2rad(config->get_float(CFG_PREFIX"vertical_viewing_angle")); 00126 cfg_cluster_tolerance_ = config->get_float(CFG_PREFIX"cluster_tolerance"); 00127 cfg_cluster_min_size_ = config->get_uint(CFG_PREFIX"cluster_min_size"); 00128 cfg_cluster_max_size_ = config->get_uint(CFG_PREFIX"cluster_max_size"); 00129 cfg_result_frame_ = config->get_string(CFG_PREFIX"result_frame"); 00130 00131 finput_ = pcl_manager->get_pointcloud<PointType>("openni-pointcloud-xyz"); 00132 input_ = pcl_utils::cloudptr_from_refptr(finput_); 00133 00134 try { 00135 double rotation[4] = {0., 0., 0., 1.}; 00136 table_pos_if_ = NULL; 00137 table_pos_if_ = blackboard->open_for_writing<Position3DInterface>("Tabletop"); 00138 table_pos_if_->set_rotation(rotation); 00139 table_pos_if_->write(); 00140 00141 switch_if_ = NULL; 00142 switch_if_ = blackboard->open_for_writing<SwitchInterface>("tabletop-objects"); 00143 switch_if_->set_enabled(true); 00144 switch_if_->write(); 00145 00146 pos_ifs_.clear(); 00147 pos_ifs_.resize(MAX_CENTROIDS, NULL); 00148 for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) { 00149 char *tmp; 00150 if (asprintf(&tmp, "Tabletop Object %u", i + 1) != -1) { 00151 // Copy to get memory freed on exception 00152 std::string id = tmp; 00153 free(tmp); 00154 Position3DInterface *iface = 00155 blackboard->open_for_writing<Position3DInterface>(id.c_str()); 00156 pos_ifs_[i] = iface; 00157 iface->set_rotation(rotation); 00158 iface->write(); 00159 } 00160 } 00161 } catch (Exception &e) { 00162 blackboard->close(table_pos_if_); 00163 blackboard->close(switch_if_); 00164 for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) { 00165 if (pos_ifs_[i]) { 00166 blackboard->close(pos_ifs_[i]); 00167 } 00168 } 00169 pos_ifs_.clear(); 00170 throw; 00171 } 00172 00173 fclusters_ = new pcl::PointCloud<ColorPointType>(); 00174 fclusters_->header.frame_id = finput_->header.frame_id; 00175 fclusters_->is_dense = false; 00176 pcl_manager->add_pointcloud<ColorPointType>("tabletop-object-clusters", fclusters_); 00177 clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_); 00178 00179 ftable_model_ = new Cloud(); 00180 table_model_ = pcl_utils::cloudptr_from_refptr(ftable_model_); 00181 table_model_->header.frame_id = finput_->header.frame_id; 00182 pcl_manager->add_pointcloud("tabletop-table-model", ftable_model_); 00183 pcl_utils::set_time(ftable_model_, fawkes::Time(clock)); 00184 00185 fsimplified_polygon_ = new Cloud(); 00186 simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_); 00187 simplified_polygon_->header.frame_id = finput_->header.frame_id; 00188 pcl_manager->add_pointcloud("tabletop-simplified-polygon", fsimplified_polygon_); 00189 pcl_utils::set_time(fsimplified_polygon_, fawkes::Time(clock)); 00190 00191 grid_.setFilterFieldName("x"); 00192 grid_.setFilterLimits(cfg_depth_filter_min_x_, cfg_depth_filter_max_x_); 00193 grid_.setLeafSize(cfg_voxel_leaf_size_, cfg_voxel_leaf_size_, cfg_voxel_leaf_size_); 00194 00195 seg_.setOptimizeCoefficients(true); 00196 seg_.setModelType(pcl::SACMODEL_PLANE); 00197 seg_.setMethodType(pcl::SAC_RANSAC); 00198 seg_.setMaxIterations(cfg_segm_max_iterations_); 00199 seg_.setDistanceThreshold(cfg_segm_distance_threshold_); 00200 00201 loop_count_ = 0; 00202 00203 #ifdef USE_TIMETRACKER 00204 tt_ = new TimeTracker(); 00205 tt_loopcount_ = 0; 00206 ttc_full_loop_ = tt_->add_class("Full Loop"); 00207 ttc_msgproc_ = tt_->add_class("Message Processing"); 00208 ttc_voxelize_ = tt_->add_class("Downsampling"); 00209 ttc_plane_ = tt_->add_class("Plane Segmentation"); 00210 ttc_extract_plane_ = tt_->add_class("Plane Extraction"); 00211 ttc_plane_downsampling_ = tt_->add_class("Plane Downsampling"); 00212 ttc_cluster_plane_ = tt_->add_class("Plane Clustering"); 00213 ttc_convex_hull_ = tt_->add_class("Convex Hull"); 00214 ttc_simplify_polygon_ = tt_->add_class("Polygon Simplification"); 00215 ttc_find_edge_ = tt_->add_class("Polygon Edge"); 00216 ttc_transform_ = tt_->add_class("Transform"); 00217 ttc_transform_model_ = tt_->add_class("Model Transformation"); 00218 ttc_extract_non_plane_ = tt_->add_class("Non-Plane Extraction"); 00219 ttc_polygon_filter_ = tt_->add_class("Polygon Filter"); 00220 ttc_table_to_output_ = tt_->add_class("Table to Output"); 00221 ttc_cluster_objects_ = tt_->add_class("Object Clustering"); 00222 ttc_visualization_ = tt_->add_class("Visualization"); 00223 #endif 00224 } 00225 00226 00227 void 00228 TabletopObjectsThread::finalize() 00229 { 00230 input_.reset(); 00231 clusters_.reset(); 00232 simplified_polygon_.reset(); 00233 00234 pcl_manager->remove_pointcloud("tabletop-object-clusters"); 00235 pcl_manager->remove_pointcloud("tabletop-table-model"); 00236 pcl_manager->remove_pointcloud("tabletop-simplified-polygon"); 00237 00238 blackboard->close(table_pos_if_); 00239 blackboard->close(switch_if_); 00240 for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) { 00241 blackboard->close(pos_ifs_[i]); 00242 } 00243 pos_ifs_.clear(); 00244 00245 finput_.reset(); 00246 fclusters_.reset(); 00247 ftable_model_.reset(); 00248 fsimplified_polygon_.reset(); 00249 } 00250 00251 template <typename PointType> 00252 inline bool 00253 comparePoints2D(const PointType &p1, const PointType &p2) 00254 { 00255 double angle1 = atan2(p1.y, p1.x) + M_PI; 00256 double angle2 = atan2(p2.y, p2.x) + M_PI; 00257 return (angle1 > angle2); 00258 } 00259 00260 00261 // Criteria for *not* choosing a segment: 00262 // 1. the existing current best is clearly closer in base-relative X direction 00263 // 2. the existing current best is longer 00264 bool 00265 TabletopObjectsThread::is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, 00266 PointType &br_p1p, PointType &br_p2p) 00267 { 00268 // current best base-relative points 00269 Eigen::Vector3f cb_br_p1(cb_br_p1p.x, cb_br_p1p.y, cb_br_p1p.z); 00270 Eigen::Vector3f cb_br_p2(cb_br_p2p.x, cb_br_p2p.y, cb_br_p2p.z); 00271 Eigen::Vector3f cb_br_p1_p2_center = (cb_br_p1 + cb_br_p2) * 0.5; 00272 00273 Eigen::Vector3f br_p1(br_p1p.x, br_p1p.y, br_p1p.z); 00274 Eigen::Vector3f br_p2(br_p2p.x, br_p2p.y, br_p2p.z); 00275 Eigen::Vector3f br_p1_p2_center = (br_p2 + br_p1) * 0.5; 00276 00277 double dist_x = (cb_br_p1_p2_center[0] - br_p1_p2_center[0]); 00278 00279 // Criteria for *not* choosing a segment: 00280 // 1. the existing current best is clearly closer in base-relative X direction 00281 // 2. the existing current best is longer 00282 if ( (dist_x < -0.25) || 00283 ((abs(dist_x) <= 0.25) && ((br_p2 - br_p1).norm() < (cb_br_p2 - cb_br_p1).norm())) ) 00284 return false; 00285 else 00286 return true; 00287 } 00288 00289 00290 void 00291 TabletopObjectsThread::loop() 00292 { 00293 TIMETRACK_START(ttc_full_loop_); 00294 00295 ++loop_count_; 00296 00297 TIMETRACK_START(ttc_msgproc_); 00298 00299 while (! switch_if_->msgq_empty()) { 00300 if (SwitchInterface::EnableSwitchMessage *msg = 00301 switch_if_->msgq_first_safe(msg)) 00302 { 00303 switch_if_->set_enabled(true); 00304 switch_if_->write(); 00305 } else if (SwitchInterface::DisableSwitchMessage *msg = 00306 switch_if_->msgq_first_safe(msg)) 00307 { 00308 switch_if_->set_enabled(false); 00309 switch_if_->write(); 00310 } 00311 00312 switch_if_->msgq_pop(); 00313 } 00314 00315 if (! switch_if_->is_enabled()) { 00316 TimeWait::wait(250000); 00317 return; 00318 } 00319 00320 TIMETRACK_INTER(ttc_msgproc_, ttc_voxelize_) 00321 00322 CloudPtr temp_cloud(new Cloud); 00323 CloudPtr temp_cloud2(new Cloud); 00324 pcl::ExtractIndices<PointType> extract_; 00325 CloudPtr cloud_hull_; 00326 CloudPtr model_cloud_hull_; 00327 CloudPtr cloud_proj_; 00328 CloudPtr cloud_filt_; 00329 CloudPtr cloud_above_; 00330 CloudPtr cloud_objs_; 00331 pcl::search::KdTree<PointType> kdtree_; 00332 00333 grid_.setInputCloud(input_); 00334 grid_.filter(*temp_cloud); 00335 00336 if (temp_cloud->points.size() <= 10) { 00337 // this can happen if run at startup. Since tabletop threads runs continuous 00338 // and not synchronized with main loop, but point cloud acquisition thread is 00339 // synchronized, we might start before any data has been read 00340 //logger->log_warn(name(), "Empty voxelized point cloud, omitting loop"); 00341 TimeWait::wait(50000); 00342 return; 00343 } 00344 00345 TIMETRACK_INTER(ttc_voxelize_, ttc_plane_) 00346 00347 pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients()); 00348 pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 00349 Eigen::Vector4f table_centroid, baserel_table_centroid(0,0,0,0); 00350 00351 // This will search for the first plane which: 00352 // 1. has a considerable amount of points (>= some percentage of input points) 00353 // 2. is parallel to the floor (transformed normal angle to Z axis in specified epsilon) 00354 // 3. is on a typical table height (at a specified height range in robot frame) 00355 // Planes found along the way not satisfying any of the criteria are removed, 00356 // the first plane either satisfying all criteria, or violating the first 00357 // one end the loop 00358 bool happy_with_plane = false; 00359 while (! happy_with_plane) { 00360 happy_with_plane = true; 00361 00362 if (temp_cloud->points.size() <= 10) { 00363 logger->log_warn(name(), "[L %u] no more points for plane detection, skipping loop", loop_count_); 00364 set_position(table_pos_if_, false); 00365 TIMETRACK_ABORT(ttc_plane_); 00366 TIMETRACK_ABORT(ttc_full_loop_); 00367 TimeWait::wait(50000); 00368 return; 00369 } 00370 00371 seg_.setInputCloud(temp_cloud); 00372 seg_.segment(*inliers, *coeff); 00373 00374 // 1. check for a minimum number of expected inliers 00375 if ((double)inliers->indices.size() < (cfg_segm_inlier_quota_ * (double)temp_cloud->points.size())) { 00376 logger->log_warn(name(), "[L %u] no table in scene, skipping loop (%zu inliers, required %f, voxelized size %zu)", 00377 loop_count_, inliers->indices.size(), 00378 (cfg_segm_inlier_quota_ * temp_cloud->points.size()), temp_cloud->points.size()); 00379 set_position(table_pos_if_, false); 00380 TIMETRACK_ABORT(ttc_plane_); 00381 TIMETRACK_ABORT(ttc_full_loop_); 00382 TimeWait::wait(50000); 00383 return; 00384 } 00385 00386 // 2. Check angle between normal vector and Z axis of the 00387 // base_link robot frame since tables are usually parallel to the ground... 00388 try { 00389 tf::Stamped<tf::Vector3> 00390 table_normal(tf::Vector3(coeff->values[0], coeff->values[1], coeff->values[2]), 00391 fawkes::Time(0,0), input_->header.frame_id); 00392 00393 tf::Stamped<tf::Vector3> baserel_normal; 00394 tf_listener->transform_vector("/base_link", table_normal, baserel_normal); 00395 tf::Vector3 z_axis(0, 0, copysign(1.0, baserel_normal.z())); 00396 00397 if (fabs(z_axis.angle(baserel_normal)) > cfg_max_z_angle_deviation_ ) { 00398 happy_with_plane = false; 00399 logger->log_warn(name(), "[L %u] table normal (%f,%f,%f) Z angle deviation |%f| > %f, excluding", 00400 loop_count_, baserel_normal.x(), baserel_normal.y(), baserel_normal.z(), 00401 z_axis.angle(baserel_normal), cfg_max_z_angle_deviation_); 00402 } 00403 } catch (tf::TransformException &e) { 00404 //logger->log_warn(name(), "Transforming normal failed, exception follows"); 00405 //logger->log_warn(name(), e); 00406 } 00407 00408 if (happy_with_plane) { 00409 // ok so far 00410 00411 // 3. Calculate table centroid, then transform it to the base_link system 00412 // to make a table height sanity check, they tend to be at a specific height... 00413 try { 00414 pcl::compute3DCentroid(*temp_cloud, *inliers, table_centroid); 00415 tf::Stamped<tf::Point> 00416 centroid(tf::Point(table_centroid[0], table_centroid[1], table_centroid[2]), 00417 fawkes::Time(0, 0), input_->header.frame_id); 00418 tf::Stamped<tf::Point> baserel_centroid; 00419 tf_listener->transform_point("/base_link", centroid, baserel_centroid); 00420 baserel_table_centroid[0] = baserel_centroid.x(); 00421 baserel_table_centroid[1] = baserel_centroid.y(); 00422 baserel_table_centroid[2] = baserel_centroid.z(); 00423 00424 if ((baserel_centroid.z() < cfg_table_min_height_) || 00425 (baserel_centroid.z() > cfg_table_max_height_)) 00426 { 00427 happy_with_plane = false; 00428 logger->log_warn(name(), "[L %u] table height %f not in range [%f, %f]", 00429 loop_count_, baserel_centroid.z(), 00430 cfg_table_min_height_, cfg_table_max_height_); 00431 } 00432 } catch (tf::TransformException &e) { 00433 //logger->log_warn(name(), "Transforming centroid failed, exception follows"); 00434 //logger->log_warn(name(), e); 00435 } 00436 } 00437 00438 00439 if (! happy_with_plane) { 00440 // throw away 00441 Cloud extracted; 00442 extract_.setNegative(true); 00443 extract_.setInputCloud(temp_cloud); 00444 extract_.setIndices(inliers); 00445 extract_.filter(extracted); 00446 *temp_cloud = extracted; 00447 } 00448 } 00449 00450 // If we got here we found the table 00451 // Do NOT set it here, we will still try to determine the rotation as well 00452 // set_position(table_pos_if_, true, table_centroid); 00453 00454 TIMETRACK_INTER(ttc_plane_, ttc_extract_plane_) 00455 00456 extract_.setNegative(false); 00457 extract_.setInputCloud(temp_cloud); 00458 extract_.setIndices(inliers); 00459 extract_.filter(*temp_cloud2); 00460 00461 00462 // Project the model inliers 00463 pcl::ProjectInliers<PointType> proj; 00464 proj.setModelType(pcl::SACMODEL_PLANE); 00465 proj.setInputCloud(temp_cloud2); 00466 proj.setModelCoefficients(coeff); 00467 cloud_proj_.reset(new Cloud()); 00468 proj.filter(*cloud_proj_); 00469 00470 TIMETRACK_INTER(ttc_extract_plane_, ttc_plane_downsampling_); 00471 00472 // *** 00473 // In the following cluster the projected table plane. This is done to get 00474 // the largest continuous part of the plane to remove outliers, for instance 00475 // if the intersection of the plane with a wall or object is taken into the 00476 // table points. 00477 // To achieve this cluster, if an acceptable cluster was found, extract this 00478 // cluster as the new table points. Otherwise continue with the existing 00479 // point cloud. 00480 00481 // further downsample table 00482 CloudPtr cloud_table_voxelized(new Cloud()); 00483 pcl::VoxelGrid<PointType> table_grid; 00484 table_grid.setLeafSize(0.04, 0.04, 0.04); 00485 table_grid.setInputCloud(cloud_proj_); 00486 table_grid.filter(*cloud_table_voxelized); 00487 00488 TIMETRACK_INTER(ttc_plane_downsampling_, ttc_cluster_plane_); 00489 00490 // Creating the KdTree object for the search method of the extraction 00491 pcl::search::KdTree<PointType>::Ptr 00492 kdtree_table(new pcl::search::KdTree<PointType>()); 00493 kdtree_table->setInputCloud(cloud_table_voxelized); 00494 00495 std::vector<pcl::PointIndices> table_cluster_indices; 00496 pcl::EuclideanClusterExtraction<PointType> table_ec; 00497 table_ec.setClusterTolerance(0.044); 00498 table_ec.setMinClusterSize(0.8 * cloud_table_voxelized->points.size()); 00499 table_ec.setMaxClusterSize(cloud_table_voxelized->points.size()); 00500 table_ec.setSearchMethod(kdtree_table); 00501 table_ec.setInputCloud(cloud_table_voxelized); 00502 table_ec.extract(table_cluster_indices); 00503 00504 if (! table_cluster_indices.empty()) { 00505 // take the first, i.e. the largest cluster 00506 CloudPtr cloud_table_extracted(new Cloud()); 00507 pcl::PointIndices::ConstPtr table_cluster_indices_ptr(new pcl::PointIndices(table_cluster_indices[0])); 00508 pcl::ExtractIndices<PointType> table_cluster_extract; 00509 table_cluster_extract.setNegative(false); 00510 table_cluster_extract.setInputCloud(cloud_table_voxelized); 00511 table_cluster_extract.setIndices(table_cluster_indices_ptr); 00512 table_cluster_extract.filter(*cloud_table_extracted); 00513 *cloud_proj_ = *cloud_table_extracted; 00514 } else { 00515 // Don't mess with the table, clustering didn't help to make it any better 00516 logger->log_info(name(), "[L %u] table plane clustering did not generate any clusters", loop_count_); 00517 } 00518 00519 TIMETRACK_INTER(ttc_cluster_plane_, ttc_convex_hull_) 00520 00521 00522 // Estimate 3D convex hull -> TABLE BOUNDARIES 00523 pcl::ConvexHull<PointType> hr; 00524 //hr.setAlpha(0.1); // only for ConcaveHull 00525 hr.setInputCloud(cloud_proj_); 00526 cloud_hull_.reset(new Cloud()); 00527 hr.reconstruct(*cloud_hull_); 00528 00529 00530 if (cloud_hull_->points.empty()) { 00531 logger->log_warn(name(), "[L %u] convex hull of table empty, skipping loop", loop_count_); 00532 TIMETRACK_ABORT(ttc_convex_hull_); 00533 TIMETRACK_ABORT(ttc_full_loop_); 00534 set_position(table_pos_if_, false); 00535 return; 00536 } 00537 00538 TIMETRACK_INTER(ttc_convex_hull_, ttc_simplify_polygon_) 00539 00540 CloudPtr simplified_polygon = simplify_polygon(cloud_hull_, 0.02); 00541 *simplified_polygon_ = *simplified_polygon; 00542 //logger->log_debug(name(), "Original polygon: %zu simplified: %zu", cloud_hull_->points.size(), 00543 // simplified_polygon->points.size()); 00544 *cloud_hull_ = *simplified_polygon; 00545 00546 TIMETRACK_INTER(ttc_simplify_polygon_, ttc_find_edge_) 00547 00548 #ifdef HAVE_VISUAL_DEBUGGING 00549 TabletopVisualizationThreadBase::V_Vector4f good_hull_edges; 00550 good_hull_edges.resize(cloud_hull_->points.size() * 2); 00551 #endif 00552 00553 try { 00554 00555 // Get transform Input camera -> base_link 00556 tf::StampedTransform t; 00557 fawkes::Time input_time(0,0); 00558 //pcl_utils::get_time(finput_, input_time); 00559 tf_listener->lookup_transform("/base_link", finput_->header.frame_id, 00560 input_time, t); 00561 00562 tf::Quaternion q = t.getRotation(); 00563 Eigen::Affine3f affine_cloud = 00564 Eigen::Translation3f(t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z()) 00565 * Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z()); 00566 00567 // Transform polygon cloud into base_link frame 00568 CloudPtr baserel_polygon_cloud(new Cloud()); 00569 pcl::transformPointCloud(*cloud_hull_, *baserel_polygon_cloud, affine_cloud); 00570 00571 // Setup plane normals for left, right, and lower frustrum 00572 // planes for line segment verification 00573 Eigen::Vector3f left_frustrum_normal = 00574 Eigen::AngleAxisf( cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ()) 00575 * (-1. * Eigen::Vector3f::UnitY()); 00576 00577 Eigen::Vector3f right_frustrum_normal = 00578 Eigen::AngleAxisf(- cfg_horizontal_va_ * 0.5, Eigen::Vector3f::UnitZ()) 00579 * Eigen::Vector3f::UnitY(); 00580 00581 Eigen::Vector3f lower_frustrum_normal = 00582 Eigen::AngleAxisf(cfg_vertical_va_ * 0.5, Eigen::Vector3f::UnitY()) 00583 * Eigen::Vector3f::UnitZ(); 00584 00585 // point and good edge indexes of chosen candidate 00586 size_t pidx1, pidx2; 00587 #ifdef HAVE_VISUAL_DEBUGGING 00588 size_t geidx1 = std::numeric_limits<size_t>::max(); 00589 size_t geidx2 = std::numeric_limits<size_t>::max(); 00590 #endif 00591 // lower frustrum potential candidate 00592 size_t lf_pidx1, lf_pidx2; 00593 pidx1 = pidx2 = lf_pidx1 = lf_pidx2 = std::numeric_limits<size_t>::max(); 00594 00595 // Search for good edges and backup edge candidates 00596 // Good edges are the ones with either points close to 00597 // two separate frustrum planes, and hence the actual line is 00598 // well inside the frustrum, or with points inside the frustrum. 00599 // Possible backup candidates are lines with both points 00600 // close to the lower frustrum plane, i.e. lines cutoff by the 00601 // vertical viewing angle. If we cannot determine a suitable edge 00602 // otherwise we fallback to this line as it is a good rough guess 00603 // to prevent at least worst things during manipulation 00604 const size_t psize = cloud_hull_->points.size(); 00605 #ifdef HAVE_VISUAL_DEBUGGING 00606 size_t good_edge_points = 0; 00607 #endif 00608 for (size_t i = 0; i < psize; ++i) { 00609 //logger->log_debug(name(), "Checking %zu and %zu of %zu", i, (i+1) % psize, psize); 00610 PointType &p1p = cloud_hull_->points[i ]; 00611 PointType &p2p = cloud_hull_->points[(i+1) % psize]; 00612 00613 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z); 00614 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z); 00615 00616 PointType &br_p1p = baserel_polygon_cloud->points[i ]; 00617 PointType &br_p2p = baserel_polygon_cloud->points[(i + 1) % psize]; 00618 00619 // check if both end points are close to left or right frustrum plane 00620 if ( ! (((left_frustrum_normal.dot(p1) < 0.03) && 00621 (left_frustrum_normal.dot(p2) < 0.03)) || 00622 ((right_frustrum_normal.dot(p1) < 0.03) && 00623 (right_frustrum_normal.dot(p2) < 0.03)) ) ) 00624 { 00625 // candidate edge, i.e. it's not too close to left or right frustrum planes 00626 00627 // check if both end points close to lower frustrum plane 00628 if ((lower_frustrum_normal.dot(p1) < 0.01) && 00629 (lower_frustrum_normal.dot(p2) < 0.01)) { 00630 // it's a lower frustrum line, keep just in case we do not 00631 // find a better one 00632 if ( (lf_pidx1 == std::numeric_limits<size_t>::max()) || 00633 is_polygon_edge_better(br_p1p, br_p2p, 00634 baserel_polygon_cloud->points[lf_pidx1], baserel_polygon_cloud->points[lf_pidx2])) 00635 { 00636 // there was no backup candidate, yet, or this one is closer 00637 // to the robot, take it. 00638 lf_pidx1 = i; 00639 lf_pidx2 = (i + 1) % psize; 00640 } 00641 00642 continue; 00643 } 00644 00645 #ifdef HAVE_VISUAL_DEBUGGING 00646 // Remember as good edge for visualization 00647 for (unsigned int j = 0; j < 3; ++j) 00648 good_hull_edges[good_edge_points][j] = p1[j]; 00649 good_hull_edges[good_edge_points][3] = 0.; 00650 ++good_edge_points; 00651 for (unsigned int j = 0; j < 3; ++j) 00652 good_hull_edges[good_edge_points][j] = p2[j]; 00653 good_hull_edges[good_edge_points][3] = 0.; 00654 ++good_edge_points; 00655 #endif 00656 00657 if (pidx1 != std::numeric_limits<size_t>::max()) { 00658 // current best base-relative points 00659 PointType &cb_br_p1p = baserel_polygon_cloud->points[pidx1]; 00660 PointType &cb_br_p2p = baserel_polygon_cloud->points[pidx2]; 00661 00662 if (! is_polygon_edge_better(cb_br_p1p, cb_br_p2p, br_p1p, br_p2p)) 00663 { 00664 //logger->log_info(name(), "Skipping: cb(%f,%f)->(%f,%f) c(%f,%f)->(%f,%f)", 00665 // cb_br_p1p.x, cb_br_p1p.y, cb_br_p2p.x, cb_br_p2p.y, 00666 // br_p1p.x, br_p1p.y, br_p2p.x, br_p2p.y); 00667 continue; 00668 } else { 00669 //logger->log_info(name(), "Taking: cb(%f,%f)->(%f,%f) c(%f,%f)->(%f,%f)", 00670 // cb_br_p1p.x, cb_br_p1p.y, cb_br_p2p.x, cb_br_p2p.y, 00671 // br_p1p.x, br_p1p.y, br_p2p.x, br_p2p.y); 00672 } 00673 //} else { 00674 //logger->log_info(name(), "Taking because we had none"); 00675 } 00676 00677 // Was not sorted out, therefore promote candidate to current best 00678 pidx1 = i; 00679 pidx2 = (i + 1) % psize; 00680 #ifdef HAVE_VISUAL_DEBUGGING 00681 geidx1 = good_edge_points - 2; 00682 geidx2 = good_edge_points - 1; 00683 #endif 00684 } 00685 } 00686 00687 //logger->log_debug(name(), "Current best is %zu -> %zu", pidx1, pidx2); 00688 00689 // in the case we have a backup lower frustrum edge check if we should use it 00690 // Criteria: 00691 // 0. we have a backup point 00692 // 1. no other suitable edge was chosen at all 00693 // 2. angle(Y_axis, chosen_edge) > threshold 00694 // 3.. p1.x or p2.x > centroid.x 00695 if (lf_pidx1 != std::numeric_limits<size_t>::max()) { 00696 PointType &lp1p = baserel_polygon_cloud->points[lf_pidx1]; 00697 PointType &lp2p = baserel_polygon_cloud->points[lf_pidx2]; 00698 00699 Eigen::Vector4f lp1(lp1p.x, lp1p.y, lp1p.z, 0.); 00700 Eigen::Vector4f lp2(lp2p.x, lp2p.y, lp2p.z, 0.); 00701 00702 // None found at all 00703 if (pidx1 == std::numeric_limits<size_t>::max()) { 00704 pidx1 = lf_pidx1; 00705 pidx2 = lf_pidx2; 00706 00707 #ifdef HAVE_VISUAL_DEBUGGING 00708 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x; 00709 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y; 00710 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z; 00711 geidx1 = good_edge_points++; 00712 00713 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x; 00714 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y; 00715 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z; 00716 geidx2 = good_edge_points++; 00717 #endif 00718 00719 } else { 00720 00721 PointType &p1p = baserel_polygon_cloud->points[pidx1]; 00722 PointType &p2p = baserel_polygon_cloud->points[pidx2]; 00723 00724 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0.); 00725 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0.); 00726 00727 // Unsuitable "good" line until now? 00728 if (//(pcl::getAngle3D(p2 - p1, Eigen::Vector4f::UnitZ()) > M_PI * 0.5) || 00729 (p1[0] > baserel_table_centroid[0]) || (p2[0] > baserel_table_centroid[0])) 00730 { 00731 //logger->log_warn(name(), "Choosing backup candidate!"); 00732 00733 pidx1 = lf_pidx1; 00734 pidx2 = lf_pidx2; 00735 00736 #ifdef HAVE_VISUAL_DEBUGGING 00737 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx1].x; 00738 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx1].y; 00739 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx1].z; 00740 geidx1 = good_edge_points++; 00741 00742 good_hull_edges[good_edge_points][0] = cloud_hull_->points[lf_pidx2].x; 00743 good_hull_edges[good_edge_points][1] = cloud_hull_->points[lf_pidx2].y; 00744 good_hull_edges[good_edge_points][2] = cloud_hull_->points[lf_pidx2].z; 00745 geidx2 = good_edge_points++; 00746 #endif 00747 } 00748 } 00749 } 00750 00751 //logger->log_info(name(), "Chose %zu -> %zu", pidx1, pidx2); 00752 00753 #ifdef HAVE_VISUAL_DEBUGGING 00754 if (good_edge_points > 0) { 00755 good_hull_edges[geidx1][3] = 1.0; 00756 good_hull_edges[geidx2][3] = 1.0; 00757 } 00758 good_hull_edges.resize(good_edge_points); 00759 #endif 00760 00761 TIMETRACK_INTER(ttc_find_edge_, ttc_transform_) 00762 00763 // Calculate transformation parameters based on determined 00764 // convex hull polygon segment we decided on as "the table edge" 00765 PointType &p1p = cloud_hull_->points[pidx1]; 00766 PointType &p2p = cloud_hull_->points[pidx2]; 00767 00768 Eigen::Vector3f p1(p1p.x, p1p.y, p1p.z); 00769 Eigen::Vector3f p2(p2p.x, p2p.y, p2p.z); 00770 00771 // Normal vectors for table model and plane 00772 Eigen::Vector3f model_normal = Eigen::Vector3f::UnitZ(); 00773 Eigen::Vector3f normal(coeff->values[0], coeff->values[1], coeff->values[2]); 00774 normal.normalize(); // just in case 00775 00776 Eigen::Vector3f table_centroid_3f = 00777 Eigen::Vector3f(table_centroid[0], table_centroid[1], table_centroid[2]); 00778 00779 // Rotational parameters to align table to polygon segment 00780 Eigen::Vector3f p1_p2 = p2 - p1; 00781 Eigen::Vector3f p1_p2_center = (p2 + p1) * 0.5; 00782 p1_p2.normalize(); 00783 Eigen::Vector3f p1_p2_normal_cross = p1_p2.cross(normal); 00784 p1_p2_normal_cross.normalize(); 00785 00786 // For N=(A,B,C), and hessian Ax+By+Cz+D=0 and N dot X=(Ax+By+Cz) 00787 // we get N dot X + D = 0 -> -D = N dot X 00788 double nD = - p1_p2_normal_cross.dot(p1_p2_center); 00789 double p1_p2_centroid_dist = p1_p2_normal_cross.dot(table_centroid_3f) + nD; 00790 if (p1_p2_centroid_dist < 0) { 00791 // normal points to the "wrong" side fo our purpose 00792 p1_p2_normal_cross *= -1; 00793 } 00794 00795 Eigen::Vector3f table_center = 00796 p1_p2_center + p1_p2_normal_cross * (cfg_table_model_width_ * 0.5); 00797 00798 for (unsigned int i = 0; i < 3; ++i) table_centroid[i] = table_center[i]; 00799 table_centroid[3] = 0.; 00800 00801 // calculate table corner points 00802 std::vector<Eigen::Vector3f> tpoints(4); 00803 tpoints[0] = p1_p2_center + p1_p2 * (cfg_table_model_length_ * 0.5); 00804 tpoints[1] = tpoints[0] + p1_p2_normal_cross * cfg_table_model_width_; 00805 tpoints[3] = p1_p2_center - p1_p2 * (cfg_table_model_length_ * 0.5); 00806 tpoints[2] = tpoints[3] + p1_p2_normal_cross * cfg_table_model_width_; 00807 00808 model_cloud_hull_.reset(new Cloud()); 00809 model_cloud_hull_->points.resize(4); 00810 model_cloud_hull_->height = 1; 00811 model_cloud_hull_->width = 4; 00812 model_cloud_hull_->is_dense = true; 00813 for (int i = 0; i < 4; ++i) { 00814 model_cloud_hull_->points[i].x = tpoints[i][0]; 00815 model_cloud_hull_->points[i].y = tpoints[i][1]; 00816 model_cloud_hull_->points[i].z = tpoints[i][2]; 00817 } 00818 //std::sort(model_cloud_hull_->points.begin(), 00819 // model_cloud_hull_->points.end(), comparePoints2D<PointType>); 00820 00821 // Rotational parameters to rotate table model from camera to 00822 // determined table position in 3D space 00823 Eigen::Vector3f rotaxis = model_normal.cross(normal); 00824 rotaxis.normalize(); 00825 double angle = acos(normal.dot(model_normal)); 00826 00827 // Transformation to translate model from camera center into actual pose 00828 Eigen::Affine3f affine = 00829 Eigen::Translation3f(table_centroid.x(), table_centroid.y(), 00830 table_centroid.z()) 00831 * Eigen::AngleAxisf(angle, rotaxis); 00832 00833 Eigen::Vector3f 00834 model_p1(-cfg_table_model_width_ * 0.5, cfg_table_model_length_ * 0.5, 0.), 00835 model_p2(-cfg_table_model_width_ * 0.5, -cfg_table_model_length_ * 0.5, 0.); 00836 model_p1 = affine * model_p1; 00837 model_p2 = affine * model_p2; 00838 00839 // Calculate the vector between model_p1 and model_p2 00840 Eigen::Vector3f model_p1_p2 = model_p2 - model_p1; 00841 model_p1_p2.normalize(); 00842 // Calculate rotation axis between model_p1 and model_p2 00843 Eigen::Vector3f model_rotaxis = model_p1_p2.cross(p1_p2); 00844 model_rotaxis.normalize(); 00845 double angle_p1_p2 = acos(model_p1_p2.dot(p1_p2)); 00846 //logger->log_info(name(), "Angle: %f Poly (%f,%f,%f) -> (%f,%f,%f) model (%f,%f,%f) -> (%f,%f,%f)", 00847 // angle_p1_p2, p1.x(), p1.y(), p1.z(), p2.x(), p2.y(), p2.z(), 00848 // model_p1.x(), model_p1.y(), model_p1.z(), model_p2.x(), model_p2.y(), model_p2.z()); 00849 00850 // Final full transformation of the table within the camera coordinate frame 00851 affine = 00852 Eigen::Translation3f(table_centroid.x(), table_centroid.y(), 00853 table_centroid.z()) 00854 * Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) 00855 * Eigen::AngleAxisf(angle, rotaxis); 00856 00857 00858 // Just the rotational part 00859 Eigen::Quaternionf qt; 00860 qt = Eigen::AngleAxisf(angle_p1_p2, model_rotaxis) 00861 * Eigen::AngleAxisf(angle, rotaxis); 00862 00863 // Set position again, this time with the rotation 00864 set_position(table_pos_if_, true, table_centroid, qt); 00865 00866 TIMETRACK_INTER(ttc_transform_, ttc_transform_model_) 00867 00868 // to show fitted table model 00869 CloudPtr table_model = generate_table_model(cfg_table_model_length_, cfg_table_model_width_, cfg_table_model_step_); 00870 pcl::transformPointCloud(*table_model, *table_model_, affine.matrix()); 00871 //*table_model_ = *model_cloud_hull_; 00872 //*table_model_ = *table_model; 00873 table_model_->header.frame_id = finput_->header.frame_id; 00874 00875 TIMETRACK_END(ttc_transform_model_); 00876 00877 } catch (Exception &e) { 00878 set_position(table_pos_if_, false); 00879 logger->log_warn(name(), "Failed to transform convex hull cloud, exception follows"); 00880 logger->log_warn(name(), e); 00881 TIMETRACK_ABORT(ttc_find_edge_); 00882 } 00883 00884 TIMETRACK_START(ttc_extract_non_plane_); 00885 // Extract all non-plane points 00886 cloud_filt_.reset(new Cloud()); 00887 extract_.setNegative(true); 00888 extract_.filter(*cloud_filt_); 00889 00890 TIMETRACK_INTER(ttc_extract_non_plane_, ttc_polygon_filter_); 00891 00892 // Use only points above tables 00893 // Why coeff->values[3] > 0 ? ComparisonOps::GT : ComparisonOps::LT? 00894 // The model coefficients are in Hessian Normal Form, hence coeff[0..2] are 00895 // the normal vector. We need to distinguish the cases where the normal vector 00896 // points towards the origin (camera) or away from it. This can be checked 00897 // by calculating the distance towards the origin, which conveniently in 00898 // dist = N * x + p is just p which is coeff[3]. Therefore, if coeff[3] is 00899 // positive, the normal vector points towards the camera and we want all 00900 // points with positive distance from the table plane, otherwise it points 00901 // away from the origin and we want points with "negative distance". 00902 // We make use of the fact that we only have a boring RGB-D camera and 00903 // not an X-Ray... 00904 pcl::ComparisonOps::CompareOp op = 00905 coeff->values[3] > 0 ? pcl::ComparisonOps::GT : pcl::ComparisonOps::LT; 00906 pcl_utils::PlaneDistanceComparison<PointType>::ConstPtr 00907 above_comp(new pcl_utils::PlaneDistanceComparison<PointType>(coeff, op)); 00908 pcl::ConditionAnd<PointType>::Ptr 00909 above_cond(new pcl::ConditionAnd<PointType>()); 00910 above_cond->addComparison(above_comp); 00911 pcl::ConditionalRemoval<PointType> above_condrem(above_cond); 00912 above_condrem.setInputCloud(cloud_filt_); 00913 //above_condrem.setKeepOrganized(true); 00914 cloud_above_.reset(new Cloud()); 00915 above_condrem.filter(*cloud_above_); 00916 00917 //printf("Before: %zu After: %zu\n", cloud_filt_->points.size(), 00918 // cloud_above_->points.size()); 00919 if (cloud_filt_->points.size() < cfg_cluster_min_size_) { 00920 //logger->log_warn(name(), "Less points than cluster min size"); 00921 TIMETRACK_ABORT(ttc_polygon_filter_); 00922 TIMETRACK_ABORT(ttc_full_loop_); 00923 return; 00924 } 00925 00926 // Extract only points on the table plane 00927 pcl::PointIndices::Ptr polygon(new pcl::PointIndices()); 00928 00929 pcl::ConditionAnd<PointType>::Ptr 00930 polygon_cond(new pcl::ConditionAnd<PointType>()); 00931 00932 pcl_utils::PolygonComparison<PointType>::ConstPtr 00933 inpoly_comp(new pcl_utils::PolygonComparison<PointType>( 00934 (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) ? *model_cloud_hull_ : *cloud_hull_)); 00935 polygon_cond->addComparison(inpoly_comp); 00936 00937 // build the filter 00938 pcl::ConditionalRemoval<PointType> condrem(polygon_cond); 00939 condrem.setInputCloud(cloud_above_); 00940 //condrem.setKeepOrganized(true); 00941 cloud_objs_.reset(new Cloud()); 00942 condrem.filter(*cloud_objs_); 00943 00944 //CloudPtr table_points(new Cloud()); 00945 //condrem.setInputCloud(temp_cloud2); 00946 //condrem.filter(*table_points); 00947 00948 // CLUSTERS 00949 // extract clusters of OBJECTS 00950 00951 TIMETRACK_INTER(ttc_polygon_filter_, ttc_table_to_output_) 00952 00953 ColorCloudPtr tmp_clusters(new ColorCloud()); 00954 tmp_clusters->header.frame_id = clusters_->header.frame_id; 00955 std::vector<int> &indices = inliers->indices; 00956 tmp_clusters->height = 1; 00957 //const size_t tsize = table_points->points.size(); 00958 //tmp_clusters->width = tsize; 00959 //tmp_clusters->points.resize(tsize); 00960 tmp_clusters->width = indices.size(); 00961 tmp_clusters->points.resize(indices.size()); 00962 for (size_t i = 0; i < indices.size(); ++i) { 00963 PointType &p1 = temp_cloud2->points[i]; 00964 //PointType &p1 = table_points->points[i]; 00965 ColorPointType &p2 = tmp_clusters->points[i]; 00966 p2.x = p1.x; 00967 p2.y = p1.y; 00968 p2.z = p1.z; 00969 p2.r = table_color[0]; 00970 p2.g = table_color[1]; 00971 p2.b = table_color[2]; 00972 } 00973 00974 TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_) 00975 00976 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00977 centroids.resize(MAX_CENTROIDS); 00978 unsigned int centroid_i = 0; 00979 00980 if (cloud_objs_->points.size() > 0) { 00981 // Creating the KdTree object for the search method of the extraction 00982 pcl::search::KdTree<PointType>::Ptr 00983 kdtree_cl(new pcl::search::KdTree<PointType>()); 00984 kdtree_cl->setInputCloud(cloud_objs_); 00985 00986 std::vector<pcl::PointIndices> cluster_indices; 00987 pcl::EuclideanClusterExtraction<PointType> ec; 00988 ec.setClusterTolerance(cfg_cluster_tolerance_); 00989 ec.setMinClusterSize(cfg_cluster_min_size_); 00990 ec.setMaxClusterSize(cfg_cluster_max_size_); 00991 ec.setSearchMethod(kdtree_cl); 00992 ec.setInputCloud(cloud_objs_); 00993 ec.extract(cluster_indices); 00994 00995 //logger->log_debug(name(), "Found %zu clusters", cluster_indices.size()); 00996 00997 ColorCloudPtr colored_clusters(new ColorCloud()); 00998 colored_clusters->header.frame_id = clusters_->header.frame_id; 00999 std::vector<pcl::PointIndices>::const_iterator it; 01000 unsigned int cci = 0; 01001 //unsigned int i = 0; 01002 unsigned int num_points = 0; 01003 for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) 01004 num_points += it->indices.size(); 01005 01006 if (num_points > 0) { 01007 colored_clusters->points.resize(num_points); 01008 for (it = cluster_indices.begin(); 01009 it != cluster_indices.end() && centroid_i < MAX_CENTROIDS; 01010 ++it, ++centroid_i) 01011 { 01012 pcl::compute3DCentroid(*cloud_objs_, it->indices, centroids[centroid_i]); 01013 01014 std::vector<int>::const_iterator pit; 01015 for (pit = it->indices.begin(); pit != it->indices.end(); ++pit) { 01016 ColorPointType &p1 = colored_clusters->points[cci++]; 01017 PointType &p2 = cloud_objs_->points[*pit]; 01018 p1.x = p2.x; 01019 p1.y = p2.y; 01020 p1.z = p2.z; 01021 p1.r = cluster_colors[centroid_i][0]; 01022 p1.g = cluster_colors[centroid_i][1];; 01023 p1.b = cluster_colors[centroid_i][2];; 01024 } 01025 } 01026 01027 *tmp_clusters += *colored_clusters; 01028 } else { 01029 logger->log_info(name(), "No clustered points found"); 01030 } 01031 } else { 01032 logger->log_info(name(), "Filter left no points for clustering"); 01033 } 01034 01035 for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) { 01036 set_position(pos_ifs_[i], i < centroid_i, centroids[i]); 01037 } 01038 centroids.resize(centroid_i); 01039 01040 TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_) 01041 01042 *clusters_ = *tmp_clusters; 01043 pcl_utils::copy_time(finput_, fclusters_); 01044 pcl_utils::copy_time(finput_, ftable_model_); 01045 pcl_utils::copy_time(finput_, fsimplified_polygon_); 01046 01047 #ifdef HAVE_VISUAL_DEBUGGING 01048 if (visthread_) { 01049 Eigen::Vector4f normal; 01050 normal[0] = coeff->values[0]; 01051 normal[1] = coeff->values[1]; 01052 normal[2] = coeff->values[2]; 01053 normal[3] = 0.; 01054 01055 TabletopVisualizationThreadBase::V_Vector4f hull_vertices; 01056 hull_vertices.resize(cloud_hull_->points.size()); 01057 for (unsigned int i = 0; i < cloud_hull_->points.size(); ++i) { 01058 hull_vertices[i][0] = cloud_hull_->points[i].x; 01059 hull_vertices[i][1] = cloud_hull_->points[i].y; 01060 hull_vertices[i][2] = cloud_hull_->points[i].z; 01061 hull_vertices[i][3] = 0.; 01062 } 01063 01064 TabletopVisualizationThreadBase::V_Vector4f model_vertices; 01065 if (model_cloud_hull_ && ! model_cloud_hull_->points.empty()) { 01066 model_vertices.resize(model_cloud_hull_->points.size()); 01067 for (unsigned int i = 0; i < model_cloud_hull_->points.size(); ++i) { 01068 model_vertices[i][0] = model_cloud_hull_->points[i].x; 01069 model_vertices[i][1] = model_cloud_hull_->points[i].y; 01070 model_vertices[i][2] = model_cloud_hull_->points[i].z; 01071 model_vertices[i][3] = 0.; 01072 } 01073 } 01074 01075 visthread_->visualize(input_->header.frame_id, 01076 table_centroid, normal, hull_vertices, model_vertices, 01077 good_hull_edges, centroids); 01078 } 01079 #endif 01080 01081 TIMETRACK_END(ttc_visualization_); 01082 TIMETRACK_END(ttc_full_loop_); 01083 01084 #ifdef USE_TIMETRACKER 01085 if (++tt_loopcount_ >= 5) { 01086 tt_loopcount_ = 0; 01087 tt_->print_to_stdout(); 01088 } 01089 #endif 01090 } 01091 01092 01093 void 01094 TabletopObjectsThread::set_position(fawkes::Position3DInterface *iface, 01095 bool is_visible, const Eigen::Vector4f ¢roid, 01096 const Eigen::Quaternionf &attitude) 01097 { 01098 tf::Stamped<tf::Pose> baserel_pose; 01099 try{ 01100 tf::Stamped<tf::Pose> 01101 spose(tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()), 01102 tf::Vector3(centroid[0], centroid[1], centroid[2])), 01103 fawkes::Time(0, 0), input_->header.frame_id); 01104 tf_listener->transform_pose(cfg_result_frame_, spose, baserel_pose); 01105 iface->set_frame(cfg_result_frame_.c_str()); 01106 } catch (tf::TransformException &e) { 01107 is_visible = false; 01108 } 01109 01110 int visibility_history = iface->visibility_history(); 01111 if (is_visible) { 01112 if (visibility_history >= 0) { 01113 iface->set_visibility_history(visibility_history + 1); 01114 } else { 01115 iface->set_visibility_history(1); 01116 } 01117 tf::Vector3 &origin = baserel_pose.getOrigin(); 01118 tf::Quaternion quat = baserel_pose.getRotation(); 01119 double translation[3] = { origin.x(), origin.y(), origin.z() }; 01120 double rotation[4] = { quat.x(), quat.y(), quat.z(), quat.w() }; 01121 iface->set_translation(translation); 01122 iface->set_rotation(rotation); 01123 01124 } else { 01125 if (visibility_history <= 0) { 01126 iface->set_visibility_history(visibility_history - 1); 01127 } else { 01128 iface->set_visibility_history(-1); 01129 double translation[3] = { 0, 0, 0 }; 01130 double rotation[4] = { 0, 0, 0, 1 }; 01131 iface->set_translation(translation); 01132 iface->set_rotation(rotation); 01133 } 01134 } 01135 iface->write(); 01136 } 01137 01138 01139 TabletopObjectsThread::CloudPtr 01140 TabletopObjectsThread::generate_table_model(const float length, const float width, 01141 const float thickness, const float step, 01142 const float max_error) 01143 { 01144 CloudPtr c(new Cloud()); 01145 01146 const float length_2 = fabs(length) * 0.5; 01147 const float width_2 = fabs(width) * 0.5; 01148 const float thickness_2 = fabs(thickness) * 0.5; 01149 01150 // calculate table points 01151 const unsigned int l_base_num = std::max(2u, (unsigned int)floor(length / step)); 01152 const unsigned int num_w = l_base_num + 01153 ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1)); 01154 const unsigned int w_base_num = std::max(2u, (unsigned int)floor(width / step)); 01155 const unsigned int num_h = w_base_num + 01156 ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1)); 01157 const unsigned int t_base_num = std::max(2u, (unsigned int)floor(thickness / step)); 01158 const unsigned int num_t = t_base_num + 01159 ((thickness < t_base_num * step) ? 0 : ((thickness - t_base_num * step) > max_error ? 2 : 1)); 01160 01161 //logger->log_debug(name(), "Generating table model %fx%fx%f (%ux%ux%u=%u points)", 01162 // length, width, thickness, 01163 // num_w, num_h, num_t, num_t * num_w * num_h); 01164 01165 c->height = 1; 01166 c->width = num_t * num_w * num_h; 01167 c->is_dense = true; 01168 c->points.resize(num_t * num_w * num_h); 01169 01170 unsigned int idx = 0; 01171 for (unsigned int t = 0; t < num_t; ++t) { 01172 for (unsigned int w = 0; w < num_w; ++w) { 01173 for (unsigned int h = 0; h < num_h; ++h) { 01174 PointType &p = c->points[idx++]; 01175 01176 p.x = h * step - width_2; 01177 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2; 01178 01179 p.y = w * step - length_2; 01180 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2; 01181 01182 p.z = t * step - thickness_2; 01183 if ((t == num_t - 1) && fabs(p.z - thickness_2) > max_error) p.z = thickness_2; 01184 } 01185 } 01186 } 01187 01188 return c; 01189 } 01190 01191 TabletopObjectsThread::CloudPtr 01192 TabletopObjectsThread::generate_table_model(const float length, const float width, 01193 const float step, const float max_error) 01194 { 01195 CloudPtr c(new Cloud()); 01196 01197 const float length_2 = fabs(length) * 0.5; 01198 const float width_2 = fabs(width) * 0.5; 01199 01200 // calculate table points 01201 const unsigned int l_base_num = std::max(2u, (unsigned int)floor(length / step)); 01202 const unsigned int num_w = l_base_num + 01203 ((length < l_base_num * step) ? 0 : ((length - l_base_num * step) > max_error ? 2 : 1)); 01204 const unsigned int w_base_num = std::max(2u, (unsigned int)floor(width / step)); 01205 const unsigned int num_h = w_base_num + 01206 ((width < w_base_num * step) ? 0 : ((width - w_base_num * step) > max_error ? 2 : 1)); 01207 01208 //logger->log_debug(name(), "Generating table model %fx%f (%ux%u=%u points)", 01209 // length, width, num_w, num_h, num_w * num_h); 01210 01211 c->height = 1; 01212 c->width = num_w * num_h; 01213 c->is_dense = true; 01214 c->points.resize(num_w * num_h); 01215 01216 unsigned int idx = 0; 01217 for (unsigned int w = 0; w < num_w; ++w) { 01218 for (unsigned int h = 0; h < num_h; ++h) { 01219 PointType &p = c->points[idx++]; 01220 01221 p.x = h * step - width_2; 01222 if ((h == num_h - 1) && fabs(p.x - width_2) > max_error) p.x = width_2; 01223 01224 p.y = w * step - length_2; 01225 if ((w == num_w - 1) && fabs(p.y - length_2) > max_error) p.y = length_2; 01226 01227 p.z = 0.; 01228 } 01229 } 01230 01231 return c; 01232 } 01233 01234 01235 TabletopObjectsThread::CloudPtr 01236 TabletopObjectsThread::simplify_polygon(CloudPtr polygon, float dist_threshold) 01237 { 01238 const float sqr_dist_threshold = dist_threshold * dist_threshold; 01239 CloudPtr result(new Cloud()); 01240 const size_t psize = polygon->points.size(); 01241 result->points.resize(psize); 01242 size_t res_points = 0; 01243 size_t i_dist = 1; 01244 for (size_t i = 1; i <= psize; ++i) { 01245 PointType &p1p = polygon->points[i - i_dist ]; 01246 01247 if (i == psize) { 01248 if (result->points.empty()) { 01249 // Simplification failed, got something too "line-ish" 01250 return polygon; 01251 } 01252 } 01253 01254 PointType &p2p = polygon->points[i % psize]; 01255 PointType &p3p = (i == psize) ? result->points[0] : polygon->points[(i + 1) % psize]; 01256 01257 Eigen::Vector4f p1(p1p.x, p1p.y, p1p.z, 0); 01258 Eigen::Vector4f p2(p2p.x, p2p.y, p2p.z, 0); 01259 Eigen::Vector4f p3(p3p.x, p3p.y, p3p.z, 0); 01260 01261 Eigen::Vector4f line_dir = p3 - p1; 01262 01263 double sqr_dist = pcl::sqrPointToLineDistance(p2, p1, line_dir); 01264 if (sqr_dist < sqr_dist_threshold) { 01265 ++i_dist; 01266 } else { 01267 i_dist = 1; 01268 result->points[res_points++] = p2p; 01269 } 01270 } 01271 01272 result->header.frame_id = polygon->header.frame_id; 01273 result->header.stamp = polygon->header.stamp; 01274 result->width = res_points; 01275 result->height = 1; 01276 result->is_dense = false; 01277 result->points.resize(res_points); 01278 01279 return result; 01280 } 01281 01282 01283 #ifdef HAVE_VISUAL_DEBUGGING 01284 void 01285 TabletopObjectsThread::set_visualization_thread(TabletopVisualizationThreadBase *visthread) 01286 { 01287 visthread_ = visthread; 01288 } 01289 #endif