# Path: lfd/tpsopt/registration.py
# def unit_boxify(x_na):    
#     ranges = x_na.ptp(axis=0)
#     dlarge = ranges.argmax()
#     unscaled_translation = - (x_na.min(axis=0) + x_na.max(axis=0))/2
#     scaling = 1./ranges[dlarge]
#     scaled_translation = unscaled_translation * scaling
#     return x_na*scaling + scaled_translation, (scaling, scaled_translation)    
# 
# def loglinspace(a,b,n):
#     "n numbers between a to b (inclusive) with constant ratio between consecutive numbers"
#     return np.exp(np.linspace(np.log(a),np.log(b),n))    
# 
# Path: lfd/rapprentice/clouds.py
# DEFAULT_F = 535.
#     X = (x - cx)*(Z/f)
#     Y = (y - cy)*(Z/f)
#     XYZ = np.empty((480,640,3))
#     Z = XYZ[:,:,2] = depth / 1000. # convert mm -> meters
#     XYZ[:,:,0] = (x - cx)*(Z/f)
#     XYZ[:,:,1] = (y - cy)*(Z/f)
# def xyZ_to_XY(x,y,Z,f=DEFAULT_F):
# def XYZ_to_xy(X,Y,Z,f=DEFAULT_F):
# def depth_to_xyz(depth,f=DEFAULT_F):
# def downsample(xyz, v):


import h5py
import argparse
import sys
import numpy as np
import scipy.linalg
import scikits.cuda.linalg
from pycuda import gpuarray
from scikits.cuda.linalg import pinv as cu_pinv
from settings import GRIPPER_OPEN_CLOSE_THRESH
from tps import tps_kernel_matrix, tps_kernel_matrix2
from lfd.tpsopt.registration import unit_boxify, loglinspace
from lfd.rapprentice import clouds
from culinalg_exts import get_gpu_ptrs, dot_batch_nocheck, m_dot_batch
from settings import N_ITER_CHEAP, DEFAULT_LAMBDA, DS_SIZE, BEND_COEF_DIGITS, EXACT_LAMBDA, N_ITER_EXACT
                    seg_info['%s_closing_inds'%lr] = closing_inds
#
            ds_g['cloud_xyz']          = x_na
            ds_g['scaled_cloud_xyz']   = scaled_x_na
            ds_g['scaling']            = scale_params[0]
            ds_g['scaled_translation'] = scale_params[1]
            ds_g['scaled_K_nn']        = K_nn


        for bend_coef in bend_coefs:
            if str(bend_coef) in inv_group:
                continue
            
            bend_coef_g = inv_group.create_group(str(bend_coef))
            _, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
            for k, v in res.iteritems():
                bend_coef_g[k] = v

        if args.verbose:
            sys.stdout.write('\rprecomputed tps solver for segment {}'.format(seg_name))
            sys.stdout.flush()
    print ""

    if args.test:
        atol = 1e-7
        print 'Running batch get sol params test with atol = ', atol
        test_batch_get_sol_params(f, [bend_coef], atol=atol)
        print 'batch sol params test succeeded'
    print ""


# Path: lfd/tpsopt/registration.py
# def unit_boxify(x_na):    
#     ranges = x_na.ptp(axis=0)
#     dlarge = ranges.argmax()
#     unscaled_translation = - (x_na.min(axis=0) + x_na.max(axis=0))/2
#     scaling = 1./ranges[dlarge]
#     scaled_translation = unscaled_translation * scaling
#     return x_na*scaling + scaled_translation, (scaling, scaled_translation)    
# 
# def loglinspace(a,b,n):
#     "n numbers between a to b (inclusive) with constant ratio between consecutive numbers"
#     return np.exp(np.linspace(np.log(a),np.log(b),n))    
# 
# Path: lfd/rapprentice/clouds.py
# DEFAULT_F = 535.
#     X = (x - cx)*(Z/f)
#     Y = (y - cy)*(Z/f)
#     XYZ = np.empty((480,640,3))
#     Z = XYZ[:,:,2] = depth / 1000. # convert mm -> meters
#     XYZ[:,:,0] = (x - cx)*(Z/f)
#     XYZ[:,:,1] = (y - cy)*(Z/f)
# def xyZ_to_XY(x,y,Z,f=DEFAULT_F):
# def XYZ_to_xy(X,Y,Z,f=DEFAULT_F):
# def depth_to_xyz(depth,f=DEFAULT_F):
# def downsample(xyz, v):


# Path: lfd/tpsopt/precompute.py
import h5py
import argparse
import sys
import numpy as np
import scipy.linalg
import scikits.cuda.linalg
from pycuda import gpuarray
from scikits.cuda.linalg import pinv as cu_pinv
from settings import GRIPPER_OPEN_CLOSE_THRESH
from tps import tps_kernel_matrix, tps_kernel_matrix2
from lfd.tpsopt.registration import unit_boxify, loglinspace
from lfd.rapprentice import clouds
from culinalg_exts import get_gpu_ptrs, dot_batch_nocheck, m_dot_batch
from settings import N_ITER_CHEAP, DEFAULT_LAMBDA, DS_SIZE, BEND_COEF_DIGITS, EXACT_LAMBDA, N_ITER_EXACT

                    seg_info['%s_closing_inds'%lr] = closing_inds
#
            ds_g['cloud_xyz']          = x_na
            ds_g['scaled_cloud_xyz']   = scaled_x_na
            ds_g['scaling']            = scale_params[0]
            ds_g['scaled_translation'] = scale_params[1]
            ds_g['scaled_K_nn']        = K_nn


        for bend_coef in bend_coefs:
            if str(bend_coef) in inv_group:
                continue
            
            bend_coef_g = inv_group.create_group(str(bend_coef))
            _, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
            for k, v in res.iteritems():
                bend_coef_g[k] = v

        if args.verbose:
            sys.stdout.write('\rprecomputed tps solver for segment {}'.format(seg_name))
            sys.stdout.flush()
    print ""

    if args.test:
        atol = 1e-7
        print 'Running batch get sol params test with atol = ', atol
        test_batch_get_sol_params(f, [bend_coef], atol=atol)
        print 'batch sol params test succeeded'
    print ""


    bend_coefs = np.around(loglinspace(args.exact_bend_coef_init, args.exact_bend_coef_final,