# Path: rapprentice/testing.py
# TEST_FUNCS = {}
# def testme(func):    
# def test_all(stop=False):
# 
# Path: rapprentice/tps.py
# VERBOSE = False
# ENABLE_SLOW_TESTS = False
#     _N, D = x_na.shape
#     M = x_ma.shape[0]
#     N, D = x_na.shape
#     M = x_ma.shape[0]
#         J = np.c_[Jl, Jt, Jw]
#     M,D = x_ma.shape
#     D = lin_ag.shape[0]
#     D = lin_ag.shape[0]
#     E,D = x_ea.shape
#     N = y_ng.shape[0]
#     M = nr_ma.shape[0]
#     N,D = x_na.shape
#     A = np.zeros((N+D+1, N+D+1))
#     A[:N, :N] = K_nn
#     A[:N, N:N+D] = x_na
#     A[:N, N+D] = 1
#     A[N:N+D,:N] = x_na.T
#     A[N+D,:N] = 1
#     A[N:N+D, N:N+D] = coef_ratio*np.eye(D)
#     B = np.empty((N+D+1, D))
#     B[:N] = y_ng
#     B[N:N+D] = coef_ratio*np.eye(D)
#     B[N+D] = 0
#     X = np.linalg.solve(A, B)
#     N = _u[:,n_cnts:]
#     Q = np.c_[np.ones((n,1)), x_na, K_nn]
#     WQ = wt_n[:,None] * Q
#     QWQ = Q.T.dot(WQ)
#     H = QWQ
#     A = np.r_[np.zeros((d+1,d+1)), np.c_[np.ones((n,1)), x_na]].T
#     N,D = x_na.shape 
#     A = QQ_nn    
#     B = Q_nn.T.dot(y_ng)
#     X = np.linalg.solve(A,B)
#     N,D = x_na.shape
#                 A = nr_coef*JJ_nn + QQ_nn + bend_coef*Bend_nn
#                 X0 = X_ng[:,g]
#                 B = nr_coef*np.dot(J_zn.T, np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(y_ng[:,g])
#     N,D = x_na.shape
#     M = nr_ma.shape[0]
#     E = N + 4*M
#     F = E - M
#     Q = N + 3*M - 4
#             X0 = X_fg[:,g]
#     N,_D = x_na.shape
#     A = QQ_nn
#     B = Q_nn.T.dot(y_ng-x_na.dot(lin_ag))
#     X = np.linalg.solve(A,B)
#     N,_D = x_na.shape
# def nan2zero(x):
# def tps_apply_kernel(distmat, dim):
# def tps_kernel_matrix(x_na):
# def tps_kernel_matrix2(x_na, y_ma):
# def tps_eval(x_ma, lin_ag, trans_g, w_ng, x_na):
# def tps_grad(x_ma, lin_ag, _trans_g, w_ng, x_na):
# def tps_nr_grad(x_ma, lin_ag, _trans_g, w_ng, x_na, return_tuple = False):
# def tps_nr_err(x_ma, lin_ag, trans_g, w_ng, x_na):
# def tps_cost(lin_ag, trans_g, w_ng, x_na, y_ng, bend_coef, K_nn=None, return_tuple=False, wt_n = None):
# def tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, y_ng, xnr_ma, bend_coef, nr_coef, K_nn = None, return_tuple=False):
# def tps_nr_cost_eval_general(lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, K_ee = None, return_tuple=True):
# def tps_fit(x_na, y_ng, bend_coef, rot_coef, wt_n=None, K_nn = None):
# def solve_eqp1(H, f, A):
# def tps_fit3(x_na, y_ng, bend_coef, rot_coef, wt_n):
# def tps_fit2(x_na, y_ng, bend_coef, rot_coef, wt_n=None):
# def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
# def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef):
# def tps_fit_fixedrot(x_na, y_ng, bend_coef, lin_ag, K_nn = None, wt_n=None):
# def tps_fit_regrot(x_na, y_ng, bend_coef, rfunc, wt_n=None, max_iter = 1, inner_max_iter=100, rgrad=None, l_init=None):
#         def f(x): 
# def tps_cost_regrot(lin_ag, trans_g, w_ng, x_na, y_ng, bend_coef, rfunc, K_nn = None, wt_n=None):


from rapprentice import testing, tps
    from jds_image_proc.clouds import voxel_downsample
    from numpy import sin,cos
    from rapprentice import registration
import numpy as np
    import numdifftools as ndt
    import numdifftools as ndt
    import fastrapp


@testing.testme
def nonrigidity_gradient():
    D = 3
    pts0 = np.random.randn(10,D)
    pts_eval = np.random.randn(3,D)
    def err_partial(params):
        lin_ag = params[0:9].reshape(3,3)

# Path: rapprentice/testing.py
# TEST_FUNCS = {}
# def testme(func):    
# def test_all(stop=False):
# 
# Path: rapprentice/tps.py
# VERBOSE = False
# ENABLE_SLOW_TESTS = False
#     _N, D = x_na.shape
#     M = x_ma.shape[0]
#     N, D = x_na.shape
#     M = x_ma.shape[0]
#         J = np.c_[Jl, Jt, Jw]
#     M,D = x_ma.shape
#     D = lin_ag.shape[0]
#     D = lin_ag.shape[0]
#     E,D = x_ea.shape
#     N = y_ng.shape[0]
#     M = nr_ma.shape[0]
#     N,D = x_na.shape
#     A = np.zeros((N+D+1, N+D+1))
#     A[:N, :N] = K_nn
#     A[:N, N:N+D] = x_na
#     A[:N, N+D] = 1
#     A[N:N+D,:N] = x_na.T
#     A[N+D,:N] = 1
#     A[N:N+D, N:N+D] = coef_ratio*np.eye(D)
#     B = np.empty((N+D+1, D))
#     B[:N] = y_ng
#     B[N:N+D] = coef_ratio*np.eye(D)
#     B[N+D] = 0
#     X = np.linalg.solve(A, B)
#     N = _u[:,n_cnts:]
#     Q = np.c_[np.ones((n,1)), x_na, K_nn]
#     WQ = wt_n[:,None] * Q
#     QWQ = Q.T.dot(WQ)
#     H = QWQ
#     A = np.r_[np.zeros((d+1,d+1)), np.c_[np.ones((n,1)), x_na]].T
#     N,D = x_na.shape 
#     A = QQ_nn    
#     B = Q_nn.T.dot(y_ng)
#     X = np.linalg.solve(A,B)
#     N,D = x_na.shape
#                 A = nr_coef*JJ_nn + QQ_nn + bend_coef*Bend_nn
#                 X0 = X_ng[:,g]
#                 B = nr_coef*np.dot(J_zn.T, np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(y_ng[:,g])
#     N,D = x_na.shape
#     M = nr_ma.shape[0]
#     E = N + 4*M
#     F = E - M
#     Q = N + 3*M - 4
#             X0 = X_fg[:,g]
#     N,_D = x_na.shape
#     A = QQ_nn
#     B = Q_nn.T.dot(y_ng-x_na.dot(lin_ag))
#     X = np.linalg.solve(A,B)
#     N,_D = x_na.shape
# def nan2zero(x):
# def tps_apply_kernel(distmat, dim):
# def tps_kernel_matrix(x_na):
# def tps_kernel_matrix2(x_na, y_ma):
# def tps_eval(x_ma, lin_ag, trans_g, w_ng, x_na):
# def tps_grad(x_ma, lin_ag, _trans_g, w_ng, x_na):
# def tps_nr_grad(x_ma, lin_ag, _trans_g, w_ng, x_na, return_tuple = False):
# def tps_nr_err(x_ma, lin_ag, trans_g, w_ng, x_na):
# def tps_cost(lin_ag, trans_g, w_ng, x_na, y_ng, bend_coef, K_nn=None, return_tuple=False, wt_n = None):
# def tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, y_ng, xnr_ma, bend_coef, nr_coef, K_nn = None, return_tuple=False):
# def tps_nr_cost_eval_general(lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, K_ee = None, return_tuple=True):
# def tps_fit(x_na, y_ng, bend_coef, rot_coef, wt_n=None, K_nn = None):
# def solve_eqp1(H, f, A):
# def tps_fit3(x_na, y_ng, bend_coef, rot_coef, wt_n):
# def tps_fit2(x_na, y_ng, bend_coef, rot_coef, wt_n=None):
# def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
# def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef):
# def tps_fit_fixedrot(x_na, y_ng, bend_coef, lin_ag, K_nn = None, wt_n=None):
# def tps_fit_regrot(x_na, y_ng, bend_coef, rfunc, wt_n=None, max_iter = 1, inner_max_iter=100, rgrad=None, l_init=None):
#         def f(x): 
# def tps_cost_regrot(lin_ag, trans_g, w_ng, x_na, y_ng, bend_coef, rfunc, K_nn = None, wt_n=None):


# Path: test/tps_unit_tests.py
from rapprentice import testing, tps
    from jds_image_proc.clouds import voxel_downsample
    from numpy import sin,cos
    from rapprentice import registration
import numpy as np
    import numdifftools as ndt
    import numdifftools as ndt
    import fastrapp



@testing.testme
def nonrigidity_gradient():
    D = 3
    pts0 = np.random.randn(10,D)
    pts_eval = np.random.randn(3,D)
    def err_partial(params):
        lin_ag = params[0:9].reshape(3,3)

        trans_g = params[9:12]