opencv的相机标定与姿态解算

首先我们要知道四个重要的坐标系

世界坐标系
相机坐标系
图像成像坐标系
图像像素坐标系

坐标系之间的转换

世界坐标系——相机坐标系
从世界坐标系到相机坐标系，涉及到旋转和平移（其实所有的运动也可以用旋转矩阵和平移向量来描述）。绕着不同的坐标轴旋转不同的角度，得到相应的旋转矩阵，如下图所示：
假设世界坐标系中点坐标为[x1,y1,z1]，对应的相机坐标系中的点坐标为[x,y,z]，世界坐标系转换至相机坐标系遵循如下推导：
在这里插入图片描述
相机坐标系——图像坐标系
从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。整个变换推导为两个相似三角形的变换，

像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。

在这里插入图片描述
那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。

在这里插入图片描述
其中相机的内参和外参可以通过张正友标定获取。

下面使用 OpenCV 的 cv.calibrateCamera 和 MATLAB 的 estimateCameraParameters 函数来完成相机标定，然后使用这些参数推算图像中的坐标点在真实世界中的位置，并通过相机位姿（位置和方向）的估计绘制相机的空间位置。

1. 添加 `mexopencv` 路径并编译 OpenCV

addpath('D:\\围棋\\坐标解算\\坐标解算相关内容\\mexopencv');
mexopencv.make('opencv_path', 'D:\Tools\opencv341\opencv\build');

addpath 将 mexopencv 工具包的路径添加到 MATLAB 的搜索路径中。mexopencv 是一个连接 MATLAB 和 OpenCV 的接口工具包。
mexopencv.make 编译 mexopencv 以使用 OpenCV 库，指定了 OpenCV 的安装路径。

2. 读取 YAML 文件中的数据

fs1 = cv.FileStorage('D:\\围棋\\坐标解算\\坐标解算相关内容\\mexopencv\\samples\\01_1.yml');
fs2 = cv.FileStorage('D:\\围棋\\坐标解算\\坐标解算相关内容\\mexopencv\\samples\\01_2.yml');

cv.FileStorage 读取 YAML 格式的文件，这些文件包含图像的相关点（角点或其他特征点）的坐标数据，用于标定相机参数。

3. 读取图像并显示

img = imread("D:\围棋\坐标解算\坐标解算相关内容\1.jpg");
imshow(img);

使用 imread 读取图像文件，并通过 imshow 显示图像。

4. 使用 OpenCV 进行相机标定

标定的结果包括相机的内参矩阵（calib.M）和畸变系数（calib.D），还有相机的旋转矩阵（calib.R）和平移向量（calib.T），这四个值共同描述了相机的内外参数。

opts = struct();
% 初始化标定选项，比如图像尺寸、纵横比、内参矩阵是否猜测等
params = st2kv(opts.flags);

初始化标定时所需的参数（如图像尺寸、纵横比等），并创建了相机矩阵 calib.M 和畸变系数 calib.D，作为标定的输入。

[calib.M, calib.D, calib.rms, calib.R, calib.T] = cv.calibrateCamera(...);

使用 OpenCV 的 cv.calibrateCamera 函数进行相机的内外参数标定。输出包括相机矩阵 M、畸变系数 D、旋转矩阵 R 和平移向量 T。

5. 旋转矩阵转换与位姿计算

R = cv.Rodrigues(calib.R{1});
[cR,cT]=cv.solvePnP(cvobjp{1},cvimgp{1},calib.M);

使用 cv.Rodrigues 将旋转向量转换为旋转矩阵，并使用 cv.solvePnP 计算相机的位姿（旋转和位移）。

6. 使用 MATLAB 函数进行相机标定

cameraParams = estimateCameraParameters(imagePoints,objectPoints, ...
                              'ImageSize',imageSize);

使用 MATLAB 的 estimateCameraParameters 函数基于图像点和物体点估算相机的内参数。

[rotationMatrix, translationVector] = extrinsics(...);

extrinsics 函数通过图像点和物体点计算相机的外参（旋转矩阵和位移向量）。

7. 计算图像中的点在世界坐标中的位置

worldPoints1 = pointsToWorld(cameraParams, rotationMatrix, translationVector, box);

使用 pointsToWorld 函数将图像中的特定点转换为实际世界中的坐标。

8. 相机位置和方向的绘制

[orientation, location] = extrinsicsToCameraPose(rotationMatrix, translationVector);
plotCamera('Location', location, 'Orientation', orientation, 'Size', 20);