Changes have been pushed for the project "Fawkes Robotics Software Framework".
Gitweb: http://git.fawkesrobotics.org/fawkes.git Trac: http://trac.fawkesrobotics.org The branch, thofmann/tabletop-obj-tracking has been updated to 0980951a3f8a89cba3e20645ca4f68e9369632ed (commit) via 74914d8fc5915337062ff1c7a47db298aa5fd090 (commit) via fcbccefc43c8e186b7af1853484dc8c9ca2156f1 (commit) via 94b305cb99160a1c2f963c22164a7b3e7643290f (commit) via 09140314ec8bb26c6a990afd59eac839130c4858 (commit) via 29560ee187c65717d7f6acab103e757ccd4a3c4c (commit) via da5284e7057f3d2161fe26e2d4422beb809b649a (commit) via 7ca3edd831080304e5d81eb98cfa0000a20fd7e6 (commit) from 6073a647e9999b055673c7c34737269e99cb1da1 (commit) http://git.fawkesrobotics.org/fawkes.git/thofmann/tabletop-obj-tracking Those revisions listed above that are new to this repository have not appeared on any other notification email; so we list those revisions in full, below. - *Log* --------------------------------------------------------------- commit 7ca3edd831080304e5d81eb98cfa0000a20fd7e6 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Feb 26 13:52:00 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:35:22 2013 +0100 tabletop-objects: visualize tracking http://git.fawkesrobotics.org/fawkes.git/commit/7ca3edd http://trac.fawkesrobotics.org/changeset/7ca3edd - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit da5284e7057f3d2161fe26e2d4422beb809b649a Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Feb 26 14:36:49 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:35:47 2013 +0100 tabletop-objects: only use points above the table as input for tracking http://git.fawkesrobotics.org/fawkes.git/commit/da5284e http://trac.fawkesrobotics.org/changeset/da5284e - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 29560ee187c65717d7f6acab103e757ccd4a3c4c Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed Feb 27 15:06:52 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:36:09 2013 +0100 tabletop-objects: change tracking particle number (400->100) http://git.fawkesrobotics.org/fawkes.git/commit/29560ee http://trac.fawkesrobotics.org/changeset/29560ee - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 09140314ec8bb26c6a990afd59eac839130c4858 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed Feb 27 15:39:50 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:36:17 2013 +0100 tabletop-objects: only track if cloud_objs_ isn't empty If cloud_objs_ is empty (e.g. something blocks the view and the table isn't visible), simply keep old positions. http://git.fawkesrobotics.org/fawkes.git/commit/0914031 http://trac.fawkesrobotics.org/changeset/0914031 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 94b305cb99160a1c2f963c22164a7b3e7643290f Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Feb 28 13:02:45 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:36:24 2013 +0100 tabletop-objects: prevent tracker from printing expected errors If there are no points near the original position of the tracked object, the coherence prints an error (input is empty). This happens if the view is blocked or the object moved too fast. In those situations, the tracker works correctly, therefore we just turn off error printing. http://git.fawkesrobotics.org/fawkes.git/commit/94b305c http://trac.fawkesrobotics.org/changeset/94b305c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit fcbccefc43c8e186b7af1853484dc8c9ca2156f1 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Feb 28 13:42:30 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:36:31 2013 +0100 tabletop-objects: use static_cast in tracker initialization use static_cast instead of implicit cast http://git.fawkesrobotics.org/fawkes.git/commit/fcbccef http://trac.fawkesrobotics.org/changeset/fcbccef - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 74914d8fc5915337062ff1c7a47db298aa5fd090 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Feb 28 14:18:35 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:36:35 2013 +0100 tabletop-objects: refactor: extract method extract_object_clusters extract_object_clusters returns a vector of object clusters for a given cloud http://git.fawkesrobotics.org/fawkes.git/commit/74914d8 http://trac.fawkesrobotics.org/changeset/74914d8 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 0980951a3f8a89cba3e20645ca4f68e9369632ed Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Feb 28 16:19:15 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Feb 28 16:36:39 2013 +0100 tabletop-objects: refactor: extract method colorize_cluster colorize_cluster colorizes either the whole cloud or a cluster within the cloud and returns the colored cloud. http://git.fawkesrobotics.org/fawkes.git/commit/0980951 http://trac.fawkesrobotics.org/changeset/0980951 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- cfg/conf.d/tabletop_objects.yaml | 2 +- .../tabletop-objects/tabletop_objects_thread.cpp | 138 +++++++++++++------- .../tabletop-objects/tabletop_objects_thread.h | 8 + 3 files changed, 100 insertions(+), 48 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* 7ca3edd831080304e5d81eb98cfa0000a20fd7e6 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Feb 26 13:52:00 2013 +0100 Subject: tabletop-objects: visualize tracking .../tabletop-objects/tabletop_objects_thread.cpp | 16 ++++++++++++++++ .../tabletop-objects/tabletop_objects_thread.h | 2 ++ 2 files changed, 18 insertions(+), 0 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index f5eaf09..bd67c1b 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -188,6 +188,12 @@ TabletopObjectsThread::init() pcl_manager->add_pointcloud("tabletop-table-model", ftable_model_); pcl_utils::set_time(ftable_model_, fawkes::Time(clock)); + ftracking_cloud_ = new Cloud(); + tracking_cloud_ = pcl_utils::cloudptr_from_refptr(ftracking_cloud_); + tracking_cloud_->header.frame_id = finput_->header.frame_id; + pcl_manager->add_pointcloud("tabletop-tracking-points", ftracking_cloud_); + pcl_utils::set_time(ftracking_cloud_, fawkes::Time(clock)); + fsimplified_polygon_ = new Cloud(); simplified_polygon_ = pcl_utils::cloudptr_from_refptr(fsimplified_polygon_); simplified_polygon_->header.frame_id = finput_->header.frame_id; @@ -299,6 +305,7 @@ TabletopObjectsThread::finalize() pcl_manager->remove_pointcloud("tabletop-object-clusters"); pcl_manager->remove_pointcloud("tabletop-table-model"); pcl_manager->remove_pointcloud("tabletop-simplified-polygon"); + pcl_manager->remove_pointcloud("tabletop-tracking-points"); blackboard->close(table_pos_if_); blackboard->close(switch_if_); @@ -1048,7 +1055,12 @@ TabletopObjectsThread::loop() p2.g = table_color[1]; p2.b = table_color[2]; } + TIMETRACK_INTER(ttc_table_to_output_, ttc_cluster_objects_) + + CloudPtr tracking_cloud(new Cloud()); + tracking_cloud->header.frame_id = input_->header.frame_id; + if (first_run_) { if (cloud_objs_->points.size() > 0) { // cloud_objs_ contains only points above the table // Creating the KdTree object for the search method of the extraction @@ -1107,6 +1119,7 @@ TabletopObjectsThread::loop() tracker_[centroid_i]->setTrans (trans); tracker_[centroid_i]->setMinIndices (int (segmented_cloud_->size()) / 2); active_trackers[centroid_i] = true; + *tracking_cloud += *segmented_cloud_; } first_run_ = false; if (centroid_i > 0) @@ -1134,6 +1147,7 @@ TabletopObjectsThread::loop() Eigen::Affine3f transformation = tracker_[centroid_i]->toEigenMatrix (result); RefCloudPtr result_cloud (new RefCloud ()); pcl::transformPointCloud<RefPointType> (*(tracker_[centroid_i]->getReferenceCloud ()), *result_cloud, transformation); + *tracking_cloud += *result_cloud; pcl::compute3DCentroid<RefPointType>(*result_cloud, centroids[centroid_i]); // logger->log_warn(name(), "centroid %d: %f %f %f", centroid_i, centroids[centroid_i][0], centroids[centroid_i][1], centroids[centroid_i][2]); @@ -1167,9 +1181,11 @@ TabletopObjectsThread::loop() TIMETRACK_INTER(ttc_cluster_objects_, ttc_visualization_) *clusters_ = *tmp_clusters; + *tracking_cloud_ = *tracking_cloud; pcl_utils::copy_time(finput_, fclusters_); pcl_utils::copy_time(finput_, ftable_model_); pcl_utils::copy_time(finput_, fsimplified_polygon_); + pcl_utils::copy_time(finput_, ftracking_cloud_); #ifdef HAVE_VISUAL_DEBUGGING if (visthread_) { diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h index c83d94e..ca4d5d8 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h @@ -164,6 +164,8 @@ class TabletopObjectsThread CloudPtr table_model_; fawkes::RefPtr<Cloud> fsimplified_polygon_; CloudPtr simplified_polygon_; + fawkes::RefPtr<Cloud> ftracking_cloud_; + CloudPtr tracking_cloud_; unsigned int loop_count_; - *commit* da5284e7057f3d2161fe26e2d4422beb809b649a - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Feb 26 14:36:49 2013 +0100 Subject: tabletop-objects: only use points above the table as input for tracking .../tabletop-objects/tabletop_objects_thread.cpp | 2 +- 1 files changed, 1 insertions(+), 1 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index bd67c1b..fa9e77d 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1141,7 +1141,7 @@ TabletopObjectsThread::loop() for (centroid_i = 0; centroid_i < MAX_CENTROIDS; centroid_i++) { if (active_trackers[centroid_i]) { - tracker_[centroid_i]->setInputCloud(temp_cloud); + tracker_[centroid_i]->setInputCloud(cloud_objs_); tracker_[centroid_i]->compute(); pcl::tracking::ParticleXYZRPY result = tracker_[centroid_i]->getResult (); Eigen::Affine3f transformation = tracker_[centroid_i]->toEigenMatrix (result); - *commit* 29560ee187c65717d7f6acab103e757ccd4a3c4c - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed Feb 27 15:06:52 2013 +0100 Subject: tabletop-objects: change tracking particle number (400->100) cfg/conf.d/tabletop_objects.yaml | 2 +- 1 files changed, 1 insertions(+), 1 deletions(-) _Diff for modified files_: diff --git a/cfg/conf.d/tabletop_objects.yaml b/cfg/conf.d/tabletop_objects.yaml index 9127d78..dc092ca 100644 --- a/cfg/conf.d/tabletop_objects.yaml +++ b/cfg/conf.d/tabletop_objects.yaml @@ -80,7 +80,7 @@ perception/tabletop-objects: tracking_maxdistance: 0.01 # Tracking: number of the particles - tracking_particle_number: 400 + tracking_particle_number: 100 # Tracking: threshold to re-initialize the particles of the tracker tracking_resample_likelihood: 0.00 - *commit* 09140314ec8bb26c6a990afd59eac839130c4858 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed Feb 27 15:39:50 2013 +0100 Subject: tabletop-objects: only track if cloud_objs_ isn't empty .../tabletop-objects/tabletop_objects_thread.cpp | 17 ++++++++++++----- 1 files changed, 12 insertions(+), 5 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index fa9e77d..a9f4c93 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1141,12 +1141,19 @@ TabletopObjectsThread::loop() for (centroid_i = 0; centroid_i < MAX_CENTROIDS; centroid_i++) { if (active_trackers[centroid_i]) { - tracker_[centroid_i]->setInputCloud(cloud_objs_); - tracker_[centroid_i]->compute(); - pcl::tracking::ParticleXYZRPY result = tracker_[centroid_i]->getResult (); - Eigen::Affine3f transformation = tracker_[centroid_i]->toEigenMatrix (result); RefCloudPtr result_cloud (new RefCloud ()); - pcl::transformPointCloud<RefPointType> (*(tracker_[centroid_i]->getReferenceCloud ()), *result_cloud, transformation); + if (cloud_objs_ && !cloud_objs_->points.empty()) { +// if (!tracker_[centroid_i]->getReferenceCloud() || tracker_[centroid_i]->getReferenceCloud()->points.empty()) +// logger->log_warn(name(), "tracker %u: Reference Cloud is empty", centroid_i); + tracker_[centroid_i]->setInputCloud(cloud_objs_); + tracker_[centroid_i]->compute(); + pcl::tracking::ParticleXYZRPY result = tracker_[centroid_i]->getResult (); + Eigen::Affine3f transformation = tracker_[centroid_i]->toEigenMatrix (result); + pcl::transformPointCloud<RefPointType> (*(tracker_[centroid_i]->getReferenceCloud ()), *result_cloud, transformation); + } else { + logger->log_debug(name(), "cloud_objs_ is empty, don't track."); + *result_cloud = *tracker_[centroid_i]->getReferenceCloud(); + } *tracking_cloud += *result_cloud; pcl::compute3DCentroid<RefPointType>(*result_cloud, centroids[centroid_i]); // logger->log_warn(name(), "centroid %d: %f %f %f", centroid_i, centroids[centroid_i][0], centroids[centroid_i][1], centroids[centroid_i][2]); - *commit* 94b305cb99160a1c2f963c22164a7b3e7643290f - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Feb 28 13:02:45 2013 +0100 Subject: tabletop-objects: prevent tracker from printing expected errors .../tabletop-objects/tabletop_objects_thread.cpp | 7 +++++++ 1 files changed, 7 insertions(+), 0 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index a9f4c93..d11e527 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1146,7 +1146,14 @@ TabletopObjectsThread::loop() // if (!tracker_[centroid_i]->getReferenceCloud() || tracker_[centroid_i]->getReferenceCloud()->points.empty()) // logger->log_warn(name(), "tracker %u: Reference Cloud is empty", centroid_i); tracker_[centroid_i]->setInputCloud(cloud_objs_); + + // the coherences print errors if there are no points near the original position of the tracked object + // turn off PCL_ERRORs for the computation because this is expected behavior + // TODO find a proper way to prevent the coherences from reporting errors which are expected + pcl::console::VERBOSITY_LEVEL verbosity = pcl::console::getVerbosityLevel(); + pcl::console::setVerbosityLevel(pcl::console::VERBOSITY_LEVEL::L_ALWAYS); tracker_[centroid_i]->compute(); + pcl::console::setVerbosityLevel(verbosity); pcl::tracking::ParticleXYZRPY result = tracker_[centroid_i]->getResult (); Eigen::Affine3f transformation = tracker_[centroid_i]->toEigenMatrix (result); pcl::transformPointCloud<RefPointType> (*(tracker_[centroid_i]->getReferenceCloud ()), *result_cloud, transformation); - *commit* fcbccefc43c8e186b7af1853484dc8c9ca2156f1 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Feb 28 13:42:30 2013 +0100 Subject: tabletop-objects: use static_cast in tracker initialization .../tabletop-objects/tabletop_objects_thread.cpp | 2 +- 1 files changed, 1 insertions(+), 1 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index d11e527..0b5c4e7 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1117,7 +1117,7 @@ TabletopObjectsThread::loop() pcl::transformPointCloud(*segmented_cloud_, *transed_ref, trans.inverse()); tracker_[centroid_i]->setReferenceCloud (transed_ref); tracker_[centroid_i]->setTrans (trans); - tracker_[centroid_i]->setMinIndices (int (segmented_cloud_->size()) / 2); + tracker_[centroid_i]->setMinIndices(static_cast<int> (segmented_cloud_->size() / 2)); active_trackers[centroid_i] = true; *tracking_cloud += *segmented_cloud_; } - *commit* 74914d8fc5915337062ff1c7a47db298aa5fd090 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Feb 28 14:18:35 2013 +0100 Subject: tabletop-objects: refactor: extract method extract_object_clusters .../tabletop-objects/tabletop_objects_thread.cpp | 37 ++++++++++++-------- .../tabletop-objects/tabletop_objects_thread.h | 3 ++ 2 files changed, 25 insertions(+), 15 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index 0b5c4e7..ee25d41 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1063,21 +1063,7 @@ TabletopObjectsThread::loop() if (first_run_) { if (cloud_objs_->points.size() > 0) { // cloud_objs_ contains only points above the table - // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree<PointType>::Ptr - kdtree_cl(new pcl::search::KdTree<PointType>()); - kdtree_cl->setInputCloud(cloud_objs_); - - std::vector<pcl::PointIndices> cluster_indices; - pcl::EuclideanClusterExtraction<PointType> ec; - ec.setClusterTolerance(cfg_cluster_tolerance_); - ec.setMinClusterSize(cfg_cluster_min_size_); - ec.setMaxClusterSize(cfg_cluster_max_size_); - ec.setSearchMethod(kdtree_cl); - ec.setInputCloud(cloud_objs_); - ec.extract(cluster_indices); - - //logger->log_debug(name(), "Found %zu clusters", cluster_indices.size()); + std::vector<pcl::PointIndices> cluster_indices = extract_object_clusters(cloud_objs_); colored_clusters->header.frame_id = clusters_->header.frame_id; std::vector<pcl::PointIndices>::const_iterator it; @@ -1246,6 +1232,27 @@ TabletopObjectsThread::loop() #endif } +std::vector<pcl::PointIndices> +TabletopObjectsThread::extract_object_clusters(CloudConstPtr input) { + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree<PointType>::Ptr + kdtree_cl(new pcl::search::KdTree<PointType>()); + kdtree_cl->setInputCloud(input); + + std::vector<pcl::PointIndices> cluster_indices; + pcl::EuclideanClusterExtraction<PointType> ec; + ec.setClusterTolerance(cfg_cluster_tolerance_); + ec.setMinClusterSize(cfg_cluster_min_size_); + ec.setMaxClusterSize(cfg_cluster_max_size_); + ec.setSearchMethod(kdtree_cl); + ec.setInputCloud(input); + ec.extract(cluster_indices); + + //logger->log_debug(name(), "Found %zu clusters", cluster_indices.size()); + + return cluster_indices; +} + void TabletopObjectsThread::extractSegmentCluster (const CloudConstPtr &cloud, const std::vector<pcl::PointIndices> cluster_indices, const int segment_index, diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h index ca4d5d8..2ec30b0 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h @@ -117,6 +117,9 @@ class TabletopObjectsThread const int segment_index, Cloud &result); + std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input); + + /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ protected: virtual void run() { Thread::run(); } - *commit* 0980951a3f8a89cba3e20645ca4f68e9369632ed - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Feb 28 16:19:15 2013 +0100 Subject: tabletop-objects: refactor: extract method colorize_cluster .../tabletop-objects/tabletop_objects_thread.cpp | 57 +++++++++++--------- .../tabletop-objects/tabletop_objects_thread.h | 3 + 2 files changed, 35 insertions(+), 25 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index ee25d41..cb9b5fd 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -401,6 +401,7 @@ TabletopObjectsThread::loop() CloudPtr cloud_above_; CloudPtr cloud_objs_; ColorCloudPtr colored_clusters(new ColorCloud()); + colored_clusters->header.frame_id = clusters_->header.frame_id; pcl::search::KdTree<PointType> kdtree_; grid_.setInputCloud(input_); @@ -1065,9 +1066,7 @@ TabletopObjectsThread::loop() if (cloud_objs_->points.size() > 0) { // cloud_objs_ contains only points above the table std::vector<pcl::PointIndices> cluster_indices = extract_object_clusters(cloud_objs_); - colored_clusters->header.frame_id = clusters_->header.frame_id; std::vector<pcl::PointIndices>::const_iterator it; - unsigned int cci = 0; //unsigned int i = 0; unsigned int num_points = 0; for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) @@ -1083,17 +1082,8 @@ TabletopObjectsThread::loop() pcl::compute3DCentroid(*cloud_objs_, it->indices, centroids[centroid_i]); // logger->log_warn(name(), "centroid: %f %f %f", centroids[centroid_i][0], centroids[centroid_i][1], centroids[centroid_i][2]); - std::vector<int>::const_iterator pit; - for (pit = it->indices.begin(); pit != it->indices.end(); ++pit) { - ColorPointType &p1 = colored_clusters->points[cci++]; - PointType &p2 = cloud_objs_->points[*pit]; - p1.x = p2.x; - p1.y = p2.y; - p1.z = p2.z; - p1.r = cluster_colors[centroid_i][0]; - p1.g = cluster_colors[centroid_i][1];; - p1.b = cluster_colors[centroid_i][2];; - } + *colored_clusters += colorize_cluster(*cloud_objs_, it->indices, centroid_i); + CloudPtr segmented_cloud_(new Cloud()); // TODO we are iterating over cluster_indices, why not just use it->indices here? extractSegmentCluster(cloud_objs_, cluster_indices, centroid_i, *segmented_cloud_); @@ -1122,7 +1112,6 @@ TabletopObjectsThread::loop() } else { - unsigned int cci = 0; unsigned int size = 0; for (centroid_i = 0; centroid_i < MAX_CENTROIDS; centroid_i++) { @@ -1153,17 +1142,8 @@ TabletopObjectsThread::loop() size += result_cloud->size(); colored_clusters->points.resize(size); - Cloud::const_iterator pit; - for (pit = result_cloud->begin(); pit != result_cloud->end(); pit++) { - ColorPointType &p1 = colored_clusters->points[cci++]; - PointType p2 = *pit; - p1.x = p2.x; - p1.y = p2.y; - p1.z = p2.z; - p1.r = cluster_colors[centroid_i][0]; - p1.g = cluster_colors[centroid_i][1];; - p1.b = cluster_colors[centroid_i][2];; - } + *colored_clusters += colorize_cluster(*result_cloud, centroid_i); + if (result_cloud->size() > 0) highest_obj_id = centroid_i; else @@ -1253,6 +1233,33 @@ TabletopObjectsThread::extract_object_clusters(CloudConstPtr input) { return cluster_indices; } +TabletopObjectsThread::ColorCloud TabletopObjectsThread::colorize_cluster ( + const Cloud &input_cloud, + const std::vector<int> &cluster, + uint color) { + return colorize_cluster(Cloud(input_cloud, cluster), color); +} + +TabletopObjectsThread::ColorCloud TabletopObjectsThread::colorize_cluster ( + const Cloud &input_cloud, + uint color) { + ColorCloud result; + result.resize(input_cloud.size()); + Cloud::const_iterator pit; + uint i = 0; + for (pit = input_cloud.begin(); pit != input_cloud.end(); ++pit, ++i) { + ColorPointType &p1 = result.points[i]; + const PointType &p2 = *pit; + p1.x = p2.x; + p1.y = p2.y; + p1.z = p2.z; + p1.r = cluster_colors[color][0]; + p1.g = cluster_colors[color][1]; + p1.b = cluster_colors[color][2]; + } + return result; +} + void TabletopObjectsThread::extractSegmentCluster (const CloudConstPtr &cloud, const std::vector<pcl::PointIndices> cluster_indices, const int segment_index, diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h index 2ec30b0..53dbb28 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h @@ -119,6 +119,9 @@ class TabletopObjectsThread std::vector<pcl::PointIndices> extract_object_clusters(CloudConstPtr input); + ColorCloud colorize_cluster(const Cloud &input_cloud, const std::vector<int> &cluster, uint color); + + ColorCloud colorize_cluster (const Cloud &input_cloud, uint color); /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ protected: virtual void run() { Thread::run(); } -- Fawkes Robotics Framework http://www.fawkesrobotics.org _______________________________________________ fawkes-commits mailing list fawkes-commits@lists.kbsg.rwth-aachen.de https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
