cv::align not optimized (windows 10, C++)

[WeiChihChern]

Required Info
Camera Model	D415
Firmware Version	5.10.03
Operating System & Version	Win10
Kernel Version (Linux Only)	(e.g. 4.14.13)
Platform	PC
SDK Version	2.15.0
Language	C++

Issue Description

CPU: i5-8400
As described in #2321, aligning function slow down overall performance. I checked the 2.16.0 pre-release note, and haven't found any update regarding the align function yet.
(Don't worry about the project name, I'm using v2.15.0)

I modify few lines of code from the example code . To use cv::imshow to show the result, and the rest are basically the same. Code shown below:

#include <librealsense2/rs.hpp>
#include "opencv2/opencv.hpp"

#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstring>

using namespace cv;


void remove_background(rs2::video_frame& other, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist);
float get_depth_scale(rs2::device dev);
rs2_stream find_stream_to_align(const std::vector<rs2::stream_profile>& streams);
bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev);

int main(int argc, char * argv[]) try
{
	rs2::colorizer c;                          // Helper to colorize depth images
										  // Create a pipeline to easily configure and start the camera
	rs2::pipeline pipe;
	//Calling pipeline's start() without any additional parameters will start the first device
	// with its default streams.
	//The start function returns the pipeline profile which the pipeline used to start the device
	rs2::pipeline_profile profile = pipe.start();

	// Each depth camera might have different units for depth pixels, so we get it here
	// Using the pipeline's profile, we can retrieve the device that the pipeline uses
	float depth_scale = get_depth_scale(profile.get_device());

	//Pipeline could choose a device that does not have a color stream
	//If there is no color stream, choose to align depth to another stream
	rs2_stream align_to = find_stream_to_align(profile.get_streams());

	// Create a rs2::align object.
	// rs2::align allows us to perform alignment of depth frames to others frames
	//The "align_to" is the stream type to which we plan to align depth frames.
	rs2::align align(align_to);

	// Define a variable for controlling the distance to clip
	float depth_clipping_distance = 1.f;

	while (true) // Application still alive?
	{
		// Using the align object, we block the application until a frameset is available
		rs2::frameset frameset = pipe.wait_for_frames();

		// rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
		// Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
		//  after the call to wait_for_frames();
		if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
		{
			//If the profile was changed, update the align object, and also get the new device's depth scale
			profile = pipe.get_active_profile();
			align_to = find_stream_to_align(profile.get_streams());
			align = rs2::align(align_to);
			depth_scale = get_depth_scale(profile.get_device());
		}

		//Get processed aligned frame
		auto processed = align.process(frameset);

		// Trying to get both other and aligned depth frames
		rs2::video_frame other_frame = processed.first(align_to);
		rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();

		//If one of them is unavailable, continue iteration
		if (!aligned_depth_frame || !other_frame)
		{
			continue;
		}

		remove_background(other_frame, aligned_depth_frame, depth_scale, depth_clipping_distance);

		//*******************************************************************************************
		int rows = other_frame.get_height();
		int cols = other_frame.get_width();
		Mat result = Mat(Size(cols, rows), CV_8UC3, (void*)other_frame.get_data());
		imshow("",result); cv::waitKey(1);
		//*******************************************************************************************

	}
	return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
	std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::exception & e)
{
	std::cerr << e.what() << std::endl;
	return EXIT_FAILURE;
}

float get_depth_scale(rs2::device dev)
{
	// Go over the device's sensors
	for (rs2::sensor& sensor : dev.query_sensors())
	{
		// Check if the sensor if a depth sensor
		if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
		{
			return dpt.get_depth_scale();
		}
	}
	throw std::runtime_error("Device does not have a depth sensor");
}


void remove_background(rs2::video_frame& other_frame, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist)
{
	const uint16_t* p_depth_frame = reinterpret_cast<const uint16_t*>(depth_frame.get_data());
	uint8_t* p_other_frame = reinterpret_cast<uint8_t*>(const_cast<void*>(other_frame.get_data()));

	int width = other_frame.get_width();
	int height = other_frame.get_height();
	int other_bpp = other_frame.get_bytes_per_pixel();

#pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
	for (int y = 0; y < height; y++)
	{
		auto depth_pixel_index = y * width;
		for (int x = 0; x < width; x++, ++depth_pixel_index)
		{
			// Get the depth value of the current pixel
			auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index];

			// Check if the depth value is invalid (<=0) or greater than the threashold
			if (pixels_distance <= 0.f || pixels_distance > clipping_dist)
			{
				// Calculate the offset in other frame's buffer to current pixel
				auto offset = depth_pixel_index * other_bpp;

				// Set pixel to "background" color (0x999999)
				std::memset(&p_other_frame[offset], 0x99, other_bpp);
			}
		}
	}
}

rs2_stream find_stream_to_align(const std::vector<rs2::stream_profile>& streams)
{
	//Given a vector of streams, we try to find a depth stream and another stream to align depth with.
	//We prioritize color streams to make the view look better.
	//If color is not available, we take another stream that (other than depth)
	rs2_stream align_to = RS2_STREAM_ANY;
	bool depth_stream_found = false;
	bool color_stream_found = false;
	for (rs2::stream_profile sp : streams)
	{
		rs2_stream profile_stream = sp.stream_type();
		if (profile_stream != RS2_STREAM_DEPTH)
		{
			if (!color_stream_found)         //Prefer color
				align_to = profile_stream;

			if (profile_stream == RS2_STREAM_COLOR)
			{
				color_stream_found = true;
			}
		}
		else
		{
			depth_stream_found = true;
		}
	}

	if (!depth_stream_found)
		throw std::runtime_error("No Depth stream available");

	if (align_to == RS2_STREAM_ANY)
		throw std::runtime_error("No stream found to align with Depth");

	return align_to;
}

bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev)
{
	for (auto&& sp : prev)
	{
		//If previous profile is in current (maybe just added another)
		auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); });
		if (itr == std::end(current)) //If it previous stream wasn't found in current
		{
			return true;
		}
	}
	return false;
}

[WeiChihChern]

@dorodnic
SKDver: 2.16.0 (pre-release)
Platform: win10, VS15, C++
OPENMP = TRUE;
CPU: i5-8400, 6 cores

so visually it seems to be better than v2.15.0, and if no changes with rs2::align between 2.15 & 2.16 then the improvement might come from the openmp = TRUE;
For streaming at 30fps, if aligning, how much performance hit should I expect? @dorodnic Thank you.

[RealSense-Customer-Engineering]

[Realsense Customer Engineering Team Comment]
Before anything, you have run release build. Right?

[HippoEug]

I actually get pretty bad FPS too running this example.. maybe about 10FPS?

[ev-mp]

Hello @HippoEug ,
The latest version added CUDA implementation for rs2::align functionality.
Please check and update

[HippoEug]

I have tried it and there is indeed better FPS now.

[RealSense-Customer-Engineering]

[Realsense Customer Engineering Team Comment]
Hi @WeiChihChern, please have a try new release version v2.16.5.
if you have any news, please let us know.

[WeiChihChern]

@RealSense-Customer-Engineering Will do as long as I've tried it. If the new release version is using CUDA, I'm sure it's going to be better than the previous openMP one.