Pointcloud: comparing deprojected points vs points.get_vertices()

[grigala]

---

Required Info
Camera Model	D415
Firmware Version	05.11.01.100
Operating System & Version	Linux (Ubuntu 18)
Kernel Version (Linux Only)	4.15.0
Platform	PC
SDK Version	2.20.0
Language	Python

Comparing rs.rs2_deproject_pixel_to_point() and points.get_vertices()

I'm trying to understand what is the difference (and most importantly where does it come from) between a point which is produced by rs.rs2_deproject_pixel_to_point() and the one obtained from the point cloud generated by call points.get_vertices().

TLDR;

x coordinates are slightly off almost always ✘
y coordinates are slightly off almost always ✘
z coordinates are always the same ✓

by saying almost always I mean that there were cases when, if I round up values to 2 decimals I might get the same coordinates but not always. Whereas z value is exactly the same up until 7 decimals.

ACTUAL OUTPUT(See the example code bellow):

[INFO] Starting pipeline
[INFO] Width:  640 Height:  480
[INFO] Focal length:  602.9031982421875
[INFO] Principal point X:  318.238525390625
[INFO] Principal point Y:  237.61407470703125

Number of pixels ignored: 38471
Projected point: index = 146928 x = 1.6002562 y = 0.20419632 z = 6.039000511169434
SDK point:       index = 146928 x = 1.6427026 y = 0.25497285 z = 6.0390005

Can anybody tell me where does this difference might be coming from? I tried to find the exact implementation how SDK(C++) obtains point cloud points to examine and compare what I'm doing wrong but I couldn't find one.

Code

import random

import cv2
import numpy as np
import pyrealsense2 as rs

pipeline = rs.pipeline()
config = rs.config()

config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
pipeline.start(config)

profile = pipeline.get_active_profile()
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()

pc = rs.pointcloud()
colorizer = rs.colorizer()

align_to = rs.stream.color
align = rs.align(align_to)
try:
    while True:
        frames = pipeline.wait_for_frames()
        aligned_frames = align.process(frames)

        aligned_depth_frame = aligned_frames.get_depth_frame()
        aligned_color_frame = aligned_frames.get_color_frame()

        depth_colormap = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
        color_image = np.asanyarray(aligned_color_frame.get_data())

        images = np.hstack((cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB), depth_colormap))
        cv2.imshow("win", images)

        pts = pc.calculate(aligned_depth_frame)
        pc.map_to(aligned_color_frame)

        if not aligned_depth_frame or not aligned_color_frame:
            continue

        key = cv2.waitKey(1)

        if key == ord("d"):
            min_distance = 1e-6
            v = pts.get_vertices()
            vertices = np.asanyarray(v).view(np.float32).reshape(-1, 3)  # xyz
            h, w, _ = color_image.shape
            sdk_pts = []
            projected_pts = []
            counter = 0
            for x in range(h):
                for y in range(w):
                    z = aligned_depth_frame.get_distance(y, x)
                    if z:
                        point = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [y, x], z)
                        if (np.math.fabs(point[0]) >= min_distance
                                or np.math.fabs(point[1]) >= min_distance
                                or np.math.fabs(point[2]) >= min_distance):
                            projected_pts.append([round(point[0], 8), round(point[1], 8), point[2], 3])
                    else:
                        # Ignoring pixels which doesn't have depth value
                        # print("No info at:", [y, x], z)
                        counter += 1
                        pass  

            for i in range(pts.size()):
                if (np.math.fabs(vertices[i][0]) >= min_distance
                        or np.math.fabs(vertices[i][1]) >= min_distance
                        or np.math.fabs(vertices[i][2]) >= min_distance):
                    sdk_pts.append(vertices[i])
            
            # Checking if the number of points generated are the same
            assert len(projected_pts) == len(sdk_pts)  # PASS

            print("Number of pixels ignored:", counter)

            rnd = random.randint(0, len(projected_pts))
            print("Projected point:",
                  "index =", rnd,
                  "x =", projected_pts[rnd][0],
                  "y =", projected_pts[rnd][1],
                  "z =", projected_pts[rnd][2]
                  )
            print("SDK point:",
                  "index =", rnd,
                  "x =", sdk_pts[rnd][0],
                  "y =", sdk_pts[rnd][1],
                  "z =", sdk_pts[rnd][2]
                  )
        if key == ord("q"):
            break
finally:
    pipeline.stop()

Both projected_pts and sdk_pts list are filled up the same way export_to_ply() method fills vertices list:

librealsense/include/librealsense2/hpp/rs_export.hpp

Lines 64 to 71 in d6f6be8

	static const auto min_distance = 1e-6;
	for (size_t i = 0; i < p.size(); ++i) {
	if (fabs(verts[i].x) >= min_distance || fabs(verts[i].y) >= min_distance ||
	fabs(verts[i].z) >= min_distance)
	{
	idx_map[i] = new_verts.size();
	new_verts.push_back(verts[i]);

[dorodnic]

Hi @grigala
Please note that you are using depth_intrinsics but taking values from aligned_depth_frame. These two viewports are not identical, hence the discrepancy. If you skip align step, or change the code to use aligned intrinsics you should get the right results.

[grigala]

Hi @dorodnic,

Thanks very much for your suggestion, sounds like that could indeed be the problem. I'm gonna give it a try tomorrow.

Are those two approaches you suggested identical? Ignoring align process is easy but that might cause additional problems down the road - how do I get aligned intrinsics?

[dorodnic]

Almost identical, yes. Alignment will introduce small differences because aligned image is resampled from the original (so these are not exactly the same points).
Also, in previous versions of the SDK we did encounter bugs with these types of combined operations, but they should be solved by now and the expected behavior is for two methods to yield very similar results.

[grigala]

Hi @dorodnic,

Thanks, it helped to pass all my tests. However, the reason I was running those tests in the first place was to understand why my deprojected points are getting shifted - they are not where they should be. I still couldn't wrap my head around what could be causing this issue.

I'm obtaining pixel landmarks, deprojecting them to points and when visualizing them together with point cloud mesh the landmark points are shifted( x and y coordinates are off with roughly 5mm from the desired location) - when I align frames landmarks are moved up and right, and when I don't align them they are moved up and left. i.e. in the first case tip of the nose is moved up(x coordinate) about 6mm and left(y coordinate) for about 3mm.

The only thing that came to my mind was to manually add some fixed values to the positive axis of x and y coordinates - in the example case above (x + 0.006, y + 0.003), but in this case I cannot guarantee that those shifted points will be still positioned on the mesh itself - sometimes they might be outside of mesh region, which is not desirable.

Any thoughts what could be wrong?

Thanks in advance, appreciate your follow up.

[grigala]

Could this be an issue of aligning frames to color stream?
Because it seems like if I align frames to depth stream, problem dissappears:

align_to = rs.stream.colordepth
align = rs.align(align_to)

[ev-mp]

@grigala , deprojecting depth-aligned-to color frame will produce different, and more important, less accurate results than the x,y,z coordinates generated by the pointcloud. See #4536

[grigala]

@ev-mp Thanks for reply. Yes, I come to the same conclusion just by analyzing two different point clouds, one produced by pipeline with stream aligned to color frame and the other aligned to depth frame.

The only disadvantage is that color image generated after aligning stream to depth frame gets black pixels(depth info missing) on the image. Which might not be desirable for some applications.

[ev-mp]

@gridala, the root cause for the resulted inaccuracy was, imo, elaborated here
#2523