多台RealsenseD435生成点云（PCL显示）

PCL为3D点云数据处理提供丰富强大的接口，本文记录利用多台Realsense D435生成并显示点云。代码在显示部分仅适用于小于等于两台设备的接入，大于两台需要稍微修改多台接入的点云处理部分程序。

PCL可视化：PCLVisualizer

PCLVisualizer可视化类是PCL中功能最全的可视化类，与CloudViewer可视化类相比，PCLVisualizer使用起来更为复杂，但该类具有更全面的功能，如显示法线、绘制多种形状和多个视口。

主要用法

boost::shared_ptr viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer1->addCoordinateSystem(0.1);////添加坐标系，输入是坐标系三个圆柱体的规模因子
viewer1->initCameraParameters();////初始化相机角度等参数
viewer1->setBackgroundColor(0, 0, 0);//背景颜色

viewer1->addPointCloud(cloud, "cloud1");
viewer1>setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");
viewer1->spinOnce(1);
viewer1->removePointCloud("cloud1");////addPointCloud（）后需要使用removePointCloud（）来清楚指定ID的点云

实验效果：

双台相机点云

代码主要参考的链接如下：Realsense github官网的PCL wrappers,Realsense github官网的examples中的multicam例程，PCL中国官方教程。
完整代码如下：

#include 
#include  
#include 
#include 
#include 
#include "stdafx.h"
#include 
#include                     // std::mutex, std::lock_guard
#include                     // std::ceil

// Intel Realsense Headers
#include  // Include RealSense Cross Platform API
#include "example.hpp"   

// PCL Headers
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

// Include OpenCV API
#include    
#include "cv-helpers.hpp"

using namespace std;
using namespace cv;

typedef pcl::PointXYZRGB RGB_Cloud;
typedef pcl::PointCloud point_cloud;
typedef point_cloud::Ptr cloud_pointer;
typedef point_cloud::Ptr prevCloud;
boost::shared_ptr viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));////全局共享指针
boost::shared_ptr viewer2(new pcl::visualization::PCLVisualizer("3D Viewer"));////全局共享指针

////声明
std::tuple RGB_Texture(rs2::video_frame texture, rs2::texture_coordinate Texture_XY);
cloud_pointer PCL_Conversion(const rs2::points& points, const rs2::video_frame& color);
//boost::shared_ptr rgbVis(pcl::PointCloud::ConstPtr cloud);


const std::string no_camera_message = "No camera connected, please connect 1 or more";
const std::string platform_camera_name = "Platform Camera";


class device_container
{
    // Helper struct per pipeline
    struct view_port
    {
        std::map frames_per_stream;
        rs2::colorizer colorize_frame;
        texture tex;
        rs2::pipeline pipe;
        rs2::pipeline_profile profile;
    };

public:

    void enable_device(rs2::device dev)
    {
        std::string serial_number(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
        std::lock_guard lock(_mutex);

        if (_devices.find(serial_number) != _devices.end())
        {
            return; //already in
        }

        // Ignoring platform cameras (webcams, etc..)
        if (platform_camera_name == dev.get_info(RS2_CAMERA_INFO_NAME))
        {
            return;
        }
        // Create a pipeline from the given device
        rs2::pipeline p;
        rs2::config cfg;
        cfg.enable_device(serial_number);
        cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
        cfg.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
        cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
        // Start the pipeline with the configuration
        rs2::pipeline_profile profile = p.start(cfg);
        // Hold it internally
        _devices.emplace(serial_number, view_port{ {},{},{}, p, profile });

    }

    void remove_devices(const rs2::event_information& info)
    {
        std::lock_guard lock(_mutex);
        // Go over the list of devices and check if it was disconnected
        auto itr = _devices.begin();
        while (itr != _devices.end())
        {
            if (info.was_removed(itr->second.profile.get_device()))
            {
                itr = _devices.erase(itr);
            }
            else
            {
                ++itr;
            }
        }
    }

    size_t device_count()
    {
        std::lock_guard lock(_mutex);
        return _devices.size();
    }

    int stream_count()
    {
        std::lock_guard lock(_mutex);
        int count = 0;
        for (auto&& sn_to_dev : _devices)
        {
            for (auto&& stream : sn_to_dev.second.frames_per_stream)
            {
                if (stream.second)
                {
                    count++;
                }
            }
        }
        return count;
    }

    void poll_frames()
    {
        std::lock_guard lock(_mutex);
        // Go over all device
        for (auto&& view : _devices)
        {
            // Ask each pipeline if there are new frames available
            rs2::frameset frameset;
            if (view.second.pipe.poll_for_frames(&frameset))
            {
                for (int i = 0; i < frameset.size(); i++)
                {
                    rs2::frame new_frame = frameset[i];
                    int stream_id = new_frame.get_profile().unique_id();
                    //view.second.frames_per_stream[stream_id] = view.second.colorize_frame.process(new_frame); //update view port with the new stream
                    view.second.frames_per_stream[stream_id] = new_frame; ////不将深度图彩色化
                }
            }
        }
    }

    void render_textures()
    {
        std::lock_guard lock(_mutex);
        int stream_num = 0;
        rs2::colorizer color_map;
        for (auto&& view : _devices)
        {
            // For each device get its frames
            for (auto&& id_to_frame : view.second.frames_per_stream)
            {
                
                if (rs2::video_frame vid_frame = id_to_frame.second.as())
                {
                    
                    auto format = vid_frame.get_profile().format();
                    auto w = vid_frame.get_width();
                    auto h = vid_frame.get_height();
                    
                    if (format == RS2_FORMAT_BGR8)  ////彩色图
                    {
                        auto colorMat = Mat(Size(w, h), CV_8UC3, (void*)vid_frame.get_data(), Mat::AUTO_STEP);
                        imshow("color1_"+to_string(stream_num) , colorMat);
                    }
                    
                    else if (format == RS2_FORMAT_RGB8) 
                    {
                        auto colorMat = Mat(Size(w, h), CV_8UC3, (void*)vid_frame.get_data(), Mat::AUTO_STEP);
                        cvtColor(colorMat, colorMat, COLOR_RGB2BGR);
                        imshow("color2_" + to_string(stream_num), colorMat);
                    }

                    else if (format == RS2_FORMAT_Z16)
                    {
                        auto depth = vid_frame.apply_filter(color_map);
                        auto colorMat = Mat(Size(w, h), CV_8UC3, (void*)depth.get_data(), Mat::AUTO_STEP);
                        imshow("color_depth_" + to_string(stream_num), colorMat);
                        //auto depthMat = Mat(Size(w, h), CV_16UC1, (void*)vid_frame.get_data(), Mat::AUTO_STEP);
                    }
                                    
                    waitKey(1);
                    
                }
                stream_num++;
                
            }
        }
    }

    void pointcloud_process() {
        std::lock_guard lock(_mutex);
        int stream_num = 0;
        rs2::frame color_frame_1, color_frame_2, depth_frame_1, depth_frame_2;
        cloud_pointer cloud1, cloud2;
        for (auto&& view : _devices) ////遍历每个设备
        {
            // For each device get its frames
            for (auto&& id_to_frame : view.second.frames_per_stream) ////每个设备一个stream里有3 帧 数据：深度帧，红外帧，彩色帧
            {
                if (rs2::video_frame vid_frame = id_to_frame.second.as())
                {

                    auto format = vid_frame.get_profile().format();
                    auto w = vid_frame.get_width();
                    auto h = vid_frame.get_height();
                    int cur_num = stream_num / 3; ////cur_num等于0,1,2属于一个设备，等于3，4，5属于另一个设备，以此类推

                    ////只获取深度帧和彩色帧
                    if (format == RS2_FORMAT_BGR8)  ////彩色帧
                    {
                        if (cur_num == 0) 
                            color_frame_1 = vid_frame;
                        else
                            color_frame_2 = vid_frame;

                    }

                    else if (format == RS2_FORMAT_Z16) ////深度帧
                    {
                        if (cur_num == 0)
                            depth_frame_1 = vid_frame;
                        else
                            depth_frame_2 = vid_frame;
                    }
                }
                stream_num++;
            
            }
        
        }
        
        if (color_frame_1 && depth_frame_1 && color_frame_2 && depth_frame_2) ////若两个设备的深度帧和彩色帧均获取到，则生成点云
        {
            
            pc1.map_to(color_frame_1);
            auto points1 = pc1.calculate(depth_frame_1);
            cloud1 = PCL_Conversion(points1, color_frame_1);
            
        
            pc2.map_to(color_frame_2);
            auto points2 = pc2.calculate(depth_frame_2);
            cloud2 = PCL_Conversion(points2, color_frame_2);
            
            ////可视化
            viewer1->addPointCloud(cloud1, "cloud1");
            viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");
            viewer1->spinOnce(1);
            viewer1->removePointCloud("cloud1");
            
            viewer2->addPointCloud(cloud2, "cloud2");
            viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud2");
            viewer2->spinOnce(1);
            viewer2->removePointCloud("cloud2");
            
        }
        
        
        else
        {
            cout << "depth frame and color frame not aligned" << endl;
        }
        


    }
private:
    std::mutex _mutex;
    std::map _devices;

public:
    rs2::pointcloud pc1, pc2;

};



//======================================================
// RGB Texture
// - Function is utilized to extract the RGB data from
// a single point return R, G, and B values. 
// Normals are stored as RGB components and
// correspond to the specific depth (XYZ) coordinate.
// By taking these normals and converting them to
// texture coordinates, the RGB components can be
// "mapped" to each individual point (XYZ).
//======================================================
std::tuple RGB_Texture(rs2::video_frame texture, rs2::texture_coordinate Texture_XY)
{
    // Get Width and Height coordinates of texture
    int width = texture.get_width();  // Frame width in pixels
    int height = texture.get_height(); // Frame height in pixels

                                       // Normals to Texture Coordinates conversion
    int x_value = min(max(int(Texture_XY.u * width + .5f), 0), width - 1);
    int y_value = min(max(int(Texture_XY.v * height + .5f), 0), height - 1);

    int bytes = x_value * texture.get_bytes_per_pixel();   // Get # of bytes per pixel
    int strides = y_value * texture.get_stride_in_bytes(); // Get line width in bytes
    int Text_Index = (bytes + strides);

    const auto New_Texture = reinterpret_cast(texture.get_data());

    // RGB components to save in tuple
    int NT1 = New_Texture[Text_Index];
    int NT2 = New_Texture[Text_Index + 1];
    int NT3 = New_Texture[Text_Index + 2];

    return std::tuple(NT1, NT2, NT3);
}

//===================================================
//  PCL_Conversion
// - Function is utilized to fill a point cloud
//  object with depth and RGB data from a single
//  frame captured using the Realsense.
//=================================================== 
cloud_pointer PCL_Conversion(const rs2::points& points, const rs2::video_frame& color) {

    // Object Declaration (Point Cloud)
    cloud_pointer cloud(new point_cloud);

    // Declare Tuple for RGB value Storage (, , )
    std::tuple RGB_Color;

    //================================
    // PCL Cloud Object Configuration
    //================================
    // Convert data captured from Realsense camera to Point Cloud
    auto sp = points.get_profile().as();

    cloud->width = static_cast(sp.width());
    cloud->height = static_cast(sp.height());
    cloud->is_dense = false;
    cloud->points.resize(points.size());

    auto Texture_Coord = points.get_texture_coordinates();
    auto Vertex = points.get_vertices();

    // Iterating through all points and setting XYZ coordinates
    // and RGB values
    for (int i = 0; i < points.size(); i++)
    {
        //===================================
        // Mapping Depth Coordinates
        // - Depth data stored as XYZ values
        //===================================
        cloud->points[i].x = Vertex[i].x;
        cloud->points[i].y = Vertex[i].y;
        cloud->points[i].z = Vertex[i].z;

        // Obtain color texture for specific point
        RGB_Color = RGB_Texture(color, Texture_Coord[i]);

        // Mapping Color (BGR due to Camera Model)
        cloud->points[i].r = get<2>(RGB_Color); // Reference tuple<2>
        cloud->points[i].g = get<1>(RGB_Color); // Reference tuple<1>
        cloud->points[i].b = get<0>(RGB_Color); // Reference tuple<0>

    }

    return cloud; // PCL RGB Point Cloud generated
}

//boost::mutex updateModelMutex;

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

    bool captureLoop = true; // Loop control for generating point clouds
    device_container connected_devices;

    rs2::context ctx;    // Create librealsense context for managing devices

                         // Register callback for tracking which devices are currently connected
    ctx.set_devices_changed_callback([&](rs2::event_information& info)
    {
        connected_devices.remove_devices(info);
        for (auto&& dev : info.get_new_devices())
        {
            connected_devices.enable_device(dev);
        }
    });

    // Initial population of the device list
    for (auto&& dev : ctx.query_devices()) // Query the list of connected RealSense devices
    {
        connected_devices.enable_device(dev);
    }

    if ((int)connected_devices.device_count() == 0)
    {
        
        cerr << no_camera_message << endl;
        return  EXIT_FAILURE;
    }
    ////单台接入点云显示
    else if ((int)connected_devices.device_count() == 1)
    {
        rs2::pointcloud pc;
        rs2::pipeline pipe;
        rs2::config cfg;
        rs2::colorizer color_map;
        cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
        cfg.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
        cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
        rs2::pipeline_profile selection = pipe.start(cfg);
        rs2::device selected_device = selection.get_device();
        auto depth_sensor = selected_device.first();

        if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
        {
            depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter
            depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0.f); // Disable emitter
        }
        if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
        {
            // Query min and max values:
            auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
            depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power
            depth_sensor.set_option(RS2_OPTION_LASER_POWER, 0.f); // Disable laser
        }

        // Wait for frames from the camera to settle
        for (int i = 0; i < 30; i++) {
            auto frames = pipe.wait_for_frames(); //Drop several frames for auto-exposure
        }
        ////单台相机

        viewer1->addCoordinateSystem(0.1);
        viewer1->initCameraParameters();
        viewer1->setBackgroundColor(0, 0, 0);

        while (captureLoop) {
            //// 单台相机
            auto frames = pipe.wait_for_frames();
            auto depth = frames.get_depth_frame();
            auto depth1 = depth.apply_filter(color_map);
            auto RGB = frames.get_color_frame();
            auto rgbMat = frame_to_mat(RGB);
            auto depthMat = frame_to_mat(depth1);
            //imshow("rgb", rgbMat);
            //imshow("depth", depthMat);
            //waitKey(1);
            //// 单台相机


            pc.map_to(RGB);
            auto points = pc.calculate(depth);
            cloud_pointer cloud = PCL_Conversion(points, RGB);
            ////可视化
            viewer1->addPointCloud(cloud, "cloud1");
            viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");
            viewer1->spinOnce(1);
            viewer1->removePointCloud("cloud1");

        }
    }
    ////多台相机接入，这里是两台
    else
    {       
        viewer1->addCoordinateSystem(0.1);
        viewer1->initCameraParameters();
        viewer1->setBackgroundColor(0, 0, 0);
        viewer2->addCoordinateSystem(0.1);
        viewer2->initCameraParameters();
        viewer2->setBackgroundColor(0, 0, 0);


        // Loop and take frame captures upon user input
        while (captureLoop) {


            ////多台相机
            connected_devices.poll_frames();
            auto total_number_of_streams = connected_devices.stream_count();

            //connected_devices.render_textures(); ////显示深度图和彩色图
            connected_devices.pointcloud_process();////点云处理

                                                   ////多台相机
        }

        //========================================
        // Filter PointCloud (PassThrough Method)
        //========================================
        /*
        pcl::PassThrough Cloud_Filter; // Create the filtering object
        Cloud_Filter.setInputCloud(cloud);           // Input generated cloud to filter
        Cloud_Filter.setFilterFieldName("z");        // Set field name to Z-coordinate
        Cloud_Filter.setFilterLimits(0.0, 1.0);      // Set accepted interval values
        Cloud_Filter.filter(*newCloud);              // Filtered Cloud Outputted
        */
    }

    cout << "Exiting Program... " << endl;
    return EXIT_SUCCESS;
    
}

catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

多台RealsenseD435生成点云（PCL显示）

PCL为3D点云数据处理提供丰富强大的接口，本文记录利用多台Realsense D435生成并显示点云。代码在显示部分仅适用于小于等于两台设备的接入，大于两台需要稍微修改多台接入的点云处理部分程序。

PCL可视化：PCLVisualizer

PCLVisualizer可视化类是PCL中功能最全的可视化类，与CloudViewer可视化类相比，PCLVisualizer使用起来更为复杂，但该类具有更全面的功能，如显示法线、绘制多种形状和多个视口。

主要用法

实验效果：

代码主要参考的链接如下：Realsense github官网的PCL wrappers,Realsense github官网的examples中的multicam例程，PCL中国官方教程。

完整代码如下：

你可能感兴趣的:(多台RealsenseD435生成点云（PCL显示）)