How to project points to pixel? Librealsense SDK 2.0

[SEber3]

Hi,

With the help of rs2::pointcloud I could compute 3D-Points from a depth_frame in the color-camera-coordinate system.

The examples are very short and the documentation is missing. Therefore, I need first a confirmation of the following to be true:

rs2::frameset m_oFrameset;

rs2::points m_oPoints;

rs2::pointcloud pc;

pc.map_to(m_oFrameset.get_color_frame()); //the object pc is set to the coordinate system of the color-camera

m_oPoints = pc.calculate(m_oFrameset.get_depth_frame()); //de-projects every depth_frame pixel to a 3D-Point; the coordinates for the points are given with respect to the color-camera coordinate system

pPoints2DepthBuffer = (void *) m_oPoints.get_vertices(); //pPoints2DepthBuffer points to the begining of an array with the same size like the number of pixels in the depth_frame. Each element being a rs2::vertex object with x,y,z elments. And these x,y,z are coordinates in the color-camera coordinate system with units in meter

Now, how do I project m_oPoints back to the "pixel space" of the color_frame?

What does m_oPoints.get_texture_coordinates() do? In what units are these coordinates?

Thank you,

Sven

(I am working with the D435)

[MartyG]

Whilst I do not have information on your technical questions, you may be interested in the meantime on reading the documentation of the Projection system that SDK 2.0 uses .

https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0 Projection in RealSense SDK 2.0 · IntelRealSense/librealsense Wiki · GitHub

[SEber3]

Hi MartyG,

thank you

I did read this part already. It seems like I need the function "rs2_project_point_to_pixel"

But I am confused by several things:

-Why is this function not living in the rs.hpp? And where do I get the camera intrinsics from?

-Do I really have to apply this function for every pixel by myself? Is there not a wrapper or something in the pointcloud-class?

[MartyG]

rs2_project_point_to_pixel is referenced in the file rsutil.h. A Python binding for it in this file location was added in the 2.10.0 SDK, the version before the current one. In version, 2.10.0, a feature to use Python to "allow extrinsics and intrinsics creation and modification" was also added.

[SEber3]

Hi MartyG,

I am using C++...

I will wait for some technical answers about my understanding of the pointcloud-class.

[PSnip]

Hi MartyG,

Sorry for hijacking this thread. I had a question which I wanted to ask, and can't resist.

Question relates to the camera extrinsic parameter on D435.

The Wiki page "Projection in RealSense" is very interesting.

This page says that there is no rotation between infra1 and infra2 on D4 series. (under last section Model specific details.)

Also, there is translation only on one axis.

Can I assume that same is the case with Depth to Color transformation?

i.e. If I have depth points, then can I obtain color points just by translation on x-axis [translation on y & z axis being zero].

Also is there any way to grab the extrinsic parameters, without having to set up a full fledge application?

For example, I am using D4xx under ROS, and I am just able to dump the intrinsic matrix from camera_info message.

Is there a similar easy way to dump extrinsic matrix somewhere?

Regards,

PS

[MartyG]

Your translation question is a bit outside of my programming experience. I will field it to stream programming expert jb455 to see if he can offer advice on it.

In regard to your other question, the ability to create and edit intrinsics and extrinsics using the Python language was recently added to the SDK.

[jb455]

I would suspect that the simple translation between the IR cameras will be as a result of them being identical sensors fixed on the same board to be in-line on the horizontal axis. The colour camera will likely be different, with different intrinsics, and possibly on a different plane to the others. Maybe you can get by in your app with a very rough depth to colour translation but I don't think it would be very accurate across the whole image.

I'm not familiar with ROS, but in C# getting extrinsics is pretty simple:

var pipeline = new Pipeline();

var cfg = new Config();

cfg.EnableStream(Stream.Depth, 640, 480);

cfg.EnableStream(Stream.Color, Format.Bgr8);

var profile = pipeline.Start(cfg);

var depthStream = profile.GetStream(Stream.Depth);

var colourStream = profile.GetStream(Stream.Color);

var extrinsics = depthStream.GetExtrinsicsTo(colourStream);

Hopefully that's easy to translate!

[PSnip]

Hi JB,

Thanks for your suggestion.

I managed to print the Depth to Color extrinsics under ROS. There is a code section where these matrices are computed, and I just added logs around that place.

Regarding camera calibration, there are two things: Extrinsics, Intrinsics.

While Intrinsic depends on camera construction [focal length, aspect ratio etc], the extrinsic depends purely on the Camera Pose.

If you consider the pose of Color sensor w.r.t. Depth sensor in a 3-D co-ordinate system, then it is similar to how two infra cameras are positioned w.r.t. each other.

So, I had a feel that it will require only X translation. Which means that if you move these cameras on x-axis w.r.t. each other, their centres will eventually align.

Here are the rotation and translation value which I got, and which confirm this understanding

Translation (if we just consider 3-decimal points then y & z are 0)

0.014696 -0.000154 -0.000151

Rotation (if we round off the values, its an identity matrix almost)

0.999958 -0.005230 -0.007479

0.005223 0.999986 -0.000844

0.007483 0.000805 0.999972

This resolves my question. I posted the above information here in case it will benefit any one else.

[jb455]

I don't think pc.map_to actually maps the point cloud to the colour image. I believe it instead creates a mapping from the colour image to the point cloud. So in your case, the point cloud is still aligned to the depth image instead of colour

It looks like this code may help you: https://github.com/IntelRealSense/librealsense/blob/master/common/model-views.cpp# L189 librealsense/model-views.cpp at master · IntelRealSense/librealsense · GitHub

The texture coordinates return uv coordinates mapping to the colour image with the coordinates normalised to 0-1. So to get from uv to xy coordinates (ie, pixel coordinates in the colour image) you have to multiply the u and v values by the colour image width and height (though that code is slightly more complex to account for the extreme edges of the image).

Alternatively, I believe you can first align the depth image to colour using Align, then pass that aligned depth image to the point cloud to get your point cloud aligned to the colour image if you'd prefer. Then getting the colour of a vertex or the vertex of a colour pixel is simple as they will have the same indices of their respective arrays. (so (x,y) in colour image has corresponding vertex points[x + y * colourWidth] and vice-versa)

[SEber3]

Hi jb455,

thank you for the clarification. Actually, I did already multiply the image dimensions to the texture_coordinates but I did not trust in the result Now I trust a little bit more in this projection.

The route using the align class is also an option to solve my projection problem, thank you.

Still, I have some more questions about the map_to function of the pointcloud class:

The Kinect API did work with three different coordinate systems: Camera Space (3D, origin at IR sensor), Color Space (2D, plane of rgbx image) and Depth Space (2D, plane of depth image).

Now, they had three mapping functions (and many more):

-MapDepthFrameToColorSpace -->results in 2D point cloud, mapping every depth pixel to a corresponding pixel of the rgb camera

-MapDepthFrameToCameraSpace --> results in 3D point cloud. Every depth pixel is mapped to a 3D point in camera space

-MapColorFrameToCameraSpace -->results in 3D point cloud. Every rgbx pixel is mapped to a 3D point in camera space

How do use the pointcloud class to get similar results. What is the origin of the pointcloud returned by the "calculate" function? Is it the mid-point between the two IR cameras of the D415/435?

As I understood, "MapDepthFrameToColorSpace" has to be replicated via the align class or via get_texture_coordinates with additional scaling.

[jb455]

I believe the point cloud is aligned to the depth image, which itself is aligned to one of the IR images (from streaming with the viewer app and putting my finger over the lenses in turn, it appears to be the left lens as you're looking at it, which is called "Infrared 1" in the UI).

From what I understand, the standard point cloud is equivalent to MapDepthFrameToCameraSpace, using align will be like MapDepthFrameToColorSpace, and passing the aligned depth image to pointcloud should be like MapColorFrameToCameraSpace.

We're fast approaching the limits of my knowledge here so hopefully someone else will be able to double-check my assertions!

[SEber3]

void * MapDepthFrameToColorSpace()

{

void * pDepthProjected = NULL;

if(!m_bIsAligned) //boolean which flags whether the alignment witht the proedure align::process is used or not (somewhere else in the class)

{

rs2::pointcloud pc;

pc.map_to(m_oFrameset.get_color_frame()); //all points have coordinate system of the color camera

m_oDepthPointsInColorCoords = pc.calculate(m_oFrameset.get_depth_frame()); //calculate the coordinates of the depth frame in the color camera coordinate system

pDepthProjected = (void *)m_oDepthPointsInColorCoords.get_texture_coordinates(); //project 3D-points to color-camera plane //be aware, that these coordinates are in an interval [0..1], which has to be scaled by the image width and height

}

else

{

//if depth and color are aligned and the mapping was not produced before, compute (trivial) mapping

if (m_pDepthPointsInColorSpace == NULL) //compute mapping only once

{

int width = m_oFrameset.get_depth_frame().get_width();

int height = m_oFrameset.get_depth_frame().get_height();

m_pDepthPointsInColorSpace = new point2D[width*height];

point2D * pTmp = m_pDepthPointsInColorSpace;

for (int y = 0; y < height; y++)

{

for (int x = 0; x < width; x++)

{

pTmp->x = x;

pTmp->y = y;

}

}

}

pDepthProjected = m_pDepthPointsInColorSpace;

}

return pDepthProjected;

}

void * MapDepthFrameToCameraSpace()

{

void * pVertices = NULL;

rs2::pointcloud pc;

pc.map_to(m_oFrameset.get_depth_frame()); //all points have coordinate system of the depth camera

m_oDepthPointsInDepthCoords = pc.calculate(m_oFrameset.get_depth_frame()); //calculate the coordinates of the depth frame in the depth camera coordinate system

pVerticesTmp = m_oDepthPointsInDepthCoords.get_vertices();

pVertices = (void *)pVerticesTmp;

return pVertices;

}

void * ClRealSenseD400::MapColorFrameToCameraSpace()

{

void * pVertices = NULL;

rs2::pointcloud pc;

pc.map_to(m_oFrameset.get_depth_frame()); //all points have coordinate system of the depth camera

m_oColorPointsInColorCoords = pc.calculate(m_oFrameset.get_color_frame()); //calculate the coordinates of the color frame in the depth camera coordinate system

pVertices = (void *) m_oColorPointsInColorCoords.get_vertices();

return pVertices;

}

I wrote three functions to resemble the three functions MapDepthFrameToColorSpace, MapDepthFrameToCameraSpace and MapColorFrameToCameraSpace; I am ok with the ...ToCameraSpace functions.

I am still unhappy with the solution of MapDepthFrameToColorSpace, because get_texture_coordinates does not map directly to the color frame pixels but to an interval [0...1], so that the coordinates have to be scaled by color image width and height.

Also, the approach with the align class is not nice, because now, color or depth image dimension will be changed to have same image dimension in depth and color frame

A different approach would use rs2_project_point_to_pixel from librealsense2/rsutil.h instead of get_texture_coordinates(), but

• I am not sure which camera intrinsics I need to use. color or depth stream?
• These functions work only per pixel. Thus I have to loop over every space point and apply the function; In my case, where I want to return a pointer to the pointcloud, I would need to allocate a second pointcloud in memory with the result of rs2_project_point_to_pixel;

As this is a very complicated topic, I wonder why there is no function for accomplishing MapDepthFrameToColorSpace in the SDK2.0;

[jb455]

The intrinsics to use in project_point_to_pixel will be whichever image your depth is aligned to. So if it's the raw depth image, use depth intrinsics; if it's aligned to colour use colour intrinsics.

I'm not aware of a way to map colour to depth without going via camera space, though I haven't had cause to try it myself. Perhaps one of the devs/Intel staff will be able to give you advice on GitHub: https://github.com/IntelRealSense/librealsense/issues Issues · IntelRealSense/librealsense · GitHub