利用Azure Kinect 生成点云(C++)

准备:VS2019、Azure Kinect、SDKV1.4.1、OpenCV4.4.0、PCL1.12、VTK9.0

 软件安装地址:

SDKV1.4.1下载地址:Azure-Kinect-Sensor-SDK/usage.md at develop · microsoft/Azure-Kinect-Sensor-SDK · GitHubOpenCV4.4.0下载地址:

Releases - OpenCVhttps://opencv.org/releases/

PCL1.12.0下载地址:

https://github.com/PointCloudLibrary/pcl/releases

项目配置:

VS2019配置OpenCV4.4.0

(99条消息) openCV4+vs2019环境搭建_vs2020用多少版本的opencv4_zhong_zihao的博客-CSDN博客https://blog.csdn.net/zhong_zihao/article/details/107811457

VS2019配置PCL1.12

(99条消息) Win10 系统下VisualStudio2019 配置点云库 PCL1.12.0_pcl1.12下载_点云侠的博客-CSDN博客https://blog.csdn.net/qq_36686437/article/details/119044299

VS2019配置Azure Kinect SDKV1.4.1

1、选择项目中的引用,右键选择管理NuGet包

利用Azure Kinect 生成点云(C++)_第1张图片

 点击管理NuGet程序包(N)

利用Azure Kinect 生成点云(C++)_第2张图片

点击第一个进行下载

C++程序

1、获取Azure Kinect相机参数值

#include
#include
#include
#include
#include
#include
using namespace std;

static string get_serial(k4a_device_t device)
{
    size_t serial_number_length = 0;

    if (K4A_BUFFER_RESULT_TOO_SMALL != k4a_device_get_serialnum(device, NULL, &serial_number_length))
    {
        cout << "Failed to get serial number length" << endl;
        k4a_device_close(device);
        exit(-1);
    }

    char* serial_number = new (std::nothrow) char[serial_number_length];
    if (serial_number == NULL)
    {
        cout << "Failed to allocate memory for serial number (" << serial_number_length << " bytes)" << endl;
        k4a_device_close(device);
        exit(-1);
    }

    if (K4A_BUFFER_RESULT_SUCCEEDED != k4a_device_get_serialnum(device, serial_number, &serial_number_length))
    {
        cout << "Failed to get serial number" << endl;
        delete[] serial_number;
        serial_number = NULL;
        k4a_device_close(device);
        exit(-1);
    }

    string s(serial_number);
    delete[] serial_number;
    serial_number = NULL;
    return s;
}

static void print_calibration()
{
    uint32_t device_count = k4a_device_get_installed_count();
    cout << "Found " << device_count << " connected devices:" << endl;
    cout << fixed << setprecision(6);

    for (uint8_t deviceIndex = 0; deviceIndex < device_count; deviceIndex++)
    {
        k4a_device_t device = NULL;
        if (K4A_RESULT_SUCCEEDED != k4a_device_open(deviceIndex, &device))
        {
            cout << deviceIndex << ": Failed to open device" << endl;
            exit(-1);
        }

        k4a_calibration_t calibration;

        k4a_device_configuration_t deviceConfig = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
        deviceConfig.color_format = K4A_IMAGE_FORMAT_COLOR_MJPG;
        deviceConfig.color_resolution = K4A_COLOR_RESOLUTION_1080P;
        deviceConfig.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
        deviceConfig.camera_fps = K4A_FRAMES_PER_SECOND_30;
        deviceConfig.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE;
        deviceConfig.synchronized_images_only = true;

        // 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;

        cout << "\n===== Device " << (int)deviceIndex << ": " << get_serial(device) << " =====\n";
        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;

        k4a_device_close(device);
    }
}

static void calibration_blob(uint8_t deviceIndex = 0, string filename = "calibration.json")
{
    k4a_device_t device = NULL;

    if (K4A_RESULT_SUCCEEDED != k4a_device_open(deviceIndex, &device))
    {
        cout << deviceIndex << ": Failed to open device" << endl;
        exit(-1);
    }

    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 calibration_buffer = vector(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(&calibration_buffer[0]), (long)calibration_size);
            file.close();
            cout << "Calibration blob for device " << (int)deviceIndex << " (serial no. " << get_serial(device)
                << ") 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);
    }
}

static void print_usage()
{
    cout << "Usage: calibration_info [device_id] [output_file]" << endl;
    cout << "Using calibration_info.exe without any command line arguments will display" << endl
        << "calibration info of all connected devices in stdout. If a device_id is given" << endl
        << "(0 for default device), the calibration.json file of that device will be" << endl
        << "saved to the current directory." << endl;
}

int main(int argc, char** argv)
{
    if (argc == 1)
    {
        print_calibration();
    }
    else if (argc == 2)
    {
        calibration_blob((uint8_t)atoi(argv[1]), "calibration.json");
    }
    else if (argc == 3)
    {
        calibration_blob((uint8_t)atoi(argv[1]), argv[2]);
    }
    else
    {
        print_usage();
    }

    return 0;
}

2、获取RGB、IR、Depth、//深度图和RGB图配准获取白色点云,需要提前建立rgb、ir、depth三个子文件夹。

如果想要带RGB值,需要在static void generate_point_cloud(const k4a::image depth_image, const k4a_image_t xy_table, k4a_image_t point_cloud, int *point_count)里加入const k4a::image clour_image

//C++
#include
#include
#include
#include
 //OpenCV
#include
#include
#include
 //Kinect DK
#include
//#include
#include
#include

using namespace cv;
using namespace std;

//宏
//方便控制是否 std::cout 信息
#define DEBUG_std_cout 0


static void create_xy_table(const k4a_calibration_t* calibration, k4a_image_t xy_table)
{
    k4a_float2_t* table_data = (k4a_float2_t*)(void*)k4a_image_get_buffer(xy_table);

    int width = calibration->depth_camera_calibration.resolution_width;
    int height = calibration->depth_camera_calibration.resolution_height;

    k4a_float2_t p;
    k4a_float3_t ray;
    int valid;

    for (int y = 0, idx = 0; y < height; y++)
    {
        p.xy.y = (float)y;
        for (int x = 0; x < width; x++, idx++)
        {
            p.xy.x = (float)x;

            k4a_calibration_2d_to_3d(
                calibration, &p, 1.f, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_DEPTH, &ray, &valid);

            if (valid)
            {
                table_data[idx].xy.x = ray.xyz.x;
                table_data[idx].xy.y = ray.xyz.y;
            }
            else
            {
                table_data[idx].xy.x = nanf("");
                table_data[idx].xy.y = nanf("");
            }
        }
    }
}

static void generate_point_cloud(const k4a::image depth_image, const k4a_image_t xy_table, k4a_image_t point_cloud, int* point_count)
{
    int width = depth_image.get_width_pixels();
    int height = depth_image.get_height_pixels();


    uint16_t* depth_data = (uint16_t*)(void*)depth_image.get_buffer();
    k4a_float2_t* xy_table_data = (k4a_float2_t*)(void*)k4a_image_get_buffer(xy_table);
    k4a_float3_t* point_cloud_data = (k4a_float3_t*)(void*)k4a_image_get_buffer(point_cloud);

    *point_count = 0;
    for (int i = 0; i < width * height; i++)
    {
        if (depth_data[i] != 0 && !isnan(xy_table_data[i].xy.x) && !isnan(xy_table_data[i].xy.y))
        {
            point_cloud_data[i].xyz.x = xy_table_data[i].xy.x * (float)depth_data[i];
            point_cloud_data[i].xyz.y = xy_table_data[i].xy.y * (float)depth_data[i];
            point_cloud_data[i].xyz.z = (float)depth_data[i];
            (*point_count)++;
        }
        else
        {
            point_cloud_data[i].xyz.x = nanf("");
            point_cloud_data[i].xyz.y = nanf("");
            point_cloud_data[i].xyz.z = nanf("");
        }
    }
}

static void write_point_cloud(const char* file_name, const k4a_image_t point_cloud, int point_count)
{
    int width = k4a_image_get_width_pixels(point_cloud);
    int height = k4a_image_get_height_pixels(point_cloud);

    k4a_float3_t* point_cloud_data = (k4a_float3_t*)(void*)k4a_image_get_buffer(point_cloud);

    //save to the ply file
    std::ofstream ofs(file_name); // text mode first
    ofs << "ply" << std::endl;
    ofs << "format ascii 1.0" << std::endl;
    ofs << "element vertex"
        << " " << point_count << std::endl;
    ofs << "property float x" << std::endl;
    ofs << "property float y" << std::endl;
    ofs << "property float z" << std::endl;
    ofs << "end_header" << std::endl;
    ofs.close();

    std::stringstream ss;
    for (int i = 0; i < width * height; i++)
    {
        if (isnan(point_cloud_data[i].xyz.x) || isnan(point_cloud_data[i].xyz.y) || isnan(point_cloud_data[i].xyz.z))
        {
            continue;
        }

        ss << (float)point_cloud_data[i].xyz.x << " " << (float)point_cloud_data[i].xyz.y << " "
            << (float)point_cloud_data[i].xyz.z << std::endl;
    }

    std::ofstream ofs_text(file_name, std::ios::out | std::ios::app);
    ofs_text.write(ss.str().c_str(), (std::streamsize)ss.str().length());
}

int main(int argc, char* argv[]) {
    /*

        找到并打开 Azure Kinect 设备
    */
    //发现已连接的设备数

    const uint32_t device_count = k4a::device::get_installed_count();
    if (0 == device_count) {
        cout << "Error: no K4A devices found. " << 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 device = k4a::device::open(K4A_DEVICE_DEFAULT);
    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_30;
    config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
    config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
    config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
    //config.depth_mode = K4A_DEPTH_MODE_WFOV_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;

    //写入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_depthImage;

    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;

    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图配准
                        //Get the camera calibration for the entire K4A device, which is used for all transformation functions.
                    k4a::calibration k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);

                    k4a::transformation k4aTransformation = k4a::transformation(k4aCalibration);

                    transformed_depthImage = k4aTransformation.depth_image_to_color_camera(depthImage);

                    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 = cv::Mat(transformed_depthImage.get_height_pixels(), transformed_depthImage.get_width_pixels(), CV_16U,
                        (void*)transformed_depthImage.get_buffer(), static_cast(transformed_depthImage.get_stride_bytes()));

                    normalize(cv_depth, cv_depth_8U, 0, 256 * 256, NORM_MINMAX);
                    cv_depth_8U.convertTo(cv_depth, CV_8U, 1);

                    cv_irImage = cv::Mat(irImage.get_height_pixels(), irImage.get_width_pixels(), CV_16U,
                        (void*)irImage.get_buffer(), static_cast(irImage.get_stride_bytes()));
                    normalize(cv_irImage, cv_irImage_8U, 0, 256 * 256, NORM_MINMAX);
                    cv_irImage.convertTo(cv_irImage_8U, CV_8U, 1);


                    //k4a::image xyzImage;
                    //cv::Mat cv_xyzImage;// 16位有符号
                    //cv::Mat cv_xyzImage_32F;// 32位float
                    
                        //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(std::chrono::duration_cast(
                        rgbImage.get_device_timestamp()).count());

                    std::string filename_rgb = std::to_string(time_rgb / 1000000) + ".png";

                    double time_d = static_cast(std::chrono::duration_cast(
                        depthImage.get_device_timestamp()).count());

                    std::string filename_d = std::to_string(time_d / 1000000) + ".png";

                    double time_ir = static_cast(std::chrono::duration_cast(
                        irImage.get_device_timestamp()).count());
                    std::string filename_ir = std::to_string(time_ir / 1000000) + ".png";
                    imwrite("./rgb/" + filename_rgb, cv_rgbImage_no_alpha);
                    imwrite("./depth/" + filename_d, cv_depth_8U);
                    imwrite("./ir/" + filename_ir, cv_irImage_8U);


                    //const int32_t TIMEOUT_IN_MS = 1000;
                    //std::string file_name;
                    //uint32_t device_count = 0;

                    //k4a_device_t device1 = NULL;
                    //k4a_device_configuration_t config1 = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
                    //k4a_capture_t capture1 = NULL;
                    //k4a_image_t depth_image = NULL;
                    //k4a_calibration_t calibration1;

                    k4a_image_t xy_table = NULL;
                    k4a_image_t point_cloud = NULL;
                    int point_count = 0;

                    double time_point = static_cast(std::chrono::duration_cast(
                        rgbImage.get_device_timestamp()).count());
                    std::string filename_point = std::to_string(time_point / 1000000) + ".ply";
                    

                    k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
                        k4aCalibration.depth_camera_calibration.resolution_width,
                        k4aCalibration.depth_camera_calibration.resolution_height,
                        k4aCalibration.depth_camera_calibration.resolution_width * (int)sizeof(k4a_float2_t),
                        &xy_table);

                    create_xy_table(&k4aCalibration, xy_table);

                    k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
                        k4aCalibration.depth_camera_calibration.resolution_width,
                        k4aCalibration.depth_camera_calibration.resolution_height,
                        k4aCalibration.depth_camera_calibration.resolution_width * (int)sizeof(k4a_float3_t),
                        &point_cloud);
                    /*    k4a_device_start_cameras(device, &config);
                        k4a_device_get_capture(device, &capture, TIMEOUT_IN_MS);*/

                        //depth_image = k4a_capture_get_depth_image(capture1);
                    if (depthImage == 0)
                    {
                        printf("Failed to get depth image from capture\n");
                    }

                    generate_point_cloud(depthImage, xy_table, point_cloud, &point_count);

                    write_point_cloud(filename_point.c_str(), point_cloud, point_count);

                    /*    k4a_image_release(depthImage);
                        k4a_capture_release(capture);*/
                    k4a_image_release(xy_table);
                    k4a_image_release(point_cloud);
                    //returnCode = 0;
                    //k4a_device_close(device1);


                    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;

                    k4aTransformation.destroy();

                    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();

                    if (cv::waitKey() == '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;
}
 

3、获取三维彩色点云

核心代码

k4a::calibration k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);
            k4a::transformation k4aTransformation = k4a::transformation(k4aCalibration);

            //PointCloud::Ptr cloud(new PointCloud);
            int color_image_width_pixels = rgbImage.get_width_pixels();
            int color_image_height_pixels = rgbImage.get_height_pixels();
            transformed_depthImage = NULL;
            transformed_depthImage = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
                color_image_width_pixels,
                color_image_height_pixels,
                color_image_width_pixels * (int)sizeof(uint16_t));
            k4a::image point_cloud_image = NULL;
            point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
                color_image_width_pixels,
                color_image_height_pixels,
                color_image_width_pixels * 3 * (int)sizeof(int16_t));

            if (depthImage.get_width_pixels() == rgbImage.get_width_pixels() && depthImage.get_height_pixels() == rgbImage.get_height_pixels()) {
                std::copy(depthImage.get_buffer(), depthImage.get_buffer() + depthImage.get_height_pixels() * depthImage.get_width_pixels() * (int)sizeof(uint16_t), transformed_depthImage.get_buffer());
            }
            else {
                k4aTransformation.depth_image_to_color_camera(depthImage, &transformed_depthImage);
            }
            k4aTransformation.depth_image_to_point_cloud(transformed_depthImage, K4A_CALIBRATION_TYPE_COLOR, &point_cloud_image);

            pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
            cloud->width = color_image_width_pixels;
            cloud->height = color_image_height_pixels;
            cloud->is_dense = false;
            cloud->resize(static_cast(color_image_width_pixels) * color_image_height_pixels);

            const int16_t* point_cloud_image_data = reinterpret_cast(point_cloud_image.get_buffer());
            const uint8_t* color_image_data = rgbImage.get_buffer();

            for (int i = 0; i < color_image_width_pixels * color_image_height_pixels; i++) {
                PointT point;

                point.x = point_cloud_image_data[3 * i + 0] / 1000.0f;
                point.y = point_cloud_image_data[3 * i + 1] / 1000.0f;
                point.z = point_cloud_image_data[3 * i + 2] / 1000.0f;

                point.b = color_image_data[4 * i + 0];
                point.g = color_image_data[4 * i + 1];
                point.r = color_image_data[4 * i + 2];
                uint8_t alpha = color_image_data[4 * i + 3];
                if (point.x == 0 && point.y == 0 && point.z == 0 && alpha == 0)
                    continue;
                cloud->points[i] = point;
            }
            pcl::io::savePLYFile("D:\\data\\3.ply", *cloud);   //将点云数据保存为ply文件
        }
        else {
            std::cout << "false: K4A_WAIT_RESULT_TIMEOUT." << std::endl;
        }

完整代码:

//C++
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
 //OpenCV
#include
#include
#include
 //Kinect DK
#include
//#include
// PCL 库
#include
#include
#include

//定义点云类型
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud PointCloud;

using namespace cv;
using namespace std;

//宏
//方便控制是否 std::cout 信息
#define DEBUG_std_cout 0


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 device = k4a::device::open(K4A_DEVICE_DEFAULT);
    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_30;
    //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_WFOV_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;

    //写入txt文件流
    ofstream rgb_out;
    ofstream d_out;

    rgb_out.open("./rgb.txt");
    d_out.open("./depth.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;

    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_depthImage;

    cv::Mat cv_rgbImage_with_alpha;
    cv::Mat cv_rgbImage_no_alpha;
    cv::Mat cv_depth;
    cv::Mat cv_depth_8U;

    int index = 0;
    while (index < 1) {
        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
            //获取彩色点云
            k4a::calibration k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);
            k4a::transformation k4aTransformation = k4a::transformation(k4aCalibration);

            //PointCloud::Ptr cloud(new PointCloud);
            int color_image_width_pixels = rgbImage.get_width_pixels();
            int color_image_height_pixels = rgbImage.get_height_pixels();
            transformed_depthImage = NULL;
            transformed_depthImage = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
                color_image_width_pixels,
                color_image_height_pixels,
                color_image_width_pixels * (int)sizeof(uint16_t));
            k4a::image point_cloud_image = NULL;
            point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
                color_image_width_pixels,
                color_image_height_pixels,
                color_image_width_pixels * 3 * (int)sizeof(int16_t));

            if (depthImage.get_width_pixels() == rgbImage.get_width_pixels() && depthImage.get_height_pixels() == rgbImage.get_height_pixels()) {
                std::copy(depthImage.get_buffer(), depthImage.get_buffer() + depthImage.get_height_pixels() * depthImage.get_width_pixels() * (int)sizeof(uint16_t), transformed_depthImage.get_buffer());
            }
            else {
                k4aTransformation.depth_image_to_color_camera(depthImage, &transformed_depthImage);
            }
            k4aTransformation.depth_image_to_point_cloud(transformed_depthImage, K4A_CALIBRATION_TYPE_COLOR, &point_cloud_image);

            pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
            cloud->width = color_image_width_pixels;
            cloud->height = color_image_height_pixels;
            cloud->is_dense = false;
            cloud->resize(static_cast(color_image_width_pixels) * color_image_height_pixels);

            const int16_t* point_cloud_image_data = reinterpret_cast(point_cloud_image.get_buffer());
            const uint8_t* color_image_data = rgbImage.get_buffer();

            for (int i = 0; i < color_image_width_pixels * color_image_height_pixels; i++) {
                PointT point;

                point.x = point_cloud_image_data[3 * i + 0] / 1000.0f;
                point.y = point_cloud_image_data[3 * i + 1] / 1000.0f;
                point.z = point_cloud_image_data[3 * i + 2] / 1000.0f;

                point.b = color_image_data[4 * i + 0];
                point.g = color_image_data[4 * i + 1];
                point.r = color_image_data[4 * i + 2];
                uint8_t alpha = color_image_data[4 * i + 3];
                if (point.x == 0 && point.y == 0 && point.z == 0 && alpha == 0)
                    continue;
                cloud->points[i] = point;
            }
            pcl::io::savePLYFile("D:\\data\\4.ply", *cloud);   //将点云数据保存为ply文件
        }
        else {
            std::cout << "false: K4A_WAIT_RESULT_TIMEOUT." << std::endl;
        }
        index++;
    }
    cv::destroyAllWindows();
    rgb_out << flush;
    d_out << flush;
    rgb_out.close();
    d_out.close();

    // 释放,关闭设备
    rgbImage.reset();
    depthImage.reset();
    capture.reset();
    device.close();

    return 1;
}
 

成功后界面

利用Azure Kinect 生成点云(C++)_第3张图片

 参考

(99条消息) 基于Azure Kinect SDK获取物体rgb图、深度图、红外IR图和点云数据并保存到本地_深度相机ir图_农机AI小白的博客-CSDN博客https://blog.csdn.net/weixin_42532587/article/details/111054649?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522168446494016800192299929%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=168446494016800192299929&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-13-111054649-null-null.142%5Ev87%5Einsert_down28,239%5Ev2%5Einsert_chatgpt&utm_term=Azure%20Kinect%20DK%E8%8E%B7%E5%8F%96%E7%82%B9%E4%BA%91&spm=1018.2226.3001.4449(98条消息) 基于Azure Kinect DK相机的安装配置,获取并保存RGB、Depth、IR图、点云,点云融合(Windows)_yyyyygq的博客-CSDN博客https://blog.csdn.net/y18771025420/article/details/113468859?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522168422794616800225536534%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=168422794616800225536534&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-2-113468859-null-null.142%5Ev87%5Einsert_down28,239%5Ev2%5Einsert_chatgpt&utm_term=Azure%20Kinect%20DK%E7%94%9F%E6%88%90%E7%82%B9%E4%BA%91&spm=1018.2226.3001.4187(99条消息) Win10 系统下VisualStudio2019 配置点云库 PCL1.12.0_pcl1.12下载_点云侠的博客-CSDN博客https://blog.csdn.net/qq_36686437/article/details/119044299

你可能感兴趣的:(点云处理&深度相机,azure,c++)