VINS全局优化残差函数分析

VINS全局优化残差函数分析

VINS的全局优化包含有与GPS低耦合的优化．即GPS作为一个观测(边)加入到整体的优化中去．
其优化的依据是：

1. 帧与帧之间相对平移和旋转在局部是准确的
2. 含有gps的帧其位置应与gps重合．

第一个约束对应的是RelativeRTError,第二个约束对应的是TError
总体的优化由于包括姿态，因此为非线性优化．
参数是：每一帧的位置和姿态
观测是：某帧的gps，以及每一帧的相对位置和姿态
整体的抽象优化效果可解释为：当一帧GPS带来了残差时，把它在尽量保证多帧相对位置的情况下消化掉．即均摊掉．

但要注意的是，gps位置的准确与多帧相对姿态的切合(多帧的拼图效果)是成负相关的．即gps越准，拼图越差．
其中相关可权衡的参数有：

gps权
相对位资的权
迭代次数

---

其分析写在了注释部分．

struct TError
{
	TError(double t_x, double t_y, double t_z, double var)
				  :t_x(t_x), t_y(t_y), t_z(t_z), var(var){}

	template <typename T>
	bool operator()(const T* tj, T* residuals) const
	{
		// GPS残差
		residuals[0] = (tj[0] - T(t_x)) / T(var);
		residuals[1] = (tj[1] - T(t_y)) / T(var);
		residuals[2] = (tj[2] - T(t_z)) / T(var);
		return true;
	}

	static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var) 
	{
	  return (new ceres::AutoDiffCostFunction<
	          TError, 3, 3>(
	          	new TError(t_x, t_y, t_z, var)));
	}

	double t_x, t_y, t_z, var;

};
struct RelativeRTError
{
	RelativeRTError(double t_x, double t_y, double t_z, 
					double q_w, double q_x, double q_y, double q_z,
					double t_var, double q_var)
				  :t_x(t_x), t_y(t_y), t_z(t_z), 
				   q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
				   t_var(t_var), q_var(q_var){}

	template <typename T>
	bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
	{
		// t_w_ij是在世界坐标系(vo原点)下的相对位移
		T t_w_ij[3];
		t_w_ij[0] = tj[0] - ti[0];
		t_w_ij[1] = tj[1] - ti[1];
		t_w_ij[2] = tj[2] - ti[2];

		// i_q_w是第i个点到世界坐标系下的四元数　= w_q_i的逆
		T i_q_w[4];
		QuaternionInverse(w_q_i, i_q_w);

		// t_i_ij是第i个点到第j个点在(相对于)第i个点坐标系下的相对位移
		T t_i_ij[3];
		ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);

		residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
		residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
		residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);

		// relative_q 是第i帧到第j帧相对于第i帧的相对旋转 (观测值)
		T relative_q[4];
		relative_q[0] = T(q_w);
		relative_q[1] = T(q_x);
		relative_q[2] = T(q_y);
		relative_q[3] = T(q_z);

		// q_i_j是第i帧到第j帧相对于第i帧的相对旋转       (估计值)
		T q_i_j[4];
		ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);


		// 衡量两个四元数相近的公式：q1^-1*q2越接近[0,0,0,1]越相近
		T relative_q_inv[4];
		QuaternionInverse(relative_q, relative_q_inv);

		T error_q[4];
		ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); 

		residuals[3] = T(2) * error_q[1] / T(q_var);
		residuals[4] = T(2) * error_q[2] / T(q_var);
		residuals[5] = T(2) * error_q[3] / T(q_var);

		return true;
	}

	static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
									   const double q_w, const double q_x, const double q_y, const double q_z,
									   const double t_var, const double q_var) 
	{
	  return (new ceres::AutoDiffCostFunction<
	          RelativeRTError, 6, 4, 3, 4, 3>(
	          	new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));
	}

	double t_x, t_y, t_z, t_norm;
	double q_w, q_x, q_y, q_z;
	double t_var, q_var;

};