GLFWAPI int glfwGetOSMesaDepthBuffer(GLFWwindow* handle,
int* width, int* height,
int* bytesPerValue,
void** buffer)
{
void* mesaBuffer;
GLint mesaWidth, mesaHeight, mesaBytes;
_GLFWwindow* window = (_GLFWwindow*) handle;
assert(window != NULL);
_GLFW_REQUIRE_INIT_OR_RETURN(GLFW_FALSE);
if (!OSMesaGetDepthBuffer(window->context.osmesa.handle,
&mesaWidth, &mesaHeight,
&mesaBytes, &mesaBuffer))
{
_glfwInputError(GLFW_PLATFORM_ERROR,
"OSMesa: Failed to retrieve depth buffer");
return GLFW_FALSE;
}
if (width)
*width = mesaWidth;
if (height)
*height = mesaHeight;
if (bytesPerValue)
*bytesPerValue = mesaBytes;
if (buffer)
*buffer = mesaBuffer;
return GLFW_TRUE;
}
void
Renderer3dImpl::get_buffers(int width, int height, void* rgb, void* depth) const
{
GLint tmp_width;
GLint tmp_height;
GLint tmp_format;
void* tmp;
if (rgb) {
OSMesaGetColorBuffer( ctx_, &tmp_width, &tmp_height, &tmp_format, &tmp );
std::memcpy(rgb, tmp, tmp_width*tmp_height*tmp_format);
}
if (depth) {
OSMesaGetDepthBuffer( ctx_, &tmp_width, &tmp_height, &tmp_format, &tmp );
std::memcpy(depth, tmp, tmp_width*tmp_height*tmp_format);
}
}
float* renderDepth(Mesh3D* model, float* projection, int render_width, int render_height){
unsigned char * pbufferRGB = new unsigned char [3 * render_width * render_height];
float m_near = 0.1; // 0.3;
float m_far = 1e8;
//float m_far = 10; //1e8;
//int m_level = 0;
// Step 1: setup off-screen mesa's binding
OSMesaContext ctx = OSMesaCreateContextExt(OSMESA_RGB, 32, 0, 0, NULL );
// Bind the buffer to the context and make it current
if (!OSMesaMakeCurrent(ctx, (void*)pbufferRGB, GL_UNSIGNED_BYTE, render_width, render_height)) {
std::cerr << "OSMesaMakeCurrent failed!: " << render_width << ' ' << render_height << std::endl;
return NULL;
}
OSMesaPixelStore(OSMESA_Y_UP, 1);
/*
GLint vup;
OSMesaGetIntegerv(OSMESA_Y_UP, &vup);
std::cout<<"OSMESA_Y_UP="<<vup<<std::endl;
*/
// Step 2: Setup basic OpenGL setting
glEnable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
//glEnable(GL_CULL_FACE);
//glCullFace(GL_BACK);
glPolygonMode(GL_FRONT, GL_FILL);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//glClearColor(m_clearColor[0], m_clearColor[1], m_clearColor[2], 1.0f); // this line seems useless
glViewport(0, 0, render_width, render_height);
// Step 3: Set projection matrices
//double scale = (0x0001) << m_level;
double final_matrix[16];
// new way: faster way by reuse computation and symbolic derive. See sym_derive.m to check the math.
//double inv_width_scale = 1.0/(m_width*scale);
// double inv_height_scale = 1.0/(m_height*scale);
//double inv_width_scale_1 =inv_width_scale - 1.0;
//double inv_height_scale_1_s = -(inv_height_scale - 1.0);
//double inv_width_scale_2 = inv_width_scale*2.0;
//double inv_height_scale_2_s = -inv_height_scale*2.0;
double cx_rgb = render_width/2;
double cy_rgb = render_height/2;
double inv_width_scale_1 = 1.0 - 4 * cx_rgb / render_width;
double inv_height_scale_1_s = -1.0 + 4 * cy_rgb / render_height;
double inv_width_scale_2 = 2.0 / render_width;
double inv_height_scale_2_s = -2.0 / render_height;
double m_far_a_m_near = m_far + m_near;
double m_far_s_m_near = m_far - m_near;
double m_far_d_m_near = m_far_a_m_near/m_far_s_m_near;
final_matrix[ 0]= projection[2+0*3]*inv_width_scale_1 + projection[0+0*3]*inv_width_scale_2;
final_matrix[ 1]= projection[2+0*3]*inv_height_scale_1_s + projection[1+0*3]*inv_height_scale_2_s;
final_matrix[ 2]= projection[2+0*3]*m_far_d_m_near;
final_matrix[ 3]= projection[2+0*3];
final_matrix[ 4]= projection[2+1*3]*inv_width_scale_1 + projection[0+1*3]*inv_width_scale_2;
final_matrix[ 5]= projection[2+1*3]*inv_height_scale_1_s + projection[1+1*3]*inv_height_scale_2_s;
final_matrix[ 6]= projection[2+1*3]*m_far_d_m_near;
final_matrix[ 7]= projection[2+1*3];
final_matrix[ 8]= projection[2+2*3]*inv_width_scale_1 + projection[0+2*3]*inv_width_scale_2;
final_matrix[ 9]= projection[2+2*3]*inv_height_scale_1_s + projection[1+2*3]*inv_height_scale_2_s;
final_matrix[10]= projection[2+2*3]*m_far_d_m_near;
final_matrix[11]= projection[2+2*3];
final_matrix[12]= projection[2+3*3]*inv_width_scale_1 + projection[0+3*3]*inv_width_scale_2;
final_matrix[13]= projection[2+3*3]*inv_height_scale_1_s + projection[1+3*3]*inv_height_scale_2_s;
final_matrix[14]= projection[2+3*3]*m_far_d_m_near - (2*m_far*m_near)/m_far_s_m_near;
final_matrix[15]= projection[2+3*3];
/*
// new way: faster way by reuse computation and symbolic derive. See sym_derive.m to check the math.
double inv_width_scale = 1.0/(render_width*scale);
double inv_height_scale = 1.0/(render_height*scale);
double inv_width_scale_1 =inv_width_scale - 1.0;
double inv_height_scale_1_s = -(inv_height_scale - 1.0);
double inv_width_scale_2 = inv_width_scale*2.0;
double inv_height_scale_2_s = -inv_height_scale*2.0;
double m_far_a_m_near = m_far + m_near;
double m_far_s_m_near = m_far - m_near;
double m_far_d_m_near = m_far_a_m_near/m_far_s_m_near;
final_matrix[ 0]= projection[2+0*3]*inv_width_scale_1 + projection[0+0*3]*inv_width_scale_2;
final_matrix[ 1]= projection[2+0*3]*inv_height_scale_1_s + projection[1+0*3]*inv_height_scale_2_s;
final_matrix[ 2]= projection[2+0*3]*m_far_d_m_near;
final_matrix[ 3]= projection[2+0*3];
final_matrix[ 4]= projection[2+1*3]*inv_width_scale_1 + projection[0+1*3]*inv_width_scale_2;
final_matrix[ 5]= projection[2+1*3]*inv_height_scale_1_s + projection[1+1*3]*inv_height_scale_2_s;
final_matrix[ 6]= projection[2+1*3]*m_far_d_m_near;
final_matrix[ 7]= projection[2+1*3];
final_matrix[ 8]= projection[2+2*3]*inv_width_scale_1 + projection[0+2*3]*inv_width_scale_2;
final_matrix[ 9]= projection[2+2*3]*inv_height_scale_1_s + projection[1+2*3]*inv_height_scale_2_s;
final_matrix[10]= projection[2+2*3]*m_far_d_m_near;
final_matrix[11]= projection[2+2*3];
final_matrix[12]= projection[2+3*3]*inv_width_scale_1 + projection[0+3*3]*inv_width_scale_2;
final_matrix[13]= projection[2+3*3]*inv_height_scale_1_s + projection[1+3*3]*inv_height_scale_2_s;
final_matrix[14]= projection[2+3*3]*m_far_d_m_near - (2*m_far*m_near)/m_far_s_m_near;
final_matrix[15]= projection[2+3*3];
*/
// matrix is ready. use it
glMatrixMode(GL_PROJECTION);
glLoadMatrixd(final_matrix);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// Step 3: render the mesh
for (unsigned int i = 0; i < model->face.size() ; ++i) {
glBegin(GL_POLYGON);
for (unsigned int j=0; j < model->face[i].size(); ++j){
int vi = model->face[i][j];
glVertex3f(model->vertex[vi].x, model->vertex[vi].y, model->vertex[vi].z);
}
glEnd();
}
glFinish(); // done rendering
////////////////////////////////////////////////////////////////////////////////
unsigned int* pDepthBuffer;
GLint outWidth, outHeight, bitPerDepth;
OSMesaGetDepthBuffer(ctx, &outWidth, &outHeight, &bitPerDepth, (void**)&pDepthBuffer);
float* pbufferD = new float[outWidth*outHeight];
for(int i=0; i<outWidth*outHeight; i++){
pbufferD[i] = float( m_near / (1.0 - double(pDepthBuffer[i])/(4294967296.0)) );
//double d = double(pDepthBuffer[i])/4294967296.0;
//pbufferD[i] = float( 1.0 / ( ( m_far / (m_far - m_near) ) - d * ( - (m_near * m_far) / (m_far - m_near) ) ) );
}
OSMesaDestroyContext(ctx);
delete [] pbufferRGB;
return pbufferD;
}
int main(int argc, char* argv[]) {
if (argc < 3) {
printf("Usage: ./zbuffer input.ply [output.pcd,outputFolder]\n");
return 1;
}
std::vector<Point> vertices;
std::vector<Triangle> faces;
std::vector<Point> pointcloud;
char buffer[128];
bool merge = opendir(argv[2]) == NULL;
readPLY(argv[1],&vertices,&faces);
//get bounding box
double minX=vertices[0].x,maxX=vertices[0].x;
double minY=vertices[0].y,maxY=vertices[0].y;
double minZ=vertices[0].z,maxZ=vertices[0].z;
for (size_t i=1; i<vertices.size(); i++) {
if (vertices[i].x < minX) minX = vertices[i].x;
else if (vertices[i].x > maxX) maxX = vertices[i].x;
if (vertices[i].y < minY) minY = vertices[i].y;
else if (vertices[i].y > maxY) maxY = vertices[i].y;
if (vertices[i].z < minZ) minZ = vertices[i].z;
else if (vertices[i].z > maxZ) maxZ = vertices[i].z;
}
printf("Bounding box: x:(%.2f %.2f) y:(%.2f %.2f) z:(%.2f %.2f)\n",minX,maxX,minY,maxY,minZ,maxZ);
Point centroid = {
(minX + maxX) / 2,
(minY + maxY) / 2,
(minZ + maxZ) / 2
};
for (size_t i = 0; i < vertices.size(); i++) {
vertices[i].x -= centroid.x;
vertices[i].y -= centroid.y;
vertices[i].z -= centroid.z;
}
int width = RESOLUTION;
int height = RESOLUTION;
OSMesaContext ctx;
ctx = OSMesaCreateContextExt(OSMESA_RGB, 32, 0, 0, NULL );
unsigned char * pbuffer = new unsigned char [3 * width * height];
// Bind the buffer to the context and make it current
if (!OSMesaMakeCurrent(ctx, (void*)pbuffer, GL_UNSIGNED_BYTE, width, height))
printf("fail to create MESA context\n");
OSMesaPixelStore(OSMESA_Y_UP, 0);
glEnable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glPolygonMode(GL_FRONT, GL_FILL);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
float fov = 70;
float fov_scale = 2 * tan(fov / 2 / 180 * M_PI);
float zfar = 100000;
gluPerspective(fov,1,1,zfar);
glViewport(0, 0, width, height);
float cameraX = maxX - minX;
float cameraY = maxY - minY;
float cameraZ = maxZ - minZ;
float cx = 0.5 * (width + 1);
float cy = 0.5 * (height + 1);
unsigned int* depth = new unsigned int[width * height];
float rho = sqrt(cameraX*cameraX + cameraY*cameraY);
float theta = atan2(cameraY, cameraX);
int depthBits=0;
glGetIntegerv(GL_DEPTH_BITS, &depthBits);
printf("depth buffer bits %d\n",depthBits);
int numViews = INCLUDE_TOP ? NUM_CAMERAS + 2 : NUM_CAMERAS;
for (int k = 0; k < numViews; k++) {
// if (!merge)
// pointcloud.clear();
float rx = rho * cos(theta);
float ry = rho * sin(theta);
theta += 2 * 3.14159265 / NUM_CAMERAS;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
if (k < NUM_CAMERAS)
gluLookAt(rx,ry, cameraZ, 0,0,0, 0,0,1);
else if (k == NUM_CAMERAS)
gluLookAt(0,0, cameraZ*4, 0,0,0, 1,0,0);
else if (k == NUM_CAMERAS + 1)
gluLookAt(0,0, -cameraZ*4, 0,0,0, 1,0,0);
GLfloat R[16] = {};
glGetFloatv(GL_MODELVIEW_MATRIX, R);
// printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",R[0],R[1],R[2],R[3],R[4],R[5],R[6],R[7],R[8],R[9],R[10],R[11],R[12],R[13],R[14],R[15]);
// printf("camera: %f %f %f\n",rx,ry,cameraZ);
glBegin(GL_TRIANGLES);
glColor3ub(150, 150, 150);
for (size_t i = 0; i < faces.size(); i++) {
Point p1 = vertices[faces[i].id1];
Point p2 = vertices[faces[i].id2];
Point p3 = vertices[faces[i].id3];
glVertex3f(p1.x, p1.y, p1.z);
glVertex3f(p2.x, p2.y, p2.z);
glVertex3f(p3.x, p3.y, p3.z);
}
glEnd();
glFinish(); // done rendering
GLint outWidth, outHeight, bitPerDepth;
GLboolean ret = OSMesaGetDepthBuffer(ctx, &outWidth, &outHeight, &bitPerDepth, (void**)&depth);
for (int i = 0; i < width; i++) {
for (int j = 0; j < height; j++) {
float buffer_z = (float) depth[j * width + i] / 0xFFFFFFFF;
if (buffer_z > 0 && buffer_z < 1) {
// float z = -1 / (1 - buffer_z);
float z = 1 / (buffer_z - 1 - buffer_z / zfar);
float x = (i - cx) * -z / width * fov_scale;
float y = (j - cy) * -z / height * fov_scale;
x -= R[12];
y -= R[13];
z -= R[14];
Point p = {
R[0] * x + R[1] * y + R[2] * z,
R[4] * x + R[5] * y + R[6] * z,
R[8] * x + R[9] * y + R[10] * z
};
pointcloud.push_back(p);
}
}
}
// printf("pointcloud: %lu\n",pointcloud.size());
if (!merge && pointcloud.size() > 0) {
int n=0;
while (true) {
sprintf(buffer,"%s/%d-cloud.pcd",argv[2],n);
FILE* f = fopen(buffer,"r");
if (!f) {
writeToPCD(buffer,&pointcloud);
break;
}
fclose(f);
n++;
}
}
}
if (merge && pointcloud.size() > 0) {
sprintf(buffer,"%s",argv[2]);
writeToPCD(buffer,&pointcloud);
}
// sprintf(buffer,"%s/vertex.pcd",argv[2]);
// writeToPCD(buffer,&vertices);
OSMesaDestroyContext(ctx);
return 0;
}