深度图像和彩色图像对齐
原因：由于RGB图像数据与深度图像数据的空间坐标系是不同的，前者的原点是RGB摄像头，后者的原点是红外摄像头，因此两者会有相应的误差。没对齐之前的结果如下图所示：（执行这个 bin/cpp-capture,我用opencv显示的）
执行bin/cpp-alignimages 实现图像对齐，如下图所示：
对齐的原理：
深度图上的2D点转换到世界坐标的3D点，世界坐标的3D点再投影到彩色图像上；如下图所示：
代码如下：
void alignmentDepthandColor(const Mat depth, const Mat color, const rs::device &dev)
{
rs::float2 pt_depth, pt_color;
rs::float3 pt3_depth, pt3_color;
auto intrinDepth = dev.get_stream_intrinsics(rs::stream::depth);
auto intrinColor = dev.get_stream_intrinsics(rs::stream::color);
auto extrinsics = dev.get_extrinsics(rs::stream::depth, rs::stream::color);
int y=0, x= 0;
Mat mat = Mat::zeros(color.rows,color.cols,CV_8UC3);
for (int row=0; row<depth.rows; row++)
{
for (int col=0; col<depth.cols; col++)
{
pt_depth.x = row;
pt_depth.y = col;
// cout<<"depth 2d: x="<<pt_depth.x<<",y="<<pt_depth.y<<endl;
pt3_depth = intrinDepth.deproject(pt_depth, depth.at<ushort>(row,col));
// cout<<"depth 3d: x="<<pt3_depth.x<<",y="<<pt3_depth.y<<", z="<<pt3_depth.z<<endl;
pt3_color = extrinsics.transform(pt3_depth);
pt_color = intrinColor.project(pt3_color);
// cout<<"color 2d: x="<<pt_color.x<<",y="<<pt_color.y<<endl;
x = (int)pt_color.x;///(depth.at<ushort>(row,col)+1);
x = x<0? 0 : x;
x = x>depth.cols-1? depth.cols-1 : x;
y = (int)pt_color.y;///(depth.at<ushort>(row,col) + 1);
y = y<0? 0 : y;
y = y>depth.rows-1? depth.rows-1 : y;
// cout<<"xy 2d: x="<<x<<",y="<<y<<endl;
for (int k=0; k<3; k++)
{
mat.at<Vec3b>(y, x)[k] = color.at<Vec3b>(row,col)[k];
}
}
}
Mat color1,color2;
transpose(mat, color1); //旋转
flip(color1, color2,1); //水平镜像
imshow("color1", color1);
imshow("color2", color2);
imwrite("color.bmp", color2);
imshow("alignment", mat);
}
效果图：
备注：
最后一张图是彩色图与深度图对齐后的图像，对齐图像的大小变了，需要调整成同样大小。
