p04798526
/
OmniGibson
mirror of https://github.com/StanfordVL/OmniGibson.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

pose debug

This commit is contained in:
fxia22 2017-09-15 14:43:45 -07:00
parent f47d1f8501
commit 96a9fc0cfa
3 changed files with 410 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
depth/depth_render/hwclient.py
View File

@ -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)

337
depth/depth_render/render.cpp
View File

@ -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<std::string>("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);

233
dev/pose_debug.ipynb Normal file
View File

@ -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
}