Touch based localization

Assuming that we have collected a set of 'touch' measurements including positions and normals and would like to estimate the object pose.

The following code snippets shows basic usage of 'cope' in estimating an object pose given 'touch' measurements.

First, import necessary functions

import trimesh
import numpy as np
import cope.SE3lib as SE3
import cope.particlelib as ptcl
import pickle

Then load the object model or create a simple box as below

extents    = [0.05,0.1,0.2]
mesh       = trimesh.creation.box(extents)

Here we load a pre-computed offline angle dictionary which is required for the efficient face selection algorithm in the measurement likelihood evaluation step.

pkl_file   = open('data/woodstick_w_dict.p', 'rb')
angle_dict = pickle.load(pkl_file)
pkl_file.close()
# Otherwise, one can also compute this here by using 
# angle_dict = ptcl.NormalHashing(mesh,num_random_unit=100,plot_histogram=True)

Next, we input all the measurements and the program parameters

measurements = [[np.array([-0.02882446, -0.04892219,  0.00738576]),
                 np.array([-0.40190523, -0.90828342, -0.11616118])],
                [np.array([ 0.01610016,  0.04007391,  0.01259628]),
                 np.array([ 0.52140577,  0.83554119,  0.17322511])],
                # [np.array([ -1.470e-05,  2.2329e-02,  8.2384e-02]),
                #  np.array([-0.88742601,  0.40497962,  0.22015128])],
                [np.array([-0.00351179, -0.05645598,  0.10106514]),
                 np.array([ 0.14455158, -0.24319869,  0.95914506])],
                # [np.array([ 0.03399573,  0.01704082,  0.09381309]),
                #  np.array([ 0.45363072,  0.87916087,  0.14592921])],
                [np.array([-0.03732133, -0.00669343, -0.01288346]),
                 np.array([-0.88147849,  0.34850243,  0.31865615])]]
# Measurements' Errs
pos_err          = 2e-3
nor_err          = 5./180.0*np.pi
# Scaling series params
dim              = 6
prune_percentage = 0.8
ptcls0           = [np.eye(4)]
M                = 6
sigma0           = np.diag([0.0025,0.0025,0.0025,0.25,0.25,0.25],0) #trans,rot
sigma_desired    = np.diag([1e-6,1e-6,1e-6,1e-6,1e-6,1e-6],0)

Finally, use 'cope' to solve for the object pose

estimate = ptcl.RunImprovedScalingSeries( mesh, angle_dict, ptcls0, measurements, pos_err, nor_err, M, sigma0, sigma_desired, prune_percentage, dim = 6, visualize = False)
# Visualization
ptcl.Visualize(mesh,estimate,measurements)

The following figure should appear