目录
前言
记录一下ROS下的深度相机标定。emmm，这次记录的标定和其他博客有写不太一样，因为有些地方我也没有弄明白。另外，仅仅在单目标定这一块，可以使用的方法可以有很多，ROS只是其中一种方法而已。这里简单举几个例子，matlab中的工具包、opencv、halcon、ROS等等，各位小伙伴可以采取自己的方式对单目相机进行标定。这篇文章是使用ROS中的标定程序进行单目标定的，其他地方比如红外相机和彩色相机配准、彩色相机给点云赋色等都和ROS没关系，主要使用到的是opencv和pcl。
单目相机标定
标定前的准备
（单目相机的标定原理可以看我得另外一篇博客相机的成像模型_相机成像模型-CSDN博客，这篇博客讲的挺详细的，对相机标定、相机内参等地方不太明白的小伙伴可以去学习一下。）
在ROS下对单目相机标定需要安装ROS官方的标定（注意：修改ROS版本）：
sudo apt install ros-（ROS版本）-camera-calibration
安装完成之后就可以运行标定程序了。
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.108 image:=/my_camera/image camera:=/my_camera该标定程序默认是使用棋盘格进行标定的，其中的参数也是比较基本的几种，下面简单解释一下各参数含义。
--pattern：标定板的图案，这里给出了三种图案 'chessboard','circles', 'acircles'，chessboard为棋盘格，circles为对称圆点，acircles为非对称圆点；
--size：标定板中的角点数量、圆点数量等；
--square：角点或圆点的个数；
image为你要标定的图像的话题；
有的小伙伴可能手头比较紧，买不了高精度的标定板，那么可以使用这个链接Calibration Checkerboard Collection | Mark Hedley Jones自动生成标定板，然后再去打印就好了。这样的优点就是价格便宜，上手快，缺点就是精度可能会稍微差一点。
如果使用圆点标定板的小伙伴则可以使用下面的代码：
rosrun camera_calibration cameracalibrator.py --pattern 'circles' --size 7x7 --square 0.02 image:=/camera/color/image_raw我这里买的halcon的标定板，但是标定的时候想起来halcon标定需要先输入一个大概的焦距，但是我又不知道这个焦距是多少，所以才选则使用ROS进行标定的（其他小伙伴遇到这样的问题也可以使用其他的单目标定方式）。个人认为ROS标定还是比较方便的。
彩色相机标定
运行程序后可以看到下面的界面：
然后就可以标定了，等到CALIBRATE按钮变色，再点击SAVE就可以保存参数了。参数文件默认保存在/tmp路径下，同时终端会类似显示下面的画面：
其中camera matrix就是我们需要的相机内参矩阵了，distortion就是畸变参数了。
红外相机标定
通常深度相机是通过发射红外光斑，然后使用红外相机去捕捉光斑来获取深度的，方法有点类似于TOF了。如果红外摄像头有两个的话，就需要观察深度图中，是以哪个相机为主的。找到主红外摄像头之后，就只需要对主红外摄像头进行下面的标定了。
红外相机的标定与彩色相机类似。不同的是红外相机只能检测红外光。比如像下面这张图片一样黑黢黢的基本上什么都看不见。
我这里用了一个笨办法，就是在把窗户打开，让外面的阳光照进来就好了。小伙伴们也可以采用类似这样的办法，比如用镜子反射阳光照到标定板上就可以了。如果不行的话，可能就需要另外购买红外光源了。这里放一下我采图的效果。
同样在标定完成后也会/tmp目录下生成标定结果。
PS：使用ROS进行双目标定
在ROS下也可以直接用双目标定程序，同时对两个相机进行标定的。标定完成后，不仅会获得两个相机的内参，还可以获得两个相机的外参（也就是两个相机间的转换关系）。但是这个地方我没有成功，具体的原因我也不太清楚，有知道的小伙伴可以联系我~~
这里就放一下ROS官方的双目标定的代码吧。
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.108 right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw left_camera:=/my_stereo/left right_camera:=/my_stereo/right也可以选择去掉相机参数的服务，在后面加上--no-service-check就好了。
求解双目外参
emmm。。这里所说的外参指的是两个相机坐标系之间的转换关系。也就是在红外相机坐标系下的点通过某种转换关系，可以转换到彩色相机坐标系下。可能专业名称不是这样叫的，请谅解~~因为我在ROS下用双目标定程序失败了，所以选择用opencv了，哈哈哈哈，条条大路通罗马，无论黑猫白猫能够做出来的就是好猫。
这里就涉及到一些数学之间的转换了，实际上非常简单，就是彩色相机和红外相机同时对标定板拍照，然后去算两个相机到标定板的外参，由于标定板在空间中是不变的就有下面的关系。这里偷个懒就放个其他博主的博客中的公式吧。这个是原链接深度图与彩色图的配准与对齐_彩色 深度 配准-CSDN博客
公式看起来可能有一点点复杂，不过使用opencv中的代码是很容易就能够实现的。有的博主使用的是一张标定板的图像进行求解的，这样不太稳定。我这里用的是opencv中的双目标定函数进行标定的（实际上也就是做一次双目标定，哈哈哈哈），这样做的好处是可以同时对多张图像进行标定，这样求解的结果会更加稳定一些，效果也会更好一些。
代码：
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <string>
#include <filesystem>
using namespace std;
using namespace cv;
struct CameraInfo
{
Mat cameraMatrix;
Mat distort_coefficient;
int width;
int height;
};
int calculateRT(vector<string> irImgPathList, vector<string> rgbImgPathList,
CameraInfo irCamInfo, CameraInfo rgbCamInfo,
Size boardSize, // 棋盘格上的角点的大小，mxn
Size2f boardRectSize, // 单个格子的尺寸，单位m
Mat &R, Mat &T)
{
std::cout << "calculateRT:" << irImgPathList.size() << " " << rgbImgPathList.size() << std::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);
cv::Mat irImg = imread(irImgPath);
cv::Mat rgbImg = imread(rgbImgPath);
cout << "--begin find chessboard:" << i << endl;
vector<Point2f> imagePoints; /* 图像上检测到的角点 */
/* 提取角点 */
if (false == findCirclesGrid(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 == findCirclesGrid(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;
}
int main(int argc, char **argv)
{
CameraInfo rgbCam, rCam;
rgbCam.cameraMatrix = (Mat_<double>(3, 3) << 743.50639, 0., 782.21241,
0., 747.45669, 616.80663,
0., 0., 1.); // 相机的内参
rgbCam.distort_coefficient = (Mat_<double>(5, 1) << -0.011922, 0.012531, 0.003824, -0.004757, 0.000000);
rgbCam.height = 1200;
rgbCam.width = 1600;
rCam.cameraMatrix = (Mat_<double>(3, 3) << 540.16853, 0., 540.44738,
0., 510.14288, 359.8121,
0., 0., 1.); // 相机的内参
rCam.distort_coefficient = (Mat_<double>(5, 1) << 0.000767, 0.000459, -0.000013, -0.000000, 0.000000);
rCam.height = 720;
rCam.width = 1080;
Size boardSize = Size(7, 7);
Size2f boardRectSize = Size2f(0.02, 0.02);
vector<string> rgb_files, r_files;
cv::glob("../image/rgb/*.bmp", rgb_files, false);
cv::glob("../image/r/*.bmp", r_files, false);
Mat R, T;
calculateRT(r_files, rgb_files, rCam, rgbCam, boardSize, boardRectSize, R, T);
cout << "R = " << R << endl; // 第一个相机在第二个相机下的位姿
cout << "T = " << T << endl;
return 0;
}放几张图片吧，下面分别是红外相机和彩色相机下采的图：
下面就是求解结果了，会通过对每张图像进行求解，然后优化，求解出一个结果。
下面的R和T就是求解结果了。
红外相机与彩色相机对齐（d2c）
上面做了那么多铺垫，下面就是真正能够看到效果得时候了，真是不容易哇。写博客可能一会儿就写到这了，做的时候可是花了几天解决各种各样的问题了。。小伙伴如果看到这记得点个赞，收藏一下哇~~嘿嘿~~
回到主题，红外相机与彩色相机对齐的原理很简单，就是把深度图还原成点云，再把所有的点云通过外参转换到彩色相机下，然后再用RGB值给点云赋色就好了。代码和原理同样简单：
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
// 相机内参
double color_cx = 782.21241;
double color_cy = 616.80663;
double color_fx = 743.50639;
double color_fy = 747.45669;
double depth_cx = 540.44738;
double depth_cy = 359.8121;
double depth_fx = 540.16853;
double depth_fy = 510.14288;
int main(int argc, char* argv[])
{
cv::Mat color = cv::imread("../image/depth/2/Output_Webcam_00000168.bmp", 1);
cv::Mat depth = cv::imread("../image/depth/2/Depth_Channel_3_2422822005543.tif", cv::IMREAD_UNCHANGED);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
int point_nums = 0;
for (int i = 0; i < depth.rows; ++i)
{
for (int j = 0; j < depth.cols; ++j)
{
short d = depth.ptr<short>(i)[j];
if (d <= 0)
continue;
point_nums++;
pcl::PointXYZRGB p;
p.z = double(d);
p.x = (j - depth_cx) * p.z / depth_fx;
p.y = (i - depth_cy) * p.z / depth_fy;
Eigen::Matrix3f R; //d2c旋转矩阵
R << 0.9995953698390964, -0.006099681531393762, 0.02778291707966812,
0.005890378575293033, 0.9999537011789307, 0.007609135220085419,
-0.0278280440649333, -0.007442404434951745, 0.999585019185335;
Eigen::Vector3f T(0.0653115633382564,
0.001010594465843528,
0.00211337038326914); //d2c平移向量
Eigen::Vector3f Pd = p.getArray3fMap(); // 将红外相机的点云转换到彩色相机中
Eigen::Vector3f Pc = R * Pd + T * 1000;
int n = Pc.x() * color_fx / Pc.z() + color_cx;
if (n < 0)n = 0;
if (n >= color.cols)n = color.cols - 1;
int m = Pc.y() * color_fy / Pc.z() + color_cy;
if (m < 0)m = 0;
if (m >= color.rows)m = color.rows - 1;
p.b = color.at<cv::Vec3b>(m, n)[0];
p.g = color.at<cv::Vec3b>(m, n)[1];
p.r = color.at<cv::Vec3b>(m, n)[2];
cloud->points.push_back(p);
}
}
cout << "cloud size is " << cloud->size() << endl;
if (cloud->size() >= 0)
{
// // 创建可视化对象
pcl::visualization::CloudViewer viewer("Cloud Viewer");
// 显示点云
viewer.showCloud(cloud);
// 等待用户关闭窗口
while (!viewer.wasStopped())
{
}
}
return 0;
}
好了，放两张结果图~~
这里存在一下小缺陷，这个适合自己的深度相机有关系的。就比如我这里的电脑屏幕，应该是一个平面的，但是放大看就变的凹凸不平了。
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
