How to align confidence matrix from L515 with RGB image?

[erjiekai]

Required Info
Camera Model	L515
Firmware Version	01.02.03.00
Operating System & Version	Win 10
Platform	PC
SDK Version	2.39.0.2337
Language	C++
Segment	Robot

Issue Description

How to align confidence matrix from L515 with RGB image?

I can align my depth image (depth frame) to RGB image (camera frame) as follows:

rs2::align *align_to = new rs2::align(RS2_STREAM_COLOR);
frame_data = d435_pipe.wait_for_frames().apply_filter(*align_to);

depth_frame = frame_data.get_depth_frame();
camera_frame = frame_data.get_color_frame();
color_frame = colorizer.colorize(depth_frame);
confidence_frame = frame_data.first_or_default(RS2_STREAM_CONFIDENCE);

But my confidence frame is not aligned!

Is there a simple function call to align them (confidence frame to camera frame)?

[erjiekai]

I may have found a solution, but unsure of its accuracy. This is edited from align.cpp

int depth_width = depth_intrin.width;
int depth_height = depth_intrin.height;
int camera_width = camera_intrin.width;
int camera_height = camera_intrin.height;

unsigned char *aligned_confidence = new unsigned char[camera_width * camera_height];

int count = 0;
for (int i = 0; i < depth_width * depth_height; i++)
{
	//top left corner of pixel
	float conf_point[3], other_point[3], other_pixel[2]
	float conf_pixel[2] = {i % depth_width - 0.5f, i / depth_width - 0.5f};
	conf_point[0] = (conf_pixel[0] - confidence_intrin.ppx) / confidence_intrin.fx;
	conf_point[1] = (conf_pixel[1] - confidence_intrin.ppy) / confidence_intrin.fy;
	conf_point[2] = 1;

	rs2_transform_point_to_point(other_point, &confidence_extrin, conf_point);
	other_point[2] = 1;

	rs2_project_point_to_pixel(other_pixel, &camera_intrin, other_point);
	const int x0 = static_cast<int>(other_pixel[0] + 0.5f);
	const int y0 = static_cast<int>(other_pixel[1] + 0.5f);

	// bottom right corner of pixel
	conf_pixel[0] = i % depth_width + 0.5f;
	conf_pixel[1] = i / depth_width + 0.5f;
	conf_point[0] = (conf_pixel[0] - confidence_intrin.ppx) / confidence_intrin.fx;
	conf_point[1] = (conf_pixel[1] - confidence_intrin.ppy) / confidence_intrin.fy;
	conf_point[2] = 1;

	rs2_transform_point_to_point(other_point, &confidence_extrin, conf_point);
	other_point[2] = 1;

	rs2_project_point_to_pixel(other_pixel, &camera_intrin, other_point);
	const int x1 = static_cast<int>(other_pixel[0] + 0.5f);
	const int y1 = static_cast<int>(other_pixel[1] + 0.5f);

	// skip aligned pixels that do not fit in the camera frame
	if (x0 < 0 || y0 < 0 || x1 >= camera_width || y1 >= camera_height)
		continue;

	// store the confidence in the new aligned pixel coordinate
	for (int y = y0; y <= y1; ++y)
	{
		for (int x = x0; x <= x1; ++x)
		{
			aligned_confidence[y * camera_width + x] = confidence[i];
		}
	}
}

aligned_confidence array now hold the confidence frame that is aligned with the camera frame.

[RealSenseSupport]

Hi @erjiekai

Regarding the L515 camera, you're right there is no function call to align confidence with RGB. Information to note is that the IR stream and confidence stream are perfectly aligned with the depth stream. So using depth to color alignment then mapping to confidence stream is possible but again not specific function to do that. You seem to have been able to do this alignment with your code above. I was going to say and you've done it is use the align example as baseline then map from there.

[erjiekai]

Hi @RealSenseSupport ,

Thank you for your response. Glad to know that I am doing it correctly.

The output of the alignment can be seen below:

[RealSenseSupport]

Do you need anything else on this ticket?

If we don’t hear from you in 7 days, this issue will be closed.

[erjiekai]

No. Thank you for your help.

[JZhu1208]

Hi, how do you save and display the depth map and color image seen in RealSense Viewer as images? I can use C++ or matlab to program. Thank you.