ORBBEC（奥比中光）AstraPro相机在ROS2下的标定与D2C（标定与配准）
文章目录
- 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），但是从3d深度数据的获取上不是我们平时所说的双目相机，因为：【奥比中光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 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）。
为啥我知道是mm的？其实你追踪他们的源代码，可以知道他们用的是openNI，然后openNI的图像传输过来时是带格式的，可用格式如下：
要么是1mm，要么是100um=0.1mm。尝试对比一下就知道了。
#include 根据原文的介绍：
在应用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相机实用问题】
【相机内参标定究竟标了什么？相机内参外参保姆级教程】