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
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)
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]
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
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
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)
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
"""
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
# 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))
/ (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 + \
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
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
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
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