VINS-Fusion-GNSS松耦合原理

这节比较简单，将结合代码和公式进行讲解，主要是因为反推出来的公式含义和实际求解出来的结果含义会有一定歧义，我看到很多人对这部分的 GPS-IMU外参比较有疑惑，所以这里就按流程来梳理一遍

int main(int argc, char **argv)
{
    ros::init(argc, argv, "globalEstimator");
    ros::NodeHandle n("~");

    global_path = &globalEstimator.global_path;

    ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
    pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
    pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
    pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
    ros::spin();
    return 0;
}

上面的 GPS_callback 回调函数获取GPS数据 ，vio_callback获取VIO位姿数据

void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
    //printf("gps_callback! \n");
    m_buf.lock();
    gpsQueue.push(GPS_msg);
    m_buf.unlock();
}

GPS的数据都统一放到gpsQueue中，后续会在vio_callback中处理

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    //printf("vio_callback! \n");
    double t = pose_msg->header.stamp.toSec();
    last_vio_t = t;
    Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
    Eigen::Quaterniond vio_q;
    vio_q.w() = pose_msg->pose.pose.orientation.w;
    vio_q.x() = pose_msg->pose.pose.orientation.x;
    vio_q.y() = pose_msg->pose.pose.orientation.y;
    vio_q.z() = pose_msg->pose.pose.orientation.z;
    globalEstimator.inputOdom(t, vio_t, vio_q);
    m_buf.lock();
    while(!gpsQueue.empty())
    {
        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_t = GPS_msg->header.stamp.toSec();
        printf("vio t: %f, gps t: %f \n", t, gps_t);
        // 10ms sync tolerance
        if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
        {
            //printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            //int numSats = GPS_msg->status.service;
            double pos_accuracy = GPS_msg->position_covariance[0];
            if(pos_accuracy <= 0)
                pos_accuracy = 1;
            //printf("receive covariance %lf \n", pos_accuracy);
            //if(GPS_msg->status.status > 8)
                globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
            gpsQueue.pop();
            break;
        }
        else if(gps_t < t - 0.01)
            gpsQueue.pop();
        else if(gps_t > t + 0.01)
            break;
    }
    后面有一些无关紧要的就不放出来了

这上面最重要的就是

 globalEstimator.inputOdom(t, vio_t, vio_q);

和

globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);

先看inputOdom
输入了时间和PQ，这里的位姿可以定义为 $T_{I_{w}i}$ ，意思就是第 $i$ 帧VIO到IMU的世界点下的变换
下面有一个变量WGPS_T_WVIO，这个变量为 $T_{G{I}_w}$ ，按照字面含义就是IMU的世界系到GPS的世界系的变换
注意： $T_{G{I}_w}$求解的结果实际含义不是这样的，先按这个意思往下理解，后面讲到就会顺了
这里的 $globalPose$ 按照含义就是变换到GPS坐标系下的第 $i$ 帧VIO ，$T_{Gi}=T_{G{I}_w}{T}_{I_{w}i}$
$T_{G{I}_w}$ 的初始值是 $I$，所以在开始的时候这里实质是没有变化的

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
	mPoseMap.lock();
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    					     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;


    Eigen::Quaterniond globalQ;
    globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
    Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
    vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
                              globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
    globalPoseMap[t] = globalPose;

这里就是把GPS的经纬度通过第三方工具转换成笛卡尔世界坐标，并且把GPS的第一帧作为起始帧，即 $I$，这点很重要，后面理解会用到

void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
	double xyz[3];
	GPS2XYZ(latitude, longitude, altitude, xyz);
	vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
    //printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
	GPSPositionMap[t] = tmp;
    newGPS = true;

}

获取完数据就来看看残差的构建，残差约束有两个，分别为相对位姿约束和绝对位姿约束

相对位姿约束 RelativeRTError

直接看残差构建代码

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 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];

		T i_q_w[4];
		QuaternionInverse(w_q_i, i_q_w);

		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);

		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);

		T q_i_j[4];
		ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);

		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;

};

说明一下变量，这些变量是 globalPoseMap 的值，按照理论含义就系 $T_{Gi}$

bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals)

而这些变量，是 $T_{ij}$，这个变量在穿进来前是通过 $T_{I_{w}i}$ 、$T_{I_{w}j}$在外部计算得到的

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) 

这句话就是获得 $T_{Gi}$ 和 $T_{Gj}$以 $i$ 帧为起始的到 $j$ 的相对位置变化大小，旋转残差同理

ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);

整体残差为，后面还会除一下置信度，整体含义就是 $I_w$ 系下的 $T_{ij}$ 等于 $G_w$ 系下的 $T_{ij}$，根据这个 $T_{Gi}$ 构造克制，由于前面的 $T_{G{I}_w}$ 为单位阵 $I$ ，所以这里其实这么一看感觉也没约束到什么东西，就好比自己约束自己一样，所以后面还需要一个残差约束

		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);
		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);

绝对位姿约束 TError

这个约束更简单

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
	{
		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;

};

这个变量是globalPoseMap，即刚刚的 $T_{Gi}$，时刻记着此时的 $T_{G{I}_w}=I$，所以这里的 $T_{Gi}$ 本质上等于 $T_{I_{w}i}$

Create(const double t_x, const double t_y, const double t_z, const double var)

这里的 $tj$ 就是 $T_{Gg}$ ，这里是相对于起始点的绝对位移，这个 $T_{Gg}$ 的起始帧定义为单位阵

bool operator()(const T* tj, T* residuals)

上面的代码的约束公式如下
$\Delta t=t_{Gg}-t_{Gi}$

理解开始了

按照这个绝对位姿的约束，由于 $t_{Gg}$ 的起始帧为 $I$, $t_{Gg}$ 本质上等于 $t_{I_{w}i}$，而 $t_{I_{w}i}$ 的起始帧也是 $I$，所以这里相当于是默认了GPS的世界系与IMU的世界系相等，就等于通过数值让它们强行在一个起始点，然后这个残差里面的优化变量是 $t_{Gi}$ ，经过优化其实就等于是在优化第 $i$ 帧的VIO位姿，优化完后获得新的 $t_{Gi}$，这个时候第i帧的位姿比较靠近GPS的轨迹，由于GPS存在抖动，我感觉这个 $i$ 的值是靠近抖动的 $g$ 的值

然后根据公式 $T_{Gi}{T}_{i{I}_w}$ 获得更新后的外参变换 $T_{G{I}_w}$

更新后的 $T_{Gi}$ 相当于是靠近 $g$ 的值，然后根据公式 $T_{Gi}{T}_{i{I}_w}$，这里其实就等于是强行将第 $i$ 帧的值当作同一个地方，但是实际由于$T_{Gi}$ 的移动它们不是同一个地方，虽然前面默认的 $G$世界系和 $I_w$ 是相同的，但是这个公式中把 $T_{i{I}_w}$ 整一段轨迹往优化后 $T_{Gi}$ 的 $i$ 点去靠拢，那么此时 $G$ 和 $I_w$ 就会产生偏移了，所以通过通过可以算出 $T_{G{I}_w}$

但是！这个偏移按照确切含义来看应该是描述为 $Tgi$ 更为合适，因为是强迫使得 $i$ 点移动导致的 $I_w$ 移动，但是$G$ 和 $I_w$从数值上就是单位阵 $I$，这一点是不会变的，所以这个偏移应该为第$i$ 帧下VIO的位姿到 $g$ 的变换，$i$ 和 $g$ 表示同一时刻的值，所以这个是一个局部变换，因为GPS自身就存在很大的抖动，然后后面这个系统还会把这个大抖动乘上去重复上面的步骤，其实就是去靠近这个抖动值

在下一次进行时乘上 $T_{G{I}_w}$ 其实就是把当前时刻的VIO值的世界点 $Iw$ 偏移一个上一时刻位姿的抖动值，所以这个 $T_{G{I}_w}$ 本质上就是一个VIO到g的抖动值，这也是为什么这个系统输出的 $T_{G{I}_w}$ 会一直在变且变幅很大的原因，因为这玩意从本质上就不是IMU的世界系到GPS世界系的变换，不然的话应该是不动才对的

但是最后的输出位姿又很稳定，我觉得是因为根据公式 $T_{Gi}=T_{G{I}_w}{T}_{I_{w}i}$ ，其实是在变动它的世界坐标，所以抖动的是它的世界坐标，就是改变了一下参考系，但是值该是怎样还是怎样

由于这个 $T_{G{I}_w}$ 的存在，在第二次进来优化时相对约束就不会是自己约束自己了，因为此时 $G$ 系下的 $T_{ij}$ 就会由于上一次的绝对约束产生变化，并且，如果VIO的轨迹存在尺度问题的话，这个尺度信息会包含在 $T_{G{I}_w}$ 里面，所以第二次优化时相对约束中就会带有尺度信息进行优化，虽然里面也包含了GPS的抖动误差，因为会把世界系变换到真实尺度下面去，毕竟相对约束里面要的是相对值，世界系去到哪里都无所谓

假设这个 $T_{Gg}$ 每一帧都存在上下抖动，那么 $T_{G{I}_w}$ 也会上下抖动，集束优化的话最后就是输出在滑窗内，$T_{G{I}_w}$离各个 $g$ 都比较近的抖动值，然后这个值可以理解为抖动均值之类的，但是肯定不是均值，此时这个偏移量$T_{Gi}$ 是一个离各个 $g$ 点都较近的位置，但绝对不是重合，此时把这个 $T_{Gi}$ 的 $i$ 的位置当作GPS的世界系的位置，原先的 $i$ 的位置当作VIO的世界系，然后一作差就得出两个世界系的偏移 $T_{G{I}_w}$，这个差值本质上其实是当前 $i$ 时刻的VIO的位置离优化值 $g$ 点的偏移，这个 $g$ 和 $i$ 是同一时间的，但是由于他们的世界系在一开始的时候就定义为单位阵 $I$ ，即相同世界系，那么我感觉这个值其实就是一个稳定值的VIO值到一个抖动的GPS均值的变化，然后把这个值当作GPS到VIO的外参

这样优化的方式在前期 $T_{G{I}_w}$ 里面包含更多的可能会是尺度信息，后期的话才是当前 $i$ 时刻的VIO的位置离优化值 $g$ 点的偏移

这样理解个人感觉是通的，如若有错误，欢迎指正，评论区我们也可以一起讨论一下