Descripción general
Basado en VINS Mono, VINS Fusion agrega sensores como GPS que pueden obtener información de observación global, lo que permite a VINS utilizar información global para eliminar errores acumulativos y así reducir la dependencia de circuito cerrado.
Los sensores locales (como cámaras, IMU, lidar, etc.) se utilizan ampliamente en algoritmos de mapeo y localización. Aunque estos sensores pueden lograr buenos efectos de posicionamiento local y mapeo local en áreas sin información de GPS, estos sensores solo pueden proporcionar observaciones locales, lo que limita sus escenarios de aplicación:
El primer problema es que los datos de observación locales carecen de restricciones globales. Cada vez que ejecutamos el algoritmo en diferentes ubicaciones, obtendremos resultados de posicionamiento y mapeo en diferentes sistemas de coordenadas, por lo que es difícil combinar estos resultados de medición para formar un efecto global.
El segundo problema es que los algoritmos de estimación basados en observaciones locales deben tener una deriva acumulativa. Aunque la detección de cierre de bucle puede eliminar la deriva hasta cierto punto, el algoritmo aún es difícil de manejar para escenarios a gran escala con grandes cantidades de datos.
En comparación con los sensores locales, los sensores globales (como GPS, barómetro, magnetómetro, etc.) pueden proporcionar observaciones globales. Estos sensores utilizan un sistema de coordenadas unificado globalmente y la variación de los datos de observación de salida no aumenta con el tiempo. Sin embargo, estos sensores también tienen algunos problemas que impiden que se utilicen directamente para un posicionamiento y mapeo precisos. Tomando el GPS como ejemplo, los datos del GPS generalmente no son fluidos, tienen ruido y tienen una tasa de salida baja.
Por lo tanto, una idea simple e intuitiva es combinar sensores locales y sensores globales para lograr el efecto de deriva cero global local precisa. Este es el contenido principal del documento VINS Fusion.
Principio de fusión
La arquitectura del algoritmo de VINS Fusion se muestra en la figura:
La siguiente figura expresa las restricciones entre observaciones y estados en forma de gráfico de factores:
los círculos son cantidades de estado (como pose, velocidad, desplazamiento, etc.) y la los cuadrados amarillos son observaciones locales: restricciones, es decir, transformaciones de pose relativas de VO/VIO; los cuadrados de otros colores son restricciones de observaciones globales, como los cuadrados morados de GPS.
La construcción de restricciones locales (residuales) se refiere al papel vins mono, que calcula los residuos de pose entre dos cuadros adyacentes. Aquí sólo se analizan las limitaciones globales impuestas por el GPS. El primer paso es, por supuesto, convertir las coordenadas GPS, es decir, la longitud y la latitud, al sistema de coordenadas geodésicas. Es habitual elegir un sistema de coordenadas diestro, siendo las direcciones positivas de los ejes x, y y z las direcciones noreste de la Tierra o noreste del cielo, respectivamente. A continuación, se pueden calcular los residuos de las restricciones globales:
donde z es el valor de medición del GPS, X es la predicción del estado y la ecuación h es la ecuación de observación. X se puede calcular en función del estado del momento anterior y la transformación de pose entre el momento actual y el momento anterior. El método específico de este artículo es usar VIO para obtener la pose relativa dX entre el momento actual y el momento anterior, y agregarla a la pose en el momento anterior X (i-1) para obtener la pose Xi en el momento actual. . Cabe señalar que la X aquí toma el primer cuadro como origen. Al observar la ecuación h, las coordenadas del estado actual se convierten al sistema de coordenadas GPS. Esto crea una restricción global.
La optimización posterior se entrega al optimizador BA para una optimización iterativa, y vins fusion utiliza ceres como optimizador.
Código
1. Entrada de datos GPS y VIO
Una cosa que debe quedar clara es que la salida de VIO es la transformación de pose acumulativa en relación con el primer cuadro, es decir, el primer cuadro es el origen . VINS Fusion recibe la transformación de pose local (relativa al primer fotograma) generada por vio y las coordenadas de longitud y latitud generadas por gps para la fusión. La interfaz que acepta entrada de datos se encuentra en el archivo global_fusion/src/globaOptNode.cpp. El código clave para la definición de la interfaz es el siguiente:
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. Integración GPS y VIO
El código para que VINS Fusion fusione datos de GPS y VIO se encuentra en el archivo global_fusion/src/globalOpt.cpp, que se describe en detalle a continuación.
a. Recibir datos GPS, recibir datos VIO y transferirlos al sistema de coordenadas GPS
// 接收上面输入的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;
}
El proceso de cálculo para convertir las coordenadas locales de los datos VIO a coordenadas GPS aparece arriba. La fórmula es la siguiente:
GPS2VIO en esta fórmula aparece en el proceso de optimización posterior. El método de cálculo es:
b. Optimización de fusión
Aquí está el código clave de la fusión, puedes ver que el proceso es el siguiente:
1. Construya t_array y q_array para almacenar variables de traducción y rotación para facilitar la entrada de ecuaciones de optimización y su eliminación después de la optimización.
2. Use RelativeRTError::Create() para construir las restricciones entre los dos cuadros de VIO e ingrese la ecuación de optimización
3. Use TError::Create() para construir las restricciones globales compuestas de GPS e ingrese la ecuación de optimización.
4. Saque los datos optimizados.
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;
}
Una parte clave aparece en el código anterior, a saber, el cálculo de WGPS_T_WVIO. Como sabemos por el código anterior, esta matriz de 4 * 4 se utiliza para transformar el sistema de coordenadas VIO a GPS.
La estructura del programa específico en Factors.h es la siguiente:
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;
};
Nota: ¿Cuáles son los cambios entre dos fotogramas de VIO y cuáles son las posturas de bits del globalPoseMap optimizado?
Idea: Proyecte el incremento de POS optimizado por VIO y GPS en VIO, luego marque la diferencia con el incremento de POS en VIO y encuentre el residual.
Solía ser confundido al mirar estos códigos, pero después de que se familiarice con Ceres y comprenda los principios de optimización de VIO y GPS, es muy simple: la siguiente figura es un diagrama esquemático: el residuo de los datos de VIO y las cantidades de estado es definido como: (estado en el momento
j cantidad - la cantidad de estado en el momento i) el incremento obtenido - vio incremento; lo que significa que la posición después de la fusión debe
Debe coincidir con la posición GPS tanto como sea posible y el incremento entre los dos cuadros debe ser lo más igual posible al VIO. Esto es crucial para comprender la transformación de coordenadas; dicho procesamiento significa que los datos de vio
No hay restricciones sobre la posición absoluta después de la fusión, solo requiere que el error entre el incremento de posición después de la fusión y el incremento de vio sea lo más pequeño posible, por lo que la posición fusionada estará en el sistema de coordenadas GPS.
Echemos un vistazo a la estructura struct TError:
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;
};
Comprensión: Según el programa, el residuo es: posición después de la fusión VIO/GPS (variable de optimización) - valor de observación del gps ¡En este punto, la parte de optimización ha terminado!
Aviso:
WGPS_T_WVIO = WGPS_T_cuerpo * WVIO_T_cuerpo.inverse()
Esto hace que el parámetro externo WGPS_T_WVIO incluya errores en los cálculos de VIO. ¿Qué significa? Originalmente, cuando vio era lo suficientemente preciso,
El WGPS_T_WVIO calculado de esta manera debe ser un valor fijo. En realidad, al observar la salida WGPS_T_WVIO, cambiará con el tiempo.
El motivo del cambio es que tanto el resultado vio como el resultado fusionado tienen errores, y WGPS_T_body * WVIO_T_body.inverse() incluye el error.
Resumir
Proceso de optimización:
En primer lugar, para determinar si hay datos de GPS, el algoritmo general es un algoritmo de optimización bajo la arquitectura Ceres.
Entonces, el paso general es el paso de optimización de Ceres. Primero, agregue el bloque de parámetros de optimización (función AddParameterBlock), y el parámetro es la pose de 6 dimensiones en globalPoseMap (la rotación en el código está representada por cuaternión, por lo que hay 7 dimensiones en total).
Luego agregue CostFunction e impleméntelo construyendo el factor y sobrecargando el método del operador (la implementación específica requiere aprender la biblioteca ceres). Hay dos factores en esta parte, uno es la optimización del gráfico de pose y el otro es el uso de GPS para las restricciones de posición.
Después de agregar el factor, realice la solución Ceres, actualice los parámetros de conversión del sistema de coordenadas entre gps y vio en este momento y luego use la función updateGlobalPath para actualizar la pose.
https://blog.csdn.net/weixin_41843971/article/details/86748719
Plazo residual:
Luego comience a sumar los términos residuales. Hay dos términos residuales en total: factor vio y factor gps.
– vio factor：
La creación de la función de costos del término residual la proporciona el error RTE relativo. Las observaciones son proporcionadas por los resultados de vio. En este momento, el cálculo se basa en el tiempo i como referencia, y los valores de desplazamiento de i a j y el valor de rotación del cuaternión se pasan a la función de costo como valores observados. En este momento, iPj representa el desplazamiento de i a j, e iQj representa la transformación del cuaternión de i a j. Al agregar el término residual, debe agregar la pose actual en el momento i y la pose en el momento j. Es decir, los valores de observación se utilizan para estimar la pose en el momento i y la pose en el momento j.
– factor gps:
La creación de la función de costos del término residual la proporciona TError. Las observaciones son proporcionadas por los resultados de los datos del GPS. Al agregar el término residual, solo necesita agregar la pose en el momento i actual.
TError y RelativeRTError
Entender intuitivamente:
{0, 1, 2, 3, 4, 5, 6...}
Para estimar estos momentos, la pose en cada momento.
Lo que tengo son dos aspectos de los valores de observación, por un lado es la posición (x, y, z) obtenida por el GPS en cada momento (y la señal del GPS puede proporcionar la precisión posPrecisión de obtener esta posición en ese momento) No hay error acumulativo y la precisión es relativamente alta.Baja. Por otro lado, la posición (x, y, z) y el cuaternión de actitud correspondiente (w, x, y, z) obtenidos por VIO en cada momento tienen errores acumulativos y la precisión es relativamente alta en un corto período de tiempo. . Para obtener un mejor resultado de fusión, adoptamos esta estrategia: entre los valores de observación, la posición inicial la proporciona el GPS, y el valor de observación vio confía en el desplazamiento y el cambio de actitud del tiempo i al j. No confíe en el desplazamiento absoluto y la actitud de rotación absoluta obtenida por vio. Confíe únicamente en el cambio a corto plazo de i a j y utilice este cambio como el valor de observación de toda la función de costos para resolver.
Por lo tanto, los dos términos residuales TError y RelativeRTError corresponden respectivamente al impacto de la señal de posición GPS y el cambio de pose del tiempo i al j estimado por vio en un corto período de tiempo en el resultado final. Durante el proceso de solución iterativa, AutoDiffCostFunction se utiliza para resolver automáticamente los diferenciales para la iteración.
(1) TError
TError (x, y, z, precisión), el último elemento es la precisión de posicionamiento, que puede ser proporcionada por el sistema GPS. Además de restar el valor observado directo del valor real, el residual también tiene esta precisión como denominador. Esto significa que cuanto mayor sea la precisión, menor será la exactitud y mayor el impacto en los resultados.
(2) RelativeRTError
RelativeRTError (dx, dy, dz, dqw, dqx, dqy, dqz, t_var, q_var), los dos últimos elementos son la relación de distribución de peso entre desplazamiento y rotación, y para corresponder a TError. En el programa, los dos últimos elementos deben establecerse en un valor constante basado en la experiencia, correspondiente a 0,1 y 0,01 respectivamente. El residual se obtiene directamente en base a la desviación entre el valor real y el valor observado.
Enlace original: https://blog.csdn.net/huanghaihui_123/article/details/87183055
Factor local: restricción de observación local, transformación de pose relativa VIO, cálculo del residuo de la pose entre dos cuadros adyacentes