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-hungarian has been updated to 60fbad74b9c8b5739472e6e781afb8af4d798096 (commit) from 0281ca865d179cb88553de6501345cd84dcfa7fe (commit) http://git.fawkesrobotics.org/fawkes.git/thofmann/tabletop-obj-hungarian 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 60fbad74b9c8b5739472e6e781afb8af4d798096 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Fri May 31 10:42:39 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Fri May 31 10:45:29 2013 +0200 tabletop-objects: only check distance if centroid id has been reassigned -If we found a new centroid, we don't need to check whether it was moved further than max_distance. -restructure munkres assignment loop http://git.fawkesrobotics.org/fawkes.git/commit/60fbad7 http://trac.fawkesrobotics.org/changeset/60fbad7 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- .../tabletop-objects/tabletop_objects_thread.cpp | 60 ++++++++----------- 1 files changed, 25 insertions(+), 35 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* 60fbad74b9c8b5739472e6e781afb8af4d798096 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Fri May 31 10:42:39 2013 +0200 Subject: tabletop-objects: only check distance if centroid id has been reassigned .../tabletop-objects/tabletop_objects_thread.cpp | 60 ++++++++----------- 1 files changed, 25 insertions(+), 35 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 7cd8b8c..2e64577 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1130,52 +1130,42 @@ unsigned int TabletopObjectsThread::add_objects(CloudConstPtr input_cloud, Color // get assignments int assignment_size; int *assignment = solver.get_assignment(assignment_size); + unsigned int id; for (int row = 0; row < assignment_size; row++) { if (row >= hp.num_rows) { // object has disappeared old_centroids_.push_back(OldCentroid(obj_ids.at(assignment[row]), centroids_.at(obj_ids[assignment[row]]))); + continue; } - else { - unsigned int id; - if (assignment[row] >= hp.num_cols) { // object is new or has reappeared - bool assigned = false; - // first, check if there is an old centroid close enough - for (OldCentroidVector::iterator it = old_centroids_.begin(); - it != old_centroids_.end(); it++) { - double distance = pcl::distances::l2(new_centroids[row], it->getCentroid()); - if (distance < cfg_centroid_max_distance_) { - id = it->getId(); - old_centroids_.erase(it); - assigned = true; - break; - } - } - if (!assigned) { - // we still don't have an id, create as new object - id = next_id(); + else if (assignment[row] >= hp.num_cols) { // object is new or has reappeared + bool assigned = false; + // first, check if there is an old centroid close enough + for (OldCentroidVector::iterator it = old_centroids_.begin(); + it != old_centroids_.end(); it++) { + if (pcl::distances::l2(new_centroids[row], it->getCentroid()) <= cfg_centroid_max_distance_) { + id = it->getId(); + old_centroids_.erase(it); + assigned = true; + break; } } - else { - id = obj_ids[assignment[row]]; + if (!assigned) { + // we still don't have an id, create as new object + id = next_id(); } - tmp_centroids[id] = new_centroids[row]; - *tmp_clusters += *colorize_cluster(input_cloud, cluster_indices[row].indices, cluster_colors[id % MAX_CENTROIDS]); } - } - - // check if a centroid was moved further than cfg_centroid_max_distance_ - // this can happen if a centroid appears and another one disappears in the same loop - // (then, the old centroid is assigned to the new one) - for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); it++) { - // only check those centroids with the same id in centroids_ and tmp_centroids - if (tmp_centroids.count(it->first)) { - double distance = pcl::distances::l2(it->second, tmp_centroids[it->first]); - if (distance > cfg_centroid_max_distance_) { - //logger->log_debug(name(), "%u: moved by %f", it->first, distance); + else { // object has been assigned to an existing id + id = obj_ids[assignment[row]]; + // check if centroid was moved further than cfg_centroid_max_distance_ + // this can happen if a centroid appears and another one disappears in the same loop + // (then, the old centroid is assigned to the new one) + if (pcl::distances::l2(centroids_[id], new_centroids[row]) > cfg_centroid_max_distance_) { // save the centroid because we don't use it now - old_centroids_.push_back(OldCentroid(it->first, it->second)); - tmp_centroids.erase(it->first); + old_centroids_.push_back(OldCentroid(id, centroids_[id])); + continue; } } + tmp_centroids[id] = new_centroids[row]; + *tmp_clusters += *colorize_cluster(input_cloud, cluster_indices[row].indices, cluster_colors[id % MAX_CENTROIDS]); } // age all old centroids -- 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
