PCL为3D点云数据处理提供丰富强大的接口，本文记录利用多台Realsense D435生成并显示点云。代码在显示部分仅适用于小于等于两台设备的接入，大于两台需要稍微修改多台接入的点云处理部分程序。
-
PCL可视化：PCLVisualizer
PCLVisualizer可视化类是PCL中功能最全的可视化类，与CloudViewer可视化类相比，PCLVisualizer使用起来更为复杂，但该类具有更全面的功能，如显示法线、绘制多种形状和多个视口。
-
主要用法
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer1->addCoordinateSystem(0.1);////添加坐标系，输入是坐标系三个圆柱体的规模因子
viewer1->initCameraParameters();////初始化相机角度等参数
viewer1->setBackgroundColor(0, 0, 0);//背景颜色
viewer1->addPointCloud<pcl::PointXYZRGB>(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 <iostream>
#include <algorithm>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <string>
#include "stdafx.h"
#include <map>
#include <mutex> // std::mutex, std::lock_guard
#include <cmath> // std::ceil
// Intel Realsense Headers
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "example.hpp"
// PCL Headers
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>
#include <pcl/io/io.h>
#include <pcl/visualization/cloud_viewer.h>
// Include OpenCV API
#include <opencv2/opencv.hpp>
#include "cv-helpers.hpp"
using namespace std;
using namespace cv;
typedef pcl::PointXYZRGB RGB_Cloud;
typedef pcl::PointCloud<RGB_Cloud> point_cloud;
typedef point_cloud::Ptr cloud_pointer;
typedef point_cloud::Ptr prevCloud;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));////全局共享指针
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2(new pcl::visualization::PCLVisualizer("3D Viewer"));////全局共享指针
////声明
std::tuple<int, int, int> 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<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::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<int, rs2::frame> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(_mutex);
return _devices.size();
}
int stream_count()
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<rs2::video_frame>())
{
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<std::mutex> 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<rs2::video_frame>())
{
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<pcl::PointXYZRGB>(cloud1, "cloud1");
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");
viewer1->spinOnce(1);
viewer1->removePointCloud("cloud1");
viewer2->addPointCloud<pcl::PointXYZRGB>(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<std::string, view_port> _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<int, int, int> 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<const uint8_t*>(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<int, int, int>(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 (<t0>, <t1>, <t2>)
std::tuple<uint8_t, uint8_t, uint8_t> RGB_Color;
//================================
// PCL Cloud Object Configuration
//================================
// Convert data captured from Realsense camera to Point Cloud
auto sp = points.get_profile().as<rs2::video_stream_profile>();
cloud->width = static_cast<uint32_t>(sp.width());
cloud->height = static_cast<uint32_t>(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<rs2::depth_sensor>();
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<pcl::PointXYZRGB>(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<pcl::PointXYZRGB> 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;
}
