Intel Realsense SDK2.0學習：：（三）D435深度圖片對齊到彩色圖片-代碼實現

D435深度圖片對齊到彩色圖片

深度圖片對齊彩色圖片，就是我們有深度圖和彩色圖，要知道彩色圖中像素點所對應的深度值，需要利用深度圖以及相機參數進行轉換，本文先以進行原理推導，再給出實際運行代碼

一、確定所需變量

深度圖Id	深度攝像頭獲取
彩色圖Ic	RBG攝像頭獲取
深度攝像頭內參 $K_{d}$	自行標定或直接realsense sdk2.0 獲取
RGB攝像頭內參 $K_{c}$	自行標定或直接realsense sdk2.0 獲取
同一場景下A深度攝像頭外參 $T_{w2d}$ 【世界座標系轉換到深度攝像頭座標系的轉換矩陣】	標定
同一場景下A彩色攝像頭外參 $T_{w2c}$ 【世界座標系轉換到彩色攝像頭座標系的轉換矩陣】	標定
深度攝像頭到彩色攝像頭的轉移矩陣 $T_{d2c}$ 【深度攝像頭座標系轉換到彩色攝像頭座標系的轉換矩陣】	從 $T_{w2d}$ 、 $T_{w2c}$ 計算獲取或者直接從realsense sdk2.0 獲取
深度圖下某點像素 $P_{dc}$ 、 $P_{u,v}^{d}$ 深度圖下某點像素按深度還原到深度座標系下的空間點 $\dot{P}_{dc}$
彩色圖下某點像素 $P_{cc}$ 、 $P_{u,v}^{c}$ 由世界座標系轉換到彩色座標系下的空間點 $\dot{P}_{cc}$
深度圖轉換到世界座標系下的點 $P_{w}$

二、大概原理

目的：將深度座標系下的點轉換到彩色座標系下

基本步驟：

（1）將深度圖的像素點還原到深度座標系下

（2）深度座標系下的深度點還原到世界座標系

（3）世界座標系的深度點轉換到彩色座標系下

（4）彩色座標系的深度點映射到Z=1的平面上，即與彩色圖像的像素點對應起來

這裏引用https://blog.csdn.net/jay463261929/article/details/53582800的一張原理圖：

三、公式演算

（1）先進行第一步，將深度圖的像素點 $P_{u,v}^{d}$ 還原到深度座標系下

$\dot{P}_{dc}=ZK_{d}^{-1}P_{u,v}^{d}$

（2）第二步：將深度空間座標系的深度點 $\dot{P}_{dc}$ 轉換到世界座標系下

$P_{w}=T_{w2d}^{-1}\dot{P}_{dc}$

（3）第三步：將世界座標系的深度點 $P_{w}$ 轉換到彩色攝像頭座標系

$\dot{P}_{cc}=T_{w2c} P_{w}$

（4）第四步：將彩色攝像頭座標系下的深度點 $\dot{P}_{cc}$ 映射到Z=1的彩色平面上

$P_{u,v}^{c}=K_{c}(\dot{P}_{cc}/z)$

其中，
$(\dot{P}_{cc}/z)$ 表示按z軸歸一化，即該點的xyz都分別除以z值

（5）以上四步，就完成了深度平面圖的點轉換到彩色平面上，給彩色平面上的像素點增加一個信息通道{深度}

（6）實際處理中，一般把第二第三步直接結合起來，即獲取一個從深度攝像頭座標系到彩色攝像頭座標系的歐氏變換矩陣 $T_{d2c}$

具體推算如下：

$\dot{P}_{cc}=T_{w2c} P_{w}=T_{w2c}T_{w2d}^{-1} \dot{P}_{dc}$

那麼，有：

$T_{d2c}=T_{w2c}T_{w2d}^{-1}$

根據高翔的《視覺SLAM十四講》，歐氏變換矩陣T形式如下：

$T=\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}$ $T^{-1}=\begin{bmatrix} R^{-1} & -R^{-1}t \\ 0 & 1 \end{bmatrix}$

其中：

R爲旋轉矩陣，是單位正交的，所以其轉置=其逆

t爲軸的平移量

$T_{d2c}=T_{w2c} T_{w2d}^{-1}=\begin{bmatrix} R_{w2c}R_{w2d}^{-1}& t_{w2c}- R_{w2c}R_{w2d}^{-1} t_{w2d}\\ 0& 1 \end{bmatrix}$

有了這個，就可以直接將深度攝像頭空間座標系下的點轉換到彩色攝像頭空間座標系下了

代碼部分：

#include <iostream>

using namespace std;
#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstring>

#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
using namespace cv;

#include<librealsense2/rs.hpp>
#include<librealsense2/rsutil.h>
//獲取深度像素對應長度單位（米）的換算比例
float get_depth_scale(rs2::device dev)
{
    // Go over the device's sensors
    for (rs2::sensor& sensor : dev.query_sensors())
    {
        // Check if the sensor if a depth sensor
        if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
        {
            return dpt.get_depth_scale();
        }
    }
    throw std::runtime_error("Device does not have a depth sensor");
}
//深度圖對齊到彩色圖函數
Mat align_Depth2Color(Mat depth,Mat color,rs2::pipeline_profile profile){
    //聲明數據流
    auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
    auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();

    //獲取內參
    const auto intrinDepth=depth_stream.get_intrinsics();
    const auto intrinColor=color_stream.get_intrinsics();

    //直接獲取從深度攝像頭座標系到彩色攝像頭座標系的歐式變換矩陣
    //auto  extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
    rs2_extrinsics  extrinDepth2Color;
    rs2_error *error;
    rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);

    //平面點定義
    float pd_uv[2],pc_uv[2];
    //空間點定義
    float Pdc3[3],Pcc3[3];

    //獲取深度像素與現實單位比例（D435默認1毫米）
    float depth_scale = get_depth_scale(profile.get_device());
//    uint16_t depth_max=0;
//    for(int row=0;row<depth.rows;row++){
//        for(int col=0;col<depth.cols;col++){
//            if(depth_max<depth.at<uint16_t>(row,col))
//                depth_max=depth.at<uint16_t>(row,col);
//        }
//    }
    int y=0,x=0;
    //初始化結果
    Mat result=Mat::zeros(color.rows,color.cols,CV_8UC3);
    //對深度圖像遍歷
    for(int row=0;row<depth.rows;row++){
        for(int col=0;col<depth.cols;col++){
            //將當前的(x,y)放入數組pd_uv，表示當前深度圖的點
            pd_uv[0]=col;
            pd_uv[1]=row;
            //取當前點對應的深度值
            uint16_t depth_value=depth.at<uint16_t>(row,col);
            //換算到米
            float depth_m=depth_value*depth_scale;
            //將深度圖的像素點根據內參轉換到深度攝像頭座標系下的三維點
            rs2_deproject_pixel_to_point(Pdc3,&intrinDepth,pd_uv,depth_m);
            //將深度攝像頭座標系的三維點轉化到彩色攝像頭座標系下
            rs2_transform_point_to_point(Pcc3,&extrinDepth2Color,Pdc3);
            //將彩色攝像頭座標系下的深度三維點映射到二維平面上
            rs2_project_point_to_pixel(pc_uv,&intrinColor,Pcc3);

            //取得映射後的（u,v)
            x=(int)pc_uv[0];
            y=(int)pc_uv[1];
//            if(x<0||x>color.cols)
//                continue;
//            if(y<0||y>color.rows)
//                continue;
            //最值限定
            x=x<0? 0:x;
            x=x>depth.cols-1 ? depth.cols-1:x;
            y=y<0? 0:y;
            y=y>depth.rows-1 ? depth.rows-1:y;

            //將成功映射的點用彩色圖對應點的RGB數據覆蓋
            for(int k=0;k<3;k++){
                //這裏設置了只顯示1米距離內的東西
                if(depth_m<1)
                result.at<cv::Vec3b>(y,x)[k]=
                        color.at<cv::Vec3b>(y,x)[k];
            }
        }
    }
    return result;
}

int main()
{
    const char* depth_win="depth_Image";
    namedWindow(depth_win,WINDOW_AUTOSIZE);
    const char* color_win="color_Image";
    namedWindow(color_win,WINDOW_AUTOSIZE);

    //深度圖像顏色map
    rs2::colorizer c;                          // Helper to colorize depth images

    //創建數據管道
    rs2::pipeline pipe;
    rs2::config pipe_config;
    pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
    pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);

    //start()函數返回數據管道的profile
    rs2::pipeline_profile profile = pipe.start(pipe_config);

    //定義一個變量去轉換深度到距離
    float depth_clipping_distance = 1.f;

    //聲明數據流
    auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
    auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();

    //獲取內參
    auto intrinDepth=depth_stream.get_intrinsics();
    auto intrinColor=color_stream.get_intrinsics();

    //直接獲取從深度攝像頭座標系到彩色攝像頭座標系的歐式變換矩陣
    auto  extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);

    while (cvGetWindowHandle(depth_win)&&cvGetWindowHandle(color_win)) // Application still alive?
    {
        //堵塞程序直到新的一幀捕獲
        rs2::frameset frameset = pipe.wait_for_frames();
        //取深度圖和彩色圖
        rs2::frame color_frame = frameset.get_color_frame();//processed.first(align_to);
        rs2::frame depth_frame = frameset.get_depth_frame();
        rs2::frame depth_frame_4_show = frameset.get_depth_frame().apply_filter(c);
        //獲取寬高
        const int depth_w=depth_frame.as<rs2::video_frame>().get_width();
        const int depth_h=depth_frame.as<rs2::video_frame>().get_height();
        const int color_w=color_frame.as<rs2::video_frame>().get_width();
        const int color_h=color_frame.as<rs2::video_frame>().get_height();

        //創建OPENCV類型 並傳入數據
        Mat depth_image(Size(depth_w,depth_h),
                                CV_16U,(void*)depth_frame.get_data(),Mat::AUTO_STEP);
        Mat depth_image_4_show(Size(depth_w,depth_h),
                                CV_8UC3,(void*)depth_frame_4_show.get_data(),Mat::AUTO_STEP);
        Mat color_image(Size(color_w,color_h),
                                CV_8UC3,(void*)color_frame.get_data(),Mat::AUTO_STEP);
        //實現深度圖對齊到彩色圖
        Mat result=align_Depth2Color(depth_image,color_image,profile);

        //顯示
        imshow(depth_win,depth_image_4_show);
        imshow(color_win,color_image);
        imshow("result",result);
        waitKey(10);
    }
    return 0;
}

Intel Realsense SDK2.0學習：：（三）D435深度圖片對齊到彩色圖片-代碼實現

D435深度圖片對齊到彩色圖片

代碼部分：

實現效果：

C/C++：：淺談.obj .exe .dll .lib關係&& .o .a .so關係

樹莓派學習：：qt5.10.1交叉編譯【帶opengl ES2】到非官方64位系統

ROS：：ROS_INFO帶顏色的輸出

SLAM：：g2o安裝和使用

聯想 Yoga C740：：關於Ubuntu16.04下無法識別Intel WIFI6 AX201無線網卡的解決方案

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結