Getting wrong values for 3D point and rs2_deproject_pixel_to_point

[DaveBGld]

Using ROS1 C++. Realsense D431i, latest version of realsense libraries, on Ubuntu 20.04

Introduction

In the following piece of code, I have a point from a D435i camera, post processed to do object detection, and I am interested in the 3D dimensions marked by point sideA.x, sideA.y which correspond to column, row in the color frame.

rs2_intrinsics intrinsics;
intrinsics = selection.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>().get_intrinsics();float pixel[2];
float my3Dpoint[3]; // x, y, z
pixel[0] = sideA.x;
pixel[1] = sideA.y;
rs2_deproject_pixel_to_point (my3Dpoint, &intrinsics, pixel, heightOfCamera);

The camera is mounted on the wrist of a 6dof arm, and positioned pot a pose double r=0., p=1.5, y=0 which means aligned with the world frame x and y, rotated facing and parallel to the flat surface below formed by x and y, at a known camera height through a value of z in the pose adjusted by the camera actual position from the end effector:

float heightOfCamera = nextPose.position.z + 0.03;

Before using the robot's pose z as a source of depth of the point, I used this ...

float depthofPoint = depth.get_distance (sideA.x, sideA.y);
rs2_deproject_pixel_to_point (my3Dpoint, &intrinsics, pixel, depthofPoint);

(BTW I am doing this exercise in an effort to diagnose problems iwith code I wrote to use a Eye-On-Hand calibration matrix...)

The Problem

The return values for x and y in my3Dpoint are smaller by about 40% from the actual values with both methdos for determining the depth of the pixel needed by rs2_deproject_pixel_to_point ...

Ive read the material pointed to in this reply but did not find a guiding light there.

Guidance is much appreciated... Thanks in advance, Dave

[MartyG-RealSense]

Hi @DaveBGld Are you using depth-color alignment, please? Alignment can introduce large XY value errors on coordinates that are further out from the center of the image, whilst coordinates at and around the center are accurate. The issue can be resolved by not using alignment if you are aligning, or alternatively by using aligned intrinsics instead of depth intrinsics if you are using alignment.

If you are aligning, #4315 and #2523 (comment) may be helpful references in regards to why XY values are incorrect.

[DaveBGld]

@MartyG-RealSense . Thanks for the reply. No, I'm not using either alignment or camera calibration. I am assuming the on camera intrinsics are good.

Do you have a test process to run and test the camera results?

[MartyG-RealSense]

I am not able to replicate your development setup on my computer for testing purposes, unfortunately. I do apologize.

[DaveBGld]

I switched the camera to a brand new one, and got the same results.

You don't have a testing program??

Here is a picture showing the issue...

I have two rulers originating at the 0, 0, 0 point. I marked the pixel in the original color frame to confirm I am requesting the 3d point projection for the same point I want. In the resulting 3D point, the y is acceptable at around 10cm, but the X is at 16.6954 cm instead of the measured 9.8 cm, and that's a very large difference.

### Where it says X32, I mean to say X3D

[MartyG-RealSense]

I mean that I do not have a ROS installation and a script compiler environment on my particular computer, or a 6DOF robot arm and hand-eye calibration.

Without performing alignment between RGB and depth you may have difficulty obtaining accurate values, as the RGB sensor on the D435i camera model has a smaller field of view than the depth sensor. Performing depth to color alignment will automatically resize the depth sensor's field of view to match the RGB sensor's smaller field of view size and also auto-adjust for any differences between depth and RGB resolutions.

[DaveBGld]

I am developing a simple aruco detector with librealsense, so that you can mount the camera on something and see this problem.. I'll send you the repo link as soon as I'm done...

I am mounting the camera on a pole...

[MartyG-RealSense]

I would recommend checking first for an environmental reason for the inaccuracy rather than an issue in the code. For example, reflective areas of the scene such as the reflection on the ruler in your RGB image will make that area more dfficult for the camera to accurately analyze for depth information.

A plain white wall with low texture detail will also be difficult to analyze for depth detail unless a dot pattern from the camera's projector is being cast onto the wall (as the camera can use the dots as a 'texture source' to read depth from). C++ code for controlling the emitter with the RS2_OPTION_EMITTER_ENABLED instruction can be found in the 'librealsense2' section of the link below.

https://dev.intelrealsense.com/docs/api-how-to#controlling-the-laser

Alternatively, the strength of the lighting in the scene could be increased instead of projecting a dot pattern, as strong light cast onto plain surfaces helps to bring out depth information from them. This could make the ruler even more reflective though unless the ruler is changed for a non-reflective one.

Black markings on the wall would not be able to be read for depth information. This is because it is a general physics principle (not specific to RealSense) that dark grey or black absorbs light and so makes it more difficult for depth cameras to read depth information from such surfaces. The darker the color shade, the more light that is absorbed and so the less depth detail that the camera can obtain. Again, casting a strong light source onto black textured surfaces can help to make them easier for the camera to analyze for depth detail.

[MartyG-RealSense]

Going back to the code at the start of this case:

rs2_intrinsics intrinsics;
intrinsics = selection.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>().get_intrinsics();float pixel[2];
float my3Dpoint[3]; // x, y, z
pixel[0] = sideA.x;
pixel[1] = sideA.y;
rs2_deproject_pixel_to_point (my3Dpoint, &intrinsics, pixel, heightOfCamera);

If you are not performing alignment then instead of using rs2_deproject_pixel_to_point (where alignment is typically performed before deprojecting), it may be better to use rs2_project_color_pixel_to_depth_pixel, which provides XYZ coordinates for a single specific 2D color pixel by converting a color pixel to a depth pixel. A C++ example of use of this instruction is at #2982

[DaveBGld]

@MartyG-RealSense Many thanks for your comments!!

I finished on a program that isolates the realsense code away from ROS and hardware. And the issues replicate.

Before sending to you I'll try your suggestions.

I noticed also that there is an offset from the base of the camera to the center of the physical imager frame set. Where can I find that?

[MartyG-RealSense]

Thanks very much, @DaveBGld My ruler measures the height between the base of the camera and the sensor center-line to be 1 cm / 10 mm.

[DaveBGld]

Oh no.. @MartyG-RealSense ... I'm talking about the unexpected shift to the left of the mount.. in this picture the mount is marked buy a red flag, while the Center of the image is aligned with the Green Cross, introducing a substantial deviation. This is the number I'm looking for.

With this deviation this camera should only be used after calibrating with either eye to hand or eye on hand, of which I have not found any that uses the 3D point 0, 0, 0 at the center of the image. I assume this is a problem that you have faced before. Can you point me in the direction of some previously used eye on hand extrinsic calibration for the camera?

[MartyG-RealSense]

A depth image is constructed primarily from the left sensor (the green cross) but also taking into account the right-side sensor. The 0,0,0 depth origin is the center-line of the left sensor. The data sheet document for the 400 Series cameras explains it in the following way.

---

The left and right imagers capture the scene and send imager data to the depth imaging (vision) processor, which calculates depth values for each pixel in the image by correlating points on the left image to the right image and via the shift between a
point on the Left image and the Right image. The depth pixel values are processed to generate a depth frame.

---

Below are a couple of hand-eye calibration solutions from previous discussions about this subject that have been stated to work with 400 Series cameras.

autoCalibration
#3569 (comment)

'Robotic Hand-eye Calibration Workspace' eye-in-hand for ROS
https://github.com/lixiny/Handeye-Calibration-ROS

The above system makes use of visp, which has been separately recommended by some for hand-eye calibration with RealSense.

http://wiki.ros.org/visp_hand2eye_calibration

[DaveBGld]

I am aware of the data sheet, and had taken guidance from the explanation related to FOV as in the picture below with a very clear Baseline image marked at the center, which in my opinion needs to be corrected. You cannot have a FOV from the center, and 0,0,0 from the left sensor... This cost me some very expensive time in tracking unexplained differences in alignment.

Thank you for clarification. This is such an important element, that I would be happy to actually see the center of image marked in the camera casing... Could you please pass along for me to the right people at Intel?

At any rate, thanks for your pointing out.

eye-hand Calibration

I had in fact read those posts about a month ago, and could not get any of those you point out to work. I spent a couple of hours on each.

After that, and before knowing these two special facts about the location of the 3D(0, 0, 0), I developed my own calibration based on Intel's OpenCV Aruco library (which I also used for the image above).

I was looking for official support from Intel at least to produce the uniform transformation matrix for the camera.

Anyhow, I'm back to trying to solve the original problem of the post, still unsolved as you can see in my previous post...

[DaveBGld]

#Back to the Original Problem

What is Intel's best practice or guidance to use the D435i camera to measure distances between points in 3D space when the points are selected from processing the color frame?

[MartyG-RealSense]

I have passed your suggestion about a camera casing mark for the center of the depth FOV to an appropriate member of the Intel RealSense team. Thanks!

If you are aiming to measure the distance between two points and not the distance between the camera lenses and a point then the RealSense SDK has an example C++ program called rs-measure.

https://github.com/IntelRealSense/librealsense/tree/master/examples/measure

rs-measure uses the very rare alignment technique of color to depth alignment instead of the common depth to color technique. This means that the color coordinates are mapped to the depth coordinates, and the color FOV will be resized to match the size of the depth FOV.

On the D435i, which has a smaller FOV size for the color sensor, this means that the aligned image will be stretched out to fill the screen instead of having a black letterbox around the edges of the aligned image.

[DaveBGld]

Thanks again, @MartyG-RealSense

Can I please have more information about the alignment between the color frame and the depth frame?

Since my application is based on a machine learning vision pipeline, which takes as input the color frame of the camera, stretching the color frame is not good for me. Therefore what are the options for aligning frames?

[MartyG-RealSense]

Here is some further information about how alignment works. This is quoted from the notes accompanying the rs-align example program, which allows either depth to color and color to depth alignment to be selected.

https://github.com/IntelRealSense/librealsense/tree/master/examples/align

---

Align process lets the user translate images from one viewport to another. That said, results of align are synthetic streams, and suffer from several artifacts:

Sampling - mapping stream to a different viewport will modify the resolution of the frame to match the resolution of target viewport. This will either cause downsampling or upsampling via interpolation. The interpolation used needs to be of type Nearest Neighbor to avoid introducing non-existing values.

Occlusion - Some pixels in the resulting image correspond to 3D coordinates that the original sensor did not see, because these 3D points were occluded in the original viewport. Such pixels may hold invalid texture values.

---

If it is important to preserve the dimensions of the color viewpoint then depth to color would be logical. The main reason that the 'rs-measure' example uses color to depth is simply because the RealSense development team wanted to provide a demonstration of programming color to depth alignment in the RealSense SDK.

You should be able to convert rs-measure to depth-to-color just by changing line 133 of rs-measure.cpp from this:

rs2::align align_to(RS2_STREAM_DEPTH);

to this:

rs2::align align_to(RS2_STREAM_COLOR);

so that the target stream to align to is color instead of depth.

https://github.com/IntelRealSense/librealsense/blob/master/examples/measure/rs-measure.cpp#L133

[MartyG-RealSense]

Hi @DaveBGld Was the information in the comment above helpful to you, please? Thanks!

[DaveBGld]

Hi @MartyG-RealSense Not Really...

One of the uses of the camera is to estimate distances used to calculate and feed movement instructions to a robot arm coming from color video feed from the camera into a machine learning algorithm.

In all of that, there are quite a number of steps where the camera participates, and either the behavior is not well documented or the camera is not behaving as published. Or the examples are irrelevant and don't actually work.

Quite frankly I am overly delayed in my project and exponentially frustrated. I already ordered 3 depth cameras from another vendor...

My question above is about best practice recommendations to be able to measure the actual distance of any pixel to the center of the FOV, and remains unanswered. Align, not align... I have no idea....

[MartyG-RealSense]

If you measure using a RealSense SDK C++ script then the rs-measure example mentioned earlier at #11031 (comment) is likely to be the best option for measuring between two points on the image instead of from the camera lens to a point.

The center pixel of the image for any resolution can be located by dividing the image's resolution by two. So on a 1280x720 resolution RGB image, the center pixel would be at coordinates 440,360 on the image. That gives you the location of one of the two pixels to measure the distance between.

[hariharan382]

hi, when passing the principle coordinates of the camera pixel to rs2.rs2_deproject_pixel_to_point shouldn't it point to coordinates in 3d like (0,0, depth) , but I am getting (random, random, correct depth) please help me here

[MartyG-RealSense]

Hi @hariharan382 If you are receiving highly inaccurate results from the XY coordinates of an image (the '0,0' of 0, 0 depth) and you are performing depth to color alignment first before performing rs2_deproject_pixel_to_point afterwards, the issue is likely caused by the alignment.

Are you using depth to color alignment in your script and using it before the part of your script where you use rs2_deproject_pixel_to_point please?

[hariharan382]

I just mismatched my index of (480,848) as X,Y now it is working for center coordinates,but yet to check other coordinates

[MartyG-RealSense]

That's great to hear, @hariharan382 - thanks very much for the update!

If you are obtaining the coordinates using the get_distance() instruction then you may find that the XY coordinates are accurate at the center coordinates but become increasingly inaccurate when moving towards the outer edges of the image. If you are using a depth-to-color aligned image and this occurs then it may be because depth intrinsics are being used when color intrinsics should instead be used.

[MartyG-RealSense]

Hi @hariharan382 Do you require further assistance with this case, please? Thanks!

[hariharan382]

Thanks sir for your kindness ,if needed will sure post ,I am yet to calibrate sensor

…

On Mon, 14 Nov, 2022, 1:23 pm MartyG-RealSense, ***@***.***> wrote: Hi @hariharan382 <https://github.com/hariharan382> Do you require further assistance with this case, please? Thanks! — Reply to this email directly, view it on GitHub <#11031 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ALJKNFRRYWY44FPFD2FHD5LWIHVX3ANCNFSM6AAAAAARPLCSJU> . You are receiving this because you were mentioned.Message ID: ***@***.***>

[MartyG-RealSense]

Thanks very much @hariharan382

[MartyG-RealSense]

I promise that your code listings and messages have not been ignored.

When using pc.calculate, you can obtain the coordinates from vertices with the points.get_vertices() instruction like in the C++ code at #10672 (comment)

One of my Intel RealSense colleagues provides advice at #3072 (comment) regarding locating the pointcloud center. They state: "Using the sensor intrinsic you may obtain the principal point that represents the focal center in pixel's coordinates".

[DaveBGld]

@MartyG-RealSense Thanks!

A: On the intrinsics ppx and ppy to find the principal point of the image, which intrinsics (color or depth) have to be used if using pc.map_to(color); ? And which if using rs2::align align_to(RS2_STREAM_COLOR); ?

B: On the use of vertices, the code in your post here, in lines

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

My understanding is that vertices is an array of vertex elements (x, y, z) that represent the points that the camera could measure a distance to in 3D space.
tex_coords is an array of u, v coordinates (which is column and which row?), so that given a vertex element in array vertices, the same index in array tex_coords points to the pixel in the color frame. Is this correct?

However, I need to go in the opposite direction, from a pixel (column, row) in the color frame to a corresponding vertex element within the vertices array. Do I need to scan one by one the elements of the tex_coords array to find the corresponding vertex element for the pixel?

I can find no code example or any documentation to allow me to map from some color frame pixel(col, row) to a specific vertex in vertices, which is what I need.

Thanks

[MartyG-RealSense]

A. In a C++ script shared by a RealSense team member at #3375 (comment) that uses map_to and also prints the Principal Point value, the script was using video_stream_profile to retrieve the intrinsics used to calculate the Principal Point.

rs2_intrinsics intrinsics = frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();

---

When using align_to() instead of map_to(color), the intrinsics of the target stream that is being aligned to would be used. So in depth to color alignment, color intrinsics are used as the color stream is the target of depth to color alignment.

B. In regard to tex_coords, they are a gap in my programming knowledge and so I will refer you to a statement from a programmer who knows more about them than myself. They stated the following:

---

The texture coordinates return uv coordinates mapping to the color image with the coordinates normalised to 0-1. So to get from uv to xy coordinates (i.e pixel coordinates in the color image) you have to multiply the u and v values by the color 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 color of a vertex or the vertex of a color pixel is simple as they will have the same indices of their respective arrays. (so (x,y) in color image has corresponding vertex points[x + y * colorWidth] and vice-versa)

---

Intel have their own official documentation about uv mapping in their white-paper Projection, Texture-Mapping and Occlusion.

https://dev.intelrealsense.com/docs/projection-texture-mapping-and-occlusion-with-intel-realsense-depth-cameras#34-stream-alignment

[DaveBGld]

@MartyG-RealSense , that was useful. Thank you

[DaveBGld]

I was able to implement both methods, aligned frames and point cloud, both aligning to color, and I get IDENTICAL VALUES

Still, values are not as precise as in realsense-viewer.

This is the code:

        float my3Dpoint[3] = {0.00000, 0.00000, 0.00000}; // x, y, z
        float depthofPoint = 0;  // Maybe substituted by actual robot Z dimension adjusted for camera??
        getRealSense3DPointAligned (my3Dpoint, depthofPoint, x, y);
        std::cout << "PointCloud x: " << globalVertices[x + y * 1280].x << ", y: " <<  globalVertices[x + y * 1280].y 
        << " || Aligned x:" <<  my3Dpoint [0] << ", y: " <<  my3Dpoint [1] << std::endl;

Also, the center of the sensor is different in realsense-viewer as can be seen in the next image (the center of my image is a color intrinsics ppx ppy, and the measurements at that point are close to 0 for both axes), which worries me that something is not as should be:

Additionally every time the realsense-viewer loads, the center is not in the same place. Nothing else moves including the camera.

[MartyG-RealSense]

The RealSense Viewer enables a range of post-processing filters by default when launched, so in a script of your own creation these filters must be implemented into the script manually in order to achieve results that are closer to those of the Viewer.

You can see which post-processing filters and their settings are being applied by expanding open the Stereo Module > Post-Processing section of the Viewer's options side-panel.

You could also try disabling all post-processing filters in the Viewer by left-clicking on the blue icon beside Post-Processing to change its color and then see whether the location of the center becomes more stable.

[DaveBGld]

@MartyG-RealSense I did both and no change.

[MartyG-RealSense]

Apart from the post-processing filters, there will be other settings being applied by default in the Viewer that may affect the accuracy of depth results, such as the Advanced Controls.

Of these settings, I would say that the 'DS Second Peak Threshold' setting in the Depth Control category will have the most impact on the accuracy of the depth coordinate reading. This parameter will disqualify a depth pixel, if the "second best" match is too close to the best match.

I also note that the Viewer uses the Custom camera configuration preset by default, whilst your script earlier at #11031 (comment) uses the different Default preset.

The Medium Density preset provides a good balance between accuracy and the amount of detail on the depth image, whilst 'High Accuracy' tends to remove an excessive amount of depth detail from the image.

[MartyG-RealSense]

Hi @DaveBGld Was the information in the comment above helpful, please? Thanks!

[DaveBGld]

@MartyG-RealSense

I presume by your nickname that you are a part of the Intel organization, and I write this post in that spirit. If you are not, please ignore.

Was the information in the comment above helpful, please? Since you ask, No, it wasn't. And very atypical from other Intel forums where I had very professional and thoughtful replies. I am trying to use Intel products in a commercial setting, where the cost of development is crucial, and when things don't go well, and suddenly a development project turns into a research project with bad documentation or support its nothing less than a frustrating nightmare.

When none of the rs2 class is documented, the support in this forum is critical for your customer's success. But your wrong answers, together with your non answers like ".... Apart from the post-processing filters, there will be other settings being applied by default in the Viewer that may affect the accuracy of depth results, such as the Advanced Controls...." Or links to unproven customer code in other posts that doesn't even compile; is the farthest thing an ongoing commercial project needs.

In this case, you excused yourself out of actually giving answers because you don't have ROS. In response, we actually spent the money (about 30 times the cost a camera) to develop a camera only application to replicate the problem in a way you can relate to, and still you did not have any interest in relating to that.

A month and a half of following your instructions leading nowhere.... Thousands of dollars wasted!!! and, PROJECT DELAYED!

And most importantly, the issues persist and we have no answers from Intel.

I have introduced cameras from another vendor for this side of the application, and we will migrate over time our existing realsense code to that.

In applications centered around a vision based machine learning / AI, the camera performance is up there with the most critical success factors for the application. I've learned my lesson and will be more careful when selecting or recommending a depth camera for a project!

If you want to reply with a solution to the problem, I'll be happy to hear, otherwise, don't bother. Just close the issue.

[MartyG-RealSense]

I am a member of the Intel RealSense support team, yes. I apologize again that a solution has not yet been found for your concern about the center of the image apparently changing each time that realsense-viewer loads. I have therefore referred your case to my colleagues on the RealSense support team to look at this problem. Thanks again very much for your ongoing patience.

[DaveBGld]

@MartyG-RealSense No, you continue to miss the point: The camera measurements are imprecise, and therefore, we cannot use on the one device that needs to measure distances for precise movements. The different behavior between our code results and the viewer are just a means to confirm that something is wrong. But ideally if they would share the same center coordinates, we could continue debugging by comparing measurement to points off the center.

The title of the post Getting wrong values for 3D point and rs2_deproject_pixel_to_point continues being the problem more than a month and a half of efforts, multiple efforts.

And I am not looking for a solution but rather information that may lead to the proper use of the API, such that there is no problems with it. But with no such minimal documentation on the rs2 API, and whatever is available is more of a conceptual description on aspects of the theory of depth cameras, and you misleading us and sending us on wild geese chases that resulted only in wasted time and money and a delayed project, has not been conducent to anything positive.

[MartyG-RealSense]

You may find the menu-driven version of the API documentation at the link below to be an easy way to navigate the RealSense API. It draws data directly from the official documentation pages and formats it into a user-friendly interface.

https://unanancyowen.github.io/librealsense2_apireference/classes.html

[DaveBGld]

@MartyG-RealSense Yes I saw that. However, this is a hierarchy description of the API without clear description of what the methods do, or what the received and returned pentameters are. Not a source to be inspired on chow to use the API.

For example, go to The documentation about callbacks and tell me you learned how to implement a frame callback.

More importantly, rs2_deproject_pixel_to_point the focus of this post is not included there.

[MartyG-RealSense]

I have passed your precise explanation of your problem today to my support colleagues so that they can look into it.

[MartyG-RealSense]

May I also ask whether you have a non-disclosure agreement (NDA) with Intel, please? If you do then you have the option of also submitting a ticket to Intel's Zendesk support channel.

[MartyG-RealSense]

My support colleagues have analyzed this case and provided feedback.

1. It should not be assumed that the center of the image will coincide with (0, 0, 0). Instead, it depends on the sensor. The principal point may not be exactly the center of the sensor. Often, there is a small offset.

2. There are multiple ways to find the 3D point in the depth coordinates system from a known pixel in the color image. Scripts for three different methods are provided below.

---

Method 1 - align depth to color, deproject pixel into point in color coordinate systems, then transform the point back to depth coordinate system.

rs2::align align_to(RS2_STREAM_COLOR);
       rs2::pipeline pipe;
       rs2::config cfg;
       cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 15);
       cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGBA8, 15);

       auto profile = pipe.start(cfg);

       while (app)
       {
              // Wait for the next set of frames from the camera
              auto frames = pipe.wait_for_frames();

       auto depth = frames.get_depth_frame();
              auto color = frames.get_color_frame();

              rs2_extrinsics color_extrin_to_depth = color.get_profile().as<rs2::video_stream_profile>().get_extrinsics_to(depth.get_profile().as<rs2::video_stream_profile>());

              frames = frames.apply_filter(align_to);
              auto color_aligned = frames.get_color_frame();
              auto depth_aligned = frames.get_depth_frame();

              rs2_intrinsics intr_depth = depth_aligned.get_profile().as<rs2::video_stream_profile>().get_intrinsics();

              // pixel in color coordinate system
              float u = 0.5;
              float v = 0.5;

              int w = color_aligned.get_width();
              int h = color_aligned.get_height();

              int c = w * u;
              int r = h * v;

              // assume depth and color same size after alignment, find depth pixel in color coordinate system
              float pixel[2];
              pixel[0] = c;
              pixel[1] = r;

              float point_in_color_coordinates[3]; // 3D point in color sensor coordinates
              float point_in_depth_coordinates[3]; // 3D point in depth sensor coordinates

              auto dist = depth.get_distance(static_cast<int>(pixel[0]), static_cast<int>(pixel[1]));
              rs2_deproject_pixel_to_point(point_in_color_coordinates, &intr_depth, pixel, dist);
              rs2_transform_point_to_point(point_in_depth_coordinates, &color_extrin_to_depth, point_in_color_coordinates);
       }

---

Method 2 – do not align, use the original depth and color images, rs2_project_color_pixel_to_depth_pixel, then rs2_deproject_pixel_to_point.

void getRealSense3DPoint(float result3DPoint[3], int c, int r, const rs2::depth_frame& depth, rs2_intrinsics* depth_intrin, rs2_intrinsics* color_intrin, rs2_extrinsics* color_extrin_to_depth, rs2_extrinsics* depth_extrin_to_color)
{

       float rgb_pixel[2], depth_pixel[2];
       int int_depth_pixel[2];
       rgb_pixel[0] = float(c); // col in RGB frame
       rgb_pixel[1] = float(r); // row  in RGB frame

       float depth_scale = depth.get_units();

       rs2_project_color_pixel_to_depth_pixel(depth_pixel,
              reinterpret_cast<const uint16_t*>(depth.get_data()),
              depth_scale,
              0.20, 2.0,  // From 0 to 2 meters Min to Max depth
              depth_intrin,
              color_intrin,
              color_extrin_to_depth,
              depth_extrin_to_color, rgb_pixel);

       int_depth_pixel[0] = (int)round(depth_pixel[0]);
       int_depth_pixel[1] = (int)round(depth_pixel[1]);
       depth_pixel[0] = int_depth_pixel[0];
       depth_pixel[1] = int_depth_pixel[1];

       auto dist = depth.get_distance(static_cast<int>(depth_pixel[0]), static_cast<int>(depth_pixel[1]));

       rs2_deproject_pixel_to_point(result3DPoint, depth_intrin, depth_pixel, dist);
}

---

Method 3 – align depth and use point cloud.

            rs2::pointcloud pc;
             rs2::points points;
             rs2::align align_to(RS2_STREAM_COLOR);

             rs2::pipeline pipe;
             rs2::config cfg;

             cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 15);
             cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGBA8, 15);

             auto profile = pipe.start(cfg);
             … …

             // Wait for the next set of frames from the camera
             auto frames = pipe.wait_for_frames();

             auto depth = frames.get_depth_frame();
             auto color = frames.get_color_frame();

             rs2_intrinsics intr_depth = depth.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
             rs2_intrinsics intr_color = color.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
             rs2_extrinsics color_extrin_to_depth = color.get_profile().as<rs2::video_stream_profile>().get_extrinsics_to(depth.get_profile().as<rs2::video_stream_profile>());
             rs2_extrinsics depth_extrin_to_color = depth.get_profile().as<rs2::video_stream_profile>().get_extrinsics_to(color.get_profile().as<rs2::video_stream_profile>());

             frames = frames.apply_filter(align_to);
             auto color_aligned = frames.get_color_frame();

             // Tell pointcloud object to map to this color frame
             pc.map_to(color_aligned);

             auto depth_aligned = frames.get_depth_frame();

             // Generate the pointcloud and texture mappings
             points = pc.calculate(depth_aligned);

             // pixel in color image
             float u = 0.609375; // 780
             float v = 0.488889; // 352

             int w = color_aligned.get_width();
             int h = color_aligned.get_height();

             int c = w * u;
             int r = h * v;

             int index = c + w * r;

             // find corresponding depth point from vertices
             const rs2::vertex* tv = points.get_vertices();
             float x = tv[index].x;
             float y = tv[index].y;
             float z = tv[index].z;

             float point_in_color_coordinates[3]; // 3D point in color sensor coordinates
             float point_in_depth_coordinates[3]; // 3D point in depth sensor coordinates

             point_in_color_coordinates[0] = x;
             point_in_color_coordinates[1] = y;
             point_in_color_coordinates[2] = z;

             // transform 3d point from color coordinates into depth coordinates
             rs2_transform_point_to_point(point_in_depth_coordinates, &color_extrin_to_depth, point_in_color_coordinates);

[MartyG-RealSense]

My colleagues also provided a sample app with all three of the above methods implemented. To work accurately, the camera should be well calibrated.

rs-pointcloud.txt

[MartyG-RealSense]

Hi @DaveBGld Was the information and sample app code in the above two comments helpful to you, please? Thanks!

[MartyG-RealSense]

Hi @DaveBGld Do you require further assistance with this case, please? Thanks!

[DaveBGld]

Thanks @MartyG-RealSense That was very helpful.

What do yo mean by

should be well calibrated?

I am using out-of-the-box cameras

[MartyG-RealSense]

My colleague is stating the importance of having a well-calibrated camera, but if the camera is new out of the box then this should not be a concern as RealSense cameras are calibrated in the factory during manufacture and so are typically well-calibrated when first taken out of the box after purchase.

[DaveBGld]

@MartyG-RealSense Thanks! And when should calibration be considered for periodic maintenance?

And, What kind of calibration? Is there documentation?

Thanks!

[MartyG-RealSense]

A camera may become mis-calibrated if it receives a physical shock such as a hard knock, severe vibration or a drop on the ground. A very high temperature surge can also cause mis-calibration.

If keeping the camera consistently well calibrated is important to your project then calibrating it once per week should be sufficient in most situations.

There are two main types of calibration, Dynamic Calibration and Self Calibration.

Dynamic Calibration is performed by installing a seperate software tool. It only calibrates the camera's extrinsics and not the intrinsics, but has the advantage of being able to calibrate RGB in addition to depth.

https://www.intel.com/content/www/us/en/support/articles/000026723/emerging-technologies/intel-realsense-technology.html

Self-Calibration includes the calibration tools On-Chip (for improving depth quality) and Tare (for improving depth measurement accuracy). It is built into the camera firmware and so does not require the installation of a seperate software package. It does not calibrate RGB, but it has the advantage of being able to calibrate both extrinsics and intrinsics (though as separate operations rather than both at the same time).

https://dev.intelrealsense.com/docs/self-calibration-for-depth-cameras

The On-Chip and Tare tools can be accessed from the More option in the RealSense Viewer's options side-panel or controlled with program scripting.

[DaveBGld]

@MartyG-RealSense Great Info! Thanks. Closing