pose debug
This commit is contained in:
fxia22
parent
f47d1f8501
commit
96a9fc0cfa
|
|
@ -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)
|
||||
|
||||
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((768, 768, 1))
|
||||
data = data[::-1,::-1,:]
|
||||
img_array.append(data)
|
||||
|
||||
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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,6 +549,10 @@ int main( int argc, char * argv[] )
|
|||
lastTime += 1.0;
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -575,7 +567,7 @@ int main( int argc, char * argv[] )
|
|||
//computeMatricesFromInputs();
|
||||
computeMatricesFromFile(name_loc);
|
||||
glm::mat4 ProjectionMatrix = getProjectionMatrix();
|
||||
glm::mat4 ViewMatrix = viewMat * initialDirections[pose_idx]; // getViewMatrix();
|
||||
glm::mat4 ViewMatrix = getViewMatrix();
|
||||
glm::mat4 ModelMatrix = glm::mat4(1.0);
|
||||
|
||||
pose_idx ++;
|
||||
|
|
@ -583,8 +575,6 @@ int main( int argc, char * argv[] )
|
|||
glm::mat4 tempMat = getViewMatrix();
|
||||
debug_mat(tempMat, "csv");
|
||||
|
||||
|
||||
|
||||
glm::mat4 MVP = ProjectionMatrix * ViewMatrix * ModelMatrix;
|
||||
|
||||
// Send our transformation to the currently bound shader,
|
||||
|
|
@ -708,7 +698,6 @@ int main( int argc, char * argv[] )
|
|||
// Swap buffers
|
||||
//glfwSwapBuffers(window);
|
||||
//glfwPollEvents();
|
||||
i ++;
|
||||
|
||||
int nSize = windowWidth*windowHeight*3;
|
||||
int nByte = nSize*sizeof(unsigned short);
|
||||
|
|
@ -730,15 +719,21 @@ int main( int argc, char * argv[] )
|
|||
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);
|
||||
memcpy (reply.data () + windowWidth*windowHeight*sizeof(unsigned short) * k, (unsigned char*)dataBuffer_c, windowWidth*windowHeight*sizeof(unsigned short));
|
||||
|
||||
free(dataBuffer);
|
||||
free(dataBuffer_c);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
socket.send (reply);
|
||||
|
||||
//free(dataBuffer);
|
||||
//free(dataBuffer_c);
|
||||
|
||||
|
||||
|
||||
} while (true);
|
||||
// Check if the ESC key was pressed or the window was closed
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
Loading…
Reference in New Issue