~mrol-dev/mrol/trunk

topn = 100

 

# approximate number of sample points selected from the scan to be aligned

 

# TODO consider pulling in table class from icra2011 code, table.py

def printtable(table, width):

        # cast points to float32 for speed

        xyzs = np.float32(xyzs)

        xyzs_sample = self._sample_points(xyzs, sample_size)

 

        if verbose: 

            print 'Number of sample points: %d' % len(xyzs_sample)

        bestpose = initialpose

        for level, ol in enumerate(self.mapvoxels):

            objective_func = ol.cost_func 

            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

            if verbose:

                print 'resolution: %f' % (ol.resolution)

            bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample,), dx, dtheta, max_iterations=20, verbosity=2)

            #bestpose, cost = optimization._cost_min_scipy(objective_func, bestpose, (xyzs_sample,))

        # run it again at the finest resolution with tighter steps to get