in opencv-kinfu-samples/main.cpp [370:653]
int main(int argc, char** argv)
{
PrintUsage();
k4a_device_t device = NULL;
if (argc > 2)
{
printf("Please read the Usage\n");
return 2;
}
// Configure the depth mode and fps
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
if (argc == 2)
{
if (!_stricmp(argv[1], "nfov_unbinned"))
{
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
}
else if (!_stricmp(argv[1], "wfov_2x2binned"))
{
config.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
}
else if (!_stricmp(argv[1], "wfov_unbinned"))
{
config.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED;
config.camera_fps = K4A_FRAMES_PER_SECOND_15;
}
else if (!_stricmp(argv[1], "nfov_2x2binned"))
{
config.depth_mode = K4A_DEPTH_MODE_NFOV_2X2BINNED;
}
else if (!_stricmp(argv[1], "/?"))
{
return 0;
}
else
{
printf("Depth mode not supported!\n");
return 1;
}
}
uint32_t device_count = k4a_device_get_installed_count();
if (device_count == 0)
{
printf("No K4A devices found\n");
return 1;
}
if (K4A_RESULT_SUCCEEDED != k4a_device_open(K4A_DEVICE_DEFAULT, &device))
{
printf("Failed to open device\n");
k4a_device_close(device);
return 1;
}
// Retrive calibration
k4a_calibration_t calibration;
if (K4A_RESULT_SUCCEEDED !=
k4a_device_get_calibration(device, config.depth_mode, config.color_resolution, &calibration))
{
printf("Failed to get calibration\n");
k4a_device_close(device);
return 1;
}
// Start cameras
if (K4A_RESULT_SUCCEEDED != k4a_device_start_cameras(device, &config))
{
printf("Failed to start device\n");
k4a_device_close(device);
return 1;
}
// Generate a pinhole model for depth camera
pinhole_t pinhole = create_pinhole_from_xy_range(&calibration, K4A_CALIBRATION_TYPE_DEPTH);
interpolation_t interpolation_type = INTERPOLATION_BILINEAR_DEPTH;
#ifdef HAVE_OPENCV
setUseOptimized(true);
// Retrieve calibration parameters
k4a_calibration_intrinsic_parameters_t *intrinsics = &calibration.depth_camera_calibration.intrinsics.parameters;
const int width = calibration.depth_camera_calibration.resolution_width;
const int height = calibration.depth_camera_calibration.resolution_height;
// Initialize kinfu parameters
Ptr<kinfu::Params> params;
params = kinfu::Params::defaultParams();
initialize_kinfu_params(
*params, width, height, pinhole.fx, pinhole.fy, pinhole.px, pinhole.py);
// Distortion coefficients
Matx<float, 1, 8> distCoeffs;
distCoeffs(0) = intrinsics->param.k1;
distCoeffs(1) = intrinsics->param.k2;
distCoeffs(2) = intrinsics->param.p1;
distCoeffs(3) = intrinsics->param.p2;
distCoeffs(4) = intrinsics->param.k3;
distCoeffs(5) = intrinsics->param.k4;
distCoeffs(6) = intrinsics->param.k5;
distCoeffs(7) = intrinsics->param.k6;
k4a_image_t lut = NULL;
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
pinhole.width,
pinhole.height,
pinhole.width * (int)sizeof(coordinate_t),
&lut);
create_undistortion_lut(&calibration, K4A_CALIBRATION_TYPE_DEPTH, &pinhole, lut, interpolation_type);
// Create KinectFusion module instance
Ptr<kinfu::KinFu> kf;
kf = kinfu::KinFu::create(params);
namedWindow("AzureKinect KinectFusion Example");
viz::Viz3d visualization("AzureKinect KinectFusion Example");
bool stop = false;
bool renderViz = false;
k4a_capture_t capture = NULL;
k4a_image_t depth_image = NULL;
k4a_image_t undistorted_depth_image = NULL;
const int32_t TIMEOUT_IN_MS = 1000;
while (!stop && !visualization.wasStopped())
{
// Get a depth frame
switch (k4a_device_get_capture(device, &capture, TIMEOUT_IN_MS))
{
case K4A_WAIT_RESULT_SUCCEEDED:
break;
case K4A_WAIT_RESULT_TIMEOUT:
printf("Timed out waiting for a capture\n");
continue;
break;
case K4A_WAIT_RESULT_FAILED:
printf("Failed to read a capture\n");
k4a_device_close(device);
return 1;
}
// Retrieve depth image
depth_image = k4a_capture_get_depth_image(capture);
if (depth_image == NULL)
{
printf("Depth16 None\n");
k4a_capture_release(capture);
continue;
}
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16,
pinhole.width,
pinhole.height,
pinhole.width * (int)sizeof(uint16_t),
&undistorted_depth_image);
remap(depth_image, lut, undistorted_depth_image, interpolation_type);
// Create frame from depth buffer
uint8_t *buffer = k4a_image_get_buffer(undistorted_depth_image);
uint16_t *depth_buffer = reinterpret_cast<uint16_t *>(buffer);
UMat undistortedFrame;
create_mat_from_buffer<uint16_t>(depth_buffer, width, height).copyTo(undistortedFrame);
if (undistortedFrame.empty())
{
k4a_image_release(depth_image);
k4a_image_release(undistorted_depth_image);
k4a_capture_release(capture);
continue;
}
// Update KinectFusion
if (!kf->update(undistortedFrame))
{
printf("Reset KinectFusion\n");
kf->reset();
k4a_image_release(depth_image);
k4a_image_release(undistorted_depth_image);
k4a_capture_release(capture);
continue;
}
// Retrieve rendered TSDF
UMat tsdfRender;
kf->render(tsdfRender);
// Retrieve fused point cloud and normals
UMat points;
UMat normals;
kf->getCloud(points, normals);
// Show TSDF rendering
imshow("AzureKinect KinectFusion Example", tsdfRender);
// Show fused point cloud and normals
if (!points.empty() && !normals.empty() && renderViz)
{
viz::WCloud cloud(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, 1, 0.01, viz::Color::cyan());
visualization.showWidget("cloud", cloud);
visualization.showWidget("normals", cloudNormals);
visualization.showWidget("worldAxes", viz::WCoordinateSystem());
Vec3d volSize = kf->getParams().voxelSize * kf->getParams().volumeDims;
visualization.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), kf->getParams().volumePose);
visualization.spinOnce(1, true);
}
// Key controls
const int32_t key = waitKey(5);
if (key == 'r')
{
printf("Reset KinectFusion\n");
kf->reset();
}
else if (key == 'v')
{
renderViz = true;
}
else if (key == 'w')
{
// Output the fused point cloud from KinectFusion
Mat out_points;
Mat out_normals;
points.copyTo(out_points);
normals.copyTo(out_normals);
printf("Saving fused point cloud into ply file ...\n");
// Save to the ply file
#define PLY_START_HEADER "ply"
#define PLY_END_HEADER "end_header"
#define PLY_ASCII "format ascii 1.0"
#define PLY_ELEMENT_VERTEX "element vertex"
string output_file_name = "kf_output.ply";
ofstream ofs(output_file_name); // text mode first
ofs << PLY_START_HEADER << endl;
ofs << PLY_ASCII << endl;
ofs << PLY_ELEMENT_VERTEX << " " << out_points.rows << endl;
ofs << "property float x" << endl;
ofs << "property float y" << endl;
ofs << "property float z" << endl;
ofs << "property float nx" << endl;
ofs << "property float ny" << endl;
ofs << "property float nz" << endl;
ofs << PLY_END_HEADER << endl;
ofs.close();
stringstream ss;
for (int i = 0; i < out_points.rows; ++i)
{
ss << out_points.at<float>(i, 0) << " "
<< out_points.at<float>(i, 1) << " "
<< out_points.at<float>(i, 2) << " "
<< out_normals.at<float>(i, 0) << " "
<< out_normals.at<float>(i, 1) << " "
<< out_normals.at<float>(i, 2) << endl;
}
ofstream ofs_text(output_file_name, ios::out | ios::app);
ofs_text.write(ss.str().c_str(), (streamsize)ss.str().length());
}
else if (key == 'q')
{
stop = true;
}
k4a_image_release(depth_image);
k4a_image_release(undistorted_depth_image);
k4a_capture_release(capture);
}
k4a_image_release(lut);
destroyAllWindows();
#endif
k4a_device_close(device);
return 0;
}