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

update

This commit is contained in:
fxia22 2017-08-04 19:44:50 -07:00
parent 18ed60ec39
commit 151dc420af
2 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
render.cpp
View File

@ -9,10 +9,11 @@ extern "C"{
void render(int h,int w,unsigned char * img, float * depth,float * pose, unsigned char * render){
int ih, iw, i, ic;
float * points3d = (float*) malloc(sizeof(float) * h * w * 4);
float * points3d = (float*) malloc(sizeof(float) * h * w * 3);
float * points3d_after = (float*) malloc(sizeof(float) * h * w * 3);
float * points3d_polar = (float*) malloc(sizeof(float) * h * w * 3);
float * depth_render = (float *) malloc(sizeof(float) * h * w);
for (i = 0; i < 5; i++) {
printf("%f ", pose[i]);
@ -23,6 +24,7 @@ void render(int h,int w,unsigned char * img, float * depth,float * pose, unsigne
for (ic = 0; ic < 3; ic ++) {
render[(ih * w + iw) * 3 +ic] = 0;
}
depth_render[ih * w + iw] = 1e10;
}
}
@ -32,10 +34,9 @@ void render(int h,int w,unsigned char * img, float * depth,float * pose, unsigne
//printf("%f %f\n", depth[ih * w + iw], depth_point);
float phi = ((float)(ih) + 0.5) / float(h) * M_PI;
float theta = ((float)(iw) + 0.5) / float(w) * 2 * M_PI;
points3d[(ih * w + iw) * 4 + 0] = depth_point * sin(phi) * cos(theta);
points3d[(ih * w + iw) * 4 + 1] = depth_point * sin(phi) * sin(theta);
points3d[(ih * w + iw) * 4 + 2] = depth_point * cos(phi);
points3d[(ih * w + iw) * 4 + 3] = 1;
points3d[(ih * w + iw) * 3 + 0] = depth_point * sin(phi) * cos(theta);
points3d[(ih * w + iw) * 3 + 1] = depth_point * sin(phi) * sin(theta);
points3d[(ih * w + iw) * 3 + 2] = depth_point * cos(phi);
}
}
@ -46,31 +47,23 @@ void render(int h,int w,unsigned char * img, float * depth,float * pose, unsigne
alpha = pose[4];
beta = pose[3];
gamma = 0;
float transformation_matrix[16];
float transformation_matrix[9];
transformation_matrix[0] = cos(alpha) * cos(beta);
transformation_matrix[1] = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
transformation_matrix[2] = cos(alpha) * sin(beta) * cos(gamma) + sin(alpha) * sin(gamma);
transformation_matrix[3] = x;
transformation_matrix[4] = sin(alpha) * cos(beta);
transformation_matrix[5] = sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma);
transformation_matrix[6] = sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma);
transformation_matrix[7] = y;
transformation_matrix[8] = -sin(beta);
transformation_matrix[9] = cos(beta) * sin(gamma);
transformation_matrix[10] = cos(beta) * cos(gamma);
transformation_matrix[11] = z;
transformation_matrix[12] = 0;
transformation_matrix[13] = 0;
transformation_matrix[14] = 0;
transformation_matrix[15] = 1;
transformation_matrix[3] = sin(alpha) * cos(beta);
transformation_matrix[4] = sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma);
transformation_matrix[5] = sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma);
transformation_matrix[6] = -sin(beta);
transformation_matrix[7] = cos(beta) * sin(gamma);
transformation_matrix[8] = cos(beta) * cos(gamma);
for (ih = 0; ih < h; ih ++ ) {
for (iw = 0; iw < w; iw ++ ) {
for (ic = 0; ic < 3; ic ++) {
points3d_after[(ih * w + iw) * 3 + ic] = points3d[(ih * w + iw) * 4 + 0] * transformation_matrix[4 * ic + 0] + points3d[(ih * w + iw) * 4 + 1] * transformation_matrix[4 * ic + 1] + points3d[(ih * w + iw) * 4 + 2] * transformation_matrix[4 * ic + 2] + points3d[(ih * w + iw) * 4 + 3] * transformation_matrix[4 * ic + 3];
points3d_after[(ih * w + iw) * 3 + ic] = (points3d[(ih * w + iw) * 3 + 0]+x) * transformation_matrix[3 * ic + 0] + (points3d[(ih * w + iw) * 3 + 1]+y) * transformation_matrix[3 * ic + 1] + (points3d[(ih * w + iw) * 3 + 2]+z) * transformation_matrix[3 * ic + 2];
}
}
}
@ -93,8 +86,11 @@ void render(int h,int w,unsigned char * img, float * depth,float * pose, unsigne
int x = round((points3d_polar[(ih * w + iw) * 3 + 1] + M_PI)/(2*M_PI) * w - 0.5);
int y = round((points3d_polar[(ih * w + iw) * 3 + 2])/M_PI * h - 0.5);
//printf("%d %d\n", x, y);
for (ic = 0; ic < 3; ic ++) {
render[(y * w + x) * 3 + ic] = img[(ih * w + iw) * 3 +ic];
if (points3d_polar[(ih * w + iw) * 3 + 0] < depth_render[(y * w + x)]) {
for (ic = 0; ic < 3; ic ++) {
render[(y * w + x) * 3 + ic] = img[(ih * w + iw) * 3 +ic];
}
depth_render[(y * w + x)] = points3d_polar[(ih * w + iw) * 3 + 0];
}
}
}
@ -103,6 +99,7 @@ void render(int h,int w,unsigned char * img, float * depth,float * pose, unsigne
free(points3d);
free(points3d_after);
free(points3d_polar);
free(depth_render);
}

3
show_3d.py
View File

@ -82,6 +82,9 @@ def showpoints(img, depth):
elif cmd == ord('d'):
x += 0.01
changed = True
elif cmd == ord('r'):
pitch,yaw,x,y,z = 0,0,0,0,0
changed = True