def optimise_alignment(self, xyzs, initialpose, full_data=False): ''' The format of this function allows us to use standard optimizers and arbitrary cost functions from the scipy.optimize library.''' dtheta_base = np.radians(0.5) if not full_data: xyzs_sample = util.volumetricsample(xyzs, self.getfinest().resolution) else: xyzs_sample = xyzs bestpose = initialpose for level, ol in enumerate(self.mapvoxels): objective_func = ol.cost_func #objective_func = self.objectivefuncMROL #ol.use_bloom = False dtheta = dtheta_base*pow(self.factor,(len(self.mapvoxels)-level-1)) dx = ol.resolution # This helps avoid local minima, but limits accuracy to voxel size # at the finest resolution bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample,), dx, dtheta, max_iterations=20, verbosity=0) #import pydb; pydb.set_trace() #bestpose, cost = _cost_min_scipy(objective_func, bestpose, (xyzs_sample,)) # run it again at the finest resolution with tighter steps to get # sub-voxel accuracy. dx = dx*pow(self.factor,-2) dtheta = dtheta_base*pow(self.factor,-2) if not full_data: # Sometimes get better results with the below resampling, but much faster. xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution) bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs,), dx, dtheta, max_iterations=100, verbosity=0) return bestpose, cost
def optimise_alignment(self, xyzs, initialpose, full_data=False): ''' The format of this function allows us to use standard optimizers and arbitrary cost functions from the scipy.optimize library.''' dtheta_base = np.radians(0.5) if not full_data: xyzs_sample = util.volumetricsample(xyzs, self.getfinest().resolution) else: xyzs_sample = xyzs bestpose = initialpose for level, ol in enumerate(self.mapvoxels): objective_func = ol.cost_func #objective_func = self.objectivefuncMROL #ol.use_bloom = False dtheta = dtheta_base * pow(self.factor, (len(self.mapvoxels) - level - 1)) dx = ol.resolution # This helps avoid local minima, but limits accuracy to voxel size # at the finest resolution bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample, ), dx, dtheta, max_iterations=20, verbosity=0) #import pydb; pydb.set_trace() #bestpose, cost = _cost_min_scipy(objective_func, bestpose, (xyzs_sample,)) # run it again at the finest resolution with tighter steps to get # sub-voxel accuracy. dx = dx * pow(self.factor, -2) dtheta = dtheta_base * pow(self.factor, -2) if not full_data: # Sometimes get better results with the below resampling, but much faster. xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution) bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs, ), dx, dtheta, max_iterations=100, verbosity=0) return bestpose, cost
def volumetricsample(self, res): '''Downsamples the point cloud volumetrically according to the resolution''' self.points = util.volumetricsample(self.points, res)
def match(self, xyzs, centrepose, spreadpose): """Does a global search of the specified pose space. Find the possible locations of the occupied list ol in the current occupied list. Takes an occupied list to match to the current one and two poses.""" debug = False # determines the orientation step size as it relates to occupancy list # resolution poses = None bestpose = poseutil.Pose3D() totaltime = time.time() lasttime = time.time() #for i in range(len(self.voxels)): # volumetric sample and each resolution equivalent to matching two # occupiedlists together but without the double quantisation error # TODO think about this number #xyzs = util.getsample(xyzs, 1000) levels = len(self.mapvoxels) table = np.zeros((levels + 2, 9), dtype=np.dtype('S8')) # header for the table of values of interest table[ 0, :] = "Res modal map scan pose pose Top Time Best".split( ) table[ 1, :] = "(m) Frac voxels points count count overlap Taken pose".split( ) for i, mapol in enumerate(self.mapvoxels): tablerow = i + 2 res = mapol.resolution deltaposition = res deltaorientation = res / self.feature_range # 8 if poses == None: poses = poseutil.uniformposes(centrepose, spreadpose, deltaposition, deltaorientation) else: poses = poseutil.refineposes(poses, deltaposition, deltaorientation) # TODO remove poses outside search space this needs tidying up/thinking about #cparr = np.array(centrepose.get()) #sparr = np.array(spreadpose.get()) # #D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation)) #maxpose = cparr + sparr + D #minpose = cparr - sparr - D #select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1)) ##print 'Culling poses: ', len(poses), sum(select) #poses = poses[select] xyzs_sample = util.volumetricsample(xyzs, res) table[tablerow, :5] = (res, modalfractions[i], len(mapol), len(xyzs_sample), len(poses)) # TODO TIME consuming line #overlaps = self.getoverlaps(ol, i, poses) overlaps = [] for pose in poses: #overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True)) overlap = mapol.calccollisions(pose, xyzs, query=False) overlaps.append(overlap) #overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample)) overlaps = np.asarray(overlaps) ind_max = np.argmax(overlaps) bestpose.set(poses[ind_max]) bestoverlap = overlaps[ind_max] #candidates = overlaps >= modalfractions[i] * bestoverlap # maybe just take the top n? if len(overlaps) > topn: candidates = overlaps.argsort()[-topn:] else: candidates = range(len(overlaps)) timetaken = time.time() - lasttime lasttime = time.time() table[tablerow, 5:] = (len(candidates), bestoverlap, timetaken, bestpose) print printtable(table, width=9) print bestpose #scansize = ol.voxels[-1].shape[0] poses = poses[candidates] print "Total time: ", time.time() - totaltime overlaps = overlaps[candidates] return bestpose, bestoverlap
def match(self, xyzs, centrepose, spreadpose): """Does a global search of the specified pose space. Find the possible locations of the occupied list ol in the current occupied list. Takes an occupied list to match to the current one and two poses.""" debug = False # determines the orientation step size as it relates to occupancy list # resolution poses = None bestpose = poseutil.Pose3D() totaltime = time.time() lasttime = time.time() #for i in range(len(self.voxels)): # volumetric sample and each resolution equivalent to matching two # occupiedlists together but without the double quantisation error # TODO think about this number #xyzs = util.getsample(xyzs, 1000) levels = len(self.mapvoxels) table = np.zeros((levels+2, 9), dtype=np.dtype('S8')) # header for the table of values of interest table[0, :] = "Res modal map scan pose pose Top Time Best".split() table[1, :] = "(m) Frac voxels points count count overlap Taken pose".split() for i, mapol in enumerate(self.mapvoxels): tablerow = i + 2 res = mapol.resolution deltaposition = res deltaorientation = res/self.feature_range # 8 if poses == None: poses = poseutil.uniformposes(centrepose, spreadpose, deltaposition, deltaorientation) else: poses = poseutil.refineposes(poses, deltaposition, deltaorientation) # TODO remove poses outside search space this needs tidying up/thinking about #cparr = np.array(centrepose.get()) #sparr = np.array(spreadpose.get()) # #D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation)) #maxpose = cparr + sparr + D #minpose = cparr - sparr - D #select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1)) ##print 'Culling poses: ', len(poses), sum(select) #poses = poses[select] xyzs_sample = util.volumetricsample(xyzs, res) table[tablerow, :5] = (res, modalfractions[i], len(mapol), len(xyzs_sample), len(poses)) # TODO TIME consuming line #overlaps = self.getoverlaps(ol, i, poses) overlaps = [] for pose in poses: #overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True)) overlap = mapol.calccollisions(pose, xyzs, query=False) overlaps.append(overlap) #overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample)) overlaps = np.asarray(overlaps) ind_max = np.argmax(overlaps) bestpose.set(poses[ind_max]) bestoverlap = overlaps[ind_max] #candidates = overlaps >= modalfractions[i] * bestoverlap # maybe just take the top n? if len(overlaps) > topn: candidates = overlaps.argsort()[-topn:] else: candidates = range(len(overlaps)) timetaken = time.time() - lasttime lasttime = time.time() table[tablerow, 5:] = (len(candidates), bestoverlap, timetaken, bestpose) print printtable(table, width=9) print bestpose #scansize = ol.voxels[-1].shape[0] poses = poses[candidates] print "Total time: ", time.time() - totaltime overlaps = overlaps[candidates] return bestpose, bestoverlap