From 8147a4cb6375345d8df9af72495c006887aaa155 Mon Sep 17 00:00:00 2001 From: hzyjerry Date: Thu, 14 Sep 2017 19:14:33 -0700 Subject: [PATCH] Read pose from file (loc + quaternion) --- depth/.gitignore | 4 +- depth/common/controls.cpp | 144 ++++++++++++++++++++++---------- depth/common/controls.hpp | 6 +- depth/depth_render/demo.cpp | 2 +- depth/depth_render/hwclient.py | 6 +- depth/depth_render/render.cpp | 79 ++++++------------ depth/depth_render/transfer.pyx | 2 +- 7 files changed, 137 insertions(+), 106 deletions(-) diff --git a/depth/.gitignore b/depth/.gitignore index 763bac138..5e2e87f24 100644 --- a/depth/.gitignore +++ b/depth/.gitignore @@ -16,4 +16,6 @@ distrib/ *.obj depth_render glew-1.13.0/ -glfw-3.1.2/ \ No newline at end of file +glfw-3.1.2/ +/11HB6XZSh1Q +/19Em1uen8Ae \ No newline at end of file diff --git a/depth/common/controls.cpp b/depth/common/controls.cpp index cab6f77ef..2dc490572 100644 --- a/depth/common/controls.cpp +++ b/depth/common/controls.cpp @@ -13,6 +13,7 @@ extern GLFWwindow* window; // The "extern" keyword here is to access the variabl #include #include + using namespace glm; #include "controls.hpp" @@ -110,7 +111,7 @@ void getPositionRotation(glm::vec3 &position, float& rotX, float& rotY, float& r -bool computeMatricesFromInputs(char* filename){ +bool computeMatricesFromInputs(){ bool do_screenshot = true; @@ -204,50 +205,7 @@ bool computeMatricesFromInputs(char* filename){ //} - // First way (deprecated) : lookAt function - /* - ViewMatrix = glm::lookAt( - position, // Camera is here - position+pose_direction, // and looks here : at the same position, plus "direction" - up // Head is up (set to 0,-1,0 to look upside-down) - ); - - printf("First view matrix (no translate)\n"); - for (int i = 0; i < 4; ++i) { - printf("\t %f %f %f %f\n", ViewMatrix[0][i], ViewMatrix[1][i], ViewMatrix[2][i], ViewMatrix[3][i]); - } - printf("Current view matrix\n"); - for (int i = 0; i < 4; ++i) { - printf("\t %f %f %f %f\n", ViewMatrix[0][i], ViewMatrix[1][i], ViewMatrix[2][i], ViewMatrix[3][i]); - } - printf("Up vector: %f %f %f\n", up[0], up[1], up[2]); - printf("pos vector: %f %f %f\n", position[0], position[1], position[2]); - */ - - - /* Second way (deprecated): manually construct up direction - glm::vec4 pose_d = glm::vec4(0.0, 0.0, -1.0, 1.0); - - glm::mat4 pose_trans = glm::mat4(1.0); - - pose_trans = glm::rotate(pose_trans, -rotationX, glm::vec3(1.0f, 0.0f, 0.0f)); - pose_trans = glm::rotate(pose_trans, -rotationY, glm::vec3(0.0f, 1.0f, 0.0f)); - pose_trans = glm::rotate(pose_trans, -rotationZ, glm::vec3(0.0f, 0.0f, 1.0f)); - - pose_d = pose_trans * pose_d; - up4 = pose_trans * up4; - - - glm::vec3 pose_direction(pose_d); - glm::vec3 up(up4); - //printf("pose direction %f %f %f %f\n", pose_d[0], pose_d[1], pose_d[2], pose_d[3]); - //printf("pose direction %f %f %f\n", pose_direction[0], pose_direction[1], pose_direction[2]); - //printf(" direction %f %f %f\n", direction[0], direction[1], direction[2]); - */ - - - // Third way glm::quat viewDirection; glm::vec3 viewDirectionEuler(rotationX, rotationY, rotationZ); viewDirection = glm::quat(viewDirectionEuler) * initial; @@ -264,3 +222,101 @@ bool computeMatricesFromInputs(char* filename){ return do_screenshot; } + + +bool computeMatricesFromFile(std::string filename){ + + bool do_screenshot = true; + + + // glfwGetTime is called only once, the first time this function is called + static double lastTime = glfwGetTime(); + + // Compute time difference between current and last frame + double currentTime = glfwGetTime(); + float deltaTime = float(currentTime - lastTime); + + // Get mouse position + double xpos, ypos; + + + // Compute new orientation + horizontalAngle += mouseSpeed * float( 512/2 - xpos ); + verticalAngle += mouseSpeed * float( 512/2 - ypos ); + + // Direction : Spherical coordinates to Cartesian coordinates conversion + glm::vec3 direction( + cos(verticalAngle) * sin(horizontalAngle), + sin(verticalAngle), + cos(verticalAngle) * cos(horizontalAngle) + ); + + // Hardcoded pose information + // Camera matrix + // Point 0 view 1 + /*float rotationX = 1.2462860345840454; + float rotationY = -0.009244712069630623; + float rotationZ = -1.2957184314727783; + */ + // Point 0 view 2 + //float rotationX = 1.3605239391326904; + //float rotationY = -0.009078502655029297; + //float rotationZ = -1.441698670387268; + //float fov = 0.9698680134771724; + float fov = glm::radians(90.0f); + + float posX = 0; + float posY = 0; + float posZ = 0; + + float rotW = 0; + float rotX = 0; + float rotY = 0; + float rotZ = 0; + + float junk[2]; + + FILE * file = fopen(filename.c_str(), "r"); + if( file == NULL ){ + printf("Impossible to open pose file %s!\n", filename.c_str()); + } + + char namebuf[50]; + + int count = fscanf(file, "%s %f %f %f %f %f %f %f %f %f\n", namebuf, &posX, &posY, &posZ, &rotW, &rotX, &rotY, &rotZ, &junk[0], &junk[1] ); + + printf("Loading pose file count: %d, namebuf: %s, rot count %d\n", count, namebuf, currentPoseRotCount); + + assert(count == 10); + + rotY = -rotY; + + position = glm::vec3(posX, posY, posZ); + + ProjectionMatrix = glm::perspective(fov, 1.0f, 0.1f, 5000.0f); // near & far are not verified, but accuracy seems to work well + + + //if (currentTime - currentPoseStartTime > 1) { + // UNCOMMENT THIS, in order to render png at a new position every second + //getPositionRotation(position, rotationX, rotationY, rotationZ, filename); + glm::quat initial = initialDirections[currentPoseRotCount]; + //convertRotation(rotationX, rotationY, rotationZ, currentPoseRotCount); + currentPoseStartTime = currentTime; + currentPoseRotCount += 1; + do_screenshot = true; + //} + + + glm::quat viewDirection; + //glm::vec3 viewDirectionEuler(rotationX, rotationY, rotationZ); + viewDirection = glm::quat(rotW, rotX, rotY, rotZ) * initial; + //viewDirection = glm::quat(viewDirectionEuler) * initial; + + ViewMatrix = glm::inverse(glm::translate(glm::mat4(1.0), position) * glm::toMat4(viewDirection)); + + // For the next frame, the "last time" will be "now" + lastTime = currentTime; + + return do_screenshot; +} + diff --git a/depth/common/controls.hpp b/depth/common/controls.hpp index b9ed28f75..4db80647b 100644 --- a/depth/common/controls.hpp +++ b/depth/common/controls.hpp @@ -1,7 +1,11 @@ #ifndef CONTROLS_HPP #define CONTROLS_HPP -bool computeMatricesFromInputs(char* filename); +#include + +bool computeMatricesFromInputs(); +bool computeMatricesFromFile(std::string filename); + glm::mat4 getViewMatrix(); glm::mat4 getProjectionMatrix(); diff --git a/depth/depth_render/demo.cpp b/depth/depth_render/demo.cpp index cab5c28e9..4b7d30a0e 100644 --- a/depth/depth_render/demo.cpp +++ b/depth/depth_render/demo.cpp @@ -367,7 +367,7 @@ int main( int argc, char * argv[] ) // Compute the MVP matrix from keyboard and mouse input char filename[50]; - bool do_screenshot = computeMatricesFromInputs(filename); + bool do_screenshot = computeMatricesFromInputs(); glm::mat4 ProjectionMatrix = getProjectionMatrix(); glm::mat4 ViewMatrix = getViewMatrix(); glm::mat4 ModelMatrix = glm::mat4(1.0); diff --git a/depth/depth_render/hwclient.py b/depth/depth_render/hwclient.py index 84b5f495a..29565d95e 100644 --- a/depth/depth_render/hwclient.py +++ b/depth/depth_render/hwclient.py @@ -41,9 +41,9 @@ for request in range(6): # todo: debug data = data[:][::-1][:] - #img = Image.fromarray(data[0]) - #img.save(img_path + str(request) + ".tiff") - scipy.misc.imsave(img_path + str(request) + ".tiff", data) + img = Image.fromarray(data) + img.save(img_path + str(request) + ".tiff") + #scipy.misc.imsave(img_path + str(request) + ".png", data, 'L', bits=16) print("Received reply %s [ %s ]" % (request, data)) diff --git a/depth/depth_render/render.cpp b/depth/depth_render/render.cpp index ce70fa0c2..3c8e715bd 100755 --- a/depth/depth_render/render.cpp +++ b/depth/depth_render/render.cpp @@ -161,9 +161,22 @@ int main( int argc, char * argv[] ) { cmdline::parser cmdp; - cmdp.add("obj", 'b', "obj file name", true, ""); + cmdp.add("datapath", 'd', "data model directory", true, ""); + + cmdp.add("model", 'm', "model id", true, ""); + cmdp.parse_check(argc, argv); - std::string name_obj = cmdp.get("obj"); + + std::string name_path = cmdp.get("datapath"); + + std::string model_id = cmdp.get("model"); + + std::string name_obj = name_path + "/" + model_id + "/" + model_id + "_HIGH.obj"; + std::string name_loc = name_path + "/" + model_id + "/" + "sweep_locations.csv"; + + + //std::string name_ply = "out_res.ply"; + glfwSetErrorCallback(error_callback); @@ -334,11 +347,17 @@ int main( int argc, char * argv[] ) bool res = loadOBJ(name_obj.c_str(), vertices, uvs, normals); // Note: use unsigned int because of too many indices + //std::vector short_indices; + //bool res = loadAssImp(name_ply.c_str(), short_indices, vertices, uvs, normals); + std::vector indices; + std::vector indexed_vertices; std::vector indexed_uvs; std::vector indexed_normals; indexVBO(vertices, uvs, normals, indices, indexed_vertices, indexed_uvs, indexed_normals); + + // Load it into a VBO @@ -503,25 +522,11 @@ int main( int argc, char * argv[] ) glUseProgram(programID); // Compute the MVP matrix from keyboard and mouse input - char filename[50]; - bool do_screenshot = computeMatricesFromInputs(filename); + computeMatricesFromFile(name_loc); glm::mat4 ProjectionMatrix = getProjectionMatrix(); glm::mat4 ViewMatrix = getViewMatrix(); glm::mat4 ModelMatrix = glm::mat4(1.0); - /* - printf("Before "); - for (int i = 0; i < 16; i++) - printf("%f ", ProjectionMatrix[i / 4][i % 4]); - - printf("\n"); - //BuildPerspProjMat(ProjectionMatrix, 1.0489180166567196, 1.0, 0.0, 128.0); - printf("After "); - for (int i = 0; i < 16; i++) - printf("%f ", ProjectionMatrix[i / 4][i % 4]); - - printf("\n"); - */ @@ -633,6 +638,7 @@ int main( int argc, char * argv[] ) */ + /* if (false) { char buffer[100]; //printf("before: %s\n", buffer); @@ -642,6 +648,7 @@ int main( int argc, char * argv[] ) //printf("saving screenshot to %s\n", buffer); save_screenshot(buffer, windowWidth, windowHeight, renderedTexture); } + */ // Swap buffers //glfwSwapBuffers(window); @@ -698,41 +705,3 @@ int main( int argc, char * argv[] ) return 0; } - -void BuildPerspProjMat(glm::mat4 &m, float fov, float aspect, - float znear, float zfar) { - float xymax = znear * tan(fov/2); - float ymin = -xymax; - float xmin = -xymax; - - float width = xymax - xmin; - float height = xymax - ymin; - - float depth = zfar - znear; - float q = -(zfar + znear) / depth; - float qn = -2 * (zfar * znear) / depth; - - float w = 2 * znear / (width + 0.000001); - w = w / aspect; - float h = 2 * znear / (height + 0.000001); - - //m[0][0] = w; - m[0][1] = (float) 0.0; - m[0][2] = (float) 0.0; - m[0][3] = (float) 0.0; - - m[1][0] = (float) 0.0; - //m[1][1] = h; - m[1][2] = (float) 0.0; - m[1][3] = (float) 0.0; - - m[2][0] = (float) 0.0; - m[2][1] = (float) 0.0; - m[2][2] = q; - m[2][3] = (float) -1.0; - - m[3][0] = (float) 0.0; - m[3][1] = (float) 0.0; - //m[3][2] = qn; - m[3][3] = (float) 0.0; -} diff --git a/depth/depth_render/transfer.pyx b/depth/depth_render/transfer.pyx index de2c21840..332b145bb 100644 --- a/depth/depth_render/transfer.pyx +++ b/depth/depth_render/transfer.pyx @@ -15,5 +15,5 @@ def transfer2(unsigned char [:,:,:,:] in_img, int [:,:,:]coords, int h, int w): corrx = coords[ycoord, xcoord, 1] corry = coords[ycoord, xcoord, 2] for c in range(3): - out_img[ycoord, xcoord, c] = in_img[ind, corrx, corry, c] + out_img[ycoord, xcoord, c] = in_img[ind, corry, corrx, c] return np.array(out_img) \ No newline at end of file