使用PCL显示Azure Kinect采集到的彩色点云

使用PCL显示Azure Kinect采集到的点云

参考文章，大佬介绍的非常好。

我这里希望能显示并保存彩色点云并借此学习下SDK的基础使用，所以并不如上述参考文章周密复杂，只写了段简单简陋的代码。

键盘退出响应可能有些问题，其他功能性尚好。

代码如下

#include <k4a/k4a.hpp>

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <chrono>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace pcl;

bool save_flag = false;
bool quit_flag = false;

void keyboard_event(const visualization::KeyboardEvent& event, void* nothing){
    if(event.getKeySym() == "s" && event.keyDown()){
        save_flag = true;
    }else if(event.getKeySym() == "q" && event.keyDown()){
        quit_flag = true;
    }
}

void pcl_azure_kinect_display(){
    uint32_t device_count = 0;
    k4a::device device = NULL;
    k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;

    //get the number of connected devices
    device_count = k4a::device::get_installed_count();
    if(device_count == 0){
        cout << "No K4A devices found" << endl;
        return;
    }

    //open the defaule device
    device = k4a::device::open(K4A_DEVICE_DEFAULT);
    if(device == NULL){
        cout << "Failed to opend device" << endl;
        device.close();
        return;
    }

    //set config parameter
    config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
    config.color_resolution = K4A_COLOR_RESOLUTION_3072P;
    config.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED;
    config.camera_fps = K4A_FRAMES_PER_SECOND_15;
    config.synchronized_images_only = true;

    //start device
    device.start_cameras(&config);
    //should catch errors here
    
    //get images and convert to PCL format
    k4a::capture capture;
    k4a::image k4a_rgba_image;
    k4a::image k4a_depth_image;

    PointCloud<PointXYZRGBA>::Ptr cloud_rgba(new PointCloud<PointXYZRGBA>);
    boost::shared_ptr<visualization::PCLVisualizer> viewer (new visualization::PCLVisualizer("Point Cloud Viewer"));
    viewer -> registerKeyboardCallback(&keyboard_event, (void *)NULL);
    viewer -> setCameraPosition(0.0, 0.0, -2500.0, 1.0, -1.0, 1.0);

    while(true){
        if(device.get_capture(&capture, chrono::milliseconds(0))){
            k4a_rgba_image = capture.get_color_image();
            k4a_depth_image = capture.get_depth_image();

            //get calibration and transformation information
            k4a::calibration k4a_device_calibration = device.get_calibration(config.depth_mode, config.color_resolution);
            k4a::transformation k4a_device_transformation = k4a::transformation(k4a_device_calibration);
            
            //create k4a point cloud image format
            k4a::image k4a_xyz_image = NULL;
            k4a_xyz_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM, 
                                            k4a_depth_image.get_width_pixels(), 
                                            k4a_depth_image.get_height_pixels(), 
                                            k4a_depth_image.get_width_pixels() * 3 * (int)sizeof(int16_t));
         
            //create k4a transformed color image format
            k4a::image k4a_transformed_rgba_image = k4a::image::create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
                                                                    k4a_depth_image.get_width_pixels(),
                                                                    k4a_depth_image.get_height_pixels(),
                                                                    k4a_depth_image.get_width_pixels() * 4 *(int)sizeof(uint8_t));

            //transformation
            k4a_device_transformation.depth_image_to_point_cloud(k4a_depth_image, K4A_CALIBRATION_TYPE_DEPTH, &k4a_xyz_image);
            k4a_device_transformation.color_image_to_depth_camera(k4a_depth_image, k4a_rgba_image, &k4a_transformed_rgba_image);

            //set point cloud parameter
            int point_height = k4a_xyz_image.get_height_pixels();
            int point_width = k4a_xyz_image.get_width_pixels();
            cloud_rgba -> width = point_width;
            cloud_rgba -> height = point_height;
            cloud_rgba -> is_dense = false;
            cloud_rgba -> points.resize(cloud_rgba -> height * cloud_rgba -> width);

            int16_t *k4a_xyz_image_data = (int16_t *)(void *)(k4a_xyz_image.get_buffer());
            uint8_t *k4a_transformed_rgba_image_data = (uint8_t *)(void *)(k4a_transformed_rgba_image.get_buffer());

            for(int i = 0; i < point_width * point_height; ++i){
                PointXYZRGBA point;
                point.x = k4a_xyz_image_data[3 * i + 0];
                point.y = k4a_xyz_image_data[3 * i + 1];
                point.z = k4a_xyz_image_data[3 * i + 2];
                point.b = k4a_transformed_rgba_image_data[4 * i + 0];
                point.g = k4a_transformed_rgba_image_data[4 * i + 1];
                point.r = k4a_transformed_rgba_image_data[4 * i + 2];
                point.a = k4a_transformed_rgba_image_data[4 * i + 3];
                cloud_rgba -> points[i] = point;
            }
            if(save_flag){
                string timestamp = to_string(k4a_rgba_image.get_device_timestamp().count());
                string fileName = "PCD/" + timestamp + ".pcd";
                io::savePCDFile(fileName, *cloud_rgba);
                save_flag = false;
            }
            if(!viewer -> wasStopped()){
                viewer -> spinOnce(100);
                //boost::this_thread::sleep (boost::posix_time::microseconds (100000));
                if(!viewer -> updatePointCloud(cloud_rgba, "cloud")){
                    viewer -> addPointCloud(cloud_rgba, "cloud");
                }
            }
        }
        if(quit_flag){
            viewer-> close();
            k4a_rgba_image.reset();
            k4a_depth_image.reset();
            device.close();
            break;
        }
    }
}