Java opencv3d计算姿态_OpenCV相机标定和姿态更新

原帖地址：

这一节我们首先介绍下计算机视觉领域中常见的三个坐标系：图像坐标系，相机坐标系，世界坐标系以及他们之间的关系，然后介绍如何使用张正友相机标定法标定相机。

图像坐标系：

理想的图像坐标系原点O1和真实的O0有一定的偏差，由此我们建立了等式(1)和(2)，可以用矩阵形式(3)表示。

相机坐标系(C)和世界坐标系(W)：

通过相机与图像的投影关系，我们得到了等式(4)和等式(5)，可以用矩阵形式(6)表示。

我们又知道相机坐标系和世界坐标的关系可以用等式(7)表示：

由等式(3)，等式(6)和等式(7)我们可以推导出图像坐标系和世界坐标系的关系：

其中M1称为相机的内参矩阵，包含内参(fx，fy，u0，v0)。M2称为相机的外参矩阵，包含外参(R:旋转矩阵，T:平移矩阵)。

众所周知，相机镜头存在一些畸变，主要是径向畸变(下图dr)，也包括切向畸变(下图dt)等。

上图右侧等式中，k1,k2,k3,k4,k5,k6为径向畸变，p1,p2为切向畸变。在OpenCV中我们使用张正友相机标定法通过10幅不同角度的棋盘图像来标定相机获得相机内参和畸变系数。函数为calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs，flag)

objectPoints: 一组世界坐标系中的3D

imagePoints: 超过10张图片的角点集合

imageSize: 每张图片的大小

cameraMatrix: 内参矩阵

distCoeffs: 畸变矩阵(默认获得5个即便参数k1,k2,p1,p2,k3，可修改)

rvecs: 外参：旋转向量

tvecs: 外参：平移向量

flag: 标定时的一些选项：

CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话，将初始化(u0,v0)图像的中心点，使用最小二乘法估算出fx，fy。

CV_CALIB_FIX_PRINCIPAL_POINT：在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置，光轴点将保持在中心或者某个输入的值。

CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。

CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数(p1,p2)为零。

CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变。

CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。

首先我们打开摄像头并按下'g'键开始标定：

VideoCapture cap(1);

cap.set(CV_CAP_PROP_FRAME_WIDTH,640);

cap.set(CV_CAP_PROP_FRAME_HEIGHT,480);if(!cap.isOpened()){

std::cout<

exit(-1);

}

namedWindow("Calibration");

std::cout<

{ mode =CAPTURING;

}

按下空格键(SPACE)后使用findChessboardCorners函数在当前帧寻找是否存在可用于标定的