文章 标签 分类 关于 工具箱

Contents

camera:realsense(D450)深度对齐优化

zhangyuhu included in 计算机视觉
   1973 words   4 minutes 

本文采用知识共享署名 4.0 国际许可协议进行许可，转载时请注明原文链接，图片在使用时请保留全部内容，可适当缩放并在引用处附上图片所在的文章链接。

SDK 中的对齐实现

源码位置

align_z_to_other

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

void align::align_z_to_other(rs2::video_frame& aligned, 
                             const rs2::video_frame& depth, const rs2::video_stream_profile& other_profile, float z_scale)
{
    byte* aligned_data = reinterpret_cast<byte*>(const_cast<void*>(aligned.get_data()));
    auto aligned_profile = aligned.get_profile().as<rs2::video_stream_profile>();
    memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel());

    auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();

    auto z_intrin = depth_profile.get_intrinsics();
    auto other_intrin = other_profile.get_intrinsics();
    auto z_to_other = depth_profile.get_extrinsics_to(other_profile);

    auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
    auto out_z = (uint16_t *)(aligned_data);

    align_images(z_intrin, z_to_other, other_intrin,
                 [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
                 [out_z, z_pixels](int z_pixel_index, int other_pixel_index)
                 {
                     out_z[other_pixel_index] = out_z[other_pixel_index] ?
                         std::min((int)out_z[other_pixel_index], (int)z_pixels[z_pixel_index]) :
                     z_pixels[z_pixel_index];
                 });
}

align_images

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

    template<class GET_DEPTH, class TRANSFER_PIXEL>
    void align_images(const rs2_intrinsics& depth_intrin, const rs2_extrinsics& depth_to_other,
        const rs2_intrinsics& other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
    {
        // Iterate over the pixels of the depth image
#pragma omp parallel for schedule(dynamic)
        for (int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
        {
            int depth_pixel_index = depth_y * depth_intrin.width;
            for (int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
            {
                // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
                if (float depth = get_depth(depth_pixel_index))
                {
                    // Map the top-left corner of the depth pixel onto the other image
                    float depth_pixel[2] = { depth_x - 0.5f, depth_y - 0.5f }, depth_point[3], other_point[3], other_pixel[2];
                    rs2_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
                    rs2_transform_point_to_point(other_point, &depth_to_other, depth_point);
                    rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
                    const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
                    const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);

                    // Map the bottom-right corner of the depth pixel onto the other image
                    depth_pixel[0] = depth_x + 0.5f; depth_pixel[1] = depth_y + 0.5f;
                    rs2_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
                    rs2_transform_point_to_point(other_point, &depth_to_other, depth_point);
                    rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
                    const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
                    const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);

                    if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
                        continue;

                    // Transfer between the depth pixels and the pixels inside the rectangle on the other image
                    for (int y = other_y0; y <= other_y1; ++y)
                    {
                        for (int x = other_x0; x <= other_x1; ++x)
                        {
                            transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
                        }
                    }
                }
            }
        }
    }

目的：将深度坐标系下的点转换到彩色坐标系下

基本步骤：

（1）将深度图的像素点还原到深度坐标系下

（2）深度坐标系下的深度点还原到世界坐标系

（3）世界坐标系的深度点转换到彩色坐标系下

（4）彩色坐标系的深度点映射到Z=1的平面上，即与彩色图像的像素点对应起来

由目标像素坐标和深度值（使用内参INTRINSICS）获取目标点的真实坐标原理：

https://cdn.jsdelivr.net/gh/zhangyuhu/share_images/images/202301301855957.png

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

//深度图对齐到彩色图函数
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;
}

D435深度图片对齐到彩色图片

More efficient way of aligning images

Align to Depth produces lossy color image

转化为opencv Mat格式

转化为opencv 格式的相机内参矩阵和畸变矩阵

1
2
3
4
5

cv::Mat intrinsicsd_D = (cv::Mat_<double>(3, 3) << depth_intrin.fx, 0, depth_intrin.ppx, 0, depth_intrin.fy,depth_intrin.ppy, 0, 0, 1);
cv::Mat distCoeffs_D = cv::Mat(1, 5, CV_64FC1, depth_intrin.coeffs).clone();

cv::Mat intrinsicsd_C = (cv::Mat_<double>(3, 3) << color_intrin.fx, 0, color_intrin.ppx, 0, color_intrin.fy,color_intrin.ppy, 0, 0, 1);
cv::Mat distCoeffs_C = cv::Mat(1, 5, CV_64FC1, color_intrin.coeffs).clone();

相机的旋转向量和平移向量

1
2
3
4
5

cv::Mat R_D2C =  Mat(3,3,CV_64FC1,depth_extrin_to_color.rotation).clone();
cv::Mat T_D2C =  Mat(4,1,CV_64FC1,depth_extrin_to_color.translation).clone();

cv::Mat R_C2D =  Mat(3,3,CV_64FC1,color_extrin_to_depth.rotation).clone();
cv::Mat T_C2D =  Mat(4,1,CV_64FC1,color_extrin_to_depth.translation).clone();

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

intrinsicsdD : 
 [641.8226318359375, 0, 637.7587890625;
 0, 641.8226318359375, 353.4434814453125;
 0, 0, 1]
distCoeffsD : 
 [0, 0, -2.889534818121034e+76, -2.889788802386637e+76, -1.833730888558527e-270]
intrinsicsdD : 
 [641.8226318359375, 0, 637.7587890625;
 0, 641.8226318359375, 353.4434814453125;
 0, 0, 1]
distCoeffsD : 
 [0, 0, -2.889534818121034e+76, -2.889788802386637e+76, -1.833730888558527e-270]
R : 
 [-1.007959509014731e-27, 9.788559947358751e-28, -8.427343739731148e-22;
 8.42383379014226e-22, -1.027880253175563e-12, 1.666459052330406e-31;
 1.5278369701274e-311, 1.398529605097543e+18, 1.415899540474143e+20]
T : 
 [3.035833948987033e-36;
 2.716631590043863e-311;
 1.462199172142216e+20]


Rl : 
 [-2.076210961424182e-56, 0, -1;
 0, -1, 0;
 -1, 0, 2.076210961424182e-56]
Pl : 
 [638.316162109375, 0, -nan, 0;
 0, 638.316162109375, -nan, 0;
 0, 0, 1, 0]
Rr : 
 [-2.076210961424182e-56, 0, -1;
 0, -1, 0;
 -1, 0, 2.076210961424182e-56]
Pr : 
 [638.316162109375, 0, -nan, -9.333453638013247e+22;
 0, 638.316162109375, -nan, 0;
 0, 0, 1, 0]

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19

rs2_intrinsics depth_intrin = m_depth_profile.as<rs2::video_stream_profile>().get_intrinsics();
cv::Mat intrinsicsdD = (cv::Mat_<double>(3, 3) << depth_intrin.fx, 0, depth_intrin.ppx, 0, depth_intrin.fy,depth_intrin.ppy, 0, 0, 1);
cv::Mat distCoeffsD = cv::Mat(1, 5, CV_64FC1, depth_intrin.coeffs).clone();
rs2_extrinsics depth_extrin_to_color = m_depth_profile.as<rs2::video_stream_profile>().get_extrinsics_to(m_color_profile);

rs2_intrinsics color_intrin = m_color_profile.as<rs2::video_stream_profile>().get_intrinsics();
cv::Mat intrinsicsdC = (cv::Mat_<double>(3, 3) << color_intrin.fx, 0, color_intrin.ppx, 0, color_intrin.fy,color_intrin.ppy, 0, 0, 1);
cv::Mat distCoeffsC = cv::Mat(1, 5, CV_64FC1, color_intrin.coeffs).clone();
rs2_extrinsics color_extrin_to_depth = m_color_profile.as<rs2::video_stream_profile>().get_extrinsics_to(m_depth_profile);

cv::Mat R =  Mat(3,3,CV_64FC1,depth_extrin_to_color.rotation).clone();
cv::Mat T =  Mat(3,1,CV_64FC1,depth_extrin_to_color.translation).clone();


Mat Rl, Rr, Pl, Pr, Q;  
cv::stereoRectify(intrinsicsdD, distCoeffsD, intrinsicsdC, distCoeffsC, cv::Size(width_org, hight_org), R, T, Rl, Rr, Pl, Pr, Q);
cv::initUndistortRectifyMap(intrinsicsdD, distCoeffsD, Rl, Pl, cv::Size(width_org, hight_org), CV_32FC1, map1_, map2_);  

cv::remap(curr_depth_org,curr_depth_org_temp,xMapArray,yMapArray,cv::INTER_NEAREST);

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

void RealsenseCamera::align_depth_to_color(const rs2::stream_profile& depth_profile , const rs2::stream_profile& color_profile,uint16_t *indepth,uint16_t *outdepth)
{
    auto d_pixels = indepth;
    auto out_d_pixels = outdepth;

    auto d_intrin = depth_profile.as<rs2::video_stream_profile>().get_intrinsics();
    auto color_intrin = color_profile.as<rs2::video_stream_profile>().get_intrinsics();
    auto d_to_color = depth_profile.as<rs2::video_stream_profile>().get_extrinsics_to(color_profile);

    // align_images(d_intrin, d_to_color, color_intrin,                            \
    //     [d_pixels](int z_pixel_index) { return d_pixels[z_pixel_index]; },      \
    //     [out_d_pixels, d_pixels](int z_pixel_index, int other_pixel_index){out_d_pixels[other_pixel_index] = out_d_pixels[other_pixel_index] ? ( std::min((int)out_d_pixels[other_pixel_index], (int)d_pixels[z_pixel_index])) : (d_pixels[z_pixel_index]);},\
    //     m_cols_,m_rows_);

    // align_images(d_intrin, d_to_color, color_intrin,                            \
    //     [d_pixels](int z_pixel_index) { return d_pixels[z_pixel_index]; },      \
    //     [out_d_pixels, d_pixels](int z_pixel_index, int other_pixel_index){out_d_pixels[other_pixel_index] = out_d_pixels[other_pixel_index] ? ( std::min((int)out_d_pixels[other_pixel_index], (int)d_pixels[z_pixel_index])) : (d_pixels[z_pixel_index]);});

    float z_scale = 0.001;
    align_images(d_intrin, d_to_color, color_intrin,                            \
    [d_pixels, z_scale](int z_pixel_index) { return z_scale * d_pixels[z_pixel_index]; },      \
    [out_d_pixels, d_pixels](int z_pixel_index, int other_pixel_index){out_d_pixels[other_pixel_index] = out_d_pixels[other_pixel_index] ? ( std::min((int)out_d_pixels[other_pixel_index], (int)d_pixels[z_pixel_index])) : (d_pixels[z_pixel_index]);});

}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11

    // if (once){
    //     cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_depth_org.jpg",curr_depth_org);
    //     // cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_depth_org_temp.jpg",curr_depth_org_temp);
    //     cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_color_.jpg",curr_color_lores);
    //     cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_depth_.jpg",curr_depth_lores);
    //     cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_depth_lores_temp.jpg",curr_depth_lores_temp);

        
    //     // cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_color_old.jpg",curr_color_lores);
    //     // cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/curr_depth_old.jpg",curr_depth_lores);
    // }

1
2
3
4
5

    tbb::tick_count t0 = tbb::tick_count::now();
    // Use homography to warp image
    cv::warpPerspective(curr_depth_lores, curr_depth_lores_align, homography, curr_depth_lores.size(),INTER_NEAREST);
    float interval = 1000 * (tbb::tick_count::now() - t0).seconds();
    printf(" --------  %.4f \n", interval);

realsense 对齐流程

1
2
3
4
5
6

std::shared_ptr<rs2::align> align_to_color;

rs2::frameset frameset;
frameset = pipe.wait_for_frames();

frameset = align_to_color->process(frameset);    

1

filter::process(frames);

1

rs2_process_frame(get(), ptr, &e);

1
2

auto depth = frames.first_or_default(RS2_STREAM_DEPTH, RS2_FORMAT_Z16).as<rs2::depth_frame>();
_depth_scale = ((librealsense::depth_frame*)depth.get())->get_units();

1

align_z_to_other(aligned, from, to_profile, _depth_scale);

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

void align::align_z_to_other(rs2::video_frame& aligned, 
                             const rs2::video_frame& depth, const rs2::video_stream_profile& other_profile, float z_scale)
{
    byte* aligned_data = reinterpret_cast<byte*>(const_cast<void*>(aligned.get_data()));
    auto aligned_profile = aligned.get_profile().as<rs2::video_stream_profile>();
    memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel());

    auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();

    auto z_intrin = depth_profile.get_intrinsics();
    auto other_intrin = other_profile.get_intrinsics();
    auto z_to_other = depth_profile.get_extrinsics_to(other_profile);

    auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
    auto out_z = (uint16_t *)(aligned_data);

    align_images(z_intrin, z_to_other, other_intrin,
                 [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
                 [out_z, z_pixels](int z_pixel_index, int other_pixel_index)
                 {
                     out_z[other_pixel_index] = out_z[other_pixel_index] ?
                         std::min((int)out_z[other_pixel_index], (int)z_pixels[z_pixel_index]) :
                     z_pixels[z_pixel_index];
                 });
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11

    static bool writeimg = true;
    if (writeimg){
# if REALSENCE_ALIGN
        cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/color_realsence.jpg",curr_color_lores);
        cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/depth_align_realsence.jpg",curr_depth_lores_align);
#else
        cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/color_opencv.jpg",curr_color_lores);
        cv::imwrite("/home/deepglint/zyh/zyh_libra/release/aarch64-rk3399pro-linux/depth_align_opencv.jpg",curr_depth_lores_align);
#endif
    }
    once = false;

points1 ,points2:

homography : [0.9783506973455497, -0.003518269926918195, 14.96135699407099; -0.0004602924762779034, 0.9741223762143064, 16.69589351807111; 6.57807138529927e-07, -7.03827767458266e-06, 1]

points2 , points1 :

homography : [1.021281290759013, 0.002739759630768195, -14.96440533526881; 0.0004858818482422798, 1.025665029417736, -17.1042255312311; -1.068779611668291e-06, 5.619754840876425e-06, 1]

https://github.com/mickeyouyou/realsenseOnCyber/blob/master/realsense_component.cc

https://blog.csdn.net/dieju8330/article/details/85346454

https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga617b1685d4059c6040827800e72ad2b6

https://github.com/IntelRealSense/librealsense/issues/1189

rs2_deproject_pixel_to_point rs2_transform_point_to_point rs2_project_point_to_pixel

Updated on 2023-01-30
 camera
Back | Home