利用IntelRealSense D435i 提取一帧pcl：：PointXYZRGB图像(C++)

        本文利用Intel Real Sense D435i，捕捉现场RGB-D图像并生成pcl::PointXYZRGB点云数据用于后续处理。主要解决了D453i样例中没有frame彩色信息的问题。

        最近打算利用基于RGB的区域生长分割算法完成现场图像分割，手边有d435i,但是d435i只提供了收集pcl::PointXYZ的样例。接下来在这个基础上写一个到PointXYZRGB的接口。

        在样例中给出了PointXYZ的转换方法：

https://github.com/IntelRealSense/librealsense/blob/master/wrappers/pcl/pcl/rs-pcl.cpp

pcl_ptr points_to_pcl(const rs2::points& points)
{
    pcl_ptr cloud(new pcl::PointCloud);

    auto sp = points.get_profile().as();
    cloud->width = sp.width();
    cloud->height = sp.height();
    cloud->is_dense = false;
    cloud->points.resize(points.size());
    auto ptr = points.get_vertices();
    for (auto& p : cloud->points)
    {
        p.x = ptr->x;
        p.y = ptr->y;
        p.z = ptr->z;
        ptr++;
    }

    return cloud;
}

        主要难点在于获取rgb信息，在官方git问题中找到了解决办法：

std::tuple get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords)
{
	const int w = texture.get_width(), h = texture.get_height();
	int x = std::min(std::max(int(texcoords.u*w + .5f), 0), w - 1);
	int y = std::min(std::max(int(texcoords.v*h + .5f), 0), h - 1);
	int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
	const auto texture_data = reinterpret_cast(texture.get_data());
	return std::tuple(
		texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]);
}

        这里就直接提取就可以了：

int main(int argc, char * argv[])
{
	boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	rs2::pointcloud pc;
	rs2::points points;
	rs2::pipeline pipe;
	pipe.start();
	pcl::PLYWriter writer;
	for (int i = 1; i < 20; i++)pipe.wait_for_frames();
	auto frames = pipe.wait_for_frames();
	auto depth = frames.get_depth_frame();
	auto color = frames.get_color_frame();

	pc.map_to(color);
	points = pc.calculate(depth);

	auto pcl_points = points_to_pcl(points, color);

	//for (int i = 1; i < 500; i++)
	//{
	//	int r = pcl_points->points[i].r;
	//	int g = pcl_points->points[i].g;
	//	int b = pcl_points->points[i].b;
	//	cout << r << "t" << g << "t" << b << endl;
	//}
	//pcl::io::savePCDFileASCIi("abc.pcd", *pcl_points);

	writer.write("colorful.ply", *pcl_points);
	pcl::io::savePCDFileASCIi("colorful.pcd", *pcl_points);
	viewer->removeAllPointClouds();
	viewer->addPointCloud(pcl_points);

	while (!viewer->wasStopped()) {
		viewer->spinonce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1));
	}
	return 0;
}

所有代码：

#include  // Include RealSense Cross Platform API
#include "example.hpp" // Include short list of convenience functions for rendering
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

std::tuple get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords)
{
	const int w = texture.get_width(), h = texture.get_height();
	int x = std::min(std::max(int(texcoords.u*w + .5f), 0), w - 1);
	int y = std::min(std::max(int(texcoords.v*h + .5f), 0), h - 1);
	int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
	const auto texture_data = reinterpret_cast(texture.get_data());
	return std::tuple(
		texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]);
}

pcl::PointCloud::Ptr points_to_pcl(const rs2::points& points, const rs2::video_frame& color)
{
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

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

	auto tex_coords = points.get_texture_coordinates();
	auto vertices = points.get_vertices();

	for (int i = 0; i < points.size(); ++i)
	{
		cloud->points[i].x = vertices[i].x;
		cloud->points[i].y = vertices[i].y;
		cloud->points[i].z = vertices[i].z;

		std::tuple current_color;
		current_color = get_texcolor(color, tex_coords[i]);

		cloud->points[i].r = std::get<0>(current_color);
		cloud->points[i].g = std::get<1>(current_color);
		cloud->points[i].b = std::get<2>(current_color);
		if(i<500)cout << int(cloud->points[i].r) << "t" <points[i].g) << "t" << int(cloud->points[i].b)<< "t"< viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	rs2::pointcloud pc;
	rs2::points points;
	rs2::pipeline pipe;
	pipe.start();
	pcl::PLYWriter writer;
	for (int i = 1; i < 20; i++)pipe.wait_for_frames();
	auto frames = pipe.wait_for_frames();
	auto depth = frames.get_depth_frame();
	auto color = frames.get_color_frame();

	pc.map_to(color);
	points = pc.calculate(depth);//这里顺序不能反，否则rgb信息不对

	auto pcl_points = points_to_pcl(points, color);

	writer.write("colorful.ply", *pcl_points);
	pcl::io::savePCDFileASCIi("colorful.pcd", *pcl_points);
	viewer->removeAllPointClouds();
	viewer->addPointCloud(pcl_points);

	while (!viewer->wasStopped()) {
		viewer->spinonce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1));
	}
	return 0;
}

下面代码看看结果:

#include 
#include 
#include 

using namespace pcl;

int main() {
	visualization::CloudViewer viewer("segmentation_test");
	PointCloud::Ptr cloud(new PointCloud);
	PLYReader reader;

	reader.read("colorful.ply", *cloud);

	viewer.showCloud(cloud);
	while (!viewer.wasStopped())
	{
	}
	return 0;
}