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
import table_score
t = table_score.diamond_positions()
for p in t:
print "Diamond", p
s = table_score.generate_hypotheses(0,0,0,0.1, 0, 0.01, 3)
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)
