En modo monocular + IMU, tras completar algunas de las configuraciones anteriores, al procesar el primer fotograma de imagen:
1. Esta función se llamará para cada cuadro de imagen:
clahe->apply(im,im);Propósito: hacer que la distribución del histograma en escala de grises sea más uniforme, fortaleciendo así el contraste general y facilitando la extracción posterior de puntos característicos y otras tareas;
2. No hay datos IMU en la imagen del primer cuadro (ni = 0) (el contenedor vImuMeas está vacío), ingrese la siguiente función:
SLAM.TrackMonocular(im, tframe, vImuMeas);Esta función primero juzga algunos estados y los maneja en consecuencia. Cuando hay datos IMU, los datos IMU se almacenan en la cola mlQueueImuData, su tarea principal es calcular la pose de la cámara en el cuadro actual, lo cual se completa llamando a la siguiente función;
Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);3. Primero convierta la imagen en color en una imagen en escala de grises de un solo canal y luego construya la clase Frame de acuerdo con si hay una IMU y un estado de seguimiento (mState), en este momento el estado de seguimiento mState == NO_IMAGES_YET. Al construir la clase Frame en el marco actual, es necesario extraer 5 * 1500 puntos característicos. Después de una inicialización exitosa, solo es necesario extraer 1500 puntos característicos;
4. En el constructor del marco, primero se completa la asignación de ID del marco:
mnId = nNextId++; // 先赋值再自增1Luego asigne los parámetros relevantes de la pirámide a las variables miembro y luego extraiga los puntos característicos de la imagen. En este momento, se aplica el funtor y la función en ejecución es:
int ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
OutputArray _descriptors, std::vector<int> &vLappingArea)Algunos puntos a tener en cuenta sobre el functor anterior son:
1) Utilice el método quadtree para calcular los puntos característicos de cada capa de imágenes y distribuirlos uniformemente. Los puntos característicos uniformes pueden mejorar la precisión del cálculo de la pose; 2) Antes de calcular los descriptores de los puntos característicos, realice un desenfoque gaussiano en la imagen, principalmente Para evitar la influencia del ruido de la imagen;
3) Finalmente, las coordenadas de los puntos característicos de la capa distinta de 0 en la pirámide deben restaurarse al sistema de coordenadas de la imagen original (capa 0). En cuanto al significado del límite, no lo entiendo, y No sé para qué sirve después.
5. Después de extraer los puntos característicos, primero use la función de corrección de OpenCV para deformar los puntos característicos, luego calcule el límite de la imagen después de la dedistorsión (este proceso generalmente se realiza en el primer cuadro o después de que cambian los parámetros de calibración de la cámara), y finalmente los puntos característicos se distribuyen en cuadrículas de imágenes (cada cuadrícula de tamaño fijo registra el índice de los puntos característicos dentro de ella), con el propósito de acelerar la coincidencia posterior de puntos característicos.
Cuando el primer fotograma de la imagen ingresa a Tracking::Track():
1. En este momento, mpLastKeyFrame = NULL, mbCreatedMap = false y el estado de seguimiento mState se actualiza a NOT_INITIALIZED;
2. Realice la inicialización monocular. En este momento, mbReadyToInitializate = false, la creación de un fotograma clave de inicialización requiere que el número de puntos característicos en el fotograma actual sea superior a 100;
3. Después de que el fotograma actual cumpla con las condiciones para crear un fotograma clave de inicialización, actualice mInitialFrame, mLastFrame y mvbPrevMatched para registrar todos los puntos característicos del "fotograma actual" y establezca todos los valores del contenedor mvIniMatches en -1. del contenedor es la característica del cuadro actual número de puntos;
4. El sesgo de mpImuPreintegratedFromLastKF se establece en 0 y *mpImuCalib se establece al leer el archivo IMU yaml. Inicialice la preintegración del marco actual mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF y finalmente establezca mbReadyToInitializate en verdadero, lo que significa que se ha creado el marco de referencia de inicialización y la matriz de transformación Tcw del marco actual es la matriz de identidad.
Cuando la imagen del segundo fotograma ingresa a Tracking::Track():
1. En este momento hay datos de IMU, mState = NOT_INITIALIZED, mpLastKeyFrame = NULL. Cuando hay una marca de tiempo anormal, es necesario procesarla;
2. Preintegración de datos IMU: hay dos preintegraciones para cada cuadro de datos IMU, una es relativa al cuadro anterior y la otra es relativa al cuadro clave anterior;
Nota: En el constructor de cuadros, establezca mpPrevFrame=*mLastFrame del cuadro actual, el giroscopio genera velocidad angular (rad/s) y el acelerómetro genera aceleración (m/s2).
void Tracking::PreintegrateIMU() // 要求当前帧的上一帧（mpPrevFrame）不为NULL且mlQueueImuData里有数据El trabajo principal de esta función es:
1) Ingrese while (verdadero) y almacene los datos de IMU que cumplan las dos condiciones siguientes en el contenedor mvImuFromLastFrame;
Condiciones: a. Marca de tiempo actual de la IMU ≥ marca de tiempo del fotograma anterior - mImuPer (0,001 s)
b. Marca de tiempo actual de IMU <marca de tiempo del fotograma actual - mImuPer (0,001 s)
Condiciones para finalizar mientras (verdadero): la marca de tiempo actual de la IMU no cumple las dos condiciones anteriores o el contenedor mlQueueImuData está vacío;
2) Construya el procesador preintegrado de IMU (pImuPreintegratedFromLastFrame), el desplazamiento utiliza el desplazamiento del fotograma anterior y el mImuCalib de cada fotograma es el mismo (se establece al leer el archivo yaml de la IMU);
3) El primer cuadro de datos IMU y el último cuadro de datos IMU deben procesarse por separado. Los datos IMU intermedios se procesan directamente. El objetivo principal es calcular la aceleración promedio (acc) y la velocidad angular promedio (angVel) del dos fotogramas antes y después de la IMU y la diferencia de tiempo entre los dos fotogramas (tstep0);
4) mpImuPreintegratedFromLastKF se establece en Tracking::ParseIMUParamFile(cv::FileStorage &fSettings), donde las compensaciones se establecen en 0;
5) El dR al principio (es decir, el cuadro anterior del cuadro actual) es la matriz unitaria, y dP y dV son ambos 0. Calcula principalmente dP, dV y dR entre el cuadro actual y el cuadro anterior y el cuadro clave anterior, calcula JPa, JPg, JVa, JVg y JRg, y finalmente calcula la matriz de covarianza C del período de tiempo;
mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc, angVel, tstep);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc, angVel, tstep);6) Actualice el mpImuPreintegratedFrame del fotograma actual (basado en el puntero del procesador preintegrado IMU del fotograma anterior), mpImuPreintegrated (basado en el puntero del procesador preintegrado IMU del fotograma clave anterior) y mpLastKeyFrame (puntero del fotograma clave anterior), y finalmente cambie el marco actual mbImuPreintegrated se establece en verdadero, lo que significa que el marco actual ha completado la preintegración.
Cuando la imagen del segundo cuadro ingresa a Tracking::MonocularInitialization():
1. En este momento, se ha creado el inicializador monocular (mInitialFrame), mbReadyToInitializate=true, que requiere que el fotograma actual tenga más de 100 puntos característicos o mSensor == System::IMU_MONOCULAR y la marca de tiempo del fotograma anterior del actual. el marco es mayor que el del marco de referencia de inicialización monocular. La marca de tiempo es mayor que 1 segundo;
2. Después de que se cumplan las condiciones anteriores, los puntos característicos se hacen coincidir entre el marco actual y el marco de referencia inicializado. El índice del contenedor mvIniMatches es el número de punto característico del marco de referencia inicializado. Si el punto característico en el marco de referencia inicializado coincide el cuadro actual, luego el contenedor. El valor correspondiente es el número de punto característico del cuadro actual; de lo contrario, es -1;
3. Primero, corrija los puntos característicos en los dos cuadros, luego calcule la matriz básica F y la matriz de homografía H a través del hilo doble RANSAC (bucle 200 veces), registre las matrices H y F correspondientes a los SH y SF con la puntuación más alta respectivamente, y finalmente pase Condiciones para determinar si se reconstruye a través de la matriz básica F o la matriz de homografía H;
4. Reconstrucción significa restaurar R, t y el número de puntos tridimensionales que cumplen con las condiciones correspondientes a través de F o H, y finalmente generar el mejor Tcw que cumpla con las condiciones relevantes, los puntos del mapa reconstruidos y qué puntos característicos en la inicialización. el marco de referencia tiene éxito. Los puntos del mapa se reconstruyen (almacenados en el contenedor vbTriangulated);
5. Utilice el marco de referencia de inicialización como sistema de coordenadas mundial, de modo que la primera matriz de transformación del marco sea la matriz de identidad y, finalmente, establezca la pose del marco actual ( Tcw ).
Función Seguimiento::CreateInitialMapMonocular():
1. Establezca tanto el cuadro de referencia de inicialización como el cuadro actual como cuadros clave, y establezca el mpImuPreintegrated (puntero del preprocesador IMU basado en el cuadro clave anterior) del cuadro de referencia de inicialización en NULL;
2. Convierta los descriptores del cuadro de referencia inicial y el cuadro clave actual en BoW, principalmente para completar la asignación de los contenedores mBowVec y mFeatVec. El tipo de datos del contenedor mBowVec es mapa, el primer dato es la identificación de la palabra y el segundo El valor es Peso. Si varios puntos característicos en la imagen corresponden a la misma palabra, el peso se superpondrá. El tipo de datos del contenedor mFeatVec es mapa. El primer dato es el ID del nodo (cuando L=2). El segundo tipo de datos es el contenedor vectorial. El contenido interno es el índice de descriptor contenido en el ID del nodo. El propósito del El contenedor es Acelerar la coincidencia posterior de puntos característicos; Nota: Sólo los fotogramas clave convertirán sus descriptores en Arco.
3. Utilice los puntos 3D inicializados para generar puntos del mapa MapPoints, donde los datos en el contenedor mvIniP3D son las coordenadas de los puntos en el sistema de coordenadas mundial:
1) Utilice puntos 3D (bajo el sistema de coordenadas mundial) para construir puntos del mapa;
2) Agregue atributos al punto del mapa:
a. Actualice el fotograma clave en el que se observa el MapPoint. El primer elemento en el contenedor mObservations es el puntero del fotograma clave. El segundo elemento es el índice del punto característico en el fotograma clave correspondiente al punto del mapa. Al mismo tiempo, el número de observaciones del punto del mapa se actualiza (nObs), cámaras binoculares y RGB: nObs=nObs+2, cámara monocular: nObs=nObs+1;
b. Seleccione el descriptor más representativo de los puntos característicos que pueden observar el MapPoint en muchos fotogramas clave. El mejor descriptor del punto del mapa debe tener el descriptor más pequeño con los descriptores de otros puntos característicos que pueden observar el punto del mapa. Distancia media; Nota: Dado que muchas cámaras observarán un MapPoint, después de insertar un fotograma clave, es necesario determinar si se actualiza el descriptor más representativo del MapPoint actual.
c.Actualice la dirección de observación promedio y el rango de distancia de observación del MapPoint.
3) Actualizar la relación de conexión entre fotogramas clave. Cada borde tiene un peso. El peso del borde es el número de puntos 3D comunes entre el fotograma clave y el fotograma clave actual;
4) Optimización global de BA, optimizando la pose y MapPoint del fotograma clave actual;
Después de la inicialización monocular, todos los puntos en el mapa inicial obtenido son puntos del mapa local.
5) mState = OK, ahora se completa la inicialización monocular.