Exemplo n.º 1
0
    def newton_method(self,
                      xk1=None,
                      alpha=1,
                      epsilon=0.01,
                      file_name='newton_method.txt',
                      color='g'):
        if xk1 is None:
            xk1 = np.zeros(2)

        double_derivation_matrix = self.double_derivation_function(xk1)
        vector_h1 = npl.tensorsolve(double_derivation_matrix,
                                    -self.gradient(xk1))

        xk_vector = xk1 + vector_h1
        count = 1

        vector_x = [xk1, xk_vector]
        const_func_xk_1 = self.func(xk_vector)

        condition_const = np.abs(const_func_xk_1 - self.func(xk1) - epsilon *
                                 alpha * np.dot(self.gradient(xk1), vector_h1))

        create_file(file_name)
        self.string_add_in_file(count, xk_vector, vector_h1, alpha, file_name)

        while condition_const > self.h:
            alpha *= 0.5

            double_derivation_matrix = self.double_derivation_function(
                xk_vector)
            vector_h1 = npl.tensorsolve(double_derivation_matrix,
                                        -self.gradient(xk_vector))

            xk_vector = xk_vector + vector_h1 * alpha

            const_func_xk_1 = self.func(xk_vector + vector_h1 * alpha)

            condition_const = np.abs(
                const_func_xk_1 - self.func(xk_vector) -
                epsilon * alpha * np.dot(self.gradient(xk_vector), vector_h1))
            count += 1
            vector_x.append(xk_vector)

            print(self.print_result(count, xk_vector, alpha))

            self.string_add_in_file(count, xk_vector, vector_h1, alpha,
                                    file_name)

        # create arrows
        create_arrows(vector_x, color=color)

        return xk_vector
Exemplo n.º 2
0
    def __find_decomposition(self, Q_Tens, h_Tens):
        # contract the vector field terms to extract the coupling matrix
        # in my notes LNM is lnm with bar
        # in the notes this is the M - matrix
        div_coup_Tens = np.einsum('LNMs,lnms->LNMlnm', np.conj( self.jacTens * self.psiTens ), Q_Tens)
        div_sol_Tens = np.einsum('LNMs,s->LNM', np.conj( self.jacTens * self.psiTens ), h_Tens)

        # in the notes this is q
        return npla.tensorsolve(div_coup_Tens, div_sol_Tens)
Exemplo n.º 3
0
def derham3dweights(jac, invjac=None, dets=None):
    from numpy.linalg import tensorsolve, det
    from numpy import ones, transpose
    if invjac is None: invjac = lambda p : tensorsolve(jac(p), numpy.tile(numpy.eye(3), (len(p),1,1))) 
    if dets is None: dets = lambda p : map(det, jac(p))
    w0 = lambda p: ones(len(p))
    w1 = lambda p: transpose(jac(p), (0,2,1))
    w2 = lambda p: invjac(p) * dets(p).reshape(-1,1,1)
    w3 = dets
    return [w0,w1,w2,w3]    
Exemplo n.º 4
0
 def get_A(N, q, Q, f, F, y, A):
     # fill the elements of y:
     for rr in range(N):
         y[2*rr] = Q[rr] - np.sum(A[2:,rr]*q[2:])
         y[2*rr + 1] = F[rr] - np.sum(A[2:,rr]*f[2:])
     # and find the solution
     x = la.tensorsolve(C,y)
     # iCC = la.inv(CC); xalt = iCC.dot(y) # is the same as tensorsolve
     # fill in the A matrix with the correct values
     if N > 2:
         A[:2,:] = x.reshape((2,N), order='F')
     else:
         A[:] = x.reshape((N,N), order='F')
     return A
Exemplo n.º 5
0
def Newton_Iteration(Df, Hf, X0):
    fx = Df(X0)
    print(0, X0, fx)
    X = X0
    nmax = 500  # Set max iteration so it does not go on forever
    ## Newton iteration where X = X - Hf(X)^(-1)*Df(X)
    for n in range(nmax):
        Dx = Df(X)
        Xn = nl.tensorsolve(Hf(X), Dx)
        X = X - Xn
        print(n, X, Dx)
        if vectorAbs(Xn) < 10**(-14):
            print("Convergence")
            return X
    return X
Exemplo n.º 6
0
def test1():
    jacobian = solver.generate_jacobian(start_vals)

    xs = jacobian.keys()
    zs = jacobian[xs[0]].keys()

    start_results = fn.calculate(start_vals)
    errors = dict([(z, (targets[z] - start_results[z])) for z in targets])
    print errors

    print '\t' + '\t'.join(xs)
    for z in zs:
        row = [jacobian[x][z] for x in xs]
        print z + '\t' + '\t'.join(map(str, row))

    # I originally got the rows an cols the wrong way round - eurgh, tired.
    jacobian_matrix = [[jacobian[x][z] for x in xs] for z in zs]
    #print jacobian_matrix

    J = np.array([[4.0, 3.0], [-2.0, 5.0]])

    J = np.array(jacobian_matrix)

    print J
    #print J.shape

    e = np.array([errors[z] for z in zs])
    print e

    #e = np.array([[-11.0],
    #             [-2.0]])

    #e = np.array([-11.,-2.0])
    #print e
    #print e.shape

    result = tensorsolve(a=J, b=e)
    print result

    next_vals = dict([(x, (start_vals[x] + result[i]))
                      for i, x in enumerate(xs)])
    print next_vals
    #    new_values = {'x': start_vals['x']-1.88461538, 'y':start_vals['y']-1.15384615}
    print fn.calculate(next_vals)
Exemplo n.º 7
0
    def next_iteration(self, current_values, errors):
        gradients = self.generate_jacobian(current_values)
        print 'GRADIENTS'
        print gradients
        xs = gradients.keys()
        zs = gradients[xs[0]].keys()

        jacobian_matrix = [[gradients[x][z] for x in xs] for z in zs]

        J = np.array(jacobian_matrix)
        e = np.array([errors[z] for z in zs])

        #        print 'jacobian\n',J
        #        print 'errors\n',e

        result = tensorsolve(a=J, b=e)

        #        print 'corrections\n', result
        #convert back from matrix to dict

        corrections = dict([(x, result[i]) for i, x in enumerate(xs)])

        return corrections
Exemplo n.º 8
0
"""
import numpy as np
import numpy.linalg as lin

matrix_A = np.array([[4, -2, 1], [3, 6, -4], [2, 1, 8]])
inverse_A = lin.inv(matrix_A)
print(np.array([[52, 17, 2], [-32, 30, 19], [-9, 8, 30]]) / 263)
print('---------')
print(inverse_A)
print('---------')
identity = np.dot(matrix_A, inverse_A)
print(identity)
print('-------')

b1 = np.array([12, -25, 32])
solution_1 = lin.tensorsolve(matrix_A, b1)
b2 = np.array([4, -10, 22])
solution_2 = lin.tensorsolve(matrix_A, b2)
b3 = np.array([20, -30, 32])
solution_3 = lin.tensorsolve(matrix_A, b3)

print(solution_1)
print('-------')
print(solution_2)
print('--------')
print(solution_3)
print('-------')

#eigen value problem
alpha = 2
beta = -1
Exemplo n.º 9
0
# Written by Vassili Savinov on 05/02/2019
# getting familiar with tensorsolve

import numpy as np
import numpy.linalg as npla
import numpy.random as npr

# I will be primarily interested in rank three tensors, and rank 6 mixing tensors

inTens = 2 * npr.rand(2, 20, 35) - 1

# mixing tensor
mixTens = 2 * npr.rand(*(*inTens.shape, *inTens.shape)) - 1

# constract
outTens = np.einsum('ijkstu,stu', mixTens, inTens)

print(outTens)

# now invert
solTens = npla.tensorsolve(mixTens, outTens)

# compare
diff = np.max(np.abs((inTens - solTens) / (inTens + 1e-12)))

print('max difference is %.3e %%' % (100 * diff))
Exemplo n.º 10
0
        / (xf(i + 1) - xf(i - 1)) / (yf(j + 2) - yf(j)),
        (d(i + 2, j + 2) - d(i + 2, j + 0) - d(i + 0, j + 2) + d(i + 0, j + 0))
        / (xf(i + 2) - xf(i)) / (yf(j + 2) - yf(j)),
    ],
                    dtype=np.float64)


for i in range(9):
    for j in range(9):
        l = i + 9 * j
        const_vec[16 * l:16 * (l + 1)] = elt_vec(i, j)

# inv_const_mat = inv(const_mat)
# coeffs = inv_const_mat.dot(const_vec)

coeffs = tensorsolve(const_mat, const_vec)


def P(i, j):
    if 0 <= i <= 8 and 0 <= j <= 8:
        l = 16 * (i + 9 * j)
        return lambda a, b: coeffs[l+0 ]*a*a*a*b*b*b + \
                            coeffs[l+1 ]*a*a*a*b*b + \
                            coeffs[l+2 ]*a*a*a*b + \
                            coeffs[l+3 ]*a*a*a + \
                            coeffs[l+4 ]*a*a*b*b*b + \
                            coeffs[l+5 ]*a*a*b*b + \
                            coeffs[l+6 ]*a*a*b + \
                            coeffs[l+7 ]*a*a + \
                            coeffs[l+8 ]*a*b*b*b + \
                            coeffs[l+9 ]*a*b*b + \
Exemplo n.º 11
0
    def RegisterRGBD_optimize(self, Image1, Image2):
        '''
        Optimize version of  RegisterRGBD
        :param Image1: First RGBD images
        :param Image2:  Second RGBD images
        :return: Transform matrix between Image1 and Image2
        '''
        res = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.],
                        [0., 0., 0., 1.]],
                       dtype=np.float32)

        column_index_ref = np.array(
            [np.array(range(Image1.Size[1])) for _ in range(Image1.Size[0])])
        line_index_ref = np.array([
            x * np.ones(Image1.Size[1], np.int) for x in range(Image1.Size[0])
        ])
        Indexes_ref = column_index_ref + Image1.Size[1] * line_index_ref

        for l in range(1, self.lvl + 1):
            for it in range(self.max_iter[l - 1]):
                #nbMatches = 0
                #row = np.array([0.,0.,0.,0.,0.,0.,0.])
                #Mat = np.zeros(27, np.float32)
                b = np.zeros(6, np.float32)
                A = np.zeros((6, 6), np.float32)

                # For each pixel find correspondinng point by projection
                Buffer = np.zeros((Image1.Size[0] * Image1.Size[1], 6),
                                  dtype=np.float32)
                Buffer_B = np.zeros((Image1.Size[0] * Image1.Size[1], 1),
                                    dtype=np.float32)
                stack_pix = np.ones((Image1.Size[0], Image1.Size[1]),
                                    dtype=np.float32)
                stack_pt = np.ones(
                    (np.size(Image1.Vtx[::l, ::l, :],
                             0), np.size(Image1.Vtx[::l, ::l, :], 1)),
                    dtype=np.float32)
                pix = np.zeros((Image1.Size[0], Image1.Size[1], 2),
                               dtype=np.float32)
                pix = np.dstack((pix, stack_pix))
                pt = np.dstack((Image1.Vtx[::l, ::l, :], stack_pt))
                pt = np.dot(res, pt.transpose(0, 2, 1)).transpose(1, 2, 0)

                # transform normales
                nmle = np.zeros(
                    (Image1.Size[0], Image1.Size[1], Image1.Size[2]),
                    dtype=np.float32)
                nmle[::l, ::l, :] = np.dot(
                    res[0:3, 0:3],
                    Image1.Nmls[::l, ::l, :].transpose(0, 2,
                                                       1)).transpose(1, 2, 0)

                #Project in 2d space
                #if (pt[2] != 0.0):
                lpt = np.dsplit(pt, 4)
                lpt[2] = General.in_mat_zero2one(lpt[2])
                # if in 1D pix[0] = pt[0]/pt[2]
                pix[::l, ::l, 0] = (lpt[0] / lpt[2]).reshape(
                    np.size(Image1.Vtx[::l, ::l, :], 0),
                    np.size(Image1.Vtx[::l, ::l, :], 1))
                # if in 1D pix[1] = pt[1]/pt[2]
                pix[::l, ::l, 1] = (lpt[1] / lpt[2]).reshape(
                    np.size(Image1.Vtx[::l, ::l, :], 0),
                    np.size(Image1.Vtx[::l, ::l, :], 1))
                pix = np.dot(
                    Image1.intrinsic,
                    pix[0:Image1.Size[0],
                        0:Image1.Size[1]].transpose(0, 2,
                                                    1)).transpose(1, 2, 0)

                #checking values are in the frame
                column_index = (np.round(pix[:, :, 0])).astype(int)
                line_index = (np.round(pix[:, :, 1])).astype(int)
                # create matrix that have 0 when the conditions are not verified and 1 otherwise
                cdt_column = (column_index > -1) * (column_index <
                                                    Image2.Size[1])
                cdt_line = (line_index > -1) * (line_index < Image2.Size[0])
                line_index = line_index * cdt_line
                column_index = column_index * cdt_column

                # Compute distance betwn matches and btwn normals
                diff_Vtx = Image2.Vtx[line_index[:][:],
                                      column_index[:][:]] - pt[:, :, 0:3]
                diff_Vtx = diff_Vtx * diff_Vtx
                norm_diff_Vtx = diff_Vtx.sum(axis=2)
                mask_vtx = (norm_diff_Vtx < self.thresh_dist)
                print "mask_vtx"
                print sum(sum(mask_vtx))

                diff_Nmle = Image2.Nmls[line_index[:][:],
                                        column_index[:][:]] - nmle
                diff_Nmle = diff_Nmle * diff_Nmle
                norm_diff_Nmle = diff_Nmle.sum(axis=2)
                mask_nmls = (norm_diff_Nmle < self.thresh_norm)
                print "mask_nmls"
                print sum(sum(mask_nmls))

                Norme_Nmle = nmle * nmle
                norm_Norme_Nmle = Norme_Nmle.sum(axis=2)

                mask_pt = (pt[:, :, 2] > 0.0)

                # Display
                # print "mask_pt"
                # print sum(sum(mask_pt)  )
                #
                # print "cdt_column"
                # print sum(sum( (cdt_column==0))  )
                #
                # print "cdt_line"
                # print sum(sum( (cdt_line==0)) )

                # mask for considering only good value in the linear system
                mask = cdt_line * cdt_column * (pt[:, :, 2] > 0.0) * (
                    norm_Norme_Nmle > 0.0) * (
                        norm_diff_Vtx < self.thresh_dist) * (norm_diff_Nmle <
                                                             self.thresh_norm)
                print "final correspondence"
                print sum(sum(mask))

                # partial derivate
                w = 1.0
                Buffer[Indexes_ref[:][:]] = np.dstack((w*mask[:,:]*nmle[ :, :,0], \
                      w*mask[:,:]*nmle[ :, :,1], \
                      w*mask[:,:]*nmle[ :, :,2], \
                      w*mask[:,:]*(-Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,2]*nmle[:,:,1] + Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,1]*nmle[:,:,2]), \
                      w*mask[:,:]*(Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,2]*nmle[:,:,0] - Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,0]*nmle[:,:,2]), \
                      w*mask[:,:]*(-Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,1]*nmle[:,:,0] + Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,0]*nmle[:,:,1]) ))
                #residual
                Buffer_B[Indexes_ref[:][:]] = np.dstack(w*mask[:,:]*(nmle[:,:,0]*(Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,0] - pt[:,:,0])\
                                                                    + nmle[:,:,1]*(Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,1] - pt[:,:,1])\
                                                                    + nmle[:,:,2]*(Image2.Vtx[line_index[:][:], column_index[:][:]][:,:,2] - pt[:,:,2])) ).transpose()

                # Solving sum(A.t * A) = sum(A.t * b) ref newcombe kinect fusion
                # fisrt part of the linear equation
                A = np.dot(Buffer.transpose(), Buffer)
                #second part of the linear equation
                b = np.dot(Buffer.transpose(), Buffer_B).reshape(6)

                sign, logdet = LA.slogdet(A)
                det = sign * np.exp(logdet)
                if (det == 0.0):
                    print "determinant null"
                    print det
                    warnings.warn("this is a warning message")
                    break

                # solve equation
                delta_qsi = -LA.tensorsolve(A, b)
                # compute 4*4 matrix
                delta_transfo = Exponential(delta_qsi)
                delta_transfo = General.InvPose(delta_transfo)
                res = np.dot(delta_transfo, res)
                print "delta_transfo"
                print delta_transfo
                print "res"
                print res
        return res
def step_forward(velDen, posQuad, negQuad):
    #matrix multiply the negative quad by the FFT column vector
    RHS = np.dot(negQuad, velDen)
    #linear algebra solve the pos_quad*newFFTvector = above-result for newFFTvector
    solution = LA.tensorsolve(posQuad, RHS)
    return solution
Exemplo n.º 13
0
    def RegisterRGBDMesh_optimize(self, NewImage, MeshVtx, MeshNmls, Pose):
        '''
        Optimize version with CPU  of RegisterRGBDMesh
        :param NewImage: RGBD image
        :param MeshVtx: list of vertices of the mesh
        :param MeshNmls: list of normales of the mesh
        :param Pose:  estimate of the pose of the current image
        :return: Transform matrix between Image1 and the mesh (transform from the first frame to the current frame)
        '''

        # Initializing the res with the current Pose so that mesh that are in a local coordinates can be
        # transform in the current frame and thus enabling ICP.
        Size = MeshVtx.shape
        res = Pose

        for l in range(1, self.lvl + 1):
            for it in range(self.max_iter[l - 1]):

                # residual matrix
                b = np.zeros(6, np.float32)
                # Jacobian matrix
                A = np.zeros((6, 6), np.float32)

                # For each pixel find correspondinng point by projection
                Buffer = np.zeros((Size[0], 6), dtype=np.float32)
                Buffer_B = np.zeros((Size[0], 1), dtype=np.float32)
                stack_pix = np.ones(Size[0], dtype=np.float32)
                stack_pt = np.ones(np.size(MeshVtx[::l, :], 0),
                                   dtype=np.float32)
                pix = np.zeros((Size[0], 2), dtype=np.float32)
                pix = np.stack((pix[:, 0], pix[:, 1], stack_pix), axis=1)
                pt = np.stack(
                    (MeshVtx[::l, 0], MeshVtx[::l, 1], MeshVtx[::l,
                                                               2], stack_pt),
                    axis=1)

                # transform closer vertices to camera pose
                pt = np.dot(res, pt.T).T
                # transform closer normales to camera pose
                nmle = np.zeros((Size[0], Size[1]), dtype=np.float32)
                nmle[::l, :] = np.dot(res[0:3, 0:3], MeshNmls[::l, :].T).T

                # Projection in 2D space
                lpt = np.split(pt, 4, axis=1)
                lpt[2] = General.in_mat_zero2one(lpt[2])

                # if in 1D pix[0] = pt[0]/pt[2]
                pix[::l,
                    0] = (lpt[0] / lpt[2]).reshape(np.size(MeshVtx[::l, :], 0))
                # if in 1D pix[1] = pt[1]/pt[2]
                pix[::l,
                    1] = (lpt[1] / lpt[2]).reshape(np.size(MeshVtx[::l, :], 0))
                pix = np.dot(NewImage.intrinsic, pix[0:Size[0], 0:Size[1]].T).T
                column_index = (np.round(pix[:, 0])).astype(int)
                line_index = (np.round(pix[:, 1])).astype(int)

                # create matrix that have 0 when the conditions are not verified and 1 otherwise
                cdt_column = (column_index > -1) * (column_index <
                                                    NewImage.Size[1])
                cdt_line = (line_index > -1) * (line_index < NewImage.Size[0])
                line_index = line_index * cdt_line
                column_index = column_index * cdt_column

                # compute vtx and nmls differences
                diff_Vtx = NewImage.Vtx[line_index[:],
                                        column_index[:]] - pt[:, 0:3]
                diff_Vtx = diff_Vtx * diff_Vtx
                norm_diff_Vtx = diff_Vtx.sum(axis=1)
                mask_vtx = (norm_diff_Vtx < self.thresh_dist)
                # print "mask_vtx"
                # print sum(mask_vtx)
                # print "norm_diff_Vtx : max, min , median"
                # print "max : %f; min : %f; median : %f; var :  %f " % (np.max(norm_diff_Vtx),np.min(norm_diff_Vtx) ,np.median(norm_diff_Vtx),np.var(norm_diff_Vtx) )

                diff_Nmle = NewImage.Nmls[line_index[:],
                                          column_index[:]] - nmle
                diff_Nmle = diff_Nmle * diff_Nmle
                norm_diff_Nmle = diff_Nmle.sum(axis=1)
                # print "norm_diff_Nmle : max, min , median"
                # print "max : %f; min : %f; median : %f; var :  %f " % (np.max(norm_diff_Nmle),np.min(norm_diff_Nmle) ,np.median(norm_diff_Nmle),np.var(norm_diff_Nmle) )

                mask_nmls = (norm_diff_Nmle < self.thresh_norm)
                # print "mask_nmls"
                # print sum(mask_nmls)

                Norme_Nmle = nmle * nmle
                norm_Norme_Nmle = Norme_Nmle.sum(axis=1)

                mask_pt = (pt[:, 2] > 0.0)
                # print "mask_pt"
                # print sum(mask_pt)

                #checking mask
                mask = cdt_line * cdt_column * mask_pt * (
                    norm_Norme_Nmle > 0.0) * mask_vtx * mask_nmls
                print "final correspondence"
                print sum(mask)

                # partial derivate
                w = 1.0
                Buffer[:] = np.stack((w*mask[:]*nmle[ :,0], \
                      w*mask[:]*nmle[ :, 1], \
                      w*mask[:]*nmle[ :, 2], \
                      w*mask[:]*(NewImage.Vtx[line_index[:], column_index[:]][:,1]*nmle[:,2] - NewImage.Vtx[line_index[:], column_index[:]][:,2]*nmle[:,1]), \
                      w*mask[:]*(- NewImage.Vtx[line_index[:], column_index[:]][:,0]*nmle[:,2] + NewImage.Vtx[line_index[:], column_index[:]][:,2]*nmle[:,0] ), \
                      w*mask[:]*(NewImage.Vtx[line_index[:], column_index[:]][:,0]*nmle[:,1] - NewImage.Vtx[line_index[:], column_index[:]][:,1]*nmle[:,0]) ) , axis = 1)
                # residual
                Buffer_B[:] = ((w*mask[:]*(nmle[:,0]*(NewImage.Vtx[line_index[:], column_index[:]][:,0] - pt[:,0])\
                                                      + nmle[:,1]*(NewImage.Vtx[line_index[:], column_index[:]][:,1] - pt[:,1])\
                                                      + nmle[:,2]*(NewImage.Vtx[line_index[:], column_index[:]][:,2] - pt[:,2])) ).transpose()).reshape(Buffer_B[:].shape)

                # Solving sum(A.t * A) = sum(A.t * b) ref newcombe kinect fusion
                # fisrt part of the linear equation
                A = np.dot(Buffer.transpose(), Buffer)
                b = np.dot(Buffer.transpose(), Buffer_B).reshape(6)

                sign, logdet = LA.slogdet(A)
                det = sign * np.exp(logdet)
                if (det == 0.0):
                    print "determinant null"
                    print det
                    warnings.warn("this is a warning message")
                    break

                # solve equation
                delta_qsi = -LA.tensorsolve(A, b)
                # compute 4*4 matrix
                delta_transfo = General.InvPose(Exponential(delta_qsi))

                res = np.dot(delta_transfo, res)
                print "delta_transfo"
                print delta_transfo
                print "res"
                print res

        return res
Exemplo n.º 14
0
    def RegisterRGBDMesh(self, NewImage, MeshVtx, MeshNmls, Pose):
        '''
        Function that estimate the relative rigid transformation between an input RGB-D images and a mesh
        :param NewImage: RGBD image
        :param MeshVtx: list of vertices of the mesh
        :param MeshNmls: list of normales of the mesh
        :param Pose:  estimate of the pose of the current image
        :return: Transform matrix between Image1 and the mesh (transform from the first frame to the current frame)
        '''
        res = Pose

        line_index = 0
        column_index = 0
        pix = np.array([0., 0., 1.])

        pt = np.array([0., 0., 0., 1.])
        nmle = np.array([0., 0., 0.])
        for l in range(1, self.lvl + 1):
            for it in range(self.max_iter[l - 1]):
                nbMatches = 0
                row = np.array([0., 0., 0., 0., 0., 0., 0.])
                Mat = np.zeros(27, np.float32)
                b = np.zeros(6, np.float32)
                A = np.zeros((6, 6), np.float32)

                # For each pixel find correspondinng point by projection
                for i in range(
                        MeshVtx.shape[0]):  # line index (i.e. vertical y axis)
                    # Transform current 3D position and normal with current transformation
                    pt[0:3] = MeshVtx[i][:]
                    if (LA.norm(pt[0:3]) < 0.1):
                        continue
                    pt = np.dot(res, pt)
                    nmle[0:3] = MeshNmls[i][0:3]
                    if (LA.norm(nmle) == 0.):
                        continue
                    nmle = np.dot(res[0:3, 0:3], nmle)

                    # Project onto Image2
                    pix[0] = pt[0] / pt[2]
                    pix[1] = pt[1] / pt[2]
                    pix = np.dot(NewImage.intrinsic, pix)
                    column_index = int(round(pix[0]))
                    line_index = int(round(pix[1]))

                    if (column_index < 0 or column_index > NewImage.Size[1] - 1
                            or line_index < 0
                            or line_index > NewImage.Size[0] - 1):
                        continue

                    # Compute distance betwn matches and btwn normals
                    match_vtx = NewImage.Vtx[line_index, column_index]

                    distance = LA.norm(pt[0:3] - match_vtx)
                    if (distance > self.thresh_dist):
                        # print "no Vtx correspondance"
                        # print distance
                        continue
                    match_nmle = NewImage.Nmls[line_index, column_index]

                    distance = LA.norm(nmle - match_nmle)
                    if (distance > self.thresh_norm):

                        # print "no Nmls correspondance"
                        # print distance

                        continue

                    w = 1.0
                    # partial derivate
                    row[0] = w * nmle[0]
                    row[1] = w * nmle[1]
                    row[2] = w * nmle[2]
                    row[3] = w * (-match_vtx[2] * nmle[1] +
                                  match_vtx[1] * nmle[2])
                    row[4] = w * (match_vtx[2] * nmle[0] -
                                  match_vtx[0] * nmle[2])
                    row[5] = w * (-match_vtx[1] * nmle[0] +
                                  match_vtx[0] * nmle[1])
                    # residual
                    row[6] = w*( nmle[0]*(match_vtx[0] - pt[0])\
                               + nmle[1]*(match_vtx[1] - pt[1])\
                               + nmle[2]*(match_vtx[2] - pt[2]))

                    nbMatches += 1
                    # upper part triangular matrix computation
                    shift = 0
                    for k in range(6):
                        for k2 in range(k, 7):
                            Mat[shift] = Mat[shift] + row[k] * row[k2]
                            shift += 1

                print("nbMatches: ", nbMatches)

                # fill up the matrix A.transpose * A and A.transpose * b (A jacobian matrix)
                shift = 0
                for k in range(6):
                    for k2 in range(k, 7):
                        val = Mat[shift]
                        shift += 1
                        if (k2 == 6):
                            b[k] = val
                        else:
                            A[k, k2] = A[k2, k] = val

                det = LA.det(A)
                if (det < 1.0e-10):
                    print "determinant null"
                    break

                #solve linear equation
                delta_qsi = -LA.tensorsolve(A, b)
                #compute 4*4 matrix
                delta_transfo = General.InvPose(Exponential(delta_qsi))

                res = np.dot(delta_transfo, res)

                print res
        return res
def step_forward(velDen,posQuad,negQuad):
    #matrix multiply the negative quad by the FFT column vector
    RHS = np.dot(negQuad,velDen)
    #linear algebra solve the pos_quad*newFFTvector = above-result for newFFTvector
    solution = LA.tensorsolve(posQuad,RHS)
    return solution
def step_forward(thermal,posCrank,negCrank):
    #matrix multiply the negative quad by the FFT column vector
    RHS = np.dot(negCrank,thermal)
    #linear algebra solve the pos_quad*newFFTvector = above-result for newFFTvector
    newThermal = LA.tensorsolve(posCrank, RHS)
    return newThermal
Exemplo n.º 17
0
    def RegisterRGBD(self, Image1, Image2):
        '''
        Function that estimate the relative rigid transformation between two input RGB-D images
        :param Image1: First RGBD images
        :param Image2:  Second RGBD images
        :return: Transform matrix between Image1 and Image2
        '''

        # Init
        res = np.identity(4)
        pix = np.array([0., 0., 1.])
        pt = np.array([0., 0., 0., 1.])
        nmle = np.array([0., 0., 0.])

        for l in range(1, self.lvl + 1):
            for it in range(self.max_iter[l - 1]):
                nbMatches = 0
                row = np.array([0., 0., 0., 0., 0., 0., 0.])
                Mat = np.zeros(27, np.float32)
                b = np.zeros(6, np.float32)
                A = np.zeros((6, 6), np.float32)

                # For each pixel find correspondinng point by projection
                for i in range(Image1.Size[0] /
                               l):  # line index (i.e. vertical y axis)
                    for j in range(Image1.Size[1] / l):
                        # Transform current 3D position and normal with current transformation
                        pt[0:3] = Image1.Vtx[i * l, j * l][:]
                        if (LA.norm(pt[0:3]) < 0.1):
                            continue
                        pt = np.dot(res, pt)
                        nmle[0:3] = Image1.Nmls[i * l, j * l][0:3]
                        if (LA.norm(nmle) == 0.):
                            continue
                        nmle = np.dot(res[0:3, 0:3], nmle)

                        # Project onto Image2
                        pix[0] = pt[0] / pt[2]
                        pix[1] = pt[1] / pt[2]
                        pix = np.dot(Image2.intrinsic, pix)
                        column_index = int(round(pix[0]))
                        line_index = int(round(pix[1]))

                        if (column_index < 0
                                or column_index > Image2.Size[1] - 1
                                or line_index < 0
                                or line_index > Image2.Size[0] - 1):
                            continue

                        # Compute distance betwn matches and btwn normals
                        match_vtx = Image2.Vtx[line_index, column_index]
                        distance = LA.norm(pt[0:3] - match_vtx)
                        print "[line,column] : [%d , %d] " % (line_index,
                                                              column_index)
                        print "match_vtx"
                        print match_vtx
                        print pt[0:3]
                        if (distance > self.thresh_dist):
                            continue

                        match_nmle = Image2.Nmls[line_index, column_index]
                        distance = LA.norm(nmle - match_nmle)
                        print "match_nmle"
                        print match_nmle
                        print nmle
                        if (distance > self.thresh_norm):
                            continue

                        w = 1.0
                        # Compute partial derivate for each point
                        row[0] = w * nmle[0]
                        row[1] = w * nmle[1]
                        row[2] = w * nmle[2]
                        row[3] = w * (-match_vtx[2] * nmle[1] +
                                      match_vtx[1] * nmle[2])
                        row[4] = w * (match_vtx[2] * nmle[0] -
                                      match_vtx[0] * nmle[2])
                        row[5] = w * (-match_vtx[1] * nmle[0] +
                                      match_vtx[0] * nmle[1])
                        #current residual
                        row[6] = w * (nmle[0] *
                                      (match_vtx[0] - pt[0]) + nmle[1] *
                                      (match_vtx[1] - pt[1]) + nmle[2] *
                                      (match_vtx[2] - pt[2]))

                        nbMatches += 1

                        # upper part triangular matrix computation
                        shift = 0
                        for k in range(6):
                            for k2 in range(k, 7):
                                Mat[shift] = Mat[shift] + row[k] * row[k2]
                                shift += 1

                print("nbMatches: ", nbMatches)

                # fill up the matrix A.transpose * A and A.transpose * b (A jacobian matrix)
                shift = 0
                for k in range(6):
                    for k2 in range(k, 7):
                        val = Mat[shift]
                        shift += 1
                        if (k2 == 6):
                            b[k] = val
                        else:
                            A[k, k2] = A[k2, k] = val

                det = LA.det(A)
                if (det < 1.0e-10):
                    print "determinant null"
                    break

                #resolve system
                delta_qsi = -LA.tensorsolve(A, b)
                # compute the 4*4 matrix transform
                delta_transfo = LA.inv(Exponential(delta_qsi))

                # update result
                res = np.dot(delta_transfo, res)

                print res
        return res