Fawkes API  Fawkes Development Version
tabletop_objects_thread.cpp
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 &centroid,
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