深度图像和彩色图像对齐

深度图像和彩色图像对齐

原因：由于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);
}

效果图：

备注：

     最后一张图是彩色图与深度图对齐后的图像，对齐图像的大小变了，需要调整成同样大小。
————————————————
版权声明：本文为CSDN博主「jay463261929」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/jay463261929/article/details/53582800