From edde463e21d6f50804b38c906a0cef49579df0d6 Mon Sep 17 00:00:00 2001
From: hzyjerry <zhiyanghe96@gmail.com>
Date: Thu, 21 Sep 2017 16:17:44 -0700
Subject: [PATCH] Debugging pose integration

---
 Dockerfile                    |  2 +-
 depth/depth_render/render.cpp |  6 ++---
 dev/show_3d2_f.py             | 45 +++++++++++++----------------------
 3 files changed, 21 insertions(+), 32 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index cd58c3fa3..ebc52e0cd 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -70,7 +70,7 @@ WORKDIR /usr/local/realenv/
 RUN ["chmod", "+x", "/usr/local/realenv/init.sh"]
 
 
-#ENTRYPOINT ["/usr/local/realenv/init.sh"]
+#ENTRYPOINT [ "/usr/local/realenv/init.sh" ]
 #ENTRYPOINT [ "/bin/bash", "-c" ]
 #CMD ["x11vnc", "-forever", "-usepw", "-create"]
 
diff --git a/depth/depth_render/render.cpp b/depth/depth_render/render.cpp
index ee4828c2a..6b7ed10ba 100755
--- a/depth/depth_render/render.cpp
+++ b/depth/depth_render/render.cpp
@@ -531,7 +531,7 @@ int main( int argc, char * argv[] )
 
         //  Wait for next request from client
         socket.recv (&request);
-        std::cout << "Received Hello " << request.data() << std::endl;
+        std::cout << "Received data " << request.data() << std::endl;
 
         //printf("%s\n", request.data());
 
@@ -541,7 +541,7 @@ int main( int argc, char * argv[] )
         //std::cout << request_str << std::endl;
 
         glm::mat4 viewMat = str_to_mat(request_str);
-        debug_mat(viewMat, "json");
+        debug_mat(viewMat, "receive");
 
 		// Measure speed
 		//double currentTime = glfwGetTime();
@@ -589,7 +589,7 @@ int main( int argc, char * argv[] )
 
             // Compute the MVP matrix from keyboard and mouse input
             //computeMatricesFromInputs();
-            computeMatricesFromFile(name_loc);
+            //computeMatricesFromFile(name_loc);
             float fov = glm::radians(90.0f);
             glm::mat4 ProjectionMatrix = glm::perspective(fov, 1.0f, 0.1f, 5000.0f); // near & far are not verified, but accuracy seems to work well
             glm::mat4 ViewMatrix =  getView(viewMat, k);
diff --git a/dev/show_3d2_f.py b/dev/show_3d2_f.py
index 24a6a257f..5c0b0192f 100644
--- a/dev/show_3d2_f.py
+++ b/dev/show_3d2_f.py
@@ -156,20 +156,14 @@ def showpoints(imgs, depths, poses, model, target, tdepth, target_pose):
         t0 = time.time()
         
         v_cam2world = np.linalg.inv(poses[0]).dot(target_pose)
-        p = pose.dot(v_cam2world)
-        
-        trans = -np.dot(p[:3, :3].T, p[:3, -1])
-        rot = np.dot(np.array([[-1,0,0],[0,-1,0],[0,0,1]]),  np.linalg.inv(p[:3, :3]))
-        p2 = np.eye(4)
-        p2[:3, :3] = rot
-        p2[:3, -1] = trans
-
+        p = v_cam2world.dot(pose)
 
 
 
         print("Sending request ...")
         #print(v_cam2world)
-        print('current viewer pose', pose)
+        #print('current viewer pose', pose)
+        print("current camera matrix", p)
         print("camera pose", p)
         print("target pose", target_pose)
         #s = mat_to_str(p2)
@@ -178,15 +172,6 @@ def showpoints(imgs, depths, poses, model, target, tdepth, target_pose):
         '''
         p = pose.dot(np.linalg.inv(poses[0])) #.dot(target_pose)
 
-        trans = -pose[:3, -1]
-        rot = np.linalg.inv(pose[:3, :3])
-
-
-        p2 = np.eye(4)
-        p2[:3, :3] = rot
-        p2[:3, -1] = trans
-
-
         s = mat_to_str(poses[0] * p2)
         '''
 
@@ -234,9 +219,9 @@ def showpoints(imgs, depths, poses, model, target, tdepth, target_pose):
             #print(poses[0])
 
             pose_after = pose.dot(np.linalg.inv(poses[0])).dot(poses[i]).astype(np.float32)
-            if i == 0:
-                print('First pose after')
-                print(pose_after)
+            #if i == 0:
+                #print('First pose after')
+                #print(pose_after)
             #from IPython import embed; embed()
             #print('Received pose ' + str(i))
             #print(pose_after)
@@ -308,7 +293,7 @@ def showpoints(imgs, depths, poses, model, target, tdepth, target_pose):
 
             cpose2 = np.eye(4)
             cpose2[0,3] = x
-            cpose2[1,3] = y
+            cpose2[1,3] = -y
             cpose2[2,3] = z
 
             cpose = np.dot(cpose, cpose2)
@@ -336,23 +321,27 @@ def showpoints(imgs, depths, poses, model, target, tdepth, target_pose):
 
         if cmd==ord('q'):
             break
+        ## z > 0: moves outward from screen
+        ## z < 0: moves inward to screen
+        ## y > 0: moves downward
+        ## y < 0: moves upward
         elif cmd == ord('w'):
-            x -= 0.05
+            z += 0.05
             changed = True
         elif cmd == ord('s'):
-            x += 0.05
+            z -= 0.05
             changed = True
         elif cmd == ord('a'):
-            y += 0.05
+            x -= 0.05
             changed = True
         elif cmd == ord('d'):
-            y -= 0.05
+            x += 0.05
             changed = True
         elif cmd == ord('z'):
-            z += 0.01
+            y += 0.03
             changed = True
         elif cmd == ord('x'):
-            z -= 0.01
+            y -= 0.03
             changed = True
 
         elif cmd == ord('r'):