diff --git a/depth/depth_render/hwclient.py b/depth/depth_render/hwclient.py index 6d6bf3a31..7540baab9 100644 --- a/depth/depth_render/hwclient.py +++ b/depth/depth_render/hwclient.py @@ -238,19 +238,18 @@ if __name__ == '__main__': img_array = [] # Do 10 requests, waiting each time for a response - for request in range(6): - print("Sending request %s ..." % request) - #socket.send(b"Hello") - socket.send(mat_str) - - # Get the reply. - message = socket.recv() - data = np.array(np.frombuffer(message, dtype=np.uint16)).reshape((768, 768, 1)) - data = data[::-1,::-1,:] - img_array.append(data) - - + + print("Sending request ..." ) + #socket.send(b"Hello") + socket.send(mat_str) + # Get the reply. + message = socket.recv() + data = np.array(np.frombuffer(message, dtype=np.uint16)).reshape((6, 768, 768, 1)) + data = data[:, ::-1,::-1,:] + + for i in range(6): + img_array.append(data[i]) #data = np.log(data / 256 * (256/np.log(256))).astype(np.uint8) diff --git a/depth/depth_render/render.cpp b/depth/depth_render/render.cpp index 1a0f25bd4..f3995588f 100755 --- a/depth/depth_render/render.cpp +++ b/depth/depth_render/render.cpp @@ -51,18 +51,6 @@ static glXCreateContextAttribsARBProc glXCreateContextAttribsARB = NULL; static glXMakeContextCurrentARBProc glXMakeContextCurrentARB = NULL; -glm::quat initialDirections[] = { - glm::quat(glm::vec3(glm::radians(90.0f), 0.0f, 0.0f)), - glm::quat(glm::vec3(0.0f, glm::radians(90.0f), 0.0f)), - glm::quat(glm::vec3(0.0f, 0.0f, 0.0f)), - glm::quat(glm::vec3(0.0f, glm::radians(-90.0f), 0.0f)), - glm::quat(glm::vec3(0.0f, glm::radians(-180.0f), 0.0f)), - //glm::quat(glm::vec3(0.0f, glm::radians(90.0f), 0.0f)), - //glm::quat(glm::vec3(0.0f, glm::radians(180.0f), 0.0f)), - //glm::quat(glm::vec3(0.0f, glm::radians(270.0f), 0.0f)), - glm::quat(glm::vec3(glm::radians(-90.0f), 0.0f, 0.0f)) -}; - glm::vec3 GetOGLPos(int x, int y) { @@ -214,7 +202,7 @@ int main( int argc, char * argv[] ) 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.c"; + std::string name_loc = name_path + "/" + model_id + "/" + "sweep_locations.csv"; //std::string name_ply = "out_res.ply"; @@ -561,183 +549,190 @@ int main( int argc, char * argv[] ) lastTime += 1.0; } - // Render to our framebuffer - glBindFramebuffer(GL_FRAMEBUFFER, FramebufferName); - glViewport(0,0,windowWidth,windowHeight); // Render on the whole framebuffer, complete from the lower left corner to the upper right + zmq::message_t reply (windowWidth*windowHeight*sizeof(unsigned short) * 6); + + for (int k = 0; k < 6; k ++ ) + { + // Render to our framebuffer + glBindFramebuffer(GL_FRAMEBUFFER, FramebufferName); + glViewport(0,0,windowWidth,windowHeight); // Render on the whole framebuffer, complete from the lower left corner to the upper right - // Clear the screen - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + // Clear the screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - // Use our shader - glUseProgram(programID); + // Use our shader + glUseProgram(programID); - // Compute the MVP matrix from keyboard and mouse input - //computeMatricesFromInputs(); - computeMatricesFromFile(name_loc); - glm::mat4 ProjectionMatrix = getProjectionMatrix(); - glm::mat4 ViewMatrix = viewMat * initialDirections[pose_idx]; // getViewMatrix(); - glm::mat4 ModelMatrix = glm::mat4(1.0); + // Compute the MVP matrix from keyboard and mouse input + //computeMatricesFromInputs(); + computeMatricesFromFile(name_loc); + glm::mat4 ProjectionMatrix = getProjectionMatrix(); + glm::mat4 ViewMatrix = getViewMatrix(); + glm::mat4 ModelMatrix = glm::mat4(1.0); - pose_idx ++; + pose_idx ++; - glm::mat4 tempMat = getViewMatrix(); - debug_mat(tempMat, "csv"); + glm::mat4 tempMat = getViewMatrix(); + debug_mat(tempMat, "csv"); + + glm::mat4 MVP = ProjectionMatrix * ViewMatrix * ModelMatrix; + + // Send our transformation to the currently bound shader, + // in the "MVP" uniform + glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &MVP[0][0]); + glUniformMatrix4fv(ModelMatrixID, 1, GL_FALSE, &ModelMatrix[0][0]); + glUniformMatrix4fv(ViewMatrixID, 1, GL_FALSE, &ViewMatrix[0][0]); + + glm::vec3 lightPos = glm::vec3(4,4,4); + glUniform3f(LightID, lightPos.x, lightPos.y, lightPos.z); + + // Bind our texture in Texture Unit 0 + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, Texture); + // Set our "myTextureSampler" sampler to use Texture Unit 0 + glUniform1i(TextureID, 0); + + // 1rst attribute buffer : vertices + glEnableVertexAttribArray(0); + glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer); + glVertexAttribPointer( + 0, // attribute + 3, // size + GL_FLOAT, // type + GL_FALSE, // normalized? + 0, // stride + (void*)0 // array buffer offset + ); + + // 2nd attribute buffer : UVs + glEnableVertexAttribArray(1); + glBindBuffer(GL_ARRAY_BUFFER, uvbuffer); + glVertexAttribPointer( + 1, // attribute + 2, // size + GL_FLOAT, // type + GL_FALSE, // normalized? + 0, // stride + (void*)0 // array buffer offset + ); + + // 3rd attribute buffer : normals + glEnableVertexAttribArray(2); + glBindBuffer(GL_ARRAY_BUFFER, normalbuffer); + glVertexAttribPointer( + 2, // attribute + 3, // size + GL_FLOAT, // type + GL_FALSE, // normalized? + 0, // stride + (void*)0 // array buffer offset + ); + + // Index buffer + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, elementbuffer); + + // Draw the triangles ! + glDrawElements( + GL_TRIANGLES, // mode + indices.size(), // count + GL_UNSIGNED_INT, // type + (void*)0 // element array buffer offset + ); + + glDisableVertexAttribArray(0); + glDisableVertexAttribArray(1); + glDisableVertexAttribArray(2); + + /* + // Render to the screen + glBindFramebuffer(GL_FRAMEBUFFER, 0); + // Render on the whole framebuffer, complete from the lower left corner to the upper right + glViewport(0,0,windowWidth,windowHeight); + + // Clear the screen + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + // Use our shader + glUseProgram(quad_programID); + + // Bind our texture in Texture Unit 0 + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, renderedTexture); + //glBindTexture(GL_TEXTURE_2D, depthTexture); + // Set our "renderedTexture" sampler to use Texture Unit 0 + glUniform1i(texID, 0); + + glUniform1f(timeID, (float)(glfwGetTime()*10.0f) ); + + // 1rst attribute buffer : vertices + glEnableVertexAttribArray(0); + glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer); + glVertexAttribPointer( + 0, // attribute 0. No particular reason for 0, but must match the layout in the shader. + 3, // size + GL_FLOAT, // type + GL_FALSE, // normalized? + 0, // stride + (void*)0 // array buffer offset + ); + + // Draw the triangles ! + glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles + + glDisableVertexAttribArray(0); + */ + /* + if (false) { + char buffer[100]; + //printf("before: %s\n", buffer); + sprintf(buffer, "/home/jerry/Pictures/%s_mist.png", filename); + //printf("after: %s\n", buffer); + //printf("file name is %s\n", filename); + //printf("saving screenshot to %s\n", buffer); + save_screenshot(buffer, windowWidth, windowHeight, renderedTexture); + } + */ - glm::mat4 MVP = ProjectionMatrix * ViewMatrix * ModelMatrix; + // Swap buffers + //glfwSwapBuffers(window); + //glfwPollEvents(); - // Send our transformation to the currently bound shader, - // in the "MVP" uniform - glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &MVP[0][0]); - glUniformMatrix4fv(ModelMatrixID, 1, GL_FALSE, &ModelMatrix[0][0]); - glUniformMatrix4fv(ViewMatrixID, 1, GL_FALSE, &ViewMatrix[0][0]); + int nSize = windowWidth*windowHeight*3; + int nByte = nSize*sizeof(unsigned short); + // First let's create our buffer, 3 channels per Pixel + unsigned short* dataBuffer = (unsigned short*)malloc(nByte); + //char* dataBuffer = (char*)malloc(nSize*sizeof(char)); - glm::vec3 lightPos = glm::vec3(4,4,4); - glUniform3f(LightID, lightPos.x, lightPos.y, lightPos.z); + if (!dataBuffer) return false; - // Bind our texture in Texture Unit 0 - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, Texture); - // Set our "myTextureSampler" sampler to use Texture Unit 0 - glUniform1i(TextureID, 0); + // Let's fetch them from the backbuffer + // We request the pixels in GL_BGR format, thanks to Berzeger for the tip + glReadPixels((GLint)0, (GLint)0, + (GLint)windowWidth, (GLint)windowHeight, + GL_BGR, GL_UNSIGNED_SHORT, dataBuffer); - // 1rst attribute buffer : vertices - glEnableVertexAttribArray(0); - glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer); - glVertexAttribPointer( - 0, // attribute - 3, // size - GL_FLOAT, // type - GL_FALSE, // normalized? - 0, // stride - (void*)0 // array buffer offset - ); + glGetTextureImage(renderedTexture, 0, GL_RGB, GL_UNSIGNED_SHORT, nSize*sizeof(unsigned short), dataBuffer); - // 2nd attribute buffer : UVs - glEnableVertexAttribArray(1); - glBindBuffer(GL_ARRAY_BUFFER, uvbuffer); - glVertexAttribPointer( - 1, // attribute - 2, // size - GL_FLOAT, // type - GL_FALSE, // normalized? - 0, // stride - (void*)0 // array buffer offset - ); + unsigned short * dataBuffer_c = (unsigned short * ) malloc(windowWidth*windowHeight * sizeof(unsigned short)); + for (int i = 0; i < windowWidth * windowHeight; i++) + dataBuffer_c[i] = dataBuffer[3*i]; + + memcpy (reply.data () + windowWidth*windowHeight*sizeof(unsigned short) * k, (unsigned char*)dataBuffer_c, windowWidth*windowHeight*sizeof(unsigned short)); + + free(dataBuffer); + free(dataBuffer_c); - // 3rd attribute buffer : normals - glEnableVertexAttribArray(2); - glBindBuffer(GL_ARRAY_BUFFER, normalbuffer); - glVertexAttribPointer( - 2, // attribute - 3, // size - GL_FLOAT, // type - GL_FALSE, // normalized? - 0, // stride - (void*)0 // array buffer offset - ); + } - // Index buffer - glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, elementbuffer); - - // Draw the triangles ! - glDrawElements( - GL_TRIANGLES, // mode - indices.size(), // count - GL_UNSIGNED_INT, // type - (void*)0 // element array buffer offset - ); - - glDisableVertexAttribArray(0); - glDisableVertexAttribArray(1); - glDisableVertexAttribArray(2); - - /* - // Render to the screen - glBindFramebuffer(GL_FRAMEBUFFER, 0); - // Render on the whole framebuffer, complete from the lower left corner to the upper right - glViewport(0,0,windowWidth,windowHeight); - - // Clear the screen - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - - // Use our shader - glUseProgram(quad_programID); - - // Bind our texture in Texture Unit 0 - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, renderedTexture); - //glBindTexture(GL_TEXTURE_2D, depthTexture); - // Set our "renderedTexture" sampler to use Texture Unit 0 - glUniform1i(texID, 0); - - glUniform1f(timeID, (float)(glfwGetTime()*10.0f) ); - - // 1rst attribute buffer : vertices - glEnableVertexAttribArray(0); - glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer); - glVertexAttribPointer( - 0, // attribute 0. No particular reason for 0, but must match the layout in the shader. - 3, // size - GL_FLOAT, // type - GL_FALSE, // normalized? - 0, // stride - (void*)0 // array buffer offset - ); - - // Draw the triangles ! - glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles - - glDisableVertexAttribArray(0); - */ - - - /* - if (false) { - char buffer[100]; - //printf("before: %s\n", buffer); - sprintf(buffer, "/home/jerry/Pictures/%s_mist.png", filename); - //printf("after: %s\n", buffer); - //printf("file name is %s\n", filename); - //printf("saving screenshot to %s\n", buffer); - save_screenshot(buffer, windowWidth, windowHeight, renderedTexture); - } - */ - - // Swap buffers - //glfwSwapBuffers(window); - //glfwPollEvents(); - i ++; - - int nSize = windowWidth*windowHeight*3; - int nByte = nSize*sizeof(unsigned short); - // First let's create our buffer, 3 channels per Pixel - unsigned short* dataBuffer = (unsigned short*)malloc(nByte); - //char* dataBuffer = (char*)malloc(nSize*sizeof(char)); - - if (!dataBuffer) return false; - - // Let's fetch them from the backbuffer - // We request the pixels in GL_BGR format, thanks to Berzeger for the tip - glReadPixels((GLint)0, (GLint)0, - (GLint)windowWidth, (GLint)windowHeight, - GL_BGR, GL_UNSIGNED_SHORT, dataBuffer); - - glGetTextureImage(renderedTexture, 0, GL_RGB, GL_UNSIGNED_SHORT, nSize*sizeof(unsigned short), dataBuffer); - - unsigned short * dataBuffer_c = (unsigned short * ) malloc(windowWidth*windowHeight * sizeof(unsigned short)); - for (int i = 0; i < windowWidth * windowHeight; i++) - dataBuffer_c[i] = dataBuffer[3*i]; - - zmq::message_t reply (windowWidth*windowHeight*sizeof(unsigned short)); - memcpy (reply.data (), (unsigned char*)dataBuffer_c, windowWidth*windowHeight*sizeof(unsigned short)); socket.send (reply); - free(dataBuffer); - free(dataBuffer_c); + //free(dataBuffer); + //free(dataBuffer_c); + } while (true); diff --git a/dev/pose_debug.ipynb b/dev/pose_debug.ipynb new file mode 100644 index 000000000..bd1e9728a --- /dev/null +++ b/dev/pose_debug.ipynb @@ -0,0 +1,233 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 23, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "import numpy as np\n", + "import json\n", + "import utils" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "json_file = '../data/11HB6XZSh1Q/pano/points/point_003869070b5641efb8a71b8a0245aae2.json'" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "csv_file = '../data/11HB6XZSh1Q/sweep_locations.csv'" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "for line in open(csv_file):\n", + " ls =line.split(' ')\n", + " if ls[0] == '003869070b5641efb8a71b8a0245aae2':\n", + " pose_csv = np.array(ls[1:8]).astype(np.float32)" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "pose_dict = json.load(open(json_file))" + ] + }, + { + "cell_type": "code", + "execution_count": 41, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "p = np.concatenate(np.array(pose_dict[0][u'camera_rt_matrix'] + [[0,0,0,1]])).astype(np.float32).reshape((4,4))\n", + "#rotation = np.array([[0,-1,0,0],[-1,0,0,0],[0,0,1,0],[0,0,0,1]])\n", + "#p = np.dot(rotation, p)" + ] + }, + { + "cell_type": "code", + "execution_count": 42, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([[ 9.49621797e-01, 3.13134640e-01, 1.28523270e-02,\n", + " 1.88963938e+00],\n", + " [ -3.13175946e-01, 9.49694276e-01, 1.28583587e-03,\n", + " -4.20118427e+00],\n", + " [ -1.18031418e-02, -5.24609722e-03, 9.99916553e-01,\n", + " -4.28687191e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 1.00000000e+00]], dtype=float32)" + ] + }, + "execution_count": 42, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p" + ] + }, + { + "cell_type": "code", + "execution_count": 55, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[-3.1607511 3.37563992 4.2676301 ]\n", + "[[-0.99991715 -0.0052462 -0.01180312]\n", + " [-0.00128594 0.94969475 -0.31317616]\n", + " [ 0.01285234 -0.31313485 -0.94962227]]\n" + ] + } + ], + "source": [ + "trans = pose_csv[:3]\n", + "quat = pose_csv[3:]\n", + "rot = utils.to_r(quat)\n", + "print trans\n", + "print rot" + ] + }, + { + "cell_type": "code", + "execution_count": 60, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([-3.16075134, 3.37563992, 4.2676301 ], dtype=float32)" + ] + }, + "execution_count": 60, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "-np.dot(p[:3, :3].T, p[:3, -1])" + ] + }, + { + "cell_type": "code", + "execution_count": 124, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [ + "rotation = np.array([[0,0,-1],[0,-1,0],[1,0,0]])" + ] + }, + { + "cell_type": "code", + "execution_count": 127, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([[-0.99991655, -0.0052461 , -0.01180314],\n", + " [-0.00128584, 0.94969428, -0.31317595],\n", + " [ 0.01285233, -0.31313464, -0.9496218 ]])" + ] + }, + "execution_count": 127, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "np.dot(np.dot(rotation, p[:3, :3]), rotation)" + ] + }, + { + "cell_type": "code", + "execution_count": 128, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([[-0.99991715, -0.0052462 , -0.01180312],\n", + " [-0.00128594, 0.94969475, -0.31317616],\n", + " [ 0.01285234, -0.31313485, -0.94962227]], dtype=float32)" + ] + }, + "execution_count": 128, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "rot" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "collapsed": true + }, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 2", + "language": "python", + "name": "python2" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 2 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython2", + "version": "2.7.13" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}