def test_cross_readings(self):
particle = FilterParticle()
old_reading = (
Odometry(),
Blob(),
)
new_odom = Odometry()
new_odom.pose.pose.orientation = heading_to_quaternion(math.pi / 2)
new_reading = (
new_odom,
Blob(),
)
result = particle.cross_readings(old_reading, new_reading)
self.assertIsNotNone(result)
self.assertEqual(result[0], 0.0)
self.assertEqual(result[1], 0.0)
new_odom = Odometry()
new_odom.pose.pose.orientation = heading_to_quaternion(0.0)
new_odom.pose.pose.position.y = -1.0
new_reading = (
new_odom,
Blob(),
)
result = particle.cross_readings(old_reading, new_reading)
self.assertIsNone(result)
def test_prob_color_match(self):
# Note to self: check and see if the 0 is a problem for high covariance
particle = FilterParticle()
f_mean = np.array([0, 0, 255, 0, 0])
f_covar = list([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 5, 0, 0],
[0, 0, 0, 5, 0], [0, 0, 0, 0, 5]])
f_covar = np.array(f_covar)
blob = Blob()
blob.color.r = 255
blob.color.g = 0
blob.color.b = 0
result1 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result1 > 0.005)
blob.color.r = 250
result2 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result2 < result1)
blob.color.b = 5
result3 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result3 < result2)
blob.color.r = 200
result4 = particle.prob_color_match(f_mean, f_covar, blob)
self.assertTrue(result4 < result2)
def test_find_nearest_reading(self):
particle = FilterParticle()
state1 = Odometry() # 0,0
blob1 = Blob()
blob1.bearing = .1
state2 = Odometry() # 0,1
state2.pose.pose.position.y = 1
blob2 = Blob()
blob2.bearing = -.1
particle.potential_features[-1] = (state1, blob1)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
# parallel to state2 option, should not match
state3 = Odometry()
state3.pose.pose.position.y = 1
blob3 = Blob()
blob3.bearing = -.1
particle.potential_features[-3] = (state3, blob3)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
# wrong color option, should not match
state4 = Odometry() # 0,0
blob4 = Blob()
blob4.bearing = .1
blob4.color.r = 255
blob4.color.g = 255
blob4.color.b = 255
particle.potential_features[-4] = (state4, blob4)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
# doesn't intersect state 2, should not match
state5 = Odometry() # 0,0
state5.pose.pose.position.y = 100
blob5 = Blob()
blob5.bearing = -.1
particle.potential_features[-5] = (state4, blob4)
min_dist_id = particle.find_nearest_reading(state2, blob2)
self.assertEqual(min_dist_id, -1)
def test_add_orphaned_reading(self):
particle = FilterParticle()
original_size = len(particle.hypothesis_set)
particle.add_orphaned_reading(Odometry(), Blob())
new_size = len(particle.hypothesis_set)
self.assertTrue(original_size < new_size)
def test_reading_distance_function(self):
particle = FilterParticle()
state1 = Odometry()
blob1 = Blob()
# lines are parallel, should have no intersection
state2 = Odometry()
state2.pose.pose.position.y = 1
blob2 = Blob()
result2 = particle.reading_distance_function(state1, blob1, state2,
blob2)
self.assertEqual(result2, float('inf'))
# lines don't intersect should have no intersection
state3 = Odometry()
state3.pose.pose.position.y = 1
blob3 = Blob()
blob3.bearing = 1.0
result3 = particle.reading_distance_function(state1, blob1, state3,
blob3)
self.assertEqual(result3, float('inf'))
# lines intersect, color is the same, distance is 0
state4 = Odometry()
state4.pose.pose.position.y = 1
blob4 = Blob()
blob4.bearing = -1.0
result4 = particle.reading_distance_function(state1, blob1, state4,
blob4)
self.assertEqual(result4, 0.0)
# lines intersect, color is close, distance is small
state5 = Odometry()
state5.pose.pose.position.y = 1
blob5 = Blob()
blob5.bearing = -1.0
blob5.color.r = 5
result5 = particle.reading_distance_function(state1, blob1, state5,
blob5)
self.assertTrue(result5 > 0.0)
def test_probability_of_match_color(self):
particle = FilterParticle()
state = Odometry()
# two 0 cases: colors far apart, _bearing_ far off
blob_color = Blob()
blob_color.color.r = 255
feature = Feature(mean=np.array([1, 0, 0, 0, 0]))
self.assertIsInstance(feature.mean, np.ndarray)
result_color = particle.probability_of_match(state, blob_color,
feature)
self.assertEqual(result_color, 0.0)
def generate_measurement(self, featureid):
'''
Generate an expected measurement for the given featureid
'''
state = self.state
s_x = state.pose.pose.position.x
s_y = state.pose.pose.position.y
feature = self.get_feature_by_id(featureid)
f_x = feature.mean[0]
f_y = feature.mean[1]
bobby = Blob()
bobby.bearing = math.atan2(f_y - s_y, f_x - s_x)
# bobby.size = 1/math.sqrt(math.pow(f_x-s_x, 2)+math.pow(f_y-s_y, 2))
bobby.color.r = feature.mean[2]
bobby.color.g = feature.mean[3]
bobby.color.b = feature.mean[4]
return bobby