def iterate(self,context):
(A, B, self.d_stats) = make_pairs(self.align_obj, self.base_obj, self.base_bvh, self.vlist, self.thresh, self.sample_factor, calc_stats = self.use_target)
if self.align_meth == '0': #rigid transform
M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True)
elif self.align_meth == '1': # rot, loc, scale
M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True)
new_mat = Matrix.Identity(4)
for y in range(0,4):
for z in range(0,4):
new_mat[y][z] = M[y][z]
self.align_obj.matrix_world = self.align_obj.matrix_world * new_mat
trans = new_mat.to_translation()
quat = new_mat.to_quaternion()
self.align_obj.update_tag()
if self.d_stats:
i = fmod(self.total_iters,5)
self.conv_t_list[i] = trans.length
self.conv_r_list[i] = abs(quat.angle)
if all(d < self.target_d for d in self.conv_t_list):
self.converged = True
print('Converged in %s iterations' % str(self.total_iters+1))
print('Final Translation: %f ' % self.conv_t_list[i])
print('Final Avg Dist: %f' % self.d_stats[0])
print('Final St Dev %f' % self.d_stats[1])
print('Avg last 5 rotation angle: %f' % np.mean(self.conv_r_list))
def iterate(self,context):
(A, B, self.d_stats) = make_pairs(self.align_obj, self.base_obj, self.base_bvh, self.vlist, self.thresh, self.sample_factor, calc_stats = self.use_target)
if self.align_meth == '0': #rigid transform
M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True)
elif self.align_meth == '1': # rot, loc, scale
M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True)
new_mat = Matrix.Identity(4)
for y in range(0,4):
for z in range(0,4):
new_mat[y][z] = M[y][z]
self.align_obj.matrix_world = self.align_obj.matrix_world * new_mat
trans = new_mat.to_translation()
quat = new_mat.to_quaternion()
self.align_obj.update_tag()
if self.d_stats:
i = fmod(self.total_iters,5)
self.conv_t_list[i] = trans.length
self.conv_r_list[i] = abs(quat.angle)
if all(d < self.target_d for d in self.conv_t_list):
self.converged = True
print('Converged in %s iterations' % str(self.total_iters+1))
print('Final Translation: %f ' % self.conv_t_list[i])
print('Final Avg Dist: %f' % self.d_stats[0])
print('Final St Dev %f' % self.d_stats[1])
print('Avg last 5 rotation angle: %f' % np.mean(self.conv_r_list))
def moment_arm(F, th, joint_name):
if joint_name=='KFE':
a = 0.0826
b = 0.1143
c = 0.0818
d = 0.0825
alpha = 4.468 - th
elif joint_name == 'HFE':
a = 0.0826
b = 0.1143
c = 0.0512
d = 0.0497
# alpha = 3.189 - th
alpha = 4.2369 - th
elif joint_name == 'HAA':
a = 0
b = 0
c = 0.0419
d = 0.1901
alpha = 1.7809 + th
e = sqrt(c**2 + d**2 - 2*c*d*cos( alpha))
beta = arccos( (e**2 + d**2 - c**2)/(2*e*d) )
if a != 0:
beta2 = arccos( (e**2 + a**2 - b**2)/(2*e*a) )
if(fmod(alpha, 2*pi) > pi):
beta = beta2 - beta
else:
beta = beta2 + beta
tau = F * d * sin(beta)
return tau
def execute(self, context):
settings = get_addon_preferences()
align_meth = settings.align_meth
start = time.time()
align_obj = context.object
base_obj = [
obj for obj in context.selected_objects if obj != align_obj
][0]
base_bvh = BVHTree.FromObject(base_obj, context.scene)
align_obj.rotation_mode = 'QUATERNION'
vlist = []
#figure out if we need to do any inclusion/exclusion
group_lookup = {g.name: g.index for g in align_obj.vertex_groups}
if 'icp_include' in align_obj.vertex_groups:
group = group_lookup['icp_include']
for v in align_obj.data.vertices:
for g in v.groups:
if g.group == group and g.weight > 0.9:
vlist.append(v.index)
elif 'icp_exclude' in align_obj.vertex_groups:
group = group_lookup['icp_exclude']
for v in align_obj.data.vertices:
v_groups = [g.group for g in v.groups]
if group not in v_groups:
vlist.append(v.index)
else:
for g in v.groups:
if g.group == group and g.weight < 0.1:
vlist.append(v.index)
#unfortunate way to do this..
else:
vlist = [v.index for v in align_obj.data.vertices]
settings = get_addon_preferences()
thresh = settings.min_start
sample = settings.sample_fraction
iters = settings.icp_iterations
target_d = settings.target_d
use_target = settings.use_target
factor = round(1 / sample)
n = 0
converged = False
conv_t_list = [target_d * 2] * 5 #store last 5 translations
conv_r_list = [None] * 5
while n < iters and not converged:
(A, B, d_stats) = make_pairs(align_obj,
base_obj,
base_bvh,
vlist,
thresh,
factor,
calc_stats=use_target)
if align_meth == '0': #rigid transform
M = affine_matrix_from_points(A,
B,
shear=False,
scale=False,
usesvd=True)
elif align_meth == '1': # rot, loc, scale
M = affine_matrix_from_points(A,
B,
shear=False,
scale=True,
usesvd=True)
new_mat = Matrix.Identity(4)
for y in range(0, 4):
for z in range(0, 4):
new_mat[y][z] = M[y][z]
align_obj.matrix_world = align_obj.matrix_world * new_mat
trans = new_mat.to_translation()
quat = new_mat.to_quaternion()
align_obj.update_tag()
context.scene.update()
if d_stats:
i = fmod(n, 5)
conv_t_list[i] = trans.length
conv_r_list[i] = abs(quat.angle)
if all(d < target_d for d in conv_t_list):
converged = True
print('Converged in %s iterations' % str(n + 1))
print('Final Translation: %f ' % conv_t_list[i])
print('Final Avg Dist: %f' % d_stats[0])
print('Final St Dev %f' % d_stats[1])
print('Avg last 5 rotation angle: %f' %
np.mean(conv_r_list))
n += 1
time_taken = time.time() - start
if use_target and not converged:
print('Maxed out iterations')
print('Final Translation: %f ' % conv_t_list[i])
print('Final Avg Dist: %f' % d_stats[0])
print('Final St Dev %f' % d_stats[1])
print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list))
print('Aligned obj in %f sec' % time_taken)
return {'FINISHED'}
def execute(self, context):
settings = get_settings()
align_meth = settings.align_meth
start = time.time()
align_obj = context.object
base_obj = [obj for obj in context.selected_objects if obj != align_obj][0]
base_bvh = BVHTree.FromObject(base_obj, context.scene)
align_obj.rotation_mode = 'QUATERNION'
vlist = []
#figure out if we need to do any inclusion/exclusion
group_lookup = {g.name: g.index for g in align_obj.vertex_groups}
if 'icp_include' in align_obj.vertex_groups:
group = group_lookup['icp_include']
for v in align_obj.data.vertices:
for g in v.groups:
if g.group == group and g.weight > 0.9:
vlist.append(v.index)
elif 'icp_exclude' in align_obj.vertex_groups:
group = group_lookup['icp_exclude']
for v in align_obj.data.vertices:
v_groups = [g.group for g in v.groups]
if group not in v_groups:
vlist.append(v.index)
else:
for g in v.groups:
if g.group == group and g.weight < 0.1:
vlist.append(v.index)
#unfortunate way to do this..
else:
vlist = [v.index for v in align_obj.data.vertices]
settings = get_settings()
thresh = settings.min_start
sample = settings.sample_fraction
iters = settings.icp_iterations
target_d = settings.target_d
use_target = settings.use_target
factor = round(1/sample)
n = 0
converged = False
conv_t_list = [target_d * 2] * 5 #store last 5 translations
conv_r_list = [None] * 5
while n < iters and not converged:
(A, B, d_stats) = make_pairs(align_obj, base_obj, base_bvh, vlist, thresh, factor, calc_stats = use_target)
if align_meth == '0': #rigid transform
M = affine_matrix_from_points(A, B, shear=False, scale=False, usesvd=True)
elif align_meth == '1': # rot, loc, scale
M = affine_matrix_from_points(A, B, shear=False, scale=True, usesvd=True)
new_mat = Matrix.Identity(4)
for y in range(0,4):
for z in range(0,4):
new_mat[y][z] = M[y][z]
align_obj.matrix_world = align_obj.matrix_world * new_mat
trans = new_mat.to_translation()
quat = new_mat.to_quaternion()
align_obj.update_tag()
context.scene.update()
if d_stats:
i = fmod(n,5)
conv_t_list[i] = trans.length
conv_r_list[i] = abs(quat.angle)
if all(d < target_d for d in conv_t_list):
converged = True
print('Converged in %s iterations' % str(n+1))
print('Final Translation: %f ' % conv_t_list[i])
print('Final Avg Dist: %f' % d_stats[0])
print('Final St Dev %f' % d_stats[1])
print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list))
n += 1
time_taken = time.time() - start
if use_target and not converged:
print('Maxed out iterations')
print('Final Translation: %f ' % conv_t_list[i])
print('Final Avg Dist: %f' % d_stats[0])
print('Final St Dev %f' % d_stats[1])
print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list))
print('Aligned obj in %f sec' % time_taken)
return {'FINISHED'}
def execute(self, context):
settings = get_settings()
ICP_sel = context.scene.ICP_area
align_meth = settings.align_meth
start = time.time()
align_obj = context.object
base_obj = [
obj for obj in context.selected_objects if obj != align_obj
][0]
# **********************choose select vertexgroup in baseobj to ICP***********************
# find polygons from vetex group
# get list of vertex in selected vertex groups
sel_poly = []
base_vlist = []
base_vlist_co = []
base_group_lookup = {g.name: g.index
for g in base_obj.vertex_groups
} #create a dictionary
base_group_vlist = []
if ICP_sel in base_obj.vertex_groups:
group = base_group_lookup[ICP_sel]
for v in base_obj.data.vertices:
for g in v.groups:
if g.group == group:
base_group_vlist.append(v.index)
#get list of polygon from selected vertex group
b = set(base_group_vlist)
for poly in base_obj.data.polygons:
a = set(poly.vertices[:])
anb = a & b
if anb:
verts_in_poly = poly.vertices[:]
sel_poly.append(poly.index) #selected polygons
base_vlist.append(
poly.vertices[:]) #selected vertex from polygons
#renumber polygons vertices
# combine all vertice in polygons list
sortlist = []
for poly in base_vlist:
for vert in poly:
sortlist.append(vert)
sortlist = sorted(
set(sortlist)) # remove duplicate and sort the list
# renumber base_vlist to index of found item in sortlist
# for poly in base_vlist:
# for i, vert in enumerate(poly):
# for j, no in enumerate(sortlist):
# if vert==no:
# # vert = j
# poly[i] = j --> error: 'tuple' object does not support item assignment
temp = [[vert for vert in poly] for poly in base_vlist
] #need to copy to another list to be able to modified
for poly in temp:
for i, vert in enumerate(poly):
for j, no in enumerate(sortlist):
if vert == no:
poly[i] = j
# Create list of vertex coordination from softlist
for vert in sortlist:
base_vlist_co.append(
base_obj.data.vertices[vert].co
) #coord of selected vertex in selected polygons
base_bvh = BVHTree.FromPolygons(
base_vlist_co, temp) #choose selected vertices in baseobj
else:
base_bvh = BVHTree.FromObject(
base_obj, context.scene) #choose whole vertices in baseobj
# **********************choose select vertexgroup in alignobj to ICP***********************
align_obj.rotation_mode = 'QUATERNION'
vlist = []
group_lookup = {g.name: g.index for g in align_obj.vertex_groups}
if ICP_sel in align_obj.vertex_groups:
group = group_lookup[ICP_sel]
for v in align_obj.data.vertices: #copy selected area vertex to list of ICP action
for g in v.groups:
if g.group == group:
vlist.append(v.index)
else: #choose the whole align objects vertex if not choose the area
vlist = [v.index for v in align_obj.data.vertices]
# ************************************************************************************
settings = get_settings()
thresh = settings.min_start
sample = settings.sample_fraction
iters = settings.icp_iterations
target_d = settings.target_d
use_target = settings.use_target
factor = round(1 / sample)
n = 0
converged = False
conv_t_list = [target_d * 2] * 5 #store last 5 translations
conv_r_list = [None] * 5
bematrixworld = align_obj.matrix_world.copy(
) #store the begining matrixworld
while n < iters and not converged:
(A, B, d_stats) = make_pairs(align_obj,
base_obj,
base_bvh,
vlist,
thresh,
factor,
calc_stats=use_target)
if align_meth == '0': #rigid transform
M = affine_matrix_from_points(A,
B,
shear=False,
scale=False,
usesvd=True)
# print (M)
elif align_meth == '1': # rot, loc, scale
M = affine_matrix_from_points(A,
B,
shear=False,
scale=True,
usesvd=True)
new_mat = Matrix.Identity(4)
for y in range(0, 4):
for z in range(0, 4):
new_mat[y][z] = M[y][z] #new_mat = affine transform matrix
align_obj.matrix_world = align_obj.matrix_world * new_mat #****This is for transformation every interations
trans = new_mat.to_translation()
quat = new_mat.to_quaternion()
align_obj.update_tag()
context.scene.update()
if d_stats:
i = fmod(n, 5) #returns the floating-point remainder of x/y
conv_t_list[
i] = trans.length #get the length of last 5 translation matrixs
conv_r_list[i] = abs(quat.angle)
if all(d < target_d for d in conv_t_list):
converged = True
print('---------Summary-----------------')
print('Converged in %s iterations' % str(n + 1))
print('Final Translation: %f ' % conv_t_list[i])
print('Final Avg Dist: %f' % d_stats[0])
print('Final St Dev %f' % d_stats[1])
print('Avg last 5 rotation angle: %f' %
np.mean(conv_r_list))
n += 1
time_taken = time.time() - start
if use_target and not converged:
print('Maxed out iterations')
print('Final Translation: %f ' % conv_t_list[i])
print('Final Avg Dist: %f' % d_stats[0])
print('Final St Dev %f' % d_stats[1])
print('Avg last 5 rotation angle: %f' % np.mean(conv_r_list))
print('Aligned obj in %f sec' % time_taken)
endmatrixworld = align_obj.matrix_world #store the last matrixworld
ICPmatrix = endmatrixworld * bematrixworld.inverted()
Initial_point = Vector([1.78237, 0.69558, 2.40553])
point_transformed = ICPmatrix * Initial_point
accessfile = bpy.path.abspath("//output.txt")
with open(accessfile, 'a') as f:
f.write('The begining world maxtrix:\n')
f.write(str(bematrixworld) + '\n')
f.write('The ended world matrix:\n')
f.write(str(endmatrixworld) + '\n')
f.write('The ICP transformation matrix is:\n')
f.write(str(ICPmatrix) + '\n')
f.write('The inverted ICP transformation matrix is:\n')
f.write(str(ICPmatrix.inverted()) + '\n')
f.write('lefteye vertex coord:\n')
f.write(str(Initial_point) + '\n')
f.write("transform to:\n")
f.write(str(point_transformed) + '\n')
f.write(
'---------------------*************------------------------\n')
return {'FINISHED'}
