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

这篇具有很好参考价值的文章主要介绍了ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

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

1.rgb、depth相机标定矫正

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

我所使用的相机型号是Astra_pro,它是一个单目结构光相机,有一个RGB摄像头+一个IR摄像头。从数量上算是一个双目相机(rgb+ir),但是从3d深度数据的获取上不是我们平时所说的双目相机,因为:【奥比中光Astra深度传感器工作原理】

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

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

在我的系统中,可以分别对这两个进行相机进行标定
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)
在标定时,具体的参数(相机、话题、格子数等等)要根据你实际的情况进行填写。

1.1.标定rgb相机

执行以下命令

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

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

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

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

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

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

1.4.取得标定结果

参考上面提到的文章进行操作后,分别可以在/tmp目录下得到标定后的数据。
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)无论是用camera_calibration进行单目标定和双目标定,貌似没有本质的区别,都是分别得到两个相机的内参。
得到的相机内参如下图所示:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)
以上图为例,可以知道相机的内参fx=628.54905,fy=631.37457;这是说相机镜头的焦距为628.54905mm吗?
不是,这个是以像素为单位的,镜头的焦距为628.54905像素。那1像素代表多少mm呢(像元尺寸)?
这个要看相机感光芯片的尺寸及芯片的分辨率。假设感光芯片尺寸为WH(单位mm),分辨率为UV,那么像元尺寸为[W/U, H/V).
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)以上面的海康威视相机为例,假如我标定时使用的是这个相机(像元尺寸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://前缀,格式如下:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)假如出现 does not match narrow_stereo in时
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)将ost.yaml里面相机的名字改成和报错的一致:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准) 由于AstraPro的rgb、ir镜头的畸变不明显,标定校准后,畸变的校准效果也不明显,这里就不贴对比图上来了。

2.rgb、depth相机配准

两个相机都标定完之后,就需要进行配准,也就是要得到从ir图到rgb图的映射(旋转矩阵R、平移矩阵T),从而得到对应深度点的颜色值。
【视觉SLAM十四讲(第二版)第5讲习题解答】
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)
在标定每个相机,并且取得各自的内参后,有两种方式进行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相机的)
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)然后按照【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摄像头的标定与配准】里面的公式:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)计算一下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)。
为啥我知道是mm的?其实你追踪他们的源代码,可以知道他们用的是openNI,然后openNI的图像传输过来时是带格式的,可用格式如下:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)要么是1mm,要么是100um=0.1mm。尝试对比一下就知道了。

#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);
}

根据原文的介绍:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)

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

2.2.多图像配准

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

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

下面的代码用到的是之前利用camera_calibration标定相机时用到的图片。共用了20张(各自10张)。
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)

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进行配准时,得到的效果如下图所示。
可以看到,近(黄色的小刀)、中(手掌)、远(快递盒)的效果都还不错。
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)

3.题外话

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

3.1.点云的坐标变换

我们用的点云来源自IR相机,点云的坐标(XYZ)是基于IR相机坐标系的。因此手眼标定时,要用IR相机去参与标定,而不是RGB相机。
我们先小试牛刀,将点云的坐标变换到标定板坐标系下,效果如下:
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)
ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)

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

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

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

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);

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

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


参考:
【1.Astra相机标定】
【【Nav2中文网】ROS2单目相机标定教程】
【深度图与彩色图的配准与对齐】
【Kinect深度图与RGB摄像头的标定与配准】
【RGBD相机实用问题】
【相机内参标定究竟标了什么?相机内参外参保姆级教程】文章来源地址https://www.toymoban.com/news/detail-456342.html

到了这里,关于ORBBEC(奥比中光)AstraPro相机在ROS2下的标定与D2C(标定与配准)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 实测 (四)NVIDIA Xavier NX + D435i / 奥比中光Astrapro 相机+ ORB-SLAM 2 + 3 稠密回环建图

    首先小白老师分享的ORB-SLAM3的可回环的稠密地图版本,具体在这篇博客,下载但是却没有相关的具体实现教程,这里我们先使用 奥比中光Astrapro 两款相机进行配置实现 其实与orb-slam2的环境配置一样,使用的仍然是pagolin0.5,和opencv3.2.0版本(3.4.x也可以),pcl1.8.1+vtk7.1.1 这里

    2024年02月16日
    浏览(42)
  • ros2 基础教程-使用ROS 2进行相机标定

    相机(摄像头)是一种非常精密的光学仪器,对外界环境的感知非常敏感。由于摄像头内部和外部的一些原因,摄像头采集的图像常常会发生一定的畸变。如果直接将采集到的图像拿来进行图像处理,会产生很大的问题。为了避免图像数据源造成的误差,需要对摄像头的相关

    2024年01月21日
    浏览(50)
  • 奥比中光Astra SDK相机SDK openni&&相机成像原理

    目录 1.1 成像原理简介 1.1.1 结构光 1.1.2 双目视觉 1.1.3 光飞行时间TOF​ 2.使用手册 参考网址 2.1 产品集成设计 2.2 SDK介绍与使用 2.3 常用API介绍 OPENNI API 2 OpenNI类(OpenNI.h) 1.1.1 结构光 结构光,英文叫做 Structured light,通常采用特定波长的不可见的红外激光作为光源,它发射出

    2024年04月14日
    浏览(63)
  • 深度相机、实验箱、扫描仪、机器狗…… 奥比中光超强性能3D视觉应用亮相VALSE 2023

    6月10日-12日,VALSE 2023视觉与学习青年学者研讨会在江苏无锡举办,奥比中光作为金牌赞助商,携Femto Mega与Gemini 2系列3D相机以及3D视觉实验箱开发套件、手持3D扫描仪、四足机器狗等创新解决方案参展,为3D视觉开发者提供多样化的开发选择。 VALSE是计算机视觉、图像处理、模

    2024年02月09日
    浏览(72)
  • 奥比中光astra深度相机通过openni直接显示rgb彩色图、depth深度图、ir红外图

    本文章的前提为astra深度相机驱动安装完成能够正常显示使用,如果没有安装好请看另一篇文章 Thinker Board 2开发板上使用奥比中光 astra 深度相机 也可以看其他人的类似奥比中光astra深度相机的教程 创建python文件复制下面代码既可使用 注意 depth图和ir图有冲突不能同时使用 这

    2024年02月14日
    浏览(83)
  • 奥比中光:进击具身智能,打造机器人之眼

    大数据产业创新服务媒体 ——聚焦数据 · 改变商业 跨过奇点的生成式人工智能是一个缸中大脑,只有赋予形体,才能与物理世界产生互动。 在5月的ITF世界半导体大会上,英伟达创世人兼CEO黄仁勋说,人工智能的下一波浪潮将是具身智能。 8月中旬,世界机器人大会在北京

    2024年02月11日
    浏览(42)
  • pyorbbecsdk奥比中光python版本SDK在Windows下环境配置笔记

            Orbbec SDK Python Wrapper基于Orbbec SDK进行设计封装,主要实现数据流接收,设备指令控制。 2.1、操作系统 Windows:Windows 10 (x64)(本文 针对windows) Linux: 18.04/20.04/22.04 (x64) Arm32: 18.04/20.04/22.04 Arm64: Ubuntu18.04/20.04/22.04 2.2、编译平台要求 Windows:Visual Studio 2017 及以上(本人用

    2024年04月12日
    浏览(43)
  • ROS进行深度相机的标定

    自己使用标定板对深度相机进行标定。 参考:http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration 在下面的网站中可下载棋盘格标定板,可用A4纸打印下来。 http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFiledo=viewtarget=check-108.pdf 1.1 进入ROS内核 1.2 打开相机

    2024年02月09日
    浏览(57)
  • ROS学习——利用电脑相机标定

    一、 安装usb-cam包和标定数据包 要把kinetic改成你自己的ros版本 。 二、启动相机 就会出现一个界面  可以通过下面命令查看相机发布了哪些参数:  可以通过下面命令查看发布消息的具体类型: 我们可以看到发布者和接收者: 三、启动相机标定包 出现黑白相机  四、进行相

    2024年02月10日
    浏览(43)
  • 【ROS】usb_cam相机标定

    1. 唠叨两句 当我们要用相机做测量用途时,就需要做相机标定了,不然得到的计算结果会有很大误差,标定的内容包括三部分:内参,外参还有畸变参数。所以标定的过程就是要求得上面这些参数。 以前弄这个事估计挺麻烦,需要做实验和计算才能得到,现在通过ros的开源

    2024年02月08日
    浏览(46)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包