def testAlignmentMethods(self):
print "Test Alignment Methods"
for method in self.aligners:
aligner_name = method.__name__.replace('_', ' ')
aligner_name = aligner_name.replace('align ', '')
print 5*'-', aligner_name, 5*'-'
# Execute the aligner and time it
start_align = time.time()
bestpose = method()[0]
taken_align = time.time() - start_align
overlap = occupiedlist.calccollisions(bestpose, self.mapset, self.res, self.Pall)
# check accuracy and timeliness
pose_error = np.fabs(bestpose - self.truepose)
print pose_error, overlap, taken_align
if not np.alltrue(pose_error < [self.lin_tol, self.lin_tol, self.lin_tol, self.ang_tol, self.ang_tol, self.ang_tol]):
print pose_error[0], pose_error[1], pose_error[2], np.rad2deg(pose_error[3]), np.rad2deg(pose_error[4]), np.rad2deg(pose_error[5])
self.assertTrue(pose_error[0] < self.lin_tol)
self.assertTrue(pose_error[1] < self.lin_tol)
self.assertTrue(pose_error[2] < self.lin_tol)
self.assertTrue(pose_error[3] < self.ang_tol)
self.assertTrue(pose_error[4] < self.ang_tol)
self.assertTrue(pose_error[5] < self.ang_tol)
# Pass/Fail metrics are specific to this dataset!
#self.assertTrue(maxo>3700) #6070?
#self.assertTrue(taken_align < 0.25)
print "truepose overlap =", occupiedlist.calccollisions(self.truepose, self.mapset, self.res, self.Pall)
def randomsearch(mapset,res,Pall,guesspose):
''' Exhaustive search for ground truth
This function is used to generate the "true pose" which is used to generate the check data
sets for the segmentation. It should not be used widely until it is implemented
properly (see notes in implementation) and has more generic input.'''
poseobj = poseutil.Pose3D()
# TODO this is only valid for small poses should do it properly by using
# random rotations from transformations.py
dx = 0.1
dq = np.radians(1)
bestoverlap = 0
while True:
#start = time.time()
delta = np.hstack((np.random.normal(0, dx, 3), np.random.normal(0, dq, 3)))
poseobj.set(guesspose + delta)
overlap = occupiedlist.calccollisions(poseobj.getTuple(), mapset, res, Pall)
if overlap > bestoverlap:
bestoverlap = overlap
# TODO now search around new pose and refine? alternatively just do this randomly
print bestoverlap, ', ', poseobj
