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和方差seme, 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 uprerr_matrix=list();for view_id, view_rerrs in enumerate(all_rerrs):if view_rerrs is not None: #if cam sees target in this viewfor rerr in view_rerrs:if not (rerr==np.array([None,None])).all(): #if corner was observedrerr_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 errorsall_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 viewcolor = 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.