WoodScape数据集之相机标定参数

WoodScape数据集中以json格式提供了每帧图像对应的相机参数，json文件在calibration文件夹下，名称与图像名一致；

相机标定参数格式：

{
  "extrinsic": {
    "quaternion": [
      0.5946970238045494,
      -0.5837953694518585,
      0.39063952590941586,
      -0.3910488170060691
    ],
    "translation": [
      3.7484,
      0.0,
      0.6577999999999999
    ]
  },
  "intrinsic": {
    "aspect_ratio": 1.0,
    "cx_offset": 3.942,
    "cy_offset": -3.093,
    "height": 966.0,
    "k1": 339.749,
    "k2": -31.988,
    "k3": 48.275,
    "k4": -7.201,
    "model": "radial_poly",
    "poly_order": 4,
    "width": 1280.0
  },
  "name": "FV"
}

其中：

1. 车体坐标系：ISO 8855标准
   
   X 轴平行于车辆的 heading 朝前；
   Y 轴垂直于 X 轴朝左；
   Z 轴垂直于 X, Y 轴朝向车辆的上方;
   坐标系原点位于车辆后轴中心点下方的地面上；

2. 相机坐标系
   与opencv中所规定的相机坐标系一致，遵循右手定则；

3. 旋转和平移变换关系为从相机坐标系到车体坐标系；
   json文件以四元数形式表征旋转矩阵；平移矩阵的单位为m；
   四元数转旋转矩阵公式：
   

void getRotation(double *Quaternion, double *rt_mat)
{
	//q:[x, y, z, w]
	rt_mat[0] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[2] * Quaternion[2]);
	rt_mat[1] = 2 * Quaternion[0] * Quaternion[1] - 2 * Quaternion[2] * Quaternion[3];
	rt_mat[2] = 2 * Quaternion[0] * Quaternion[2] + 2 * Quaternion[1] * Quaternion[3];
	rt_mat[3] = 2 * Quaternion[0] * Quaternion[1] + 2 * Quaternion[2] * Quaternion[3];
	rt_mat[4] = 1 - 2 * (Quaternion[0] * Quaternion[0]) - 2 * (Quaternion[2] * Quaternion[2]);
	rt_mat[5] = 2 * Quaternion[1] * Quaternion[2] - 2 * Quaternion[0] * Quaternion[3];
	rt_mat[6] = 2 * Quaternion[0] * Quaternion[2] - 2 * Quaternion[1] * Quaternion[3];
	rt_mat[7] = 2 * Quaternion[1] * Quaternion[2] + 2 * Quaternion[0] * Quaternion[3];
	rt_mat[8] = 1 - 2 * (Quaternion[0] * Quaternion[0]) - 2 * (Quaternion[1] * Quaternion[1]);
}

相机坐标系到车体坐标系的变换矩阵为：

	cv::Mat Trans = (cv::Mat_<double>(4, 4) << R[0], R[1], R[2],T[0],
		R[3], R[4], R[5], T[1],
		R[6], R[7], R[8], T[2],
		0, 0, 0, 1);

车体坐标系到相机坐标系的变换矩阵为：

cv::Mat invT = Trans.inv();

4. 鱼眼相机模型
   相机坐标系下的三维点(X, Y, Z)，对应点在鱼眼图像上的坐标（u，v）计算：

chi = sqrt( X ** 2 + Y ** 2)
theta = arctan2( chi, Z ) = pi / 2 - arctan2( Z, chi )
rho(theta) = k1 * theta + k2 * theta ** 2 + k3 * theta ** 3 + k4 * theta ** 4
rho = rho(theta)
u’ = rho * X / chi if chi != 0 else 0
v’ = rho * Y / chi if chi != 0 else 0
u = u’ + cx + width / 2 - 0.5
v = v’ * aspect_ratio + cy + height / 2 - 0.5