~mrol-dev/mrol/trunk

from __future__ import division

import roslib

roslib.load_manifest('mrol-mapping-ros-pkg')

import numpy as np

import pylab as p

#import matplotlib.axes3d as p3

import mpl_toolkits.mplot3d.axes3d as p3

import mrol_mapping.mapper as mapper

import mrol_mapping.poseutil as poseutil

 

import icp_mrol.icp_mrol as icp

 

 

 

def load_and_align(bestpose, fname, First=False, ICP=False):

    xyzs = np.loadtxt(fname)

    #(xyzs, _) = poseutil.transformPoints(xyzs, (0,0,0,np.deg2rad(60),0,0))

    xyzs = xyzs[(xyzs[:,0]<-0.1)]

    xyzs = xyzs[(xyzs[:,0]>-0.5)]

    xyzs = xyzs[(xyzs[:,1]<0.5)]

    xyzs = xyzs[(xyzs[:,1]>0.25)]

    xyzs = xyzs[(xyzs[:,2]>1.)]

    xyzs = xyzs[(xyzs[:,2]<1.3)]

    if First:

        return xyzs, bestpose

    else:

        if not ICP:

            bestpose = cupMapper.align_points(xyzs, bestpose)[0]

        else:

            bestpose = icp.align(bestpose, xyzs, cupMapper.get_voxels(), reject_threshold=res*10., terminate_threshold=res/100., iterations = 100, quiet=True)[0]

    (transform, _) = poseutil.transformPoints(xyzs, bestpose)

    return transform, bestpose

 

fig=p.figure()

ax = p3.Axes3D(fig)

 

res = 0.0003

cupMapper = mapper.VoxelMap(res,levels=6)

cupMapper.set_feature_range(0.25)

initpose = (0,0,0,0,0,0)

 

cup1, pose = load_and_align(initpose, 'segmented_points225.dat', First=True)

cupMapper.add_points(cup1,initpose)

 

ax.plot3D(cup1[:,0],cup1[:,1],cup1[:,2],'.')

ax.set_xlabel('X')

ax.set_ylabel('Y')

ax.set_zlabel('Z')

fig.add_axes(ax)

#p.show() # ctrl-c here?

 

print pose

 

cup2, pose = load_and_align(initpose, 'segmented_points234.dat') 

ax.plot3D(cup2[:,0],cup2[:,1],cup2[:,2],'.')

print pose

cupMapper.add_points(cup2,initpose)

 

cup3, pose = load_and_align(initpose, 'segmented_points265.dat')

ax.plot3D(cup3[:,0],cup3[:,1],cup3[:,2],'.')

print pose

cupMapper.add_points(cup3,initpose)

 

cup4, pose = load_and_align(initpose, 'segmented_points273.dat')

ax.plot3D(cup4[:,0],cup4[:,1],cup4[:,2],'.')

print pose

cupMapper.add_points(cup4,initpose)

 

cup5, pose = load_and_align(initpose, 'segmented_points296.dat')

ax.plot3D(cup5[:,0],cup5[:,1],cup5[:,2],'.')

print pose

cupMapper.add_points(cup5,initpose)

 

cup6, pose = load_and_align(initpose, 'segmented_points308.dat')

ax.plot3D(cup6[:,0],cup6[:,1],cup6[:,2],'.')

print pose

cupMapper.add_points(cup6,initpose)

 

cupModel = cupMapper.get_voxels()

cupModel = cupModel - np.mean(cupModel,axis=0)

 

np.savetxt('cupModel.dat',cupModel)

 

fig=p.figure()

ax = p3.Axes3D(fig)

ax.plot3D(cupModel[:,0],cupModel[:,1],cupModel[:,2],'.')

ax.set_xlabel('X')

ax.set_ylabel('Y')

ax.set_zlabel('Z')

fig.add_axes(ax)

 

p.show()