天天看點
傳回

淺談VINS中的global fusion節點淺談VINS中的global fusion節點

轉載自：https://zhuanlan.zhihu.com/p/75492883

淺談VINS中的global fusion節點

淺談VINS中的global fusion節點淺談VINS中的global fusion節點

劉弟弟

資深小朋友

VINS-Fusion開源已經很長時間了，但是一直也沒時間看。最近需要用到gps與VO融合，就先學習了global fusion節點。global fusion節點中資料融合的思路非常巧妙，令人贊歎，并且代碼比較簡單，很容易讀懂。下面是一些學習源碼的體會與大家分享，如有不當之處，歡迎批評指正～

1.整體流程分析

首先 global Estimator 節點訂閱兩個節點：

  • 1.VIO輸出的 nav_msgs::Odometry 類型消息，這個定位資訊包含了VIO的位置和姿态，其坐标系原點位于VIO的第一幀處。 
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
  • 2.GPS輸出的sensor_msgs::NavSatFixConstPtr 類型消息，這個是全局定位資訊，用經緯度來表示，其坐标原點位于該GPS坐标系下定義的0經度0緯度處。 
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);

我們再分别看其回調函數(這裡貼出來的代碼隻保留了主幹部分)

先看GPS回調函數，很簡單，隻是把GPS消息存儲到了隊列裡面

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

VIO回調函數，請看注釋：

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    double t = pose_msg->header.stamp.toSec();
    last_vio_t = t;
    // 擷取VIO輸出的位置(三維向量)，姿态(四元數)
    Eigen::Vector3d vio_t;
    Eigen::Quaterniond vio_q;
    ......
    /// 位姿傳入global Estimator中
    globalEstimator.inputOdom(t, vio_t, vio_q);
    m_buf.lock();
    // 尋找與VIO時間戳相對應的GPS消息
    // 細心的讀者可能會疑惑，這裡需不需要對GPS和VIO進行硬體上的時間戳同步呢？
    // 這個問題請看總結與讨論
    while(!gpsQueue.empty())
    {
        // 擷取最老的GPS資料和其時間
        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_t = GPS_msg->header.stamp.toSec();
        // 10ms sync tolerance
        // +- 10ms的時間偏差
        if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
        {   /// gps的經緯度，海拔高度
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            // gps 資料的方差
            double pos_accuracy = GPS_msg->position_covariance[0];
            if(pos_accuracy <= 0)
                pos_accuracy = 1;
            //printf("receive covariance %lf \n", pos_accuracy);
            /// GPS_msg->status.status 這個數字代表了GPS的狀态(固定解，浮點解等)
            /// 具體可以谷歌
            // if(GPS_msg->status.status > 8)
            // 向globalEstimator中輸入GPS資料
            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;
    }
    m_buf.unlock();
    ......
    // 廣播軌迹（略）......
    pub_global_odometry.publish(odometry);
    pub_global_path.publish(*global_path);
    publish_car_model(t, global_t, global_q);
    // 位姿寫入文本檔案（略）......
}

可以看出，global Fusion的優化政策是收到一幀VIO資料，就尋找相應的GPS資料來進行優化。我們下面主要來看一下globalEstimator中的inputOdom()和inputGPS()這兩個函數。

首先看下 inputGPS():

void GlobalOptimization::inputGPS(double t, double latitude, 
                                  double longitude, 
                                  double altitude, 
                                  double posAccuracy)
{
	double xyz[3];
        // 因為經緯度表示的是地球上的坐标，而地球是一個球形，
        // 需要首先把經緯度轉化到平面坐标系上
        // 值得一提的是，GPS2XYZ()并非把經緯度轉化到世界坐标系下(以0經度，0緯度為原點)，
        // 而是以第一幀GPS資料為坐标原點，這一點需要額外注意
	GPS2XYZ(latitude, longitude, altitude, xyz);
        // 存入經緯度計算出的平面坐标，存入GPSPositionMap中
	vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
	GPSPositionMap[t] = tmp;
    newGPS = true;
}

再看inputOdom()：

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
    mPoseMap.lock();
    // 把vio直接輸出的位姿存入 localPoseMap 中
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    			     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;
    Eigen::Quaterniond globalQ;
    /// 把VIO轉換到GPS坐标系下，準确的說是轉換到以第一幀GPS為原點的坐标系下
    /// 轉換之後的位姿插入到globalPoseMap 中
    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;
    lastP = globalP;
    lastQ = globalQ;
    // 把最新的全局姿态插入軌迹當中(過程略)
    ......
    global_path.poses.push_back(pose_stamped);
    mPoseMap.unlock();
}

現在兩種資料都收到以後，萬事俱備，我們看一下 void GlobalOptimization::optimize()這個函數：

這個函數開了一個線程來做優化(這個代碼太長了，貼一部分把)：

  1. 首先使用ceres建構最小二乘問題，這個沒啥可說的
  2. 狀态量賦初值，添加參數塊。可以看出來，疊代的初始值是globalPoseMap中的值，也就是VIO轉換到GPS坐标系下的值。 
int length = localPoseMap.size();
            // w^t_i   w^q_i
            double t_array[length][3];
            double q_array[length][4];
            map<double, vector<double>>::iterator iter;
            iter = globalPoseMap.begin();
            for (int i = 0; i < length; i++, iter++)
            {
                t_array[i][0] = iter->second[0];
                t_array[i][1] = iter->second[1];
                t_array[i][2] = iter->second[2];
                q_array[i][0] = iter->second[3];
                q_array[i][1] = iter->second[4];
                q_array[i][2] = iter->second[5];
                q_array[i][3] = iter->second[6];
                problem.AddParameterBlock(q_array[i], 4, local_parameterization);
                problem.AddParameterBlock(t_array[i], 3);
            }

3.然後添加殘差：

for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++) {
    //vio factor
    // 添加VIO殘差，觀測量是兩幀VIO資料之差，是相對的。而下面的GPS是絕對的
    iterVIONext = iterVIO;
    iterVIONext++;
    if (iterVIONext != localPoseMap.end()) {
        /// 計算兩幀VIO之間的相對差（略）......
        ceres::CostFunction *vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
                                                                    iQj.w(), iQj.x(), iQj.y(), iQj.z(),
                                                                    0.1, 0.01);
        problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i + 1], t_array[i + 1]);
    }
    // gps factor
    // GPS殘差，這個觀測量直接就是GPS的測量資料，
    // 殘差計算的是GPS和優化變量的差，這個是絕對的差。
    double t = iterVIO->first;
    iterGPS = GPSPositionMap.find(t);
    if (iterGPS != GPSPositionMap.end()) {
        ceres::CostFunction *gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
                                                           iterGPS->second[2], iterGPS->second[3]);
        problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
    }

}

優化完成後，再根據優化結果更新姿态就ok啦。為了防止VIO漂移過大，每次優化完成還需要計算一下VIO到GPS坐标系的變換。

2.GPS與VIO融合政策

（知乎這個文章編輯器是真的卡........）

根據上文的分析，我們可以看出整體的優化政策和位姿圖非常相似，因為觀測量是相鄰兩幀VIO之間的差和GPS坐标，是以global Fusion 節點相當于把對應時間戳的GPS位姿和VIO位姿的差，均勻分布到每兩個相鄰的VIO之間。使得整體的誤差和最小化。

3.總結與讨論

1.思考

根據上文中分析的優化政策，global fusion的應用場景應該是GPS頻率較低，VIO頻率較高的系統。fusion 預設釋出頻率位10hz，而現在的GPS可以達到20hz，如果在這種系統上使用，你可能還需要修改下VIO或者GPS頻率。

2.GPS與VIO時間不同步

上文提到了，在多傳感器融合系統中，傳感器往往需要做時鐘同步，那麼global Fusion需要麼？GPS分為為很多種，我們常見的GPS子產品精度較低，十幾米甚至幾十米的誤差，這種情況下，同不同步沒那麼重要了，因為GPS方差太大。另外一種比較常見的是RTK-GPS ，在無遮擋的情況下，室外精度可以達到 2cm之内，輸出頻率可以達到20hz，這種情況下，不同步時間戳會對系統産生影響，如果VIO要和RTK做松耦合，這點還需要注意。

無人機-SLAM-vins 無人機-自己實作VIO 無人機-SLAM-vins-GPS
上一篇: 多傳感器融合詳解0.前言
下一篇: 基于IMX6 Linux 快速啟動

繼續閱讀