예제 #1
0
    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)
예제 #2
0
    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)
예제 #3
0
    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)
예제 #4
0
    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)
예제 #5
0
    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)
예제 #6
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)
예제 #7
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