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)
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)
