Overview
Based on VINS Mono, VINS Fusion adds sensors such as GPS that can obtain global observation information, allowing VINS to use global information to eliminate cumulative errors and thereby reduce closed-loop dependence.
Local sensors (such as cameras, IMUs, lidar, etc.) are widely used in mapping and localization algorithms. Although these sensors can achieve good local positioning and local mapping effects in areas without GPS information, these sensors can only provide local observations, limiting their application scenarios:
The first problem is that local observation data lacks global constraints. Every time we run the algorithm at different locations, we will get positioning and mapping results in different coordinate systems. Therefore, it is difficult to combine these measurement results to form a global effect.
The second problem is that estimation algorithms based on local observations must have cumulative drift. Although loop closure detection can eliminate drift to a certain extent, the algorithm is still difficult to handle for large-scale scenarios with large amounts of data.
Compared with local sensors, global sensors (such as GPS, barometer, magnetometer, etc.) can provide global observations. These sensors use a globally unified coordinate system, and the variance of the output observation data does not increase over time. However, these sensors also have some problems that prevent them from being directly used for precise positioning and mapping. Taking GPS as an example, GPS data is usually not smooth, has noise, and has a low output rate.
Therefore, a simple and intuitive idea is to combine local sensors and global sensors to achieve the effect of local accurate global zero drift. This is the core content of the VINS Fusion paper.
Fusion principle
The algorithm architecture of VINS Fusion is shown in the figure:
The figure below expresses the constraints between observations and states in the form of a factor graph:
the circles are state quantities (such as pose, speed, offset, etc.), and the yellow squares are local observations. Constraints, that is, relative pose transformations from VO/VIO; squares in other colors are constraints from global observations, such as purple squares from GPS.
The construction of local constraints (residuals) refers to the vins mono paper, which calculates the pose residuals between two adjacent frames. Only the global constraints brought by GPS are discussed here. The first step is of course to convert the GPS coordinates, that is, the longitude and latitude into the geodetic coordinate system. It is customary to choose a right-handed coordinate system, with the positive directions of the x, y, and z axes being the north east earth or northeast sky directions respectively. Next, the residuals of the global constraints can be calculated:
where z is the GPS measurement value, X is the state prediction, and the h equation is the observation equation. X can be calculated based on the state of the previous moment and the pose transformation between the current moment and the previous moment. The method specific to this article is to use VIO to obtain the relative pose dX between the current moment and the previous moment, and add it to the pose at the previous moment X(i-1) to obtain the pose Xi at the current moment. It should be noted that the X here takes the first frame as the origin. By observing the equation h, the coordinates of the current state are converted to the GPS coordinate system. This creates a global constraint.
The subsequent optimization is handed over to the BA optimizer for iterative optimization, and vins fusion uses ceres as the optimizer.
Code
1. GPS and VIO data input
One thing that needs to be made clear is that the output of VIO is the cumulative pose transformation relative to the first frame, that is, the first frame is the origin . VINS Fusion receives the local pose transformation (relative to the first frame) output by vio and the longitude and latitude coordinates output by gps for fusion. The interface that accepts data input is in the global_fusion/src/globaOptNode.cpp file. The key code for the interface definition is as follows:
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
m_buf.lock();
gpsQueue.push(GPS_msg); // 每次接收到的gps消息添加到gpsQueue队列中
m_buf.unlock();
}
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
double t = pose_msg->header.stamp.toSec();
last_vio_t = t;
Eigen::Vector3d vio_t; // 平移矩阵，转换的代码省略
Eigen::Quaterniond vio_q; // 旋转四元数，转换的代码省略
globalEstimator.inputOdom(t, vio_t, vio_q); // 将时间，平移，四元数作为预测添加进globalEstimator
m_buf.lock();
while(!gpsQueue.empty())
{
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
// 找到和vio里程计数据相差在10ms以内的gps数据，作为匹配数据
if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
{
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
double pos_accuracy = GPS_msg->position_covariance[0];
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy); // 关键，将gps数据作为观测输入到globalEstimator中
gpsQueue.pop(); // 满足条件的gps信息弹出
break;
}
else if(gps_t < t - 0.01)
gpsQueue.pop(); // 将过时的gps信息弹出
else if(gps_t > t + 0.01)
break; // 说明gps信息是后来的，就不要改动gps队列了，退出
}
m_buf.unlock();
// ...其余省略
}
2. GPS and VIO integration
The code for VINS Fusion to fuse GPS and VIO data is in the global_fusion/src/globalOpt.cpp file, which is described in detail below.
a. Receive GPS data, receive VIO data and transfer to GPS coordinate system
// 接收上面输入的vio数据
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
vector<double> localPose{
OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
// 利用vio的局部坐标进行坐标变换，得到当前帧的全局位姿
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数据
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz); // 将GPS的经纬度转到地面笛卡尔坐标系
vector<double> tmp{
xyz[0], xyz[1], xyz[2], posAccuracy};
GPSPositionMap[t] = tmp;
newGPS = true;
}
The calculation process of converting the local coordinates of VIO data to GPS coordinates appears above. The formula is as follows:
GPS2VIO in this formula appears in the subsequent optimization process. The calculation method is:
b. Fusion optimization
Here is the key code of the fusion. You can see that the process is as follows:
1. Construct t_array and q_array to store translation and rotation variables to facilitate input of optimization equations and removal after optimization.
2. Use RelativeRTError::Create() to construct the constraints between the two frames of VIO, and input the optimization equation.
3. Use TError::Create() to construct the global constraints composed of GPS, and input the optimization equation.
4. Take out the optimized data.
void GlobalOptimization::optimize()
{
while(true)
{
if(newGPS)
{
newGPS = false;
// ceres定义部分略去
// add param
mPoseMap.lock();
int length = localPoseMap.size();
// ********************************************************
// *** 1. 构建t_array, q_array用来存优化变量，等优化后取出 ***
// ********************************************************
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++)
{
// 取出数据部分省略
// 添加了parameterblock
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
// ********************************************************
// ********************* 2. VIO约束 *******************
// ********************************************************
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity(); // 第i帧的变换矩阵
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity(); // 第j帧的变换矩阵
// 取出数据部分省略
Eigen::Matrix4d iTj = wTi.inverse() * wTj; // 第j帧到第i帧的变换矩阵
Eigen::Quaterniond iQj; // 第j帧到第i帧的旋转
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3); // 第j帧到第i帧的平移
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]);
}
// ********************************************************
// ********************* 3. 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]);
}
}
ceres::Solve(options, &problem, &summary);
// ********************************************************
// ******************* 4. 取出优化结果 *****************
// ********************************************************
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
// 取出优化结果
vector<double> globalPose{
t_array[i][0], t_array[i][1], t_array[i][2],
q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
iter->second = globalPose;
if(i == length - 1)
{
Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
// 剩余的存入部分省略，得到WGPS_T_WVIO
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
}
}
updateGlobalPath();
mPoseMap.unlock();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
A key part appears in the above code, namely the calculation of WGPS_T_WVIO. As we know from the previous code, this 4*4 matrix is used to transform the VIO to GPS coordinate system.
The specific program structure in Factors.h is as follows:
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]; //VIO和GPS优化后，世界坐标系 i、j帧的位置增量
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]; //i帧的四元数逆
QuaternionInverse(w_q_i, i_q_w);
//世界坐标系下，i、j帧的位置增量t_w_ij，经i_q_w 转换到i帧的坐标系，与 t_x 求差！
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]; //传入VIO观测的四元数增量
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;
};
Note: Which are the changes between two frames of VIO, and which are the bit postures of the optimized globalPoseMap!
Idea: Project the POS increment optimized by VIO and GPS onto VIO, then make the difference with the POS increment under VIO, and find the residual!
I used to be confused when looking at these codes, but after you become familiar with Ceres and understand the optimization principles of VIO and GPS, it is very simple. The following figure is a schematic diagram: The residual of VIO data and state quantities is defined as: (state at time
j quantity - the state quantity at time i) the increment obtained - vio increment; which means that the position after fusion must
It should coincide with the GPS position as much as possible, and the increment between the two frames should be as equal as possible to the VIO. This is crucial to understanding the coordinate transformation, such processing means that vio's data
There is no restriction on the absolute position after fusion. It only requires that the error between the position increment after fusion and the increment of vio is as small as possible, so the fused position will be in the GPS coordinate system.
Let’s take a look at the struct TError structure:
struct TError
{
//定义数据的 t_x 、t_y 、 t_z 和 var 的析构函数
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;
};
Understanding: According to the program, the residual is: position after VIO/GPS fusion (optimization variable) - gps observation value. At this point, the optimization part is over!
Notice:
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse()
This causes the external parameter WGPS_T_WVIO to include errors in VIO calculations. What does it mean? Originally, when vio was accurate enough,
The WGPS_T_WVIO calculated in this way must be a fixed value. Actually looking at the WGPS_T_WVIO output, it will change over time.
The reason for the change is that both the vio result and the fused result have errors, and WGPS_T_body * WVIO_T_body.inverse() makes the error included.
Summarize
Optimization process:
First of all, to determine whether there is GPS data, the overall algorithm is an optimization algorithm under the Ceres architecture.
So the overall step is the ceres optimization step. First, add the optimization parameter block (AddParameterBlock function), and the parameter is the 6-dimensional pose in globalPoseMap (the rotation in the code is represented by quaternion, so there are 7 dimensions in total).
Then add CostFunction and implement it by building factor and overloading the operator method (the specific implementation requires learning the ceres library). There are two factors in this part, one is pose graph optimization, and the other is using GPS for position constraints.
After adding the factor, perform ceres solution, update the coordinate system conversion parameters between gps and vio at this time, and then use the updateGlobalPath function to update the pose.
https://blog.csdn.net/weixin_41843971/article/details/86748719
Residual term:
Then start adding the residual terms. There are two residual terms in total: vio factor and gps factor.
– saw factor：
The costfunction creation of the residual term is provided by relativeRTError. Observations are provided by the results of vio. At this time, the calculation is based on time i as a reference, and the displacement values from i to j and the rotation value of the quaternion are passed into the cost function as observed values. At this time, iPj represents the displacement from i to j, and iQj represents the quaternion transformation from i to j. When adding the residual term, you need to add the current pose at time i and the pose at time j. That is, the observation values are used to estimate the pose at time i and the pose at time j.
– gps factor：
The costfunction creation of the residual term is provided by TError. Observations are provided by the results of Gps data. When adding the residual term, you only need to add the pose at the current i moment.
TError 及 RelativeRTError
Intuitively understand:
{0, 1, 2, 3, 4, 5, 6...}
To estimate these moments, the pose at each moment.
What I have are two aspects of observation values. On the one hand, it is the position (x, y, z) obtained by GPS at each moment (and the GPS signal can provide the accuracy posAccuracy of obtaining this position at that moment). There is no cumulative error and the accuracy is relatively high. Low. On the other hand, the position (x, y, z) and the corresponding attitude quaternion (w, x, y, z) obtained by VIO at each moment have cumulative errors, and the accuracy is relatively high in a short period of time. In order to obtain a better fusion result, we adopt this strategy: among the observation values, the initial position is provided by GPS, and the vio observation value trusts the displacement and attitude change from time i to j. Do not trust the absolute displacement and absolute rotation attitude obtained by vio. Only trust the short-term change from i to j, and use this change as the observation value of the entire cost function to solve.
Therefore, the two residual terms TError and RelativeRTError respectively correspond to the impact of the GPS position signal and the pose change from time i to j estimated by vio in a short period of time on the final result. During the iterative solution process, AutoDiffCostFunction is used to automatically solve differentials for iteration.
(1) TError
TError (x, y, z, accuracy), the last item is positioning accuracy, which can be provided by the GPS system. In addition to subtracting the direct observed value from the true value, the residual also has this accuracy as the denominator. This means that the higher the precision, the smaller the accuracy, and the greater the impact on the results.
(2) RelativeRTError
RelativeRTError (dx, dy, dz, dqw, dqx, dqy, dqz, t_var, q_var), the last two items are the weight distribution ratio between displacement and rotation, and in order to correspond to TError. In the program, the last two items should be set to a constant value based on experience, corresponding to 0.1 and 0.01 respectively. The residual is obtained directly based on the deviation between the actual value and the observed value.
Original link: https://blog.csdn.net/huanghaihui_123/article/details/87183055
Local Factor: local observation constraint, VIO relative pose transformation, calculating the residual error of pose between two adjacent frames