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