Converting RealSense Pointcloud to PCL Pointcloud

[EMráz]

Hello everyone,

I know this has been questioned few times, but I would like to make this clear - not only for me, but for everyone trying to implement this.

What I am trying to do is basically to convert the pointcloud data from RealSense camera (D415) to PCD format, which is conventional in PCL (pointcloud library). By saying pointcloud data, I mean depth data + RGB data - if you combine these two you get exactly the same as in realsense viewer, when you hit the 3D button.

What I have accomplished so far: I succesfully converted depth frame - thanks to the PCL Wrapper https://github.com/IntelRealSense/librealsense/tree/master/wrappers/pcl librealsense/wrappers/pcl at master · IntelRealSense/librealsense · GitHub

This is how I am accesing XYZ values of points - so the depth values

p.x = ptr->x;

p.y = ptr->y;

p.z = ptr->z;

Where p is pcl::PointCloud object and ptr is pointing to rs2::points.get_vertices(). Also I know I can access the RGB values just like XYZ, so p.r and so.

So what my greatest "wish" is - is to provide solution for me ( ) and of course for the others - maybe contribute to PCL wrapper in librealsense library - so the depth and color data from RS cameras would be easily convertible to PCD format. I am developing on Ubuntu.

I know how depth data is stored, at least I think I understand it - XYZ values of a single point are the coordinates (in meters), where [0,0,0] point is the camera base.

Little bit of background of my work, if you are interested. I am willing to create 3D map by registering a number of pointcloud datasets (frames). I would like to do this on the keypoints principle, that means I would like to apply SIFT algorithm - that is why I need to know correspondence between XYZ and RGB values of points in the pointcloud.

[MartyG]

Somebody who was trying to do a conversion to PCD used the sample program you linked to for the conversion and then saved it with this line:

pcl::io::savePCDFile(filename+".pcd", * cloud);

https://github.com/IntelRealSense/librealsense/issues/1455 Export an organized point cloud · Issue # 1455 · IntelRealSense/librealsense · GitHub

[EMráz]

Thank you for replying,

Saving of the Point Cloud is not a problem, I know how to save it. Function points_to_pcl(points) converts only XYZ values, not RGB. I need to find out, how I can align RGB values to Depth frame.

[MartyG]

Here's how someone did the PCL conversion for RGB:

https://github.com/IntelRealSense/librealsense/issues/1143 Generate a colorized PCL point cloud · Issue # 1143 · IntelRealSense/librealsense · GitHub

[BMana]

Hi!

You can try this !

typedef pcl::PointXYZRGB P_pcl;

typedef pcl::PointCloud<<span style="color: # b9bcd1;">P_pcl> point_cloud;typedef point_cloud::Ptr ptr_cloud;

std::tuple<<span style="color: # b9bcd1;">uint8_t, uint8_t, uint8_t> 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<<span style="color: # cc7832; font-weight: bold;">const uint8_t*>(texture.get_data()); return std::tuple<<span style="color: # b9bcd1;">uint8_t, uint8_t, uint8_t>(

texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]);

}

ptr_cloud points_to_pcl(const rs2::points& points, const rs2::video_frame& color){

ptr_cloud cloud(new point_cloud);

auto sp = points.get_profile().as<<span style="color: # b5b6e3;">rs2::video_stream_profile>(); cloud->width = static_cast<<span style="color: # b9bcd1;">uint32_t>(sp.width()); cloud->height = static_cast<<span style="color: # b9bcd1;">uint32_t>(sp.height()); cloud-><span style="color: # 9373a5;"...

[MartyG]

Thanks so much for your help with this question!

[BMana]

The sources if you need more infos : https://github.com/IntelRealSense/librealsense/issues/1601 Obtaining RGB values fora colorised PCL point cloud. · Issue # 1601 · IntelRealSense/librealsense · GitHub

You can mark the answer as resolved if it works!

[EMráz]

Hello,

Thanks for the code, I have found it before, but the pointcloud is single-colored somehow. I will post info tomorrow. I appreciate your help, thanks again!

[BMana]

The code that I post above works with the good colors

[EMráz]

Hello again,

So I will post here my code - all points have the same color, I cannot tell why. I replaced pcl::io::savePCDFileBinary with pcl::io::savePCDFileASCII. So I can see the values through text editor. Every point has RGB value of 4283717469. This is the code: https://github.com/eMrazSVK/JetsonSLAM/blob/master/pcl_testing.cpp JetsonSLAM/pcl_testing.cpp at master · eMrazSVK/JetsonSLAM · GitHub

You can also take a look at the CMakeLists.txt, but I think it is all right so far. If any additional info would help, just let me know, I will post it.

Thank you.

EDIT: MartyG sorry, I unmarked the question as answered, because it did not solve the problem for me for now.

[MartyG]

No worries, taking off the Answered marking is the right thing to do if you need further assistance.

[EMráz · Accepted solution]

So I am Replying to my own question, which was answered on Git ( https://github.com/IntelRealSense/librealsense/issues/1939 https://github.com/IntelRealSense/librealsense/issues/1939 )

So, the problem in baptiste-mnh solution was basically in exchanging two lines:

auto points = pc.calculate(depth);

pc.map_to(colored_frame);

To:

pc.map_to(colored_frame);

auto points = pc.calculate(depth);

It is described in the Git issue posted above.

So here is the full working code, it may be considered like PCL wrapper for RealSense supporting XYZ + RGB conversion - https://github.com/eMrazSVK/JetsonSLAM/blob/master/pcl_testing.cpp https://github.com/eMrazSVK/JetsonSLAM/blob/master/pcl_testing.cpp

Thanks everyone for your help!

[BMana]

Oh, you answer one of my issues

When I was exporting .ply, the first one wasn't colored but the next ones were colored. I think it's because I inversed those lines!