// 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();
}
int volumetric_knt_cuda(int argc, char **argv)
{
Timer timer;
int vol_size = vx_count * vx_size;
float half_vol_size = vol_size * 0.5f;
Eigen::Vector3i voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3i volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3i voxel_count(vx_count, vx_count, vx_count);
int total_voxels = voxel_count.x() * voxel_count.y() * voxel_count.z();
std::cout << std::fixed
<< "Voxel Count : " << voxel_count.transpose() << std::endl
<< "Voxel Size : " << voxel_size.transpose() << std::endl
<< "Volume Size : " << volume_size.transpose() << std::endl
<< "Total Voxels : " << total_voxels << std::endl
<< std::endl;
timer.start();
KinectFrame knt(filepath);
timer.print_interval("Importing knt frame : ");
Eigen::Affine3f grid_affine = Eigen::Affine3f::Identity();
grid_affine.translate(Eigen::Vector3f(0, 0, half_vol_size));
grid_affine.scale(Eigen::Vector3f(1, 1, -1)); // z is negative inside of screen
Eigen::Matrix4f grid_matrix = grid_affine.matrix();
float knt_near_plane = 0.1f;
float knt_far_plane = 10240.0f;
Eigen::Matrix4f projection = perspective_matrix<float>(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, knt_near_plane, knt_far_plane);
Eigen::Matrix4f projection_inverse = projection.inverse();
Eigen::Matrix4f view_matrix = Eigen::Matrix4f::Identity();
std::vector<float4> vertices(knt.depth.size(), make_float4(0, 0, 0, 1));
std::vector<float4> normals(knt.depth.size(), make_float4(0, 0, 1, 1));
std::vector<Eigen::Vector2f> grid_voxels_params(total_voxels);
//
// setup image parameters
//
unsigned short image_width = 2;
unsigned short image_height = 2;
uchar4* image_data = new uchar4[image_width * image_height];
memset(image_data, 0, image_width * image_height * sizeof(uchar4));
float4* debug_buffer = new float4[image_width * image_height];
memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
knt_cuda_setup(
vx_count, vx_size,
grid_matrix.data(),
projection.data(),
projection_inverse.data(),
*grid_voxels_params.data()->data(),
KINECT_V2_DEPTH_WIDTH,
KINECT_V2_DEPTH_HEIGHT,
KINECT_V2_DEPTH_MIN,
KINECT_V2_DEPTH_MAX,
vertices.data()[0],
normals.data()[0],
image_width,
image_height,
*image_data,
*debug_buffer
);
timer.start();
knt_cuda_allocate();
knt_cuda_init_grid();
timer.print_interval("Allocating gpu : ");
timer.start();
knt_cuda_copy_host_to_device();
knt_cuda_copy_depth_buffer_to_device(knt.depth.data());
timer.print_interval("Copy host to device : ");
timer.start();
knt_cuda_normal_estimation();
timer.print_interval("Normal estimation : ");
timer.start();
knt_cuda_update_grid(view_matrix.data());
timer.print_interval("Update grid : ");
timer.start();
knt_cuda_grid_params_copy_device_to_host();
knt_cuda_copy_device_to_host();
timer.print_interval("Copy device to host : ");
timer.start();
knt_cuda_free();
timer.print_interval("Cleanup gpu : ");
timer.start();
Eigen::Affine3f grid_affine_2 = Eigen::Affine3f::Identity();
grid_affine_2.translate(Eigen::Vector3f(-half_vol_size, -half_vol_size, -vol_size));
export_volume(
"../../data/grid_volume_gpu_knt.obj",
voxel_count,
voxel_size,
grid_voxels_params);
//grid_affine_2.matrix());
export_obj_with_colors("../../data/knt_grid_frame_normals.obj", vertices, normals);
timer.print_interval("Exporting volume : ");
return 0;
}
int volumetric_knt_cuda(int argc, char **argv)
{
Timer timer;
int vol_size = vx_count * vx_size;
float half_vol_size = vol_size * 0.5f;
Eigen::Vector3i voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3i volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3i voxel_count(vx_count, vx_count, vx_count);
int total_voxels = voxel_count.x() * voxel_count.y() * voxel_count.z();
std::cout << std::fixed
<< "Voxel Count : " << voxel_count.transpose() << std::endl
<< "Voxel Size : " << voxel_size.transpose() << std::endl
<< "Volume Size : " << volume_size.transpose() << std::endl
<< "Total Voxels : " << total_voxels << std::endl
<< std::endl;
timer.start();
KinectFrame knt(filepath);
timer.print_interval("Importing knt frame : ");
Eigen::Affine3f grid_affine = Eigen::Affine3f::Identity();
grid_affine.translate(Eigen::Vector3f(0, 0, half_vol_size));
grid_affine.scale(Eigen::Vector3f(1, 1, 1)); // z is negative inside of screen
Eigen::Matrix4f grid_matrix = grid_affine.matrix();
float knt_near_plane = 0.1f;
float knt_far_plane = 10240.0f;
Eigen::Matrix4f projection = perspective_matrix<float>(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, knt_near_plane, knt_far_plane);
Eigen::Matrix4f projection_inverse = projection.inverse();
Eigen::Matrix4f view_matrix = Eigen::Matrix4f::Identity();
std::vector<float4> vertices(knt.depth.size(), make_float4(0, 0, 0, 1));
std::vector<float4> normals(knt.depth.size(), make_float4(0, 0, 1, 1));
std::vector<Eigen::Vector2f> grid_voxels_params(total_voxels);
//
// setup image parameters
//
unsigned short image_width = KINECT_V2_DEPTH_WIDTH;
unsigned short image_height = image_width / aspect_ratio;
QImage img(image_width, image_height, QImage::Format::Format_RGBA8888);
img.fill(Qt::GlobalColor::gray);
uchar4* image_data = (uchar4*)img.bits();
//float4* debug_buffer = new float4[image_width * image_height];
//memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
knt_cuda_setup(
vx_count, vx_size,
grid_matrix.data(),
projection.data(),
projection_inverse.data(),
*grid_voxels_params.data()->data(),
KINECT_V2_DEPTH_WIDTH,
KINECT_V2_DEPTH_HEIGHT,
KINECT_V2_DEPTH_MIN,
KINECT_V2_DEPTH_MAX,
vertices.data()[0],
normals.data()[0],
image_width,
image_height
);
timer.start();
knt_cuda_allocate();
knt_cuda_init_grid();
timer.print_interval("Allocating gpu : ");
timer.start();
knt_cuda_copy_host_to_device();
knt_cuda_copy_depth_buffer_to_device(knt.depth.data());
timer.print_interval("Copy host to device : ");
timer.start();
knt_cuda_normal_estimation();
timer.print_interval("Normal estimation : ");
timer.start();
knt_cuda_update_grid(view_matrix.data());
timer.print_interval("Update grid : ");
timer.start();
knt_cuda_grid_params_copy_device_to_host();
knt_cuda_copy_device_to_host();
timer.print_interval("Copy device to host : ");
//
// setup camera parameters
//
timer.start();
Eigen::Affine3f camera_to_world = Eigen::Affine3f::Identity();
float cam_z = -half_vol_size;
camera_to_world.scale(Eigen::Vector3f(1, 1, -1));
camera_to_world.translate(Eigen::Vector3f(half_vol_size, half_vol_size, cam_z));
Eigen::Matrix4f camera_to_world_matrix = camera_to_world.matrix();
knt_cuda_raycast(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, camera_to_world_matrix.data());
timer.print_interval("Raycast : ");
timer.start();
knt_cuda_copy_image_device_to_host(*(uchar4*)img.bits());
timer.print_interval("Copy Img to host : ");
timer.start();
knt_cuda_free();
timer.print_interval("Cleanup gpu : ");
#if 0
//memset(image_data, 0, image_width * image_height * sizeof(uchar4));
//memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
Eigen::Vector3f camera_pos = camera_to_world_matrix.col(3).head<3>();
float fov_scale = (float)tan(DegToRad(KINECT_V2_FOVY * 0.5f));
float aspect_ratio = KINECT_V2_DEPTH_ASPECT_RATIO;
//
// for each pixel, trace a ray
//
timer.start();
for (int y = 0; y < image_height; ++y)
{
for (int x = 0; x < image_width; ++x)
{
// Convert from image space (in pixels) to screen space
// Screen Space along X axis = [-aspect ratio, aspect ratio]
// Screen Space along Y axis = [-1, 1]
float x_norm = (2.f * float(x) + 0.5f) / (float)image_width;
float y_norm = (2.f * float(y) + 0.5f) / (float)image_height;
Eigen::Vector3f screen_coord(
(x_norm - 1.f) * aspect_ratio * fov_scale,
(1.f - y_norm) * fov_scale,
1.0f);
Eigen::Vector3f direction;
multDirMatrix(screen_coord, camera_to_world_matrix, direction);
direction.normalize();
long voxels_zero_crossing[2] = { -1, -1 };
int hit_count = raycast_tsdf_volume<float>(
camera_pos,
direction,
voxel_count.cast<int>(),
voxel_size.cast<int>(),
grid_voxels_params,
voxels_zero_crossing);
if (hit_count > 0)
{
if (hit_count == 2)
{
float4 n = normals[y * image_width + x];
//image_data[y * image_width + x].x = 0;
//image_data[y * image_width + x].y = 128;
//image_data[y * image_width + x].z = 128;
//image_data[y * image_width + x].w = 255;
image_data[y * image_width + x].x = uchar((n.x * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].y = uchar((n.y * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].z = uchar((n.z * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].w = 255;
}
else
{
image_data[y * image_width + x].x = 128;
image_data[y * image_width + x].y = 128;
image_data[y * image_width + x].z = 0;
image_data[y * image_width + x].w = 255;
}
}
else
{
image_data[y * image_width + x].x = 128;
image_data[y * image_width + x].y = 0;
image_data[y * image_width + x].z = 0;
image_data[y * image_width + x].w = 255;
}
}
}
timer.print_interval("Raycasting to image : ");
//export_debug_buffer("../../data/cpu_image_data_screen_coord_f4.txt", debug_buffer, image_width, image_height);
//export_image_buffer("../../data/cpu_image_data_screen_coord_uc.txt", image_data, image_width, image_height);
#else
//export_debug_buffer("../../data/gpu_image_data_screen_coord_f4.txt", debug_buffer, image_width, image_height);
//export_image_buffer("../../data/gpu_image_data_screen_coord_uc.txt", image_data, image_width, image_height);
#endif
QImage image(&image_data[0].x, image_width, image_height, QImage::Format_RGBA8888);
//image.fill(Qt::GlobalColor::black);
QApplication app(argc, argv);
QImageWidget widget;
widget.resize(KINECT_V2_DEPTH_WIDTH, KINECT_V2_DEPTH_HEIGHT);
widget.setImage(image);
widget.show();
return app.exec();
}
