Lectura de código fuente ORB-SLAM2 (2)
Subproceso de mapeo local
Tomamos el programa de muestra mono_tim.cc como ejemplo En este ejemplo, primero necesitamos construir el sistema ORB-SLAM
// Create SLAM system. It initializes all system threads and gets ready to process frames.
//构建SLAM系统，调用有参构造函数，传入参数为：ORB字典，参数配置文件，相机类型
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);
En la definición de la función del sistema, se inicializan subprocesos como Local Mapping y Loop Closing. Para obtener más información, consulte nuestro blog anterior.
En el blog anterior, se introdujo el hilo principal de ORB-SLAM: hilo de seguimiento, hoy presentaré principalmente los dos hilos restantes
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
Se puede ver que la inicialización del hilo Local Mapping se implementa principalmente a través de la función ORB_SLAM2::LocalMapping::Run( ), echemos un vistazo a la implementación de esta función
void LocalMapping::Run()
{
mbFinished = false;
while(1)
{
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue
if(CheckNewKeyFrames()) //若队列中有新的关键帧
{
// BoW conversion and insertion in Map
ProcessNewKeyFrame();
// Check recent MapPoints
MapPointCulling();
// Triangulate new MapPoints
CreateNewMapPoints();
if(!CheckNewKeyFrames()) //队列中的关键帧处理完毕
{
//检查Current KF的Covisible KFS,对重复构建的MapPoints进行融合
// Find more matches in neighbor keyframes and fuse point duplications
SearchInNeighbors();
}
mbAbortBA = false;
if(!CheckNewKeyFrames() && !stopRequested()) //没有新的KF且无停止LoopClosing线程的请求
{
// Local BA
if(mpMap->KeyFramesInMap()>2) //地图中的关键帧要大于两帧
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
//剔除冗余关键帧
// Check redundant local Keyframes
KeyFrameCulling();
}
//筛选后的关键帧插入LoopClosing线程
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
else if(Stop())
{
//确保KF筛选完成
// Safe area to stop
while(isStopped() && !CheckFinish())
{
usleep(3000);
}
if(CheckFinish())
break;
}
//在收到请求后可以删除新添加的KF和MapPoints
ResetIfRequested();
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(true);
//等待插入完成
if(CheckFinish())
break;
usleep(3000);
}
SetFinish();
}
Podemos mirar el programa contra esta imagen de nuevo.
Se puede ver que los pasos principales son:
- Reciba el KF insertado desde el subproceso de seguimiento y realice el preprocesamiento.
Cuando el subproceso de seguimiento determina que se inserte un KF, en realidad no completa la acción de insertar el KF en el mapa. Al insertar un KF en el mapa, debe hacerlo Mucho trabajo de actualización
Actualizar el gráfico de covisibilidad;
actualizar el árbol de expansión:
calcular el BoW del nuevo KF - Elimine MapPoints de mala calidad Los MapPoints
almacenados en Map deben ser de alta calidad (buen seguimiento, triangulación correcta), por lo que es necesario tomar algunas medidas aquí para eliminar los MapPoints de mala calidad. - Generar nuevos MapPoints a través de la triangulación
ORB-SLAM coincidirá con los FeaturePoints en el KF actual que no coinciden con los MapPoints existentes, y sus KF covisibles tampoco coinciden con los FeaturePoints que coinciden con los MapPoints existentes.
Si coinciden, puede A través de la triangulación, un se genera un nuevo MapPoint (la posición, el paralaje, el error de reproyección y la consistencia de la escala deben verificarse después de la generación)
. La coincidencia entre este FeaturePoint y FeaturePoint se logra a través de la búsqueda BoW.
Después de generar un nuevo MapPoint a través de dos KF, y también verifique si aparece en otros KF. Entonces, para proyectar el MapPoint a otros KF Covisibles y hacer coincidir sus FeaturePoints
, si coinciden, vincule el MapPoint con el FeaturePoint de ese KF - Optimización BA del mapa local Optimización BA
del KF actual y sus KF covisibles y todos los MapPoints que observan - Elimine los fotogramas clave locales redundantes
En el subproceso de seguimiento, ORB-SLAM inserta una gran cantidad de KF en el mapa en condiciones muy flexibles, pero obviamente el mapa no puede mantener tantos KF de forma permanente, lo que hará que el mapa sea demasiado grande y aumente considerablemente la carga computacional. de varios BA, por lo que se debe eliminar cierta información redundante.
Si hay algún KF en el KF actual y sus KF covisibles, el 90% de los MapPoints que observa pueden ser utilizados por al menos otros 3 (la misma escala. o mejor) Se observan los KF, luego la información de este KF se considera redundante y se elimina. El propósito de esto es mantener la cantidad de KF en el Mapa no demasiado grande, y en un escenario con una cierta escala, la cantidad de KF en el Mapa no debería aumentar sin un límite superior, lo que también conduce a reducir la carga. de optimización BA.
El diagrama de flujo del programa correspondiente es el siguiente:
Hilo de cierre de lazo
El subproceso de cierre de bucle se implementa principalmente a través de la función ORB_SLAM2::LoopClosing::Run()
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
Las funciones específicas son las siguientes:
void LoopClosing::Run()
{
mbFinished =false;
while(1)
{
// Check if there are keyframes in the queue
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// 计算SIM3或SE3，确定最终的Loop KF
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
ResetIfRequested();
if(CheckFinish())
break;
usleep(5000);
}
SetFinish();
}
Aquí hay otra foto:
El hilo de cierre de bucle se puede dividir en dos pasos:
- Detección de bucle: Detección de bucle
- Corrección de bucle: Corrección de bucle
- La función DetectLoop( ) detecta un lote de Candidatos KF
- La función ComputeSim3( ) calcula la transformación de similitud entre KF actual y KF candidato, y determina el KF de bucle final en consecuencia.
- La función CorrectLoop( ) realiza la corrección de loopback
Después de completar todos los pasos anteriores, la precisión ya es muy alta, pero ORB-SLAM2 aún elige realizar un Global BA al final para agregar la guinda del pastel (este paso no parece estar en ORB-SLAM1)
Tenga en cuenta que para no afectar el trabajo de los 3 subprocesos principales, aquí se crea un 4.º subproceso para Global BA, pero Global BA puede interrumpirse en cualquier momento y solo se ejecutará cuando el sistema esté particularmente inactivo.
El diagrama de flujo del programa correspondiente es el siguiente:
referencia principal
ORB-SLAM2 Aprendizaje de papel y código - LoopClosing Thread - MingruiYu - Blog Park (cnblogs.com)
Si hay alguna infracción, póngase en contacto para eliminar