From 2586282a2e58cc9e0c92924948770885ef2f4a97 Mon Sep 17 00:00:00 2001
From: Max Bazik <max@levin.vsi-ri.com>
Date: Thu, 4 Oct 2018 11:16:20 -0400
Subject: [PATCH 1/2] Computing transformation without skew.

---
 utils/estimate_pose.py | 29 ++++++++++++++++++++++++++---
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/utils/estimate_pose.py b/utils/estimate_pose.py
index a7b8d2f..7824675 100644
--- a/utils/estimate_pose.py
+++ b/utils/estimate_pose.py
@@ -61,11 +61,34 @@ 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(coveriance_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) 
 

From 9e8075f4e02b3dd13a2fd2a29e69353a796445ea Mon Sep 17 00:00:00 2001
From: Max Bazik <max@levin.vsi-ri.com>
Date: Thu, 4 Oct 2018 13:14:02 -0400
Subject: [PATCH 2/2] Typo

---
 utils/estimate_pose.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/utils/estimate_pose.py b/utils/estimate_pose.py
index 7824675..708dc89 100644
--- a/utils/estimate_pose.py
+++ b/utils/estimate_pose.py
@@ -74,7 +74,7 @@ def compute_similarity_transform(points_static, points_to_transform):
     p1c = p1+t1
 
     covariance_matrix = p0c.dot(p1c.T)
-    U,S,V = np.linalg.svd(coveriance_matrix)
+    U,S,V = np.linalg.svd(covariance_matrix)
     R = U.dot(V)
     if np.linalg.det(R) < 0:
         R[:,2] *= -1
@@ -92,4 +92,4 @@ def estimate_pose(vertices):
     _,R,_ = P2sRt(P) # decompose affine matrix to s, R, t
     pose = matrix2angle(R) 
 
-    return P, pose
\ No newline at end of file
+    return P, pose