def estimate_table_pose(self, current_pose, diamond_positions, debug=False): # Parameters x_stdev = 0.07 y_stdev = 0.07 theta_stdev = 0.08 num_hypotheses = 5000 max_hypothesis_dist_from_table_nav = 0.15 # observed_diamonds = [(x, y) for (x, y, z) in diamond_positions] observed_z = sum([z for (x, y, z) in merged_clusters_tn]) / len(merged_clusters_tn) # Generate hypothetical poses P = { ( x^(k), y^(k), z^(k) ), ... } #hypotheses = table_score.generate_hypotheses(self._current_estimate[0], self._current_estimate[1], self._current_estimate[2], x_stdev, y_stdev, theta_stdev, num_hypotheses) hypotheses = table_score.generate_hypotheses(0.0, 0.0, 0.0, x_stdev, y_stdev, theta_stdev, num_hypotheses) hypotheses_filtered = [p for p in hypotheses if abs(p[0]) + abs(p[1]) + abs(p[2]) < max_hypothesis_dist_from_table_nav] # Find best hypothesis (best_hypothesis, best_error) = table_score.find_best_hypothesis(hypotheses_filtered, observed_diamonds) print 'Estimated pose: x=%.2f y=%.2f th=%.2f (error=%.6f)' % (best_hypothesis[0], best_hypothesis[1], best_hypothesis[2], best_error) if debug: # Visualize hypothesis for i, candidate in enumerate(hypotheses_filtered): if i % 100 != 0: continue diamond_set = table_score.computed_hypothesized_diamonds(candidate) self._publish_diamonds_to_rviz('diamond_hypothesis', i, [(x, y, observed_z) for (x, y) in diamond_set], '/table_nav', 1, 0, 0, 0.006) # Visualize observed self._publish_diamonds_to_rviz('merged_clusters_tn', 0, merged_clusters_tn, '/table_nav', 0, 1, 0, 0.04) # Visualize /table_nav table_nav_diamonds = [(x, y, 0.0) for (x, y) in table_score.computed_hypothesized_diamonds((0.0, 0.0, 0.0))] self._publish_diamonds_to_rviz('table_nav_diamonds', 0, table_nav_diamonds, '/table_nav', 1, 0, 0, 0.04) # Visualize current_estimate current_estimate_diamonds = [(x, y, observed_z) for (x, y) in table_score.computed_hypothesized_diamonds(self._current_estimate)] self._publish_diamonds_to_rviz('current_estimate_diamonds', 0, current_estimate_diamonds, '/table_nav', 1, 0, 1, 0.04) # Visualize best estimate new_estimate_points = table_score.computed_hypothesized_diamonds(new_estimate) self._publish_diamonds_to_rviz('new_estimate', 0, [(x, y, observed_z) for (x, y) in new_estimate_points], '/table_nav', 0, 0, 1, 0.04) # Debugging: visualize table in rviz self._publish_table_to_rviz('table', '/table', 0, 0, 1) return best_hypothesis
for h in s: print "hypothesis", h th = table_score.computed_hypothesized_diamonds(h) for p in th: print "twisted diamond", p print "distance2 between (1,1) and (2,4) is %f"%table_score.point_distance2((1,1), (2,4)) print "min_point_distance2 between (1,1) and [(2,4), (3,4)] is %f"%table_score.min_point_distance2((1,1), [(2,4), (3,4)]) print "errors for points [(0,0), (0,.1)], hypothesis (0,0,0)" points = [(0,0), (0,.1)] hypothesis = (0,0,0) print table_score.computed_error(hypothesis, points) print "find best hypotheses" hypotheses = [(0,0,0), (0,0,.01)] print table_score.find_best_hypothesis(hypotheses, s) (best_hyp, best_error) = table_score.find_best_hypothesis(table_score.generate_hypotheses(0,0,0,0.1, 0.1, 0.1, 100), t) print "best hyp, best error", best_hyp, best_error