VINS 初始化 ceres计算重投影误差

详细分析一下VINS在初始化的时候对计算出的相机位姿和特征点的3D坐标用ceres进行最小化重投影误差的操作。

参考资料：

【1】https://www.jianshu.com/p/e5b03cf22c80

【2】http://www.ceres-solver.org/index.html

【3】http://www.cnblogs.com/decade-dnbc66/p/5347088.html

推荐先看一下【1】中开头的小例子。

VINS的这部分代码在“initial_sfm.cpp” “initial_sfm.h”中。

（1）构建代价函数。不按照代码的顺序来，先分析最重要的部分：定义重投影误差的costfunction。VINS定义了一个名为ReprojectionError3D的仿函数，即在结构体内对“()”进行了重载，也就是说ReprojectionError3D()可以当做一个函数去使用，而且可以方便的传递参数，具体细节参考【3】。在这个结构体内，

        //"operator()" is ONE name
bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const
	{
	T p[3];
        //Pc=Rw2c*Pw+Tw2c	
        ceres::QuaternionRotatePoint(camera_R, point, p); //Rotates a point pt by a quaternion q, pose is the result
	p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; //
	T xp = p[0] / p[2];
    	T yp = p[1] / p[2];
    	residuals[0] = xp - T(observed_u);
    	residuals[1] = yp - T(observed_v);
    	return true;
	}

是对重投影误差的定义，其中的传入的参数前3个是优化变量，是有初始参数的，在 AddResidualBlock 时传入。最后一个是要求的残差项，在这个地方就是先将世界坐标系中的特征点转换到相机坐标系中，然后再投影到归一化平面中，再与测得的归一化平面中的点求坐标的误差。

ReprojectionError3D(double observed_u, double observed_v)
		:observed_u(observed_u), observed_v(observed_v)
		{}

这里就是上面说的，方便传递参数，将观测点在归一化平面上的坐标（去畸变）传入来计算残差。考虑到在优化的时候，观测值是不进行优化的，所以这个观测值应该尽可能的准确，那么

• 光流跟踪
• 畸变校正及投影（相机的参数）

的质量就很重要了。

（2）构建优化问题。首先定义类型为ceres::CostFunction*的代价函数，并使用上面构建的结构体：

ceres::CostFunction* cost_function = ReprojectionError3D::Create(sfm_f[i].observation[j].second.x(),
		sfm_f[i].observation[j].second.y());
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], sfm_f[i].position);

在problem.AddResidualBlock()中，后面的参数都是优化变量，传入前面的cost_function中。这是一个自动求导的代价函数，有固定的构造方法。参考【1】即可。

在new ceres::AutoDiffCostFunction<>中指定了代价函数的来源（结构体）、残差维度、以及输入的各个优化变量的维度。

（3）配置求解

这一部分比较简单而且也不难，参考【1】即可，然后在最后将优化后的变量进行赋值即可。

（4）另外在VINS中：

problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
problem.AddParameterBlock(c_translation[i], 3);

【2】中有说明：The user has the option of explicitly adding the parameter blocks using AddParameterBlock. This causes additional correctness checking; however, AddResidualBlock implicitly adds the parameter blocks if they are not present, so calling AddParameterBlock explicitly is not required.

也就是说没有特殊需求的话，这一部分是没有必要的，在AddResidualBlock的时候就会自动将优化变量添加为参数块，但是就VINS而言，对于四元数的运算（plus，Jacobian）需要四元数的运算方法，而且作者固定了首帧和末帧的相机姿态不变，所以单独添加了参数块，而且参数块重复添加会被忽略，所以提前添加不影响后面的AddResidualBlock。