void esProj_perspective(
esMat4f *mat, float fov, float screenratio, float near, float far,
esVec3f eye, esVec3f at, esVec3f up) {
float persp[16];
perspective_matrix(persp, fov, screenratio, near, far);
float look[16];
lookat_matrix(look, eye, at, up);
mul_matrix(mat->mat, look, persp);
}
static void
Render(unsigned int width, unsigned int height, shader_data* data)
{
float matrix_rotate[16], matrix_modelview[16], matrix_perspective[16], matrix_mvp[16];
/*
* Do some rotation with Euler angles. It is not a fixed axis as
* quaterions would be, but the effect is cool.
*/
rotate_matrix((float)data->angle_x, 1.0f, 0.0f, 0.0f, matrix_modelview);
rotate_matrix((float)data->angle_y, 0.0f, 1.0f, 0.0f, matrix_rotate);
multiply_matrix(matrix_rotate, matrix_modelview, matrix_modelview);
rotate_matrix((float)data->angle_z, 0.0f, 1.0f, 0.0f, matrix_rotate);
multiply_matrix(matrix_rotate, matrix_modelview, matrix_modelview);
/* Pull the camera back from the cube */
matrix_modelview[14] -= 2.5;
perspective_matrix(45.0f, (float)width/height, 0.01f, 100.0f, matrix_perspective);
multiply_matrix(matrix_perspective, matrix_modelview, matrix_mvp);
GL_CHECK(glUniformMatrix4fv(data->attr_mvp, 1, GL_FALSE, matrix_mvp));
data->angle_x += 3;
data->angle_y += 2;
data->angle_z += 1;
if(data->angle_x >= 360) data->angle_x -= 360;
if(data->angle_x < 0) data->angle_x += 360;
if(data->angle_y >= 360) data->angle_y -= 360;
if(data->angle_y < 0) data->angle_y += 360;
if(data->angle_z >= 360) data->angle_z -= 360;
if(data->angle_z < 0) data->angle_z += 360;
GL_CHECK(glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT));
GL_CHECK(glDrawArrays(GL_TRIANGLES, 0, 36));
}
void renderFrame() {
static float grey;
//grey += 0.01f;
//if (grey > 1.0f) {
// grey = 0.0f;
//}
glClearColor(grey, grey, grey, 1.0f);
checkGlError("glClearColor");
glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
checkGlError("glClear");
glUseProgram(gProgram);
checkGlError("glUseProgram");
#if USE_MESH
glVertexAttribPointer(gvPositionHandle, 4, GL_FLOAT, GL_FALSE, 0, gpMesh->m_pTriangleList);
#else
glVertexAttribPointer(gvPositionHandle, 3, GL_FLOAT, GL_FALSE, 0, gCubeVertices);
#endif
glEnableVertexAttribArray(gvPositionHandle);
checkGlError("glEnableVertexAttribArray");
#if USE_MESH
glVertexAttribPointer(gvColorHandle, 3, GL_FLOAT, GL_FALSE, 0, gpMesh->m_pVertexColor);
checkGlError("glVertexAttribPointer");
#else
glVertexAttribPointer(gvColorHandle, 3, GL_FLOAT, GL_FALSE, 0, gCubeColors);
checkGlError("glVertexAttribPointer");
#endif
glEnableVertexAttribArray(gvColorHandle);
checkGlError("glEnableVertexAttribArray");
//rotate - begin
rotate_matrix(iXangle, 1.0, 0.0, 0.0, aModelView);
rotate_matrix(iYangle, 0.0, 1.0, 0.0, aRotate);
multiply_matrix(aRotate, aModelView, aModelView);
rotate_matrix(iZangle, 0.0, 0.0, 1.0, aRotate);
multiply_matrix(aRotate, aModelView, aModelView);
//Pull the camera back from the geometry
aModelView[14] -= 3.5;
#if USE_MESH
//translate to lower left of geometry to lower-left of screen
aModelView[12] -= 1.0;
aModelView[13] -= 1.7;
#endif
perspective_matrix(45.0, (double)uiWidth/(double)uiHeight, 0.01, 100.0, aPerspective);
multiply_matrix(aPerspective, aModelView, aMVP);
glUniformMatrix4fv(gmvP, 1, GL_FALSE, aMVP);
glUniform1f(gAHandle, -1.0*(1.0 - animParam));
glUniform1f(gThetaHandle, animParam*_HALF_PI);
animParam -= 0.01;
if(animParam < 0.0) animParam = 1.0f;
#if !USE_MESH
//Comment the lines below to disable rotation
iZangle += 2;
iYangle += 2;
#endif
if(iXangle >= 360) iXangle -= 360;
if(iXangle < 0) iXangle += 360;
if(iYangle >= 360) iYangle -= 360;
if(iYangle < 0) iYangle += 360;
if(iZangle >= 360) iZangle -= 360;
if(iZangle < 0) iZangle += 360;
//rotate - end
#if USE_MESH
glDrawArrays(GL_TRIANGLES, 0, gpMesh->m_numTriangles);
#else
glDrawArrays(GL_TRIANGLES, 0, 36);
#endif
checkGlError("glDrawArrays");
}
// Usage: ./Volumetricd.exe ../../data/monkey.obj 256 4 2 90
int main(int argc, char **argv)
{
if (argc < 6)
{
std::cerr << "Missing parameters. Abort."
<< std::endl
<< "Usage: ./Volumetricd.exe ../../data/monkey.obj 256 8 2 90"
<< std::endl;
return EXIT_FAILURE;
}
Timer timer;
const std::string filepath = argv[1];
const int vol_size = atoi(argv[2]);
const int vx_size = atoi(argv[3]);
const int cloud_count = atoi(argv[4]);
const int rot_interval = atoi(argv[5]);
std::pair<std::vector<double>, std::vector<double>> depth_buffer;
//
// Projection and Modelview Matrices
//
Eigen::Matrix4d K = perspective_matrix(fov_y, aspect_ratio, near_plane, far_plane);
std::pair<Eigen::Matrix4d, Eigen::Matrix4d> T(Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity());
//
// Creating volume
//
Eigen::Vector3d voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3d volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3d voxel_count(volume_size.x() / voxel_size.x(), volume_size.y() / voxel_size.y(), volume_size.z() / voxel_size.z());
//
Eigen::Affine3d grid_affine = Eigen::Affine3d::Identity();
grid_affine.translate(Eigen::Vector3d(0, 0, -256));
grid_affine.scale(Eigen::Vector3d(1, 1, -1)); // z is negative inside of screen
Grid grid(volume_size, voxel_size, grid_affine.matrix());
//
// Importing .obj
//
timer.start();
std::vector<Eigen::Vector3d> points3DOrig, pointsTmp;
import_obj(filepath, points3DOrig);
timer.print_interval("Importing monkey : ");
std::cout << "Monkey point count : " << points3DOrig.size() << std::endl;
//
// Translating and rotating monkey point cloud
std::pair<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>> cloud;
//
Eigen::Affine3d rotate = Eigen::Affine3d::Identity();
Eigen::Affine3d translate = Eigen::Affine3d::Identity();
translate.translate(Eigen::Vector3d(0, 0, -256));
//
// Compute first cloud
//
for (Eigen::Vector3d p3d : points3DOrig)
{
Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
rot /= rot.w();
cloud.first.push_back(rot.head<3>());
}
//
// Update grid with first cloud
//
timer.start();
create_depth_buffer<double>(depth_buffer.first, cloud.first, K, Eigen::Matrix4d::Identity(), far_plane);
timer.print_interval("CPU compute depth : ");
timer.start();
update_volume(grid, depth_buffer.first, K, T.first);
timer.print_interval("CPU Update volume : ");
//
// Compute next clouds
Eigen::Matrix4d cloud_mat = Eigen::Matrix4d::Identity();
Timer iter_timer;
for (int i = 1; i < cloud_count; ++i)
{
std::cout << std::endl << i << " : " << i * rot_interval << std::endl;
iter_timer.start();
// Rotation matrix
rotate = Eigen::Affine3d::Identity();
rotate.rotate(Eigen::AngleAxisd(DegToRad(i * rot_interval), Eigen::Vector3d::UnitY()));
cloud.second.clear();
for (Eigen::Vector3d p3d : points3DOrig)
{
Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
rot /= rot.w();
cloud.second.push_back(rot.head<3>());
}
//export_obj("../../data/cloud_cpu_2.obj", cloud.second);
timer.start();
create_depth_buffer<double>(depth_buffer.second, cloud.second, K, Eigen::Matrix4d::Identity(), far_plane);
timer.print_interval("Compute depth buffer: ");
//export_depth_buffer("../../data/cpu_depth_buffer_2.obj", depth_buffer.second);
timer.start();
Eigen::Matrix4d icp_mat;
ComputeRigidTransform(cloud.first, cloud.second, icp_mat);
timer.print_interval("Compute rigid transf: ");
//std::cout << std::fixed << std::endl << "icp_mat " << std::endl << icp_mat << std::endl;
// accumulate matrix
cloud_mat = cloud_mat * icp_mat;
//std::cout << std::fixed << std::endl << "cloud_mat " << std::endl << cloud_mat << std::endl;
timer.start();
//update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
timer.print_interval("Update volume : ");
// copy second point cloud to first
cloud.first = cloud.second;
//depth_buffer.first = depth_buffer.second;
iter_timer.print_interval("Iteration time : ");
}
//std::cout << "------- // --------" << std::endl;
//for (int i = 0; i < grid.data.size(); ++i)
//{
// const Eigen::Vector3d& point = grid.data[i].point;
// std::cout << point.transpose() << "\t\t" << grid.data[i].tsdf << " " << grid.data[i].weight << std::endl;
//}
//std::cout << "------- // --------" << std::endl;
// timer.start();
// export_volume("../../data/grid_volume_cpu.obj", grid.data);
// timer.print_interval("Exporting volume : ");
// return 0;
QApplication app(argc, argv);
//
// setup opengl viewer
//
GLModelViewer glwidget;
glwidget.resize(640, 480);
glwidget.setPerspective(60.0f, 0.1f, 10240.0f);
glwidget.move(320, 0);
glwidget.setWindowTitle("Point Cloud");
glwidget.setWeelSpeed(0.1f);
glwidget.setDistance(-0.5f);
glwidget.show();
Eigen::Matrix4d to_origin = Eigen::Matrix4d::Identity();
to_origin.col(3) << -(volume_size.x() / 2.0), -(volume_size.y() / 2.0), -(volume_size.z() / 2.0), 1.0; // set translate
std::vector<Eigen::Vector4f> vertices, colors;
int i = 0;
for (int z = 0; z <= volume_size.z(); z += voxel_size.z())
{
for (int y = 0; y <= volume_size.y(); y += voxel_size.y())
{
for (int x = 0; x <= volume_size.x(); x += voxel_size.x(), i++)
{
const float tsdf = grid.data.at(i).tsdf;
//Eigen::Vector4d p = grid_affine.matrix() * to_origin * Eigen::Vector4d(x, y, z, 1);
Eigen::Vector4d p = to_origin * Eigen::Vector4d(x, y, z, 1);
p /= p.w();
if (tsdf > 0.1)
{
vertices.push_back(p.cast<float>());
colors.push_back(Eigen::Vector4f(0, 1, 0, 1));
}
else if (tsdf < -0.1)
{
vertices.push_back(p.cast<float>());
colors.push_back(Eigen::Vector4f(1, 0, 0, 1));
}
}
}
}
//
// setup model
//
std::shared_ptr<GLModel> model(new GLModel);
model->initGL();
model->setVertices(&vertices[0][0], vertices.size(), 4);
model->setColors(&colors[0][0], colors.size(), 4);
glwidget.addModel(model);
//
// setup kinect shader program
//
std::shared_ptr<GLShaderProgram> kinectShaderProgram(new GLShaderProgram);
if (kinectShaderProgram->build("color.vert", "color.frag"))
model->setShaderProgram(kinectShaderProgram);
return app.exec();
}
void viewer::init_program()
{
offx = 0.35f;
offy = -0.0f;
offz = -2.5f;
rot_x = 0.0f;
rot_y = 0.0f;
rot_z = 0.0f;
mouse_move_x = 0;
mouse_move_y = 0;
last_mouse_x = 0;
last_mouse_y = 0;
mouse_click = false;
active_shader_set = ShaderSet::all_white;
{
std::vector<GLuint> shaderList;
shaderList.push_back(LoadShader(GL_VERTEX_SHADER, "shaders/all_white.vert"));
shaderList.push_back(LoadShader(GL_FRAGMENT_SHADER, "shaders/all_white.frag"));
shader_programs[ShaderSet::all_white] = CreateProgram(shaderList);
}
{
std::vector<GLuint> shaderList;
shaderList.push_back(LoadShader(GL_VERTEX_SHADER, "shaders/normals.vert"));
shaderList.push_back(LoadShader(GL_GEOMETRY_SHADER, "shaders/normals.geom"));
shaderList.push_back(LoadShader(GL_FRAGMENT_SHADER, "shaders/normals.frag"));
shader_programs[ShaderSet::normals] = CreateProgram(shaderList);
}
{
std::vector<GLuint> shaderList;
shaderList.push_back(LoadShader(GL_VERTEX_SHADER, "shaders/height_map.vert"));
shaderList.push_back(LoadShader(GL_FRAGMENT_SHADER, "shaders/height_map.frag"));
shader_programs[ShaderSet::height_map] = CreateProgram(shaderList);
}
for(int i = 0; i < 3; ++i)
{
perspectiveMatrixUnif[i] = glGetUniformLocation(shader_programs[i], "perspectiveMatrix");
translationMatrixUnif[i] = glGetUniformLocation(shader_programs[i], "translationMatrix");
rotationXMatrixUnif[i] = glGetUniformLocation(shader_programs[i], "rotationXMatrix");
rotationYMatrixUnif[i] = glGetUniformLocation(shader_programs[i], "rotationYMatrix");
rotationZMatrixUnif[i] = glGetUniformLocation(shader_programs[i], "rotationZMatrix");
dirLightsUnif[i] = glGetUniformLocation(shader_programs[i], "dir_lights");
numLightsUnif[i] = glGetUniformLocation(shader_programs[i], "num_lights");
glUseProgram(shader_programs[i]);
glUniformMatrix4fv(perspectiveMatrixUnif[i], 1, GL_FALSE, &perspective_matrix()[0]);
float lights[] =
{
0.5, 0.7, 0.3,
-0.5, -0.7, -0.3,
0.5, -0.7, 0.3,
0.5, 0.7, -0.3
};
const int num_lights = sizeof(lights) / sizeof(float) / 3;
glUniform3fv(dirLightsUnif[i], num_lights, lights);
glUniform1i(numLightsUnif[i], num_lights);
glUseProgram(0);
}
}
int main(int argc, char **argv) {
EGLDisplay sEGLDisplay;
EGLContext sEGLContext;
EGLSurface sEGLSurface;
/* EGL Configuration */
EGLint aEGLAttributes[] = {
EGL_RED_SIZE, 8,
EGL_GREEN_SIZE, 8,
EGL_BLUE_SIZE, 8,
EGL_DEPTH_SIZE, 16,
EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
EGL_NONE
};
EGLint aEGLContextAttributes[] = {
EGL_CONTEXT_CLIENT_VERSION, 2,
EGL_NONE
};
EGLConfig aEGLConfigs[1];
EGLint cEGLConfigs;
#ifdef _WIN32
MSG sMessage;
#else
XSetWindowAttributes win_attrs;
int attrs[64], idx = 0, num_config = 0;
int major, minor;
Colormap colormap;
XVisualInfo *pVisual;
XEvent e;
#endif
GLint iLocPosition = 0;
GLint iLocColour, iLocTexCoord, iLocNormal, iLocMVP;
GLint iLocXangle, iLocYangle, iLocZangle;
GLint iLocAspect, iLocLightPos, iLocSampler, iLocSampler2;
GLuint uiProgram, uiFragShader, uiVertShader;
GLenum myTex, myTex2;
int bDone = 0;
const unsigned int uiWidth = 640;
const unsigned int uiHeight = 480;
int iXangle = 0, iYangle = 0, iZangle = 0;
float aTBNmatrix1[9], aTBNmatrix2[9];
float aLightPos[] = { 0.0f, 0.0f, -1.0f }; // Light is nearest camera.
unsigned char *myPixels = calloc(1, 128*128*4); // Holds texture data.
unsigned char *myPixels2 = calloc(1, 128*128*4); // Holds texture data.
float aRotate[16], aModelView[16], aPerspective[16], aMVP[16];
int i;
/* EGL Init */
#ifdef _WIN32
hDisplay = EGL_DEFAULT_DISPLAY;
#else
hDisplay = XOpenDisplay(NULL);
if (!hDisplay) {
printf("Could not open display\n");
exit(-1);
}
#endif
sEGLDisplay = EGL_CHECK(eglGetDisplay(hDisplay));
EGL_CHECK(eglInitialize(sEGLDisplay, NULL, NULL));
EGL_CHECK(eglChooseConfig(sEGLDisplay, aEGLAttributes, aEGLConfigs, 1, &cEGLConfigs));
if (cEGLConfigs == 0) {
printf("No EGL configurations were returned.\n");
exit(-1);
}
#ifdef _WIN32
hWindow = create_window(uiWidth, uiHeight);
#else
hWindow = create_window("OpenGL ES 2.0 Example on a Linux Desktop", uiWidth,
uiHeight, hDisplay, sEGLDisplay, aEGLConfigs[0], &colormap, &pVisual);
#endif
sEGLSurface = EGL_CHECK(eglCreateWindowSurface(sEGLDisplay, aEGLConfigs[0], (EGLNativeWindowType)hWindow, NULL));
if (sEGLSurface == EGL_NO_SURFACE) {
printf("Failed to create EGL surface.\n");
exit(-1);
}
sEGLContext = EGL_CHECK(eglCreateContext(sEGLDisplay, aEGLConfigs[0], EGL_NO_CONTEXT, aEGLContextAttributes));
if (sEGLContext == EGL_NO_CONTEXT) {
printf("Failed to create EGL context.\n");
exit(-1);
}
EGL_CHECK(eglMakeCurrent(sEGLDisplay, sEGLSurface, sEGLSurface, sEGLContext));
/* Shader Initialisation */
process_shader(&uiVertShader, "shader.vert", GL_VERTEX_SHADER);
process_shader(&uiFragShader, "shader.frag", GL_FRAGMENT_SHADER);
/* Create uiProgram (ready to attach shaders) */
uiProgram = GL_CHECK(glCreateProgram());
/* Attach shaders and link uiProgram */
GL_CHECK(glAttachShader(uiProgram, uiVertShader));
GL_CHECK(glAttachShader(uiProgram, uiFragShader));
GL_CHECK(glLinkProgram(uiProgram));
/* Get attribute locations of non-fixed attributes like colour and texture coordinates. */
iLocPosition = GL_CHECK(glGetAttribLocation(uiProgram, "av4position"));
iLocColour = GL_CHECK(glGetAttribLocation(uiProgram, "av3colour"));
#ifdef DEBUG
printf("iLocPosition = %i\n", iLocPosition);
printf("iLocColour = %i\n", iLocColour);
#endif
/* Get uniform locations */
iLocMVP = GL_CHECK(glGetUniformLocation(uiProgram, "mvp"));
#ifdef DEBUG
printf("iLocMVP = %i\n", iLocMVP);
#endif
GL_CHECK(glUseProgram(uiProgram));
/* Enable attributes for position, colour and texture coordinates etc. */
GL_CHECK(glEnableVertexAttribArray(iLocPosition));
GL_CHECK(glEnableVertexAttribArray(iLocColour));
/* Populate attributes for position, colour and texture coordinates etc. */
GL_CHECK(glVertexAttribPointer(iLocPosition, 3, GL_FLOAT, GL_FALSE, 0, aVertices));
GL_CHECK(glVertexAttribPointer(iLocColour, 3, GL_FLOAT, GL_FALSE, 0, aColours));
GL_CHECK(glEnable(GL_CULL_FACE));
GL_CHECK(glEnable(GL_DEPTH_TEST));
#ifndef _WIN32
XSelectInput(hDisplay, hWindow, KeyPressMask | ExposureMask | EnterWindowMask
| LeaveWindowMask | PointerMotionMask | VisibilityChangeMask | ButtonPressMask
| ButtonReleaseMask | StructureNotifyMask);
#endif
/* Enter event loop */
while (!bDone) {
#ifdef _WIN32
if(PeekMessage(&sMessage, NULL, 0, 0, PM_REMOVE)) {
if(sMessage.message == WM_QUIT) {
bDone = 1;
} else {
TranslateMessage(&sMessage);
DispatchMessage(&sMessage);
}
}
#else
while (XPending(hDisplay) > 0) {
XNextEvent(hDisplay, &e);
if (e.type == ButtonPress) {
bDone = 1;
}
}
#endif
/*
* Do some rotation with Euler angles. It is not a fixed axis as
* quaterions would be, but the effect is cool.
*/
rotate_matrix(iXangle, 1.0, 0.0, 0.0, aModelView);
rotate_matrix(iYangle, 0.0, 1.0, 0.0, aRotate);
multiply_matrix(aRotate, aModelView, aModelView);
rotate_matrix(iZangle, 0.0, 1.0, 0.0, aRotate);
multiply_matrix(aRotate, aModelView, aModelView);
/* Pull the camera back from the cube */
aModelView[14] -= 2.5;
perspective_matrix(45.0, (double)uiWidth/(double)uiHeight, 0.01, 100.0, aPerspective);
multiply_matrix(aPerspective, aModelView, aMVP);
GL_CHECK(glUniformMatrix4fv(iLocMVP, 1, GL_FALSE, aMVP));
iXangle += 3;
iYangle += 2;
iZangle += 1;
if(iXangle >= 360) iXangle -= 360;
if(iXangle < 0) iXangle += 360;
if(iYangle >= 360) iYangle -= 360;
if(iYangle < 0) iYangle += 360;
if(iZangle >= 360) iZangle -= 360;
if(iZangle < 0) iZangle += 360;
GL_CHECK(glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT));
GL_CHECK(glDrawArrays(GL_TRIANGLES, 0, 36));
if (!eglSwapBuffers(sEGLDisplay, sEGLSurface)) {
printf("Failed to swap buffers.\n");
}
#ifdef _WIN32
Sleep(20);
#else
usleep(20000);
#endif
}
/* Cleanup shaders */
GL_CHECK(glUseProgram(0));
GL_CHECK(glDeleteShader(uiVertShader));
GL_CHECK(glDeleteShader(uiFragShader));
GL_CHECK(glDeleteProgram(uiProgram));
/* EGL clean up */
EGL_CHECK(eglMakeCurrent(sEGLDisplay, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT));
EGL_CHECK(eglDestroySurface(sEGLDisplay, sEGLSurface));
EGL_CHECK(eglDestroyContext(sEGLDisplay, sEGLContext));
EGL_CHECK(eglTerminate(sEGLDisplay));
#ifndef _WIN32
/* X windows clean up */
XDestroyWindow(hDisplay, hWindow);
XFreeColormap(hDisplay, colormap);
XFree(pVisual);
XCloseDisplay(hDisplay);
#endif
return 0;
}
