参考了官方的去畸变以及采集代码
// C++
#include <iostream>
#include <chrono>
#include <string>
// OpenCV
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// Kinect DK
#include <k4a/k4a.hpp>
#include "undistort.hpp"
using namespace cv;
using namespace std;
// 宏
// 方便控制是否 std::cout 信息
#define DEBUG_std_cout 0
static void calibration_blob(k4a_device_t& device, string filename = "calibration.txt")
{
size_t calibration_size = 0;
k4a_buffer_result_t buffer_result = k4a_device_get_raw_calibration(device, NULL, &calibration_size);
if (buffer_result == K4A_BUFFER_RESULT_TOO_SMALL)
{
vector<uint8_t> calibration_buffer = vector<uint8_t>(calibration_size);
buffer_result = k4a_device_get_raw_calibration(device, calibration_buffer.data(), &calibration_size);
if (buffer_result == K4A_BUFFER_RESULT_SUCCEEDED)
{
ofstream file(filename, ofstream::binary);
file.write(reinterpret_cast<const char *>(&calibration_buffer[0]), (long)calibration_size);
file.close();
cout << "Calibration blob for device " << " (serial no. "
<< ") is saved to " << filename << endl;
}
else
{
cout << "Failed to get calibration blob" << endl;
exit(-1);
}
}
else
{
cout << "Failed to get calibration blob size" << endl;
exit(-1);
}
}
_k4a_calibration_camera_t print_calibration(k4a_device_t& device, k4a_device_configuration_t& deviceConfig)
{
k4a_calibration_t calibration;
// get calibration
if (K4A_RESULT_SUCCEEDED !=
k4a_device_get_calibration(device, deviceConfig.depth_mode, deviceConfig.color_resolution, &calibration))
{
cout << "Failed to get calibration" << endl;
exit(-1);
}
// auto calib = calibration.depth_camera_calibration;
auto calib = calibration.color_camera_calibration;
cout << "resolution width: " << calib.resolution_width << endl;
cout << "resolution height: " << calib.resolution_height << endl;
cout << "principal point x: " << calib.intrinsics.parameters.param.cx << endl;
cout << "principal point y: " << calib.intrinsics.parameters.param.cy << endl;
cout << "focal length x: " << calib.intrinsics.parameters.param.fx << endl;
cout << "focal length y: " << calib.intrinsics.parameters.param.fy << endl;
cout << "radial distortion coefficients:" << endl;
cout << "k1: " << calib.intrinsics.parameters.param.k1 << endl;
cout << "k2: " << calib.intrinsics.parameters.param.k2 << endl;
cout << "k3: " << calib.intrinsics.parameters.param.k3 << endl;
cout << "k4: " << calib.intrinsics.parameters.param.k4 << endl;
cout << "k5: " << calib.intrinsics.parameters.param.k5 << endl;
cout << "k6: " << calib.intrinsics.parameters.param.k6 << endl;
cout << "center of distortion in Z=1 plane, x: " << calib.intrinsics.parameters.param.codx << endl;
cout << "center of distortion in Z=1 plane, y: " << calib.intrinsics.parameters.param.cody << endl;
cout << "tangential distortion coefficient x: " << calib.intrinsics.parameters.param.p1 << endl;
cout << "tangential distortion coefficient y: " << calib.intrinsics.parameters.param.p2 << endl;
cout << "metric radius: " << calib.intrinsics.parameters.param.metric_radius << endl;
return calib;
}
cv::Mat undistorted(cv::Mat &image, _k4a_calibration_camera_t calib) {
float cx = calib.intrinsics.parameters.param.cx;
float cy = calib.intrinsics.parameters.param.cy;
float fx = calib.intrinsics.parameters.param.fx;
float fy = calib.intrinsics.parameters.param.fy;
float k1 = calib.intrinsics.parameters.param.k1;
float k2 = calib.intrinsics.parameters.param.k2;
float k3 = calib.intrinsics.parameters.param.k3;
float k4 = calib.intrinsics.parameters.param.k4;
float k5 = calib.intrinsics.parameters.param.k5;
float k6 = calib.intrinsics.parameters.param.k6;
float codx = calib.intrinsics.parameters.param.codx; // center of distortion is set to 0 for Brown Conrady model
float cody = calib.intrinsics.parameters.param.cody;
float p1 = calib.intrinsics.parameters.param.p1;
float p2 = calib.intrinsics.parameters.param.p2;
cv::Mat K = (cv::Mat_<double>(3, 3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0);
cv::Mat newK;
const cv::Mat D = (cv::Mat_<double>(8, 1) << k1, k2, p1, p2, k3, k4, k5, k6);
// k1,k2,p1,p2[,k3,[,k4,k5,k6]]
// cv::Mat K = (cv::Mat_<double>(3, 3) << 6.2053103020797198e+02, 0.0, 6.3205198180998377e+02
// , 0.0, 6.1656865116870870e+02, 3.5016213113500675e+02, 0.0, 0.0, 1.0);
// cv::Mat newK;
// const cv::Mat D = (cv::Mat_<double>(5, 1) << 6.8597090884417358e-02
// , -2.1407702890670174e-02, -6.8662708745400904e-03, -1.4302754640278781e-03, -5.3270781408391145e-03);
cv::Mat UndistortImage;
cv::undistort(image, UndistortImage, K, D, newK);
// cout<<K<<newK<<endl;
// cv::Mat map1, map2;
// const double alpha = 1;
// cv::Mat NewCameraMatrix = getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
// initUndistortRectifyMap(K, D, cv::Mat(), NewCameraMatrix, imageSize, CV_16SC2, map1, map2);
// cv::Mat UndistortImage;
// cv::remap(image, UndistortImage, map1, map2, cv::INTER_LINEAR);
return UndistortImage;
}
int main(int argc, char *argv[]) {
/*
找到并打开 Azure Kinect 设备
*/
// 发现已连接的设备数
const uint32_t device_count = k4a::device::get_installed_count();
if (0 == device_count) {
std::cout << "Error: no K4A devices found. " << std::endl;
return -1;
} else {
std::cout << "Found " << device_count << " connected devices. " << std::endl;
if (1 != device_count)// 超过1个设备,也输出错误信息。
{
std::cout << "Error: more than one K4A devices found. " << std::endl;
return -1;
} else// 该示例代码仅限对1个设备操作
{
std::cout << "Done: found 1 K4A device. " << std::endl;
}
}
// 打开(默认)设备
k4a_device_t device_t = NULL;
if (K4A_RESULT_SUCCEEDED != k4a_device_open(0, &device_t))
{
cout << ": Failed to open device" << endl;
exit(-1);
}
// calibration_blob(device_t);
// k4a::device device = k4a::device::open(K4A_DEVICE_DEFAULT);
k4a::device device = k4a::device(device_t);
std::cout << "Done: open device. " << std::endl;
/*
检索并保存 Azure Kinect 图像数据
*/
// 配置并启动设备
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
config.camera_fps = K4A_FRAMES_PER_SECOND_15;
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
config.color_resolution = K4A_COLOR_RESOLUTION_720P;
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
// config.depth_mode = K4A_DEPTH_MODE_NFOV_2X2BINNED;
config.synchronized_images_only = true;// ensures that depth and color images are both available in the capture
device.start_cameras(&config);
std::cout << "Done: start camera." << std::endl;
// auto calib=print_calibration(device_t,config);
//写入txt文件流
ofstream rgb_out;
ofstream d_out;
ofstream ir_out;
rgb_out.open("./rgb.txt");
d_out.open("./depth.txt");
ir_out.open("./ir.txt");
rgb_out<<"# color images"<<endl;
rgb_out<<"# file: rgbd_dataset"<<endl;
rgb_out<<"# timestamp"<<" "<<"filename"<<endl;
d_out<<"# depth images"<<endl;
d_out<<"# file: rgbd_dataset"<<endl;
d_out<<"# timestamp"<<" "<<"filename"<<endl;
ir_out<<"# ir images"<<endl;
ir_out<<"# file: rgbd_dataset"<<endl;
ir_out<<"# timestamp"<<" "<<"filename"<<endl;
rgb_out<<flush;
d_out<<flush;
// 稳定化
k4a::capture capture;
int iAuto = 0;//用来稳定,类似自动曝光
int iAutoError = 0;// 统计自动曝光的失败次数
while (true) {
if (device.get_capture(&capture)) {
std::cout << iAuto << ". Capture several frames to give auto-exposure" << std::endl;
// 跳过前 n 个(成功的数据采集)循环,用来稳定
if (iAuto != 30) {
iAuto++;
continue;
} else {
std::cout << "Done: auto-exposure" << std::endl;
break;// 跳出该循环,完成相机的稳定过程
}
} else {
std::cout << iAutoError << ". K4A_WAIT_RESULT_TIMEOUT." << std::endl;
if (iAutoError != 30) {
iAutoError++;
continue;
} else {
std::cout << "Error: failed to give auto-exposure. " << std::endl;
return -1;
}
}
}
std::cout << "-----------------------------------" << std::endl;
std::cout << "----- Have Started Kinect DK. -----" << std::endl;
std::cout << "-----------------------------------" << std::endl;
// 从设备获取捕获
k4a::image rgbImage;
k4a::image depthImage;
k4a::image irImage;
k4a::image transformed_rgbImage;
cv::Mat cv_rgbImage_with_alpha;
cv::Mat cv_rgbImage_no_alpha;
cv::Mat cv_depth;
cv::Mat cv_depth_8U;
cv::Mat cv_irImage;
cv::Mat cv_irImage_8U;
k4a_image_t lut = NULL;
k4a_image_t undistorted_rgb = NULL;
k4a_image_t undistorted_depth = NULL;
interpolation_t interpolation_type = INTERPOLATION_NEARESTNEIGHBOR;
k4a::calibration k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);// Get the camera calibration for the entire K4A device, which is used for all transformation functions.
k4a::transformation k4aTransformation = k4a::transformation(k4aCalibration);
pinhole_t pinhole;
pinhole = create_pinhole_from_xy_range(&k4aCalibration, K4A_CALIBRATION_TYPE_COLOR);
cout << "resolution width: " << pinhole.width << endl;
cout << "resolution height: " << pinhole.height << endl;
cout << "principal point x: " << pinhole.px << endl;
cout << "principal point y: " << pinhole.py << endl;
cout << "focal length x: " << pinhole.fx << endl;
cout << "focal length y: " << pinhole.fy << endl;
int count=0;
while (true)
// for (size_t i = 0; i < 100; i++)
{
// if (device.get_capture(&capture, std::chrono::milliseconds(0)))
if (device.get_capture(&capture)) {
// rgb
// * Each pixel of BGRA32 data is four bytes. The first three bytes represent Blue, Green,
// * and Red data. The fourth byte is the alpha channel and is unused in the Azure Kinect APIs.
rgbImage = capture.get_color_image();
#if DEBUG_std_cout == 1
std::cout << "[rgb] " << "\n"
<< "format: " << rgbImage.get_format() << "\n"
<< "device_timestamp: " << rgbImage.get_device_timestamp().count() << "\n"
<< "system_timestamp: " << rgbImage.get_system_timestamp().count() << "\n"
<< "height*width: " << rgbImage.get_height_pixels() << ", " << rgbImage.get_width_pixels()
<< std::endl;
#endif
// depth
// * Each pixel of DEPTH16 data is two bytes of little endian unsigned depth data. The unit of the data is in
// * millimeters from the origin of the camera.
depthImage = capture.get_depth_image();
#if DEBUG_std_cout == 1
std::cout << "[depth] " << "\n"
<< "format: " << depthImage.get_format() << "\n"
<< "device_timestamp: " << depthImage.get_device_timestamp().count() << "\n"
<< "system_timestamp: " << depthImage.get_system_timestamp().count() << "\n"
<< "height*width: " << depthImage.get_height_pixels() << ", " << depthImage.get_width_pixels()
<< std::endl;
#endif
// ir
// * Each pixel of IR16 data is two bytes of little endian unsigned depth data. The value of the data represents
// * brightness.
irImage = capture.get_ir_image();
#if DEBUG_std_cout == 1
std::cout << "[ir] " << "\n"
<< "format: " << irImage.get_format() << "\n"
<< "device_timestamp: " << irImage.get_device_timestamp().count() << "\n"
<< "system_timestamp: " << irImage.get_system_timestamp().count() << "\n"
<< "height*width: " << irImage.get_height_pixels() << ", " << irImage.get_width_pixels()
<< std::endl;
#endif
//深度图和RGB图配准
auto transformed_depthImage=k4aTransformation.depth_image_to_color_camera(depthImage);
cv_irImage = cv::Mat(irImage.get_height_pixels(), irImage.get_width_pixels(), CV_16U,
(void *) irImage.get_buffer(), static_cast<size_t>(irImage.get_stride_bytes()));
cv_irImage.convertTo(cv_irImage_8U, CV_8U, 1);
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
pinhole.width,
pinhole.height,
pinhole.width * (int)sizeof(coordinate_t),
&lut);
create_undistortion_lut(&k4aCalibration, K4A_CALIBRATION_TYPE_COLOR, &pinhole, lut, interpolation_type);
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16,
pinhole.width,
pinhole.height,
pinhole.width * (int)sizeof(uint16_t),
&undistorted_depth);
k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
pinhole.width,
pinhole.height,
pinhole.width * (int)sizeof(uint32_t),
&undistorted_rgb);
remap_BGRA(rgbImage.handle(), lut, undistorted_rgb);
remap(transformed_depthImage.handle(), lut, undistorted_depth, interpolation_type);
transformed_depthImage=k4a::image(undistorted_depth);
rgbImage=k4a::image(undistorted_rgb);
cv_depth = cv::Mat(transformed_depthImage.get_height_pixels(), transformed_depthImage.get_width_pixels(), CV_16U,
(void *) transformed_depthImage.get_buffer(), static_cast<size_t>(transformed_depthImage.get_stride_bytes()));
// cv_depth= cv::Mat(depthImage.get_height_pixels(), depthImage.get_width_pixels(), CV_16U,
// (void *) depthImage.get_buffer(), static_cast<size_t>(depthImage.get_stride_bytes()));
cv_rgbImage_with_alpha = cv::Mat(rgbImage.get_height_pixels(), rgbImage.get_width_pixels(), CV_8UC4,
(void *) rgbImage.get_buffer());
cv::cvtColor(cv_rgbImage_with_alpha, cv_rgbImage_no_alpha, cv::COLOR_BGRA2BGR);
cv_depth.convertTo(cv_depth_8U, CV_8U, 1);
// auto cv_undistorted_rgb= undistorted(cv_rgbImage_no_alpha,calib);
// auto cv_undistorted_depth= undistorted(cv_depth,calib);
// auto cv_undistorted_rgb= cv_rgbImage_no_alpha;
// auto cv_undistorted_depth= cv_depth;
// show image
// cv::imshow("color", cv_rgbImage_no_alpha);
// cv::imshow("depth", cv_depth_8U);
// cv::imshow("ir", cv_irImage_8U);
// save image
double time_rgb = static_cast<double >(std::chrono::duration_cast<std::chrono::microseconds>(
rgbImage.get_device_timestamp()).count());
auto prefix=std::to_string(count);
std::string filename_rgb = prefix+".png";
double time_d = static_cast<double >(std::chrono::duration_cast<std::chrono::microseconds>(
depthImage.get_device_timestamp()).count());
std::string filename_d_8u = prefix+"_8u.png";
std::string filename_d = prefix+".png";
double time_ir = static_cast<double >(std::chrono::duration_cast<std::chrono::microseconds>(
irImage.get_device_timestamp()).count());
std::string filename_ir = prefix+".png";
imwrite("./rgb/"+filename_rgb, cv_rgbImage_no_alpha);
imwrite("./depth/"+filename_d, cv_depth);
imwrite("./depth/"+filename_d_8u,cv_depth_8U);
// imwrite("./ir/"+filename_ir, cv_irImage_8U);
std::cout<<"Acquiring!"<<endl;
//写入depth.txt,rgb.txt文件
rgb_out<<std::to_string(time_rgb/1000000)<<" "<<"rgb/"<<filename_rgb<<endl;
d_out<<std::to_string(time_d/1000000)<<" "<<"depth/"<<filename_d<<endl;
ir_out<<std::to_string(time_ir/1000000)<<" "<<"ir/"<<filename_ir<<endl;
rgb_out<<flush;
d_out<<flush;
ir_out<<flush;
cv_rgbImage_with_alpha.release();
cv_rgbImage_no_alpha.release();
cv_depth.release();
cv_depth_8U.release();
cv_irImage.release();
cv_irImage_8U.release();
capture.reset();
count+=1;
//if (cv::waitKey(0) == 'q') {//按键采集,用户按下'q',跳出循环,结束采集
// std::cout << "----------------------------------" << std::endl;
// std::cout << "------------- closed -------------" << std::endl;
// std::cout << "----------------------------------" << std::endl;
// break;
// }
} else {
std::cout << "false: K4A_WAIT_RESULT_TIMEOUT." << std::endl;
}
}
cv::destroyAllWindows();
rgb_out<<flush;
d_out<<flush;
ir_out<<flush;
rgb_out.close();
d_out.close();
ir_out.close();
// 释放,关闭设备
rgbImage.reset();
depthImage.reset();
irImage.reset();
capture.reset();
device.close();
return 1;
}