Skip to content

Commit

Permalink
Merge pull request yfeng95#73 from MaxBazik/removed_skew
Browse files Browse the repository at this point in the history
Compute transformation with no skew
  • Loading branch information
yfeng95 authored Oct 9, 2018
2 parents a3832c1 + 9e8075f commit bf4fd18
Showing 1 changed file with 27 additions and 4 deletions.
31 changes: 27 additions & 4 deletions utils/estimate_pose.py
Original file line number Diff line number Diff line change
Expand Up @@ -61,12 +61,35 @@ def P2sRt(P):
return s, R, t2d


def compute_similarity_transform(points_static, points_to_transform):
#http://nghiaho.com/?page_id=671
p0 = np.copy(points_static).T
p1 = np.copy(points_to_transform).T

t0 = -np.mean(p0, axis=1).reshape(3,1)
t1 = -np.mean(p1, axis=1).reshape(3,1)
t_final = t1 -t0

p0c = p0+t0
p1c = p1+t1

covariance_matrix = p0c.dot(p1c.T)
U,S,V = np.linalg.svd(covariance_matrix)
R = U.dot(V)
if np.linalg.det(R) < 0:
R[:,2] *= -1

rms_d0 = np.sqrt(np.mean(np.linalg.norm(p0c, axis=0)**2))
rms_d1 = np.sqrt(np.mean(np.linalg.norm(p1c, axis=0)**2))

s = (rms_d0/rms_d1)
P = np.c_[s*np.eye(3).dot(R), t_final]
return P

def estimate_pose(vertices):
canonical_vertices = np.load('Data/uv-data/canonical_vertices.npy')

canonical_vertices_homo = np.hstack((canonical_vertices, np.ones([canonical_vertices.shape[0],1]))) #n x 4
P = np.linalg.lstsq(canonical_vertices_homo, vertices)[0].T # Affine matrix. 3 x 4
P = compute_similarity_transform(vertices, canonical_vertices)
_,R,_ = P2sRt(P) # decompose affine matrix to s, R, t
pose = matrix2angle(R)

return P, pose
return P, pose

0 comments on commit bf4fd18

Please sign in to comment.