I am trying to map the depth from the Kinectv2 to RGB space from a DSLR camera and I am stuck with weird pixel mapping.
I am working on Processing, using OpenCV and Nicolas Burrus' method where :
P3D.x = (x_d - cx_d) * depth(x_d,y_d) / fx_d
P3D.y = (y_d - cy_d) * depth(x_d,y_d) / fy_d
P3D.z = depth(x_d,y_d)
P3D' = R.P3D + T
P2D_rgb.x = (P3D'.x * fx_rgb / P3D'.z) + cx_rgb
P2D_rgb.y = (P3D'.y * fy_rgb / P3D'.z) + cy_rgb
Unfortunatly i have a problem when I reproject 3D point to RGB World Space. In order to check if the problem came from my OpenCV calibration I used MRPT Kinect & Setero Calibration in order to get the intrinsics and distorsion coefficients of the cameras and the rototranslation relative transformation between the two cameras.
eDatas from stereo calibration MRPT
Here my datas :
depth c_x = 262.573912;
depth c_y = 216.804166;
depth f_y = 462.676558;
depth f_x = 384.377033;
depthDistCoeff = {
1.975280e-001, -6.939150e-002, 0.000000e+000, -5.830770e-002, 0.000000e+000
};
DSLR c_x_R = 538.134412;
DSLR c_y_R = 359.760525;
DSLR f_y_R = 968.431461;
DSLR f_x_R = 648.480385;
rgbDistCoeff = {
2.785566e-001, -1.540991e+000, 0.000000e+000, -9.482198e-002, 0.000000e+000
};
R = {
8.4263457190597e-001, -8.9789363922252e-002, 5.3094712387890e-001,
4.4166517232817e-002, 9.9420220953803e-001, 9.8037162878270e-002,
-5.3667149820385e-001, -5.9159417476295e-002, 8.4171483671105e-001
};
T = {-4.740111e-001, 3.618596e-002, -4.443195e-002};
Then I use the data in processing in order to compute the mapping using :
PVector pixelDepthCoord = new PVector(i * offset_, j * offset_);
int index = (int) pixelDepthCoord .x + (int) pixelDepthCoord .y * depthWidth;
int depth = 0;
if (rawData[index] != 255)
{
//2D Depth Coord
depth = rawDataDepth[index];
} else
{
}
//3D Depth Coord - Back projecting pixel depth coord to 3D depth coord
float bppx = (pixelDepthCoord.x - c_x) * depth / f_x;
float bppy = (pixelDepthCoord.y - c_y) * depth / f_y;
float bppz = -depth;
//transpose 3D depth coord to 3D color coord
float x_ =(bppx * R[0] + bppy * R[1] + bppz * R[2]) + T[0];
float y_ = (bppx * R[3] + bppy * R[4] + bppz * R[5]) + T[1];
float z_ = (bppx * R[6] + bppy * R[7] + bppz * R[8]) + T[2];
//Project 3D color coord to 2D color Cood
float pcx = (x_ * f_x_R / z_) + c_x_R;
float pcy = (y_ * f_y_R / z_) + c_y_R;
Then i get the following transformations :
I think i have a probleme in my method but i don't get it. Does anyone has any ideas or a clues. I am racking my brain since many days on this problem ;)
Thanks