Kalibr源码学习(一): 重投影误差

Kalibr源码学习(一): 重投影误差

给自己挖一个大坑, 从标定结果来学习Kalibr的标定源码, 这里基本以KB模型为例, 也就是标定时, kalibr的模型设定为 --model pinhole-equi , 这里以重投影误差开始,希望能坚持;

重投影误差标定结果

首先以重投影误差的txt文档的结果开始,

可以看到上面的标定结果中, 重投影误差显示为 reprojection error: [-0.000002, -0.000000] +- [0.535272, 0.572115]
这里的两项对应的分别是 reprojection error: [x方向的重投影误差均值, y方向的重投影误差均值] +- [x方向重投影误差方差, y方向重投影误差方差]

其对应的代码片为:

//获取当前检测的角点,重投影坐标, 重投影误差
corners, reprojs, rerrs = getReprojectionErrors(cself, cidx) 
 if len(rerrs)>0:
 //获取重投影误差的均值me和方差se
 me, se = getReprojectionErrorStatistics(rerrs)
 //打印输出到txt文档中
 print >> dest, "\t reprojection error: [%f, %f] +- [%f, %f]" % (me[0], me[1], se[0], se[1])
 print >> dest

这里可以再说以下计算均值和方差的python代码:

在这里插入代码片
```def getReprojectionErrorStatistics(all_rerrs):
    #传入值是所有的误差 errors, 这里代码里面举出的例子usage是错误的,传入的应该是error,不是重投影
    if not len(all_rerrs)>0:
        raise RuntimeError("rerrs has invalid dimension")

    gc.disable() #append speed up
    rerr_matrix=list();
    for view_id, view_rerrs in enumerate(all_rerrs):
        if view_rerrs is not None: #if cam sees target in this view
            for rerr in view_rerrs:
                if not (rerr==np.array([None,None])).all(): #if corner was observed
                    rerr_matrix.append(rerr) #将误差值统一放入到rerr_matrix中
    rerr_matrix = np.array(rerr_matrix)
    gc.enable()
    
    mean = np.mean(rerr_matrix, 0, dtype=np.float)#计算均值
    std = np.std(rerr_matrix, 0, dtype=np.float)#计算方差
 
    return mean, std

重投影误差图像显示

这个图对应的代码为:

def plotAllReprojectionErrors(cself, cam_id, fno=1, noShow=False, clearFigure=True, title=""):
    # left: observations and projecitons
    # right: scatterplot of reprojection errors
    all_corners, reprojections, rerrs_xy = getReprojectionErrors(cself, cam_id)
    resolution = (cself.cameras[cam_id].geometry.projection().ru(), cself.cameras[cam_id].geometry.projection().rv())

    #创建图像
    values = np.arange(len(cself.views))/np.double(len(cself.views))
    cmap = pl.cm.jet(values,alpha=0.5)
    
    #子图一用来画重投影后的坐标 省略
    a=pl.subplot(121)

    #子图二用来画重投影误差
    sub = pl.subplot(122)
    for view_id, rerrs in enumerate(rerrs_xy):
        if rerrs is not None: #if this camerea sees the target in this view
            color = cmap[view_id,:]
            pl.plot(rerrs[:,0], rerrs[:,1], 'x', lw=3, mew=3, color=color)
   # 主要是这里,x代表的是重投影的x误差, y表示的是重投影的y值误差, 以 error_x 为横坐标, 以error_y为纵坐标,画图
    pl.axis('equal')
    pl.grid('on')
    pl.xlabel('error x (pix)')
    pl.ylabel('error y (pix)')

    #省略...

这里重点说一下获取重投影误差的函数, 经过层层调用, 最后计算出的其实是将三维点投影到图像上

//将传入的三维点投影到图像上
template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_K>
bool PinholeProjection<DISTORTION_T>::euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_K> & outKeypointConst) const {
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(Eigen::MatrixBase<DERIVED_P>, 3);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(Eigen::MatrixBase<DERIVED_K>, 2);
  Eigen::MatrixBase<DERIVED_K> & outKeypoint = const_cast<Eigen::MatrixBase<DERIVED_K> &>(outKeypointConst);
   //shinan note: 转换到归一化相机平面 --> 加畸变 -->转换到像素平面
  outKeypoint.derived().resize(2);
  double rz = 1.0 / p[2];
  outKeypoint[0] = p[0] * rz;
  outKeypoint[1] = p[1] * rz;
   //加畸变
  _distortion.distort(outKeypoint);

  outKeypoint[0] = _fu * outKeypoint[0] + _cu;
  outKeypoint[1] = _fv * outKeypoint[1] + _cv;

  return isValid(outKeypoint) && p[2] > 0;
}

其中加畸变函数如下:

//将传入的归一化相机坐标系的三维点按照公式加畸变
template<typename DERIVED_Y>
void EquidistantDistortion::distort(const Eigen::MatrixBase<DERIVED_Y> & yconst) const {
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(Eigen::MatrixBase<DERIVED_Y>, 2);
  Eigen::MatrixBase<DERIVED_Y> & y = const_cast<Eigen::MatrixBase<DERIVED_Y> &>(yconst);
  y.derived().resize(2);
  double r, theta, theta2, theta4, theta6, theta8, thetad, scaling;
  r = sqrt(y[0] * y[0] + y[1] * y[1]);
  theta = atan(r);
  theta2 = theta * theta;
  theta4 = theta2 * theta2;
  theta6 = theta4 * theta2;
  theta8 = theta4 * theta4;
  thetad = theta* (1 + _k1 * theta2 + _k2 * theta4 + _k3 * theta6 + _k4 * theta8);

  scaling = (r > 1e-8) ? thetad / r : 1.0;
  y[0] *= scaling;
  y[1] *= scaling;
}

总结

这里有一点就是这里的重投影误差的均值很小, 主要的原因可以从上面的彩色图中看出, 重投影误差有正有负,均值会出现互相抵消的状态; 总体来讲是以(-0.000002, -0.000000)为中心, 方差分别是0.535272, 0.572115的正态分布;

所以说,均值不可信,方差大概率更靠谱一些: 如下面标定的很好的结果中:
reprojection error: [0.000000, 0.000001] ± [0.115878, 0.107625]
方差仅为0.1, 其对应的重投影误差的值也都偏小:

根据3*sigma原则, 要想让大部分的重投影误差小于1个像素, 那么其所对应的方差应该是小于0.33.