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, common/tabletop-obj-merge has been updated to 0f2416128df0a18e56789c1e7c1114cbc4a48de9 (commit) from 7952febddce586af01e6d4a4b80862551fe55d47 (commit) http://git.fawkesrobotics.org/fawkes.git/common/tabletop-obj-merge 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 0f2416128df0a18e56789c1e7c1114cbc4a48de9 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed Dec 18 12:27:22 2013 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed Dec 18 12:27:22 2013 +0100 tabletop-obj: refactoring, extract method track_objects -separate object tracking (i.e. ID reassignment) from centroid/cluster manipulation -new method: track_objects: compares new_centroids and last centroids and calculates the optimal ID assignment, returns map of assignments -copy all values (tmp_centroids, cylinder fitting params) at the end and only once http://git.fawkesrobotics.org/fawkes.git/commit/0f24161 http://trac.fawkesrobotics.org/changeset/0f24161 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- .../tabletop-objects/tabletop_objects_thread.cpp | 254 +++++++++++--------- .../tabletop-objects/tabletop_objects_thread.h | 3 +- 2 files changed, 140 insertions(+), 117 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* 0f2416128df0a18e56789c1e7c1114cbc4a48de9 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed Dec 18 12:27:22 2013 +0100 Subject: tabletop-obj: refactoring, extract method track_objects .../tabletop-objects/tabletop_objects_thread.cpp | 254 +++++++++++--------- .../tabletop-objects/tabletop_objects_thread.h | 3 +- 2 files changed, 140 insertions(+), 117 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 a095aef..d400869 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.cpp @@ -1238,15 +1238,16 @@ TabletopObjectsThread::extract_object_clusters(CloudConstPtr input) { return cluster_indices; } -bool TabletopObjectsThread::next_id(unsigned int &id) { +int TabletopObjectsThread::next_id() { if (free_ids_.empty()) { logger->log_debug(name(), "free_ids is empty"); - return false; + return -1; } - id = free_ids_.front(); + int id = free_ids_.front(); free_ids_.pop_front(); - return true; + return id; } + unsigned int TabletopObjectsThread::cluster_objects(CloudConstPtr input_cloud, ColorCloudPtr tmp_clusters, @@ -1314,125 +1315,53 @@ pcl_utils::transform_pointcloud("/base_link", *single_cluster, new_centroids.resize(object_count); - if (cfg_track_objects_) { - // save cylinder_params, best_obj_guess and obj_shape_confidence - // to temporary variables to be able to reassign IDs - CentroidMap cylinder_params(cylinder_params_); - std::map<uint, signed int> best_obj_guess(best_obj_guess_); - std::map<uint, double> obj_shape_confidence(obj_shape_confidence_); - std::map<uint, std::vector<double> > obj_likelihoods(obj_likelihoods_); - cylinder_params_.clear(); - best_obj_guess_.clear(); - obj_shape_confidence_.clear(); - obj_likelihoods_.clear(); - - 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; - if (!next_id(id)) - break; - tmp_centroids[id] = new_centroids[i]; - cylinder_params_[id] = cylinder_params[i]; - obj_shape_confidence_[id] = obj_shape_confidence[i]; - best_obj_guess_[id] = best_obj_guess[i]; - obj_likelihoods_[id] = obj_likelihoods[i]; - - ColorCloudPtr colorized_cluster = colorize_cluster(input_cloud, - cluster_indices[i].indices, cluster_colors[id % MAX_CENTROIDS]); - *tmp_clusters += *colorized_cluster; - tmp_obj_clusters[id] = colorized_cluster; - } - } - else { // !first_run_ - TIMETRACK_START(ttc_hungarian_); - hungarian_problem_t hp; - // obj_ids: the id of the centroid in column i is saved in obj_ids[i] - std::vector<unsigned int> obj_ids(centroids_.size()); - // create cost matrix, - // save new centroids in rows, last centroids in columns - // distance between new centroid i and last centroid j in cost[i][j] - hp.num_rows = new_centroids.size(); - hp.num_cols = centroids_.size(); - hp.cost = (int**) calloc(hp.num_rows, sizeof(int*)); - for (int i = 0; i < hp.num_rows; i++) - hp.cost[i] = (int*) calloc(hp.num_cols, sizeof(int)); - for (int row = 0; row < hp.num_rows; row++) { // new centroids - unsigned int col = 0; - for (CentroidMap::iterator col_it = centroids_.begin(); - col_it != centroids_.end(); - col_it++, col++) { // old centroids - double distance = pcl::distances::l2(new_centroids[row], col_it->second); - hp.cost[row][col] = (int)(distance * 1000); - obj_ids[col] = col_it->first; - } - } - HungarianMethod solver; - solver.init(hp.cost, hp.num_rows, hp.num_cols, HUNGARIAN_MODE_MINIMIZE_COST); - solver.solve(); - // 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 - id = obj_ids.at(assignment[row]); - old_centroids_.push_back(OldCentroid(id, centroids_.at(id))); - continue; - } - 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; - } - } - if (!assigned) { - // we still don't have an id, create as new object - if (!next_id(id)) - continue; - } - } - 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(id, centroids_[id])); - continue; - } - } - tmp_centroids[id] = new_centroids[row]; - cylinder_params_[id] = cylinder_params[row]; - obj_shape_confidence_[id] = obj_shape_confidence[row]; - best_obj_guess_[id] = best_obj_guess[row]; - obj_likelihoods_[id] = obj_likelihoods[row]; - // remove id from old_centroids_ because we don't want the same id twices - ColorCloudPtr colorized_cluster = - colorize_cluster(input_cloud, cluster_indices[row].indices, cluster_colors[id % MAX_CENTROIDS]); - *tmp_clusters += *colorized_cluster; - tmp_obj_clusters[id] = colorized_cluster; - } + // save cylinder fitting variables + // to temporary variables to be able to reassign IDs + CentroidMap cylinder_params(cylinder_params_); + std::map<uint, signed int> best_obj_guess(best_obj_guess_); + std::map<uint, double> obj_shape_confidence(obj_shape_confidence_); + std::map<uint, std::vector<double> > obj_likelihoods(obj_likelihoods_); + cylinder_params_.clear(); + best_obj_guess_.clear(); + obj_shape_confidence_.clear(); + obj_likelihoods_.clear(); - } // !first_run_ + std::map<uint, int> assigned_ids; + if (cfg_track_objects_) { + assigned_ids = track_objects(new_centroids); } else { //! cfg_track_objects_ for (unsigned int i = 0; i < new_centroids.size(); i++) { - tmp_centroids[i] = new_centroids[i]; - ColorCloudPtr colorized_cluster = colorize_cluster(input_cloud, - cluster_indices[i].indices, cluster_colors[i]); - *tmp_clusters += *colorized_cluster; - tmp_obj_clusters[i] = colorized_cluster; + assigned_ids[i] = i; } } + // reassign IDs + for (uint i = 0; i < new_centroids.size(); i++) { + int assigned_id; + try { + assigned_id = assigned_ids.at(i); + } + catch (const std::out_of_range& e) { + logger->log_error(name(), "Object %d was not assigned", i); + // drop centroid + assigned_id = -1; + } + if (assigned_id == -1) + continue; + tmp_centroids[assigned_id] = new_centroids[i]; + cylinder_params_[assigned_id] = cylinder_params[i]; + obj_shape_confidence_[assigned_id] = obj_shape_confidence[i]; + best_obj_guess_[assigned_id] = best_obj_guess[i]; + obj_likelihoods_[assigned_id] = obj_likelihoods[i]; + ColorCloudPtr colorized_cluster = colorize_cluster(input_cloud, + cluster_indices[i].indices, + cluster_colors[assigned_id % MAX_CENTROIDS]); + *tmp_clusters += *colorized_cluster; + tmp_obj_clusters[assigned_id] = colorized_cluster; + + } + // remove all centroids too high above the table remove_high_centroids(table_centroid, tmp_centroids); @@ -1926,6 +1855,99 @@ Eigen::Vector4f TabletopObjectsThread::fit_cylinder( return new_centroid; } +/** + * calculate reassignment of IDs using the hungarian mehtod such that the total + * distance all centroids moved is minimal + * @param new_centroids current centroids which need correct ID assignment + * @return map containing the new assignments, new_centroid -> ID + * will be -1 if object is dropped, it's the caller's duty to remove any + * reference to the centroid + */ +std::map<unsigned int, int> +TabletopObjectsThread::track_objects( + std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids) +{ + std::map<uint, int> final_assignment; + 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++) { + final_assignment[i] = next_id(); + } + return final_assignment; + } + else { // !first_run_ + TIMETRACK_START(ttc_hungarian_); + hungarian_problem_t hp; + // obj_ids: the id of the centroid in column i is saved in obj_ids[i] + std::vector<unsigned int> obj_ids(centroids_.size()); + // create cost matrix, + // save new centroids in rows, last centroids in columns + // distance between new centroid i and last centroid j in cost[i][j] + hp.num_rows = new_centroids.size(); + hp.num_cols = centroids_.size(); + hp.cost = (int**) calloc(hp.num_rows, sizeof(int*)); + for (int i = 0; i < hp.num_rows; i++) + hp.cost[i] = (int*) calloc(hp.num_cols, sizeof(int)); + for (int row = 0; row < hp.num_rows; row++) { // new centroids + unsigned int col = 0; + for (CentroidMap::iterator col_it = centroids_.begin(); + col_it != centroids_.end(); col_it++, col++) { // old centroids + double distance = pcl::distances::l2(new_centroids[row], + col_it->second); + hp.cost[row][col] = (int) (distance * 1000); + obj_ids[col] = col_it->first; + } + } + HungarianMethod solver; + solver.init(hp.cost, hp.num_rows, hp.num_cols, + HUNGARIAN_MODE_MINIMIZE_COST); + solver.solve(); + // get assignments + int assignment_size; + int *assignment = solver.get_assignment(assignment_size); + int id; + for (int row = 0; row < assignment_size; row++) { + if (row >= hp.num_rows) { // object has disappeared + id = obj_ids.at(assignment[row]); + old_centroids_.push_back(OldCentroid(id, centroids_.at(id))); + continue; + } + 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; + } + } + if (!assigned) { + // we still don't have an id, create as new object + id = next_id(); + } + } + 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(id, centroids_[id])); + id = -1; + } + } + final_assignment[row] = id; + } + return final_assignment; + } +} + #ifdef HAVE_VISUAL_DEBUGGING void TabletopObjectsThread::set_visualization_thread(TabletopVisualizationThreadBase *visthread) diff --git a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h index cec4c82..5c224c8 100644 --- a/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h +++ b/src/plugins/perception/tabletop-objects/tabletop_objects_thread.h @@ -143,12 +143,13 @@ class TabletopObjectsThread unsigned int cluster_objects(CloudConstPtr input, ColorCloudPtr tmp_clusters, std::vector<ColorCloudPtr> &tmp_obj_clusters); - bool next_id(unsigned int &id); + int next_id(); void delete_old_centroids(OldCentroidVector centroids, unsigned int age); void delete_near_centroids(CentroidMap reference, OldCentroidVector centroids, float min_distance); void remove_high_centroids(Eigen::Vector4f table_centroid, CentroidMap centroids); Eigen::Vector4f fit_cylinder(ColorCloudConstPtr obj_in_base_frame, Eigen::Vector4f const ¢roid, uint const ¢roid_i); + std::map<unsigned int, int> track_objects(std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> new_centroids); /** 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
