Example #1
0
 def cost(self, threshold=999999):
     truecost = icp.cost(self.true_pose, self.P, self.Q)
     guesscost = icp.cost(self.guesspose, self.P, self.Q)
     if debug:
         print 'guesscost: ', guesscost
         print 'truecost: ', truecost
     self.assertTrue(truecost < guesscost)
 def cost(self, threshold=999999):
     truecost = icp.cost(self.true_pose, self.P, self.Q)
     guesscost = icp.cost(self.guesspose, self.P, self.Q)
     if debug:
         print 'guesscost: ', guesscost
         print 'truecost: ', truecost
     self.assertTrue(truecost < guesscost)
    def test_cost(self):
        P = np.array( ((0., 0., 0.), (1., 1., 1.)) )
        # TODO cost function fails if there is only one point
        # TODO bug if all all poinst are rejected by threshold
        #self.assertEqual(1, icp.cost((1, 0, 0, 0, 0, 0), P, P), threshold=0.5)

        self.assertAlmostEqual(0.1, icp.cost((0.1, 0, 0, 0, 0, 0), P, P))
        self.P, self.Q = generate_data(self.true_pose, pextra=0, noise_bool=False)
        truecost = icp.cost(self.true_pose, self.P, self.Q)
        self.assertEqual(truecost, 0)
Example #4
0
    def test_cost(self):
        P = np.array(((0., 0., 0.), (1., 1., 1.)))
        # TODO cost function fails if there is only one point
        # TODO bug if all all poinst are rejected by threshold
        #self.assertEqual(1, icp.cost((1, 0, 0, 0, 0, 0), P, P), threshold=0.5)

        self.assertAlmostEqual(0.1, icp.cost((0.1, 0, 0, 0, 0, 0), P, P))
        self.P, self.Q = generate_data(self.true_pose,
                                       pextra=0,
                                       noise_bool=False)
        truecost = icp.cost(self.true_pose, self.P, self.Q)
        self.assertEqual(truecost, 0)