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 eec703710c4c558bddf632e2db3eee4776d84ca3 (commit) via 89c005c858cd5dc700660967bd56e68f30941f7e (commit) from 60fbad74b9c8b5739472e6e781afb8af4d798096 (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 89c005c858cd5dc700660967bd56e68f30941f7e Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Jun 4 14:52:55 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Tue Jun 4 15:18:49 2013 +0200 tabletop-objects: only use [0,MAX_CENTROIDS) as centroid IDs -save free IDs in a list -only use ID if it wasn't used for a certain number of loops (set in tabletop_objects.yaml) -remove all old centroids sharing the same ID with a new object http://git.fawkesrobotics.org/fawkes.git/commit/89c005c http://trac.fawkesrobotics.org/changeset/89c005c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit eec703710c4c558bddf632e2db3eee4776d84ca3 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Jun 4 15:44:04 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Tue Jun 4 15:44:04 2013 +0200 tabletop-objects: static initialization of pos_ifs_ -initialize all pos_ifs_ during class initialization -use std::vector instead of std::map for pos_ifs_ http://git.fawkesrobotics.org/fawkes.git/commit/eec7037 http://trac.fawkesrobotics.org/changeset/eec7037 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- cfg/conf.d/tabletop_objects.yaml | 3 + .../tabletop-objects/tabletop_objects_thread.cpp | 83 +++++++++++++------- .../tabletop-objects/tabletop_objects_thread.h | 9 ++- 3 files changed, 63 insertions(+), 32 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* 89c005c858cd5dc700660967bd56e68f30941f7e - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Jun 4 14:52:55 2013 +0200 Subject: tabletop-objects: only use [0,MAX_CENTROIDS) as centroid IDs cfg/conf.d/tabletop_objects.yaml | 3 ++ .../tabletop-objects/tabletop_objects_thread.cpp | 33 ++++++++++++++++--- .../tabletop-objects/tabletop_objects_thread.h | 5 ++- 3 files changed, 34 insertions(+), 7 deletions(-) _Diff for modified files_: diff --git a/cfg/conf.d/tabletop_objects.yaml b/cfg/conf.d/tabletop_objects.yaml index 3749152..c0cadf3 100644 --- a/cfg/conf.d/tabletop_objects.yaml +++ b/cfg/conf.d/tabletop_objects.yaml @@ -81,3 +81,6 @@ perception/tabletop-objects: # max distance an old centroid may have moved centroid_max_distance: 0.041 + + # min age of a centroid ID before it is used again + centroid_id_min_age: 5 diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp index 2e64577..f0ebf5c 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -134,6 +134,7 @@ TabletopObjectsThread::init() cfg_result_frame_ = config->get_string(CFG_PREFIX"result_frame"); cfg_centroid_max_age_ = config->get_uint(CFG_PREFIX"centroid_max_age"); cfg_centroid_max_distance_ = config->get_float(CFG_PREFIX"centroid_max_distance"); + cfg_centroid_id_min_age_ = config->get_uint(CFG_PREFIX"centroid_id_min_age"); finput_ = pcl_manager->get_pointcloud<PointType>("openni-pointcloud-xyz"); input_ = pcl_utils::cloudptr_from_refptr(finput_); @@ -190,6 +191,8 @@ TabletopObjectsThread::init() first_run_ = true; old_centroids_.clear(); + for (unsigned int i = 0; i < MAX_CENTROIDS; i++) + free_ids_.push_back(i); #ifdef USE_TIMETRACKER tt_ = new TimeTracker(); @@ -1067,9 +1070,18 @@ TabletopObjectsThread::extract_object_clusters(CloudConstPtr input) { return cluster_indices; } -unsigned int TabletopObjectsThread::next_id() { - static unsigned int id = 0; - return id++; +bool TabletopObjectsThread::next_id(unsigned int &id) { + if (free_ids_.empty()) { + logger->log_debug(name(), "free_ids is empty"); + return false; + } + unsigned int next_id = free_ids_.front(); + if (pos_ifs_.count(next_id) && pos_ifs_[next_id]->visibility_history() > -1*static_cast<int>(cfg_centroid_id_min_age_)) { + return false; + } + id = next_id; + free_ids_.pop_front(); + return true; } unsigned int TabletopObjectsThread::add_objects(CloudConstPtr input_cloud, ColorCloudPtr tmp_clusters) { unsigned int object_count = 0; @@ -1097,7 +1109,9 @@ unsigned int TabletopObjectsThread::add_objects(CloudConstPtr input_cloud, Color if (first_run_) { // get a new id for every object since we didn't have objects before for (unsigned int i = 0; i < new_centroids.size(); i++) { - unsigned int id = next_id(); + unsigned int id; + if (!next_id(id)) + break; tmp_centroids[id] = new_centroids[i]; *tmp_clusters += *colorize_cluster(input_cloud, cluster_indices[i].indices, cluster_colors[id % MAX_CENTROIDS]); } @@ -1133,7 +1147,9 @@ unsigned int TabletopObjectsThread::add_objects(CloudConstPtr input_cloud, Color 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]]))); + id = obj_ids.at(assignment[row]); + free_ids_.push_back(id); + old_centroids_.push_back(OldCentroid(id, centroids_.at(id))); continue; } else if (assignment[row] >= hp.num_cols) { // object is new or has reappeared @@ -1144,13 +1160,15 @@ unsigned int TabletopObjectsThread::add_objects(CloudConstPtr input_cloud, Color if (pcl::distances::l2(new_centroids[row], it->getCentroid()) <= cfg_centroid_max_distance_) { id = it->getId(); old_centroids_.erase(it); + free_ids_.remove_if([&id](const unsigned int &i) { return id == i; }); assigned = true; break; } } if (!assigned) { // we still don't have an id, create as new object - id = next_id(); + if (!next_id(id)) + continue; } } else { // object has been assigned to an existing id @@ -1160,11 +1178,14 @@ unsigned int TabletopObjectsThread::add_objects(CloudConstPtr input_cloud, Color // (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 + free_ids_.push_back(id); old_centroids_.push_back(OldCentroid(id, centroids_[id])); continue; } } tmp_centroids[id] = new_centroids[row]; + // remove id from old_centroids_ because we don't want the same id twices + old_centroids_.remove_if([&id](const OldCentroid& centroid){ return centroid.getId() == id; }); *tmp_clusters += *colorize_cluster(input_cloud, cluster_indices[row].indices, cluster_colors[id % MAX_CENTROIDS]); } diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h index 7f5f7b5..f93c9a3 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h @@ -132,7 +132,7 @@ class TabletopObjectsThread unsigned int add_objects(CloudConstPtr input, ColorCloudPtr tmp_clusters); - unsigned int next_id(); + bool next_id(unsigned int &id); /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ protected: virtual void run() { Thread::run(); } @@ -149,6 +149,8 @@ class TabletopObjectsThread PosIfsMap pos_ifs_; fawkes::Position3DInterface *table_pos_if_; + std::list<unsigned int> free_ids_; + fawkes::SwitchInterface *switch_if_; float cfg_depth_filter_min_x_; @@ -171,6 +173,7 @@ class TabletopObjectsThread std::string cfg_result_frame_; uint cfg_centroid_max_age_; float cfg_centroid_max_distance_; + unsigned int cfg_centroid_id_min_age_; fawkes::RefPtr<Cloud> ftable_model_; CloudPtr table_model_; - *commit* eec703710c4c558bddf632e2db3eee4776d84ca3 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Jun 4 15:44:04 2013 +0200 Subject: tabletop-objects: static initialization of pos_ifs_ .../tabletop-objects/tabletop_objects_thread.cpp | 52 +++++++++++--------- .../tabletop-objects/tabletop_objects_thread.h | 4 +- 2 files changed, 30 insertions(+), 26 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 f0ebf5c..527b562 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -152,9 +152,30 @@ TabletopObjectsThread::init() switch_if_->write(); pos_ifs_.clear(); + pos_ifs_.resize(MAX_CENTROIDS, NULL); + for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) { + char *tmp; + if (asprintf(&tmp, "Tabletop Object %u", i + 1) != -1) { + // Copy to get memory freed on exception + std::string id = tmp; + free(tmp); + Position3DInterface *iface = + blackboard->open_for_writing<Position3DInterface>(id.c_str()); + pos_ifs_[i] = iface; + iface->set_rotation(rotation); + iface->set_visibility_history(-1*cfg_centroid_id_min_age_); + logger->log_debug(name(), "visibility: %d", iface->visibility_history()); + iface->write(); + } + } } catch (Exception &e) { blackboard->close(table_pos_if_); blackboard->close(switch_if_); + for (unsigned int i = 0; i < MAX_CENTROIDS; ++i) { + if (pos_ifs_[i]) { + blackboard->close(pos_ifs_[i]); + } + } throw; } @@ -231,8 +252,8 @@ TabletopObjectsThread::finalize() blackboard->close(table_pos_if_); blackboard->close(switch_if_); - for (PosIfsMap::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); it++) { - blackboard->close(it->second); + for (PosIfsVector::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); it++) { + blackboard->close(*it); } pos_ifs_.clear(); @@ -969,31 +990,14 @@ TabletopObjectsThread::loop() } // set all pos_ifs not in centroids_ to 'not visible' - for (PosIfsMap::const_iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); it++) { - if (!centroids_.count(it->first)) - set_position(it->second, false); + for (unsigned int i = 0; i < pos_ifs_.size(); i++) { + if (!centroids_.count(i)) { + set_position(pos_ifs_[i], false); + } } // set positions of all visible centroids for (CentroidMap::iterator it = centroids_.begin(); it != centroids_.end(); it++) { - if (!pos_ifs_.count(it->first)) { // pos_if doesn't exist, create it first - try { - char *tmp; - if (asprintf(&tmp, "Tabletop Object %u", it->first) != -1) { - // Copy to get memory freed on exception - std::string id = tmp; - free(tmp); - Position3DInterface *iface = - blackboard->open_for_writing<Position3DInterface>(id.c_str()); - pos_ifs_[it->first] = iface; - } - } catch (Exception &e) { - if (pos_ifs_[it->first]) { - blackboard->close(pos_ifs_[it->first]); - } - throw; - } - } set_position(pos_ifs_[it->first], true, it->second); } @@ -1076,7 +1080,7 @@ bool TabletopObjectsThread::next_id(unsigned int &id) { return false; } unsigned int next_id = free_ids_.front(); - if (pos_ifs_.count(next_id) && pos_ifs_[next_id]->visibility_history() > -1*static_cast<int>(cfg_centroid_id_min_age_)) { + if (pos_ifs_[next_id]->visibility_history() > -1*static_cast<int>(cfg_centroid_id_min_age_)) { return false; } id = next_id; diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h index f93c9a3..fdfa528 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h @@ -109,7 +109,7 @@ class TabletopObjectsThread Eigen::aligned_allocator<std::pair<const unsigned int, Eigen::Vector4f>>> CentroidMap; typedef std::list<OldCentroid, Eigen::aligned_allocator<OldCentroid> > OldCentroidVector; - typedef std::map<unsigned int, fawkes::Position3DInterface *> PosIfsMap; + typedef std::vector<fawkes::Position3DInterface *> PosIfsVector; private: void set_position(fawkes::Position3DInterface *iface, @@ -146,7 +146,7 @@ class TabletopObjectsThread pcl::VoxelGrid<PointType> grid_; pcl::SACSegmentation<PointType> seg_; - PosIfsMap pos_ifs_; + PosIfsVector pos_ifs_; fawkes::Position3DInterface *table_pos_if_; std::list<unsigned int> free_ids_; -- 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
