Слияние изображений RGB и глубины из Kinect

Я создаю алгоритм видения, который реализован в S-функции Simulink (код на C ++). Я достиг всего, чего хотел, кроме выравнивания цвета и глубины изображения.

У меня вопрос, как я могу сделать, чтобы 2 изображения соответствовали друг другу. Другими словами, как я могу сделать 3D-изображение с OpenCV.

Я знаю, что мой вопрос может быть немного расплывчатым, поэтому я включу свой код, который объяснит вопрос

#include "opencv2/opencv.hpp"
using namespace cv;

int main(int argc, char** argv)
{
// reading in the color and depth image
Mat color = imread("whitepaint_col.PNG", CV_LOAD_IMAGE_UNCHANGED);
Mat depth = imread("whitepaint_dep.PNG", CV_LOAD_IMAGE_UNCHANGED);

// show bouth the color and depth image
namedWindow("color", CV_WINDOW_AUTOSIZE);
imshow("color", color);
namedWindow("depth", CV_WINDOW_AUTOSIZE);
imshow("depth", depth);

// thershold the color image for the color white
Mat onlywhite;
inRange(color, Scalar(200, 200, 200), Scalar(255, 255, 255), onlywhite);

//display the mask
namedWindow("onlywhite", CV_WINDOW_AUTOSIZE);
imshow("onlywhite", onlywhite);

// apply the mask to the depth image
Mat nocalibration;
depth.copyTo(nocalibration, onlywhite);

//show the result
namedWindow("nocalibration", CV_WINDOW_AUTOSIZE);
imshow("nocalibration", nocalibration);waitKey(0);
destroyAllWindows;
return 0;
}

вывод программы:

Как видно из результатов моей программы, когда я применяю маску onlywhite к изображению глубины, тело четырехквадратного коптера не состоит из одного цвета. Причина этого заключается в том, что между двумя изображениями существует несоответствие.

Я знаю, что мне нужны параметры калибровки моей камеры, и я получил их от последнего человека, который работал с этой настройкой. Сделал калибровку в Matlab и это привело к следующему.

Результаты калибровки Matlab:

Я потратил много времени, читая следующую страницу opencv о калибровке камеры и 3D реконструкции (не могу включить ссылку из-за обмена стеками lvl)

Но я не могу на всю жизнь понять, как я могу достичь своей цели, добавив правильное значение глубины для каждого цветного пикселя.

Я попытался с помощью reprojectImageTo3D (), но я не могу понять матрицу Q.
Я также попробовал много других функций с этой страницы, но я не могу, чтобы мои входные данные были правильными.