kinect/codes/Azure-Kinect-Sensor-SDK/examples/transformation/main.cpp

495 lines
17 KiB
C++
Raw Permalink Normal View History

2024-03-06 18:05:53 +00:00
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
#include <k4a/k4a.h>
#include <k4arecord/playback.h>
#include <string>
#include "transformation_helpers.h"
#include "turbojpeg.h"
static bool point_cloud_color_to_depth(k4a_transformation_t transformation_handle,
const k4a_image_t depth_image,
const k4a_image_t color_image,
std::string file_name)
{
int depth_image_width_pixels = k4a_image_get_width_pixels(depth_image);
int depth_image_height_pixels = k4a_image_get_height_pixels(depth_image);
k4a_image_t transformed_color_image = NULL;
if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
depth_image_width_pixels,
depth_image_height_pixels,
depth_image_width_pixels * 4 * (int)sizeof(uint8_t),
&transformed_color_image))
{
printf("Failed to create transformed color image\n");
return false;
}
k4a_image_t point_cloud_image = NULL;
if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
depth_image_width_pixels,
depth_image_height_pixels,
depth_image_width_pixels * 3 * (int)sizeof(int16_t),
&point_cloud_image))
{
printf("Failed to create point cloud image\n");
return false;
}
if (K4A_RESULT_SUCCEEDED != k4a_transformation_color_image_to_depth_camera(transformation_handle,
depth_image,
color_image,
transformed_color_image))
{
printf("Failed to compute transformed color image\n");
return false;
}
if (K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_point_cloud(transformation_handle,
depth_image,
K4A_CALIBRATION_TYPE_DEPTH,
point_cloud_image))
{
printf("Failed to compute point cloud\n");
return false;
}
tranformation_helpers_write_point_cloud(point_cloud_image, transformed_color_image, file_name.c_str());
k4a_image_release(transformed_color_image);
k4a_image_release(point_cloud_image);
return true;
}
static bool point_cloud_depth_to_color(k4a_transformation_t transformation_handle,
const k4a_image_t depth_image,
const k4a_image_t color_image,
std::string file_name)
{
// transform color image into depth camera geometry
int color_image_width_pixels = k4a_image_get_width_pixels(color_image);
int color_image_height_pixels = k4a_image_get_height_pixels(color_image);
k4a_image_t transformed_depth_image = NULL;
if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16,
color_image_width_pixels,
color_image_height_pixels,
color_image_width_pixels * (int)sizeof(uint16_t),
&transformed_depth_image))
{
printf("Failed to create transformed depth image\n");
return false;
}
k4a_image_t point_cloud_image = NULL;
if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
color_image_width_pixels,
color_image_height_pixels,
color_image_width_pixels * 3 * (int)sizeof(int16_t),
&point_cloud_image))
{
printf("Failed to create point cloud image\n");
return false;
}
if (K4A_RESULT_SUCCEEDED !=
k4a_transformation_depth_image_to_color_camera(transformation_handle, depth_image, transformed_depth_image))
{
printf("Failed to compute transformed depth image\n");
return false;
}
if (K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_point_cloud(transformation_handle,
transformed_depth_image,
K4A_CALIBRATION_TYPE_COLOR,
point_cloud_image))
{
printf("Failed to compute point cloud\n");
return false;
}
tranformation_helpers_write_point_cloud(point_cloud_image, color_image, file_name.c_str());
k4a_image_release(transformed_depth_image);
k4a_image_release(point_cloud_image);
return true;
}
static int capture(std::string output_dir, uint8_t deviceId = K4A_DEVICE_DEFAULT)
{
int returnCode = 1;
k4a_device_t device = NULL;
const int32_t TIMEOUT_IN_MS = 10000;
k4a_transformation_t transformation = NULL;
k4a_transformation_t transformation_color_downscaled = NULL;
k4a_capture_t capture = NULL;
std::string file_name = "";
uint32_t device_count = 0;
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
k4a_image_t depth_image = NULL;
k4a_image_t color_image = NULL;
k4a_image_t color_image_downscaled = NULL;
device_count = k4a_device_get_installed_count();
if (device_count == 0)
{
printf("No K4A devices found\n");
return 0;
}
if (K4A_RESULT_SUCCEEDED != k4a_device_open(deviceId, &device))
{
printf("Failed to open device\n");
goto Exit;
}
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
config.color_resolution = K4A_COLOR_RESOLUTION_720P;
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
config.synchronized_images_only = true; // ensures that depth and color images are both available in the capture
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");
goto Exit;
}
transformation = k4a_transformation_create(&calibration);
if (K4A_RESULT_SUCCEEDED != k4a_device_start_cameras(device, &config))
{
printf("Failed to start cameras\n");
goto Exit;
}
// Get a capture
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");
goto Exit;
case K4A_WAIT_RESULT_FAILED:
printf("Failed to read a capture\n");
goto Exit;
}
// Get a depth image
depth_image = k4a_capture_get_depth_image(capture);
if (depth_image == 0)
{
printf("Failed to get depth image from capture\n");
goto Exit;
}
// Get a color image
color_image = k4a_capture_get_color_image(capture);
if (color_image == 0)
{
printf("Failed to get color image from capture\n");
goto Exit;
}
// Compute color point cloud by warping color image into depth camera geometry
#ifdef _WIN32
file_name = output_dir + "\\color_to_depth.ply";
#else
file_name = output_dir + "/color_to_depth.ply";
#endif
if (point_cloud_color_to_depth(transformation, depth_image, color_image, file_name.c_str()) == false)
{
goto Exit;
}
// Compute color point cloud by warping depth image into color camera geometry
#ifdef _WIN32
file_name = output_dir + "\\depth_to_color.ply";
#else
file_name = output_dir + "/depth_to_color.ply";
#endif
if (point_cloud_depth_to_color(transformation, depth_image, color_image, file_name.c_str()) == false)
{
goto Exit;
}
// Compute color point cloud by warping depth image into color camera geometry with downscaled color image and
// downscaled calibration. This example's goal is to show how to configure the calibration and use the
// transformation API as it is when the user does not need a point cloud from high resolution transformed depth
// image. The downscaling method here is naively to average binning 2x2 pixels, user should choose their own
// appropriate downscale method on the color image, this example is only demonstrating the idea. However, no matter
// what scale you choose to downscale the color image, please keep the aspect ratio unchanged (to ensure the
// distortion parameters from original calibration can still be used for the downscaled image).
k4a_calibration_t calibration_color_downscaled;
memcpy(&calibration_color_downscaled, &calibration, sizeof(k4a_calibration_t));
calibration_color_downscaled.color_camera_calibration.resolution_width /= 2;
calibration_color_downscaled.color_camera_calibration.resolution_height /= 2;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.cx /= 2;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.cy /= 2;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.fx /= 2;
calibration_color_downscaled.color_camera_calibration.intrinsics.parameters.param.fy /= 2;
transformation_color_downscaled = k4a_transformation_create(&calibration_color_downscaled);
color_image_downscaled = downscale_image_2x2_binning(color_image);
if (color_image_downscaled == 0)
{
printf("Failed to downscaled color image\n");
goto Exit;
}
#ifdef _WIN32
file_name = output_dir + "\\depth_to_color_downscaled.ply";
#else
file_name = output_dir + "/depth_to_color_downscaled.ply";
#endif
if (point_cloud_depth_to_color(transformation_color_downscaled,
depth_image,
color_image_downscaled,
file_name.c_str()) == false)
{
goto Exit;
}
returnCode = 0;
Exit:
if (depth_image != NULL)
{
k4a_image_release(depth_image);
}
if (color_image != NULL)
{
k4a_image_release(color_image);
}
if (capture != NULL)
{
k4a_capture_release(capture);
}
if (transformation != NULL)
{
k4a_transformation_destroy(transformation);
}
if (transformation_color_downscaled != NULL)
{
k4a_transformation_destroy(transformation_color_downscaled);
}
if (device != NULL)
{
k4a_device_close(device);
}
return returnCode;
}
// Timestamp in milliseconds. Defaults to 1 sec as the first couple frames don't contain color
static int playback(char *input_path, int timestamp = 1000, std::string output_filename = "output.ply")
{
int returnCode = 1;
k4a_playback_t playback = NULL;
k4a_calibration_t calibration;
k4a_transformation_t transformation = NULL;
k4a_capture_t capture = NULL;
k4a_image_t depth_image = NULL;
k4a_image_t color_image = NULL;
k4a_image_t uncompressed_color_image = NULL;
k4a_result_t result;
k4a_stream_result_t stream_result;
// Open recording
result = k4a_playback_open(input_path, &playback);
if (result != K4A_RESULT_SUCCEEDED || playback == NULL)
{
printf("Failed to open recording %s\n", input_path);
goto Exit;
}
result = k4a_playback_seek_timestamp(playback, timestamp * 1000, K4A_PLAYBACK_SEEK_BEGIN);
if (result != K4A_RESULT_SUCCEEDED)
{
printf("Failed to seek timestamp %d\n", timestamp);
goto Exit;
}
printf("Seeking to timestamp: %d/%d (ms)\n",
timestamp,
(int)(k4a_playback_get_recording_length_usec(playback) / 1000));
stream_result = k4a_playback_get_next_capture(playback, &capture);
if (stream_result != K4A_STREAM_RESULT_SUCCEEDED || capture == NULL)
{
printf("Failed to fetch frame\n");
goto Exit;
}
if (K4A_RESULT_SUCCEEDED != k4a_playback_get_calibration(playback, &calibration))
{
printf("Failed to get calibration\n");
goto Exit;
}
transformation = k4a_transformation_create(&calibration);
// Fetch frame
depth_image = k4a_capture_get_depth_image(capture);
if (depth_image == 0)
{
printf("Failed to get depth image from capture\n");
goto Exit;
}
color_image = k4a_capture_get_color_image(capture);
if (color_image == 0)
{
printf("Failed to get color image from capture\n");
goto Exit;
}
///////////////////////////////
// Convert color frame from mjpeg to bgra
k4a_image_format_t format;
format = k4a_image_get_format(color_image);
if (format != K4A_IMAGE_FORMAT_COLOR_MJPG)
{
printf("Color format not supported. Please use MJPEG\n");
goto Exit;
}
int color_width, color_height;
color_width = k4a_image_get_width_pixels(color_image);
color_height = k4a_image_get_height_pixels(color_image);
if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
color_width,
color_height,
color_width * 4 * (int)sizeof(uint8_t),
&uncompressed_color_image))
{
printf("Failed to create image buffer\n");
goto Exit;
}
tjhandle tjHandle;
tjHandle = tjInitDecompress();
if (tjDecompress2(tjHandle,
k4a_image_get_buffer(color_image),
static_cast<unsigned long>(k4a_image_get_size(color_image)),
k4a_image_get_buffer(uncompressed_color_image),
color_width,
0, // pitch
color_height,
TJPF_BGRA,
TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE) != 0)
{
printf("Failed to decompress color frame\n");
if (tjDestroy(tjHandle))
{
printf("Failed to destroy turboJPEG handle\n");
}
goto Exit;
}
if (tjDestroy(tjHandle))
{
printf("Failed to destroy turboJPEG handle\n");
}
///////////////////////////////
// Compute color point cloud by warping depth image into color camera geometry
if (point_cloud_depth_to_color(transformation, depth_image, uncompressed_color_image, output_filename) == false)
{
printf("Failed to transform depth to color\n");
goto Exit;
}
returnCode = 0;
Exit:
if (playback != NULL)
{
k4a_playback_close(playback);
}
if (depth_image != NULL)
{
k4a_image_release(depth_image);
}
if (color_image != NULL)
{
k4a_image_release(color_image);
}
if (uncompressed_color_image != NULL)
{
k4a_image_release(uncompressed_color_image);
}
if (capture != NULL)
{
k4a_capture_release(capture);
}
if (transformation != NULL)
{
k4a_transformation_destroy(transformation);
}
return returnCode;
}
static void print_usage()
{
printf("Usage: transformation_example capture <output_directory> [device_id]\n");
printf("Usage: transformation_example playback <filename.mkv> [timestamp (ms)] [output_file]\n");
}
int main(int argc, char **argv)
{
int returnCode = 0;
if (argc < 2)
{
print_usage();
}
else
{
std::string mode = std::string(argv[1]);
if (mode == "capture")
{
if (argc == 3)
{
returnCode = capture(argv[2]);
}
else if (argc == 4)
{
returnCode = capture(argv[2], (uint8_t)atoi(argv[3]));
}
else
{
print_usage();
}
}
else if (mode == "playback")
{
if (argc == 3)
{
returnCode = playback(argv[2]);
}
else if (argc == 4)
{
returnCode = playback(argv[2], atoi(argv[3]));
}
else if (argc == 5)
{
returnCode = playback(argv[2], atoi(argv[3]), argv[4]);
}
else
{
print_usage();
}
}
else
{
print_usage();
}
}
return returnCode;
}