ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)

2023-05-16

文章目录

  • 1.rgb、depth相机标定矫正
    • 1.1.标定rgb相机
    • 1.2.标定depth相机
    • 1.3.rgb、depth相机一起标定(效果重复了,但是推荐使用)
    • 1.4.取得标定结果
      • 1.4.1.得到的标定结果的意义
    • 1.5.IR、RGB相机分别应用标定结果
      • 1.5.1.openCV应用标定结果
      • 1.5.2.ros2工程应用标定结果
  • 2.rgb、depth相机配准
    • 2.1.单图像配准
      • 2.1.1.求IR、RGB相机各自的外参(R、T矩阵)
      • 2.1.2.求两个相机之间的R、T矩阵
      • 2.1.3 进行D2C操作
    • 2.2.多图像配准
      • 2.2.1.求两个相机之间的R、T矩阵
      • 2.2.2.进行D2C配准
  • 3.题外话
    • 3.1.点云的坐标变换
    • 3.2.AR(在相机画面上绘制3d物体)

相机自带的D2C效果不好,颜色和点云没有很好地匹配上,自己按照下面的介绍手动匹配一下。

1.rgb、depth相机标定矫正

在下载来的sdk,里面没有标定的文件:ost.yaml.
需要自己进行标定、生成。

我所使用的相机型号是Astra_pro,它是一个单目结构光相机,有一个RGB摄像头+一个IR摄像头。从某种上算是一个双目相机(rgb+ir),实际上不是。【奥比中光Astra深度传感器工作原理】

在ros2(humble)中,需要先安装相机标定套件:

    sudo apt install ros-humble-camera-*
    sudo apt install ros-humble-launch-testing-ament-cmake

在我的系统中,可以分别对这两个进行相机进行标定
在这里插入图片描述
在标定时,具体的参数(相机、话题、格子数等等)要根据你实际的情况进行填写。

1.1.标定rgb相机

执行以下命令

ros2 run camera_calibration cameracalibrator --size 6x7 --square 0.015 --no-service-check image:=/camera/color/image_raw camera:=/camera/color

在这里插入图片描述

1.2.标定depth相机

深度摄像头看起来和RGB摄像头差别很大,实际上有很多相似之处。就Kinect而言,其通过一个红外散斑发射器发射红外光束,光束碰到障碍物后反射回深度摄像头,然后通过返回散斑之间的几何关系计算距离。其实,Kinect的深度摄像头就是一个装了滤波片的普通摄像头,只对红外光成像的摄像头(可以这么认为)。因此要对其标定,只需用红外光源照射物体即可,LED红外光源在淘宝上就20元一个。还有一点必须注意,在拍摄红外照片时,要用黑胶带(或其他东西)将Kinect的红外发射器完全挡住,否则其发出的散斑会在红外照片中产生很多亮点,不利于棋盘角点的检测。
————————————————
原文链接:https://blog.csdn.net/aichipmunk/article/details/9264703

我这里就偷懒了,直接用自带的红外散斑发射器来标定。追求准确的同学,最好还是按照上面说的遮住红外发射器+买一个红外光源。

ros2 run camera_calibration cameracalibrator --size 6x7 --square 0.015 --no-service-check image:=/camera/ir/image_raw camera:=/camera/ir

在这里插入图片描述

1.3.rgb、depth相机一起标定(效果重复了,但是推荐使用)

假如已经进行了上面rgb、depth相机的分别标定,这一步其实没必要进行,效果是一样的。
但是这个一起标定的话,有个好处:在后面计算外参时,可以直接拿到同一时刻两个相机分别拍到的图像。
【ROS下采用camera_calibration进行双目相机标定】

ros2 run camera_calibration cameracalibrator --size 6x7 --square 0.015 --approximate 0.1 --no-service-check left:=/camera/ir/image_raw left_camera:=/camera/ir right:=/camera/color/image_raw right_camera:=/camera/color

在这里插入图片描述

1.4.取得标定结果

参考上面提到的文章进行操作后,分别可以在/tmp目录下得到标定后的数据。
在这里插入图片描述无论是用camera_calibration进行单目标定和双目标定,貌似没有本质的区别,都是分别得到两个相机的内参。
得到的相机内参入下图所示:
在这里插入图片描述
以上图为例,可以知道相机的内参fx=628.54905,fy=631.37457;这是说相机镜头的焦距为628.54905mm吗?
不是,这个是以像素为单位的,镜头的焦距为628.54905像素。那1像素代表多少mm呢(像元尺寸)?
这个要看相机感光芯片的尺寸及芯片的分辨率。假设感光芯片尺寸为WH(单位mm),分辨率为UV,那么像元尺寸为[W/U, H/V).
在这里插入图片描述以上面的海康威视相机为例,假如我标定时使用的是这个相机(像元尺寸6.9um),然后得到的内参为fx=628.54905,fy=631.37457,那么,通过标定得知的镜头焦距:f=628.54905 * 6.9 um=4336.988445um=4.3mm。也就是说这个镜头大概是4.3mm的镜头。
关于靶面、像元的介绍,可以看看这里:【相机和镜头选型需要注意哪些问题】、【相机靶面尺寸详解+工业相机选型】

1.4.1.得到的标定结果的意义

各标定参数的意义:

image_width、image_height代表图片的长宽
camera_name为摄像头名
camera_matrix规定了摄像头的内部参数矩阵
distortion_model指定了畸变模型
distortion_coefficients指定畸变模型的系数
rectification_matrix为矫正矩阵,一般为单位阵
projection_matrix为外部世界坐标到像平面的投影矩阵

也可以看看这个
【相机内参标定究竟标了什么?相机内参外参保姆级教程】

1.5.IR、RGB相机分别应用标定结果

得到标定结果后,有两种方法应用标定结果。

1.5.1.openCV应用标定结果

假如需要自己进行相机的画面矫正,可以使用opencv来进行。opencv只用到上面的camera_matrix、distortion_coefficients这两组数据
【opencv畸变校正的两种方法】

1.5.2.ros2工程应用标定结果

我这里可以直接修改卖家提供的源码里面的launch.xml文件的内容,让其加载标定结果。
设置好文件路径之后,出现 “Invalid camera calibration URL”的解决办法
要加上file://前缀,格式如下:
在这里插入图片描述假如出现 does not match narrow_stereo in时
在这里插入图片描述将ost.yaml里面相机的名字改成和报错的一致:
在这里插入图片描述 由于AstraPro的rgb、ir镜头的畸变不明显,标定校准后,畸变的校准效果也不明显,这里就不贴对比图上来了。

2.rgb、depth相机配准

两个相机都标定完之后,就需要进行配准,也就是要得到从ir图到rgb图的映射(旋转矩阵R、平移矩阵T),从而得到对应深度点的颜色值。
【视觉SLAM十四讲(第二版)第5讲习题解答】
在这里插入图片描述
在标定每个相机,并且取得各自的内参后,有两种方式进行D2C的配准:单图像配准以及多图像配准。其中,第二种效果好一点。但是第一种可以学习到更多东西,建议先看第一种。

2.1.单图像配准

这种方式只需要ir、rgb相机拍摄同一张标定图即可。
根据【Kinect深度图与RGB摄像头的标定与配准】里面分析到的,要求RT,就需要先求出外参。

2.1.1.求IR、RGB相机各自的外参(R、T矩阵)

从【Opencv——相机标定】的介绍可以看到,opencv的函数calibrateCamera在根据若干组棋盘格点坐标,计算得到相机内参时,也可以得到每张图片的外参。但是我们目前是用ros的camera_calibration得到的相机内参(虽然内部也是opencv,但是camera_calibration没有给我们保存外参),所以不能够直接得到可用的外参。
但是,我们可以从前面双目标定得到的结果中选取同一时刻拍摄的两张图片(一张是rgb相机的,一张是ir相机的)
在这里插入图片描述然后按照【OPENCV已知内参求外参】、【OPENCV标定外参】,利用上面的两张图片+各自相机的内参,分别算出每个相机的外参,主要代码如下:

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>

using namespace std;
using namespace cv;

// 根据已知的信息计算外参
int calcExtrinsics(string fileName, Mat cameraMatrix, Mat distCoeffs, Size board_size, double rectWidth,
                   Mat &R, Mat &T)
{
    vector<Point2f> image_points;  /* 图像上检测到的角点 */

    Mat imageInput = imread(fileName, IMREAD_COLOR);

    cout << "image size:" << imageInput.cols << "," << imageInput.rows << endl;

    cout << "cameraMatrix:" << cameraMatrix << endl;
    cout << "distCoeffs:"   << distCoeffs   << endl;

    // 显示一下纠正后的图像,直观地检查一下传递进来的cameraMatrix、disCoeffs有没有问题
    Mat undistortMat;
    undistort(imageInput, undistortMat, cameraMatrix, distCoeffs);
    imshow("undistort mat", undistortMat);
    cout << "undistortMat size:" << undistortMat.cols << "," << undistortMat.rows << endl;

    /* 提取角点 */
    if (0 == findChessboardCorners(imageInput, board_size, image_points))
    {
        cout << "can not find chessboard corners!\n"; //找不到角点
        return -1;
    }
    else
    {
        Mat view_gray;

        cvtColor(imageInput, view_gray, COLOR_RGB2GRAY);

        /* 亚像素精确化 */
        find4QuadCornerSubpix(view_gray, image_points, Size(11,11)); //对粗提取的角点进行精确化

        //        // 看看点的排列顺序
        //        image_points_buf[0] += Point2f(-50, -100);
        //        image_points_buf[8] += Point2f(-60, -100);

        /* 在图像上显示角点位置 */
        Mat displayImg = imageInput.clone();
        drawChessboardCorners(displayImg, board_size, image_points, true); //用于在图片中标记角点

        imshow("Camera Calibration", displayImg);//显示图片
    }

    // 上面已经得到了标定板的二维坐标,现在来填充三维坐标
    vector<Point3f> object_points;
    for(int i = 0; i < board_size.height; i++)
    {
        for(int j = 0; j < board_size.width; j++)
        {
            object_points.push_back(Point3f(rectWidth * j, rectWidth * i, 0));
        }
    }

    //    cout << "object points:" << object_points << endl;

    cout << image_points.size() << " "<< object_points.size() << endl;

    //创建旋转矩阵和平移矩阵
    //    Mat rvecs = Mat::zeros(3, 1, CV_64FC1);
    //    Mat tvecs = Mat::zeros(3, 1, CV_64FC1);
    Mat rvec;
    Mat tvec;

    //求解pnp
    bool pnpResult = false;
    //    pnpResult = solvePnP(object_points, image_points, cameraMatrix, distCoeffs, rvec, tvec);
    pnpResult = solvePnPRansac(object_points, image_points, cameraMatrix, distCoeffs, rvec, tvec);

    cout << "pnp result:" << pnpResult << endl;
    if(pnpResult == false)
    {
        return -2;
    }

    Mat rotM;
    Rodrigues(rvec, rotM);  //将旋转向量变换成旋转矩阵

    R = rotM;
    T = tvec;

    // 直观地检验一下计算到的R、T有没有问题
    {
        vector<Point2f> image_points2; /* 保存重新计算得到的投影点 */
        projectPoints(object_points, rvec, tvec, cameraMatrix, distCoeffs, image_points2);

        /* 在图像上显示角点位置 */
        Mat displayImg = imageInput.clone();
        drawChessboardCorners(displayImg, board_size, image_points2, true); //用于在图片中标记角点

        imshow("projectPoints image", displayImg);//显示图片
    }

    return 0;
}

// 分别对两个相机进行外参计算
void calcExtrinsicsDemo()
{
//#define calc_rgb // 通过注释这个来切换

    // 图像路径
    std::string fileName;

    Size board_size = Size(7, 6);   // 标定板上每行、列的角点数
    double rectWidth = 0.015;       // 标定版棋盘格的大小,单位m

    //相机内参矩阵
    Mat cameraMatrix;
    //相机畸变系数
    Mat distCoeffs;


#ifdef calc_rgb
    fileName = "/home/yong/Desktop/orbbec/calibration/dual/calibrationdata/right-0000.png";     // rgb 图像路径
#else
    fileName = "/home/yong/Desktop/orbbec/calibration/dual/calibrationdata/left-0000.png";     // ir 图像路径
#endif


#ifdef calc_rgb
    // rgb相机参数
    double camData[] = {
                        628.549051,  0.000000,   324.691230,
                        0.000000,    631.374570, 283.546017,
                        0.000000,    0.000000,   1.000000};
    cameraMatrix = Mat(3, 3, CV_64F, camData);


    double distData[] = {0.195904,
                         -0.299118,
                         0.018483,
                         0.005324,
                         0.000000};
    distCoeffs = Mat(5, 1, CV_64F, distData);
#else
    // ir相机参数
    double camData[] = {
                        588.560930, 0.000000  ,  306.212790,
                        0.000000  , 590.708852,  250.991143,
                        0.000000  , 0.000000  ,  1.000000};
    cameraMatrix = Mat(3, 3, CV_64F, camData);

    double distData[] = {-0.044573,
                         0.212463,
                         0.006501,
                         -0.006851,
                         0.000000};
    distCoeffs = Mat(5, 1, CV_64F, distData);
#endif

    Mat R,T;
    calcExtrinsics(fileName, cameraMatrix, distCoeffs, board_size, rectWidth, R, T);

    cout << "R:" << R << endl;
    cout << "T:" << T << endl;

}

2.1.2.求两个相机之间的R、T矩阵

通过上面的代码,已经计算得到了rgb、ir(depth)相机的外参:

    // rgb
//R:[0.9932208622695912, -0.104179968756072, -0.05156406561195055;
//     0.09876316392901367, 0.9902419675733234, -0.09831929163314773;
//     0.06130380251811834, 0.09256014134873089, 0.9938181242210883]
//        T:[-0.1985727556503669;
//                -0.1321870936778361;
//                0.5274038381890026]

    // ir
//R:[0.9949185009774844, -0.1001176653598934, 0.01065971367344506;
//     0.1005768682173857, 0.9931508653869539, -0.05946135014214975;
//     -0.004633572304179904, 0.06023131796689411, 0.9981736914704138]
//        T:[-0.1633382501071563;
//                -0.1003269691775472;
//                0.5162717136103199]

然后我们根据【Kinect深度图与RGB摄像头的标定与配准】里面的公式:
在这里插入图片描述计算一下R、T:

Mat R_rgb, T_rgb, R_ir, T_ir;
    {
        double data[] =  {0.9932208622695912, -0.104179968756072, -0.05156406561195055,
                         0.09876316392901367, 0.9902419675733234, -0.09831929163314773,
                         0.06130380251811834, 0.09256014134873089, 0.9938181242210883};
        R_rgb = Mat(3, 3, CV_64F, data).clone();
    }
    {
        double data[] =  {-0.1985727556503669, -0.1321870936778361, 0.5274038381890026};
        T_rgb = Mat(3, 1, CV_64F, data).clone();
    }
    {
        double data[] =  {0.9949185009774844, -0.1001176653598934, 0.01065971367344506,
                         0.1005768682173857, 0.9931508653869539, -0.05946135014214975,
                         -0.004633572304179904, 0.06023131796689411, 0.9981736914704138};
        R_ir = Mat(3, 3, CV_64F, data).clone();
    }
    {
        double data[] =  {-0.1633382501071563,
                         -0.1003269691775472,
                         0.5162717136103199};
        T_ir = Mat(3, 1, CV_64F, data).clone();
    }

    //    cout << R_rgb << endl << T_rgb;


    Mat R, T;
    R = R_rgb * R_ir.inv();
    T = T_rgb - R*T_ir;

    cout << "R:" << R << endl;
    cout << "T:" << T << endl;
    
    /*
R:[0.9980544085026888, -0.0005053133907268852, -0.06234695122237745;
 -0.00192747042416588, 0.9992391545866735, -0.03895377772019822;
 0.06231919869600642, 0.03899816148599567, 0.9972940694071134]
T:[-0.003415024268825395;
 -0.01214055388563491;
 0.02662079621125524]
 */

2.1.3 进行D2C操作

有了两个相机之间的R和T,我们就可以用rgb来对点云着色),注意,此时我们不是用ir图了,而是用depth图了,depth图是16uc1类型的,其数值表示深度(单位mm)。

#include <Eigen/Core>
#include <Eigen/LU>

void depthToColor(Mat depthMat, Mat bgrMat)
{
    Eigen::Matrix3f R_ir2rgb;
    R_ir2rgb <<
        0.9980544085026888, -0.0005053133907268852, -0.06234695122237745,
        -0.00192747042416588, 0.9992391545866735, -0.03895377772019822,
        0.06231919869600642, 0.03899816148599567, 0.9972940694071134;

    Eigen::Vector3f T_ir2rgb;
    T_ir2rgb <<
        -0.003415024268825395,
        -0.01214055388563491,
        0.02662079621125524;


    Eigen::Matrix3f K_ir;           // ir内参矩阵
    K_ir <<
        588.560930, 0.000000  ,  306.212790,
        0.000000  , 590.708852,  250.991143,
        0.000000  , 0.000000  ,  1.000000;

    Eigen::Matrix3f K_rgb;          // rgb内参矩阵
    K_rgb <<
        628.549051,  0.000000,   324.691230,
        0.000000,    631.374570, 283.546017,
        0.000000,    0.000000,   1.000000;

//    Eigen::Matrix3f R;
//    Eigen::Vector3f T;
//    R = K_rgb*parm->R_ir2rgb*K_ir.inverse();
//    T = K_rgb*parm->T_ir2rgb;

    cout << "K_rgb:\n" << K_rgb << endl;
    cout << "K_ir:\n" << K_ir << endl;
//    cout << "R:\n" << R << endl;
//    cout << "T:\n" << T << endl;

    // 4.2 计算投影
    Mat result(480, 640, CV_8UC3);
    int i = 0;
    for (int row = 0; row < 480; row++)
    {
        for (int col = 0; col < 640; col++)
        {
            unsigned short* p = (unsigned short*)depthMat.data;
            unsigned short depthValue = p[row * 640 + col];
            //cout << "depthValue       " << depthValue << endl;
            if (depthValue != std::numeric_limits<unsigned short>::infinity() &&
                depthValue != -std::numeric_limits<unsigned short>::infinity() &&
                depthValue != 0 &&
                depthValue != 65535)
            {
                // 投影到彩色图上的坐标

                // 1)构造一个三维向量p_ir = (x, y, z),其中x,y是该点的像素坐标,z是该像素的深度值;
                Eigen::Vector3f p_ir(col, row, 1.0f);

               // 2)用Kinect内参矩阵H_ir的逆,乘以p_ir得到对应的空间点坐标P_ir,具体公式见上文第四部分(配准);
                Eigen::Vector3f P_ir = K_ir.inverse() *  (p_ir *  (depthValue / 1000.f)); // (除以1000,是为了从毫米变米)
                // 现在这个P_ir,可以放到点云去显示
                
                // 3)由于P_ir是该点在Kinect坐标系下的坐标,我们需要将其转换到RGB摄像头的坐标系下,具体的,就是乘以一个旋转矩阵R,再加上一个平移向量T,得到P_rgb;
                Eigen::Vector3f P_rgb = R_ir2rgb * P_ir + T_ir2rgb;

                // 4)用RGB摄像头的内参矩阵H_rgb乘以P_rgb,得到p_rgb,
                // p_rgb也是一个三维向量,其x和y坐标即为该点在RGB图像中的像素坐标,取出该像素的颜色,作为深度图像中对应像素的颜色;
                Eigen::Vector3f p_rgb = K_rgb * P_rgb;

                int X = static_cast<int>(p_rgb [0] / p_rgb [2]);                // !!!Z_rgb*p_rgb -> p_rgb
                int Y = static_cast<int>(p_rgb [1] / p_rgb [2]);                // !!!Z_rgb*p_rgb -> p_rgb
                //cout << "X:       " << X << "     Y:      " << Y << endl;
                if ((X >= 0 && X < 640) && (Y >= 0 && Y < 480))
                {
                    //cout << "X:       " << X << "     Y:      " << Y << endl;
                    result.data[i * 3]     = bgrMat.data[3 * (Y * 640 + X)];
                    result.data[i * 3 + 1] = bgrMat.data[3 * (Y * 640 + X) + 1];
                    result.data[i * 3 + 2] = bgrMat.data[3 * (Y * 640 + X) + 2];
                }
                else
                {
                    result.data[i * 3] = 0;
                    result.data[i * 3 + 1] = 0;
                    result.data[i * 3 + 2] = 0;
                }
            }
            else
            {
                result.data[i * 3] = 0;
                result.data[i * 3 + 1] = 0;
                result.data[i * 3 + 2] = 0;
            }
            i++;
        }
    }

    imshow("result", result);
}

根据原文的介绍:
在这里插入图片描述

在应用R、T矩阵时,可以根据实际情况微调一下T矩阵的x、y
在这里插入图片描述
但是经过尝试,假如调好了近处的物体,远处的物体和颜色又对不齐;反之亦然。可能是硬件的问题。
另外,我用这种方式得到点云与官方的点云在X、Y方向有较大的偏差,可能是我通过标定得到的内参和官方的内参存在较大差异导致的。(后来经过检查他们的源码,发现他们彩色点云用的是rgb相机的内参+深度图计算的点云,而不是ir相机的内参,所以和我得到的点云存在位移也就是正常的了。)
在这里插入图片描述

2.2.多图像配准

多图像配准用到了opencv的stereoCalibrate,它可以利用多组空间点与两个相机的图像点综合来计算,效果自然比前面的单图像的要好。

2.2.1.求两个相机之间的R、T矩阵

下面的代码用到的是之前利用camera_calibration标定相机时用到的图片。共用了20张(各自10张)。
在这里插入图片描述

int calculateRT(vector<string> irImgPathList, vector<string> rgbImgPathList,
                CameraInfo irCamInfo, CameraInfo rgbCamInfo,
                Size boardSize,      // 棋盘格上的角点的大小,mxn
                Size2f boardRectSize,  // 单个格子的尺寸,单位m
                Mat &R, Mat &T)
{

    cout << "calculateRT:" << irImgPathList.size() << "  " << rgbImgPathList.size() << endl;

    // 上面已经得到了标定板的二维坐标,现在来填充三维坐标
    // 此时的坐标按照右手定则,Z轴正方向指向相机
    vector<Point3f> objectPoints;
    for(int i = 0; i <boardSize.height; i++)
    {
        for(int j = 0; j < boardSize.width; j++)
        {
            objectPoints.push_back(Point3f(boardRectSize.width * j, -boardRectSize.height * i, 0));
        }
    }

    vector<vector<Point3f>> objectPointsVec;
    vector<vector<Point2f>> imagePointsVec1;
    vector<vector<Point2f>> imagePointsVec2;

    // 填充实际坐标
    for(int i = 0; i < irImgPathList.size(); i++)
    {
        objectPointsVec.push_back(objectPoints);

        string irImgPath = irImgPathList.at(i);
        string rgbImgPath = rgbImgPathList.at(i);

        Mat irImg = imread(irImgPath);
        Mat rgbImg = imread(rgbImgPath);

        cout << "--begin find chessboard:" << i << endl;
        vector<Point2f> imagePoints;  /* 图像上检测到的角点 */
        /* 提取角点 */
        if (false == findChessboardCorners(irImg, boardSize, imagePoints))
        {
            cout << "can not find chessboard corners!" << endl; //找不到角点
            return -1;
        }
        else
        {
            cout << "ir chessboard corners found!" << "  " << irImgPath << endl;

            // 算法应该没问题,主要是深度图提供的深度有问题

            Mat grayImg;

            if(irImg.channels() == 1)
            {
                grayImg = irImg.clone();
            }
            else
            {
                cvtColor(irImg, grayImg, COLOR_BGR2GRAY);
            }


            /* 亚像素精确化 */
            find4QuadCornerSubpix(grayImg, imagePoints, Size(7, 7)); //对粗提取的角点进行精确化
        }
        imagePointsVec1.push_back(imagePoints);

        imagePoints.clear();
        /* 提取角点 */
        if (false == findChessboardCorners(rgbImg, boardSize, imagePoints))
        {
            cout << "can not find chessboard corners!" << endl; //找不到角点
            return -1;
        }
        else
        {
            cout << "rgb chessboard corners found " << rgbImgPath << endl;

            // 算法应该没问题,主要是深度图提供的深度有问题

            Mat grayImg;

            if(rgbImg.channels() == 1)
            {
                grayImg = rgbImg.clone();
            }
            else
            {
                cvtColor(rgbImg, grayImg, COLOR_BGR2GRAY);
            }


            cout << "find4QuadCornerSubpix" << grayImg.size() << endl;
            /* 亚像素精确化 */
            bool ret = find4QuadCornerSubpix(grayImg, imagePoints, Size(7, 7)); //对粗提取的角点进行精确化
            cout << "find4QuadCornerSubpix:" << ret << endl;

        }
        imagePointsVec2.push_back(imagePoints);
    }

    Mat _R, _T, _E, _F;
    Mat perViewErrors;
    // https://blog.csdn.net/weixin_43560489/article/details/123562399
    double ret = stereoCalibrate(objectPointsVec,
                                 imagePointsVec1, imagePointsVec2,
                                 irCamInfo.cameraMatrix, irCamInfo.distort_coefficient,
                                 rgbCamInfo.cameraMatrix,rgbCamInfo.distort_coefficient,
                                 Size(irCamInfo.width, irCamInfo.height),
                                 _R, _T, _E, _F,
                                 perViewErrors);

    cout << "stereoCalibrate finished:" << ret << perViewErrors << endl;

    R = _R;
    T = _T;

    return 0;
}

2.2.2.进行D2C配准

在利用stereoCalibrate得到的R、T进行配准时,得到的效果如下图所示。
可以看到,近(黄色的小刀)、中(手掌)、远(快递盒)的效果都还不错。
在这里插入图片描述

3.题外话

在取得了相机的内参,此时物体的外参后,其实可以做一些比较有趣的事情

3.1.点云的坐标变换

我们用的点云来源自IR相机,点云的坐标(XYZ)是基于IR相机坐标系的。因此手眼标定时,要用IR相机去参与标定,而不是RGB相机。
我们先小试牛刀,将点云的坐标变换到标定板坐标系下,效果如下:
在这里插入图片描述
在这里插入图片描述

可以看到,我虽然斜着放置相机,但是经过标定后,点云的姿态是能够与标定板表示的坐标系基本对齐的。

方法其实也就是拿IR相机针对标定板得到的外参(R、T)组成变换矩阵transform,由于此时的transform是标定板到相机的变换,而我们要将相机坐标系下的坐标转变为标定板坐标系下的坐标,因此需要求逆矩阵cam2ObjTransform,然后用它来对点云作变换。

在这里插入图片描述

3.2.AR(在相机画面上绘制3d物体)

利用opencv的【projectPoints】函数,可以将3d坐标的点,投影到相机画面的屏幕空间。也就可以做一些AR(增强显示)了。
比如下图,我绘制了xyz坐标轴、一个立方体骨架,实现了简单的AR显示。
同时显示了该标定板的坐标原点到相机的距离,计算距离是直接利用外参的T来计算的。可以这样算的原因是因为标定板坐标系和相机坐标系的变换是刚性变换。当然,这个值也仅供参考。

 double Tx = TMat.at<double>(0, 0);
 double Ty = TMat.at<double>(1, 0);
 double Tz = TMat.at<double>(2, 0);
 double distance = sqrt(Tx*Tx + Ty*Ty + Tz*Tz) * 1000.0;
 qDebug() << "ir 预计距离(mm):" << distance;
 putText(irDisplayMat, QString("ir distance:%1mm").arg(distance).toStdString(), Point(10, 30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(0, 0, 255), 2);

在这里插入图片描述

假如想利用opengl绘制3d物体并叠加到画面中,有了相机的内参、外参,理论上应该也是可以的。不过,这就是另外一个话题了。


参考:
【1.Astra相机标定】
【【Nav2中文网】ROS2单目相机标定教程】
【深度图与彩色图的配准与对齐】
【Kinect深度图与RGB摄像头的标定与配准】
【RGBD相机实用问题】
【相机内参标定究竟标了什么?相机内参外参保姆级教程】

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准) 的相关文章

  • 洛谷P5717-三角形分类

    洛谷P5717 三角形分类 题目 这道题更像是初中题 xff0c 但是怎么能完整的按照题目的意思来解决呢 xff0c 说实话这个题卡了我有一会儿 xff0c 要做一次性做出这个题 xff0c 我觉得需要搞清楚if if 和if else i
  • 洛谷P1424-小鱼的航程(改进版)

    洛谷P1424 小鱼的航程 xff08 改进版 xff09 这个题我第一次做的时候 xff0c 有两个没过 xff0c 后来检查的时候发现原来是没有考虑开始的时间是不是周六周日 xff0c 如果是周六要在原来的天数上 2 xff0c 如果是
  • freertos-简介(一)

    FreeRTOS 裸机 不带任何操作系统 只能先打完游戏回复信息 实时性差 xff0c 程序轮流执行delay空等待 xff0c CPU不执行其他代码结构臃肿 xff0c 实现功能都在while循环 RTOS 实时操作系统 会执行打游戏一个
  • PCB设计过程中AD使用流程详解(超详细)

    PCB设计过程中AD使用流程详解 xff08 超详细 xff09 1 设计前期部分 规则设定 xff1a Preference system file type关联文件 xff08 所有关联 xff09 PCB editor General
  • python面向对象编程

    符合python风格的对象 先来看一个向量类的例子 span class token keyword class span span class token class name Vector2d span span class token
  • DIY 一个树莓派无人机

    学习目标 xff1a DIY 一个树莓派无人机 这篇文章来源于DevicePlus com英语网站的翻译稿 提示 xff1a 这里可以添加学习目标 学习内容 xff1a 提示 xff1a 这里可以添加要学的内容 今天 xff0c 我们将利用
  • Linux开源杀毒软件CLamAV介绍

    Linux开源杀毒软件CLamAV介绍 很多用户可能不知道在Linux上会有计算机病毒 xff0c 虽然Linux上的病毒不像在Windows上那么常见 xff0c 但实际上 xff0c 很多重要系统均采用Linux系统作为服务器的操作系统
  • vrpn_cient_ros发送频率无法调整提供解决思路

    最近写了节点来订阅ros client ros 但是发现在launch文件修改update frequency不起作用 xff0c 然而我又需要通过串口给下位机发送数据 xff0c 频率一快 xff0c 串口直接堵死了 怎么调都freque
  • 封装CopyFileEx函数,实现文件复制中的暂停,控速,获取进度

    封装CopyFileEx函数 xff0c 实现文件复制中的暂停 xff0c 控速 xff0c 获取进度等 前段时间无意间想到如何控制文件复制过程的复制速度 xff0c 并且能实时获得复制进度 对于一个几兆甚至更小的文件 xff0c 调用AP
  • 字符串结束符'\0' -何时自动加- 字符串定义方法

    转载 字符串定义方法 有两种方法 1 用字符数组 xff1b 2 用字符指针 xff1b 对应两种定义方法 xff0c 有不同的初始化以及赋值方法 对字符数组 xff0c 有以下几种定义方法 xff1a 1 char str 61 34 1
  • 基于Airsim的sitl模拟环境配置(ubuntu 16.04)

    基于Airsim的sitl模拟环境配置 xff08 ubuntu 16 04 xff09 sitl仿真 xff0c 软件在环仿真可以不使用任何硬件就可以进行模拟飞行或驾驶 xff0c 实验室获取数据非常有用 基于Airsim的sitl模拟需
  • F450机架组装及飞控安装细节

    http tieba baidu com p 5342947735
  • 富斯i6接收机及PPM编码器​​​​​​​接线

    没有完成发射机和接收机对码 xff0c 则需要按照如下过程对码 xff1a 1 将对码线连接到接收机上的 B VCC 接口 2 将电源线连接到接收机上任意其他接口 3 打开发射机电源 xff0c 同时常按发射机 BINDKEY 键 xff0
  • Pixhawk指示灯和安全开关含义

    Pixhawk指示灯的含义 红灯和蓝灯闪 xff1a 初始化中 请稍等 黄灯双闪 xff1a 错误 系统拒绝解锁 蓝灯闪 xff1a 已加锁 xff0c GPS搜星中 自动导航 xff0c 悬停 xff0c 还有返回出发点模式需要GPS锁定
  • MP地面站提示

    PIX飞控或者APM飞控在装机后 xff0c 经常遇到不能解锁的情况 xff0c 地面站会有提示 xff0c 下面列出了可能出现的情况 xff0c 可以一一对应的排除故障 当然 xff0c 你也可以在地面站设置解锁不自检 xff0c 不过安
  • win7下 pixhawk (ardupilot) 的编译

    前几天都在搞pixhawk源码编译问题 xff0c 什么在window下用Console或者eclipse xff0c 还是在Ubuntu下 xff0c 都做了 xff0c 而且把 mk文件都看了 xff0c 结果还是有bug 总结一下三种
  • pixhawk 基于UART5 的NSH环境搭建

    元器件 xff1a pixhawk 六针杜邦线 USB转uart模块 PC机上的串口调试软件 xff08 比如 Putty xff09 STEP 1 xff1a 制作调试通信线 xff08 六针杜邦线和USB转uart模块 xff09 xf
  • 操作系统 | 用户态和内核态的切换(中断、系统调用与过程(库函数)调用)

    文章目录 中断过程调用系统调用过程调用和系统调用的区别 中断 用户态 内核态之间的切换是怎么实现的 用户态 内核态 是通过中断实现的 并且 中断是唯一途径 核心态 用户态 的切换是通过执行一个特权指令 xff0c 将程序状态字 PSW 的标
  • MFC鼠标响应、鼠标画线

    鼠标响应关键就是对两个函数进行操作 xff1a OnLButtonDown和OnLButtonUp xff1b 1 使用MFC AppWizard exe xff09 建立一个单文档MFC工程 2 首先要在CxxxView类的定义里加上后续
  • stm32cubeide的freertos-消息队列发送结构体或者长消息,接收不完整的问题

    今天在项目创建消息队列 xff0c 消息队列中的数据类型的是结构体 xff0c 调用接口使用的cubeide自带的创建消息队列的接口 span class token keyword typedef span span class toke

随机推荐