由于我正在使用OpenCV和C++,并且没有拥有Kinect,所以遗憾的是我无法使用官方Kinect API的MapDepthFrameToColorFrame方法。
根据给定相机的内参和畸变系数,我可以将深度映射到世界坐标,并根据此处提供的算法将其转换回RGB。
Vec3f depthToW( int x, int y, float depth ){
Vec3f result;
result[0] = (float) (x - depthCX) * depth / depthFX;
result[1] = (float) (y - depthCY) * depth / depthFY;
result[2] = (float) depth;
return result;
}
Vec2i wToRGB( const Vec3f & point ) {
Mat p3d( point );
p3d = extRotation * p3d + extTranslation;
float x = p3d.at<float>(0, 0);
float y = p3d.at<float>(1, 0);
float z = p3d.at<float>(2, 0);
Vec2i result;
result[0] = (int) round( (x * rgbFX / z) + rgbCX );
result[1] = (int) round( (y * rgbFY / z) + rgbCY );
return result;
}
void map( Mat& rgb, Mat& depth ) {
/* intrinsics are focal points and centers of camera */
undistort( rgb, rgb, rgbIntrinsic, rgbDistortion );
undistort( depth, depth, depthIntrinsic, depthDistortion );
Mat color = Mat( depth.size(), CV_8UC3, Scalar(0) );
ushort * raw_image_ptr;
for( int y = 0; y < depth.rows; y++ ) {
raw_image_ptr = depth.ptr<ushort>( y );
for( int x = 0; x < depth.cols; x++ ) {
if( raw_image_ptr[x] >= 2047 || raw_image_ptr[x] <= 0 )
continue;
float depth_value = depthMeters[ raw_image_ptr[x] ];
Vec3f depth_coord = depthToW( y, x, depth_value );
Vec2i rgb_coord = wToRGB( depth_coord );
color.at<Vec3b>(y, x) = rgb.at<Vec3b>(rgb_coord[0], rgb_coord[1]);
}
}
但是结果似乎不对齐。我无法手动设置翻译,因为数据集来自3个不同的Kinect,并且它们中的每一个都在不同方向上错位。您可以在下面看到其中之一(左:未失真的RGB,中间:未失真的深度,右:映射的RGB到深度)。
我的问题是,在这一点上我应该怎么做?在尝试将深度投影到世界或将世界回传到RGB时,我是否错过了一步?有没有经验丰富的立体相机用户能指出我的错误?