ORB_SLAM3: Обзор процесса инициализации монокуляра + IMU

В режиме монокуляр + IMU, после выполнения некоторых предыдущих настроек, при обработке первого кадра изображения:

1. Эта функция будет вызываться для каждого кадра изображения:

clahe->apply(im,im);

Цель: сделать распределение гистограммы в оттенках серого более равномерным, тем самым усилив общий контраст и облегчив последующее извлечение характерных точек и выполнение других задач;

2. В изображении первого кадра нет данных IMU (ni=0) (контейнер vImuMeas пуст), введите следующую функцию:

SLAM.TrackMonocular(im, tframe, vImuMeas);

Эта функция сначала оценивает некоторые состояния и обрабатывает их соответствующим образом. При наличии данных IMU данные IMU сохраняются в очереди mlQueueImuData.Ее основная задача — вычислить положение камеры в текущем кадре, что завершается вызовом следующей функции;

Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);

3. Сначала преобразуйте цветное изображение в одноканальное изображение в оттенках серого, а затем создайте класс Frame в соответствии с наличием IMU и состояния отслеживания (mState).В это время состояние отслеживания mState==NO_IMAGES_YET. При построении класса Frame в текущем кадре необходимо извлечь 5*1500 характерных точек. После успешной инициализации необходимо извлечь только 1500 характерных точек;

4. В конструкторе кадров сначала завершается присвоение идентификатора кадра:

mnId = nNextId++;    // 先赋值再自增1

Затем назначьте соответствующие параметры пирамиды переменным-членам, а затем извлеките характерные точки изображения. В это время применяется функтор, и бегущая функция:

int ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                                OutputArray _descriptors, std::vector<int> &vLappingArea)

Некоторые моменты, на которые следует обратить внимание в отношении приведенного выше функтора:

1) Используйте метод квадродерева для расчета характерных точек каждого слоя изображений и равномерного их распределения.Однородные характерные точки могут повысить точность расчета позы;2) Прежде чем рассчитывать дескрипторы характерных точек, выполните размытие по Гауссу на изображении, в основном Во избежание влияния шума изображения;

3) Наконец, координаты характерных точек слоя, отличного от 0, в пирамиде необходимо восстановить до системы координат исходного изображения (0-го слоя).Что касается смысла границы, то я его не понимаю, и Я не знаю, какова его польза в дальнейшем.

5. После извлечения характерных точек сначала используйте функцию коррекции OpenCV для устранения искажений характерных точек, затем рассчитайте границу изображения после устранения искажений (этот процесс обычно выполняется в первом кадре или после изменения параметров калибровки камеры) и наконец, характерные точки. Распределяются по сеткам изображений (каждая сетка фиксированного размера записывает индекс характерных точек внутри нее) с целью ускорения последующего сопоставления характерных точек.

Когда первый кадр изображения попадает в Tracking::Track():

1. В это время mpLastKeyFrame=NULL, mbCreatedMap=false, а состояние отслеживания mState обновляется до NOT_INITIALIZED;

2. Выполните монокулярную инициализацию.В это время mbReadyToInitializate=false, для создания ключевого кадра инициализации требуется, чтобы количество характерных точек в текущем кадре превышало 100;

3. После того, как текущий кадр соответствует условиям создания ключевого кадра инициализации, обновите mInitialFrame, mLastFrame и mvbPrevMatched для записи всех характерных точек «текущего кадра», а всем значениям контейнера mvIniMatches установите значение -1. контейнера — характеристика текущего кадра.Количество точек;

4. Смещение mpImuPreintegratedFromLastKF установлено в 0, а *mpImuCalib устанавливается при чтении файла yaml IMU. Инициализируйте прединтеграцию текущего кадра mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF и, наконец, установите для mbReadyToInitializate значение true, что означает, что опорный кадр инициализации создан, а матрица преобразования Tcw текущего кадра является единичной матрицей.

Когда изображение второго кадра поступает в Tracking::Track():

1. В данный момент имеются данные IMU, mState=NOT_INITIALIZED, mpLastKeyFrame=NULL.Если имеется ненормальная временная метка, ее необходимо обработать;

2. Предварительная интеграция данных IMU.Для каждого кадра данных IMU существует две предварительные интеграции: одна относительно предыдущего кадра, а другая — относительно предыдущего ключевого кадра;

Примечание. В конструкторе Frame установите mpPrevFrame=*mLastFrame текущего кадра, гироскоп выведет угловую скорость (рад/с), а акселерометр выведет ускорение (м/с2).

void Tracking::PreintegrateIMU()  // 要求当前帧的上一帧（mpPrevFrame）不为NULL且mlQueueImuData里有数据

Основная работа этой функции:

1) Введите while(true) и сохраните данные IMU, соответствующие следующим двум условиям, в контейнере mvImuFromLastFrame;

Условия: a. Текущая временная метка IMU ≥ временная метка предыдущего кадра - mimuPer (0,001 с).

б. Текущая временная метка IMU < временная метка текущего кадра — mImuPer (0,001 с).

Условия завершения while(true): текущая временная метка IMU не соответствует двум вышеуказанным условиям или контейнер mlQueueImuData пуст;

2) Создайте предварительно интегрированный процессор IMU (pImuPreintegratedFromLastFrame), смещение использует смещение предыдущего кадра, а mImuCalib каждого кадра один и тот же (он устанавливается при чтении yaml-файла IMU);

3) Первый кадр данных IMU и последний кадр данных IMU необходимо обрабатывать отдельно. Средние данные IMU обрабатываются напрямую. Основная цель - вычислить среднее ускорение (acc), среднюю угловую скорость (angVel) два кадра до и после IMU и разница во времени между двумя кадрами (tstep0);

4) mpImuPreintegratedFromLastKF установлен в Tracking::ParseIMUParamFile(cv::FileStorage &fSettings), где смещения установлены в 0;

5) dR в начале (т. е. предыдущий кадр текущего кадра) является единичной матрицей, а dP и dV оба равны 0. В основном он вычисляет dP, dV и dR между текущим кадром, предыдущим кадром и предыдущим ключевым кадром, вычисляет JPa, JPg, JVa, JVg и JRg и, наконец, вычисляет ковариационную матрицу C периода времени;

mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc, angVel, tstep);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc, angVel, tstep);

6) Обновить mpImuPreintegratedFrame текущего кадра (на основе указателя предварительно интегрированного процессора IMU предыдущего кадра), mpImuPreintegrated (на основе указателя предварительно интегрированного процессора IMU предыдущего ключевого кадра) и mpLastKeyFrame (указателя предыдущего ключевого кадра) и наконец, измените текущий кадр. Для параметра mbImuPreintegrated установлено значение true, что означает, что текущий кадр завершил предварительную интеграцию.

Когда изображение второго кадра поступает в Tracking::MonocularInitialization():

1. На данный момент создан монокулярный инициализатор (mInitialFrame), mbReadyToInitializate=true, который требует, чтобы текущий кадр имел более 100 характерных точек или mSensor == System::IMU_MONOCULAR и временную метку предыдущего кадра текущего кадр больше, чем у опорного кадра инициализации монокуляра. Временная метка больше 1 секунды;

2. После выполнения вышеуказанных условий сопоставляются характерные точки между текущим кадром и инициализированным опорным кадром.Индекс контейнера mvIniMatches представляет собой номер характерной точки инициализированного опорного кадра.Если характерная точка в инициализированном опорном кадре совпадает текущий кадр, затем контейнер. Соответствующее значение — это номер точки признака текущего кадра, в противном случае оно равно -1;

3. Сначала исправьте характерные точки в двух кадрах, затем вычислите базовую матрицу F и матрицу гомографии H с помощью двойного потока RANSAC (цикл 200 раз), запишите матрицы H и F, соответствующие SH и SF с наивысшей оценкой соответственно, и наконец, передайте Условия, чтобы определить, следует ли восстанавливать базовую матрицу F или матрицу гомографии H;

4. Реконструкция означает восстановление R, t и количества трехмерных точек, отвечающих соответствующим условиям, через F или H, и, наконец, вывод лучшего Tcw, отвечающего соответствующим условиям, восстановленных точек карты и характерных точек при инициализации. опорный кадр успешен. Точки карты перестроены (хранятся в контейнере vbTriangulated);

5. Используйте опорный кадр инициализации в качестве мировой системы координат, чтобы первая матрица преобразования кадра была единичной матрицей, и, наконец, установите текущую позу кадра ( Tcw ).

Функция Tracking::CreateInitialMapMonocular():

1. Установите опорный кадр инициализации и текущий кадр в качестве ключевых кадров и установите для mpImuPreintegrated (указатель препроцессора IMU, основанный на предыдущем ключевом кадре) опорного кадра инициализации значение NULL;

2. Преобразовать дескрипторы исходного опорного кадра и текущего ключевого кадра в BoW, главным образом для завершения назначения контейнеров mBowVec и mFeatVec.Тип данных контейнера mBowVec — карта, первые данные — это слово id, а вторые значение — Вес. Если одному и тому же слову соответствует несколько характерных точек на изображении, вес будет наложен. Тип данных контейнера mFeatVec — карта.Первые данные — это идентификатор узла (когда L=2).Второй тип данных — векторный контейнер.Содержимое внутри — это индекс дескриптора, содержащийся в идентификаторе узла.Цель контейнер — Ускорить последующее сопоставление характерных точек; Примечание. Только ключевые кадры преобразуют свои дескрипторы в Bow

3. Используйте инициализированные 3D-точки для генерации точек карты MapPoints, где данные в контейнере mvIniP3D — это координаты точек в мировой системе координат:

1) Используйте 3D-точки (в мировой системе координат) для построения точек карты;

2) Добавьте атрибуты к точке карты:

а. Обновите ключевой кадр, в котором наблюдается MapPoint. Первый элемент в контейнере mObservations — это указатель ключевого кадра. Второй элемент — индекс характерной точки в ключевом кадре, соответствующий точке карты. В то же время количество наблюдений обновляется точка карты ( nObs), бинокулярные и RGB камеры: nObs=nObs+2, монокулярная камера: nObs=nObs+1;

б. Выберите наиболее репрезентативный дескриптор из характерных точек, которые могут наблюдать MapPoint во многих ключевых кадрах. Лучший дескриптор точки карты должен иметь наименьший дескриптор с дескрипторами других характерных точек, которые могут наблюдать точку карты. Медианное расстояние; Примечание. Поскольку MapPoint будет наблюдаться многими камерами, после вставки ключевого кадра необходимо определить, следует ли обновлять наиболее репрезентативный дескриптор текущего MapPoint.

в. Обновите среднее направление наблюдения и диапазон расстояний наблюдения MapPoint.

3) Обновить связь между ключевыми кадрами.Каждое ребро имеет вес.Вес ребра — это количество общих 3D-точек между ключевым кадром и текущим ключевым кадром;

4) Глобальная оптимизация БА, оптимизация позы и MapPoint текущего ключевого кадра;

После монокулярной инициализации все точки полученной исходной карты являются точками локальной карты.

5) mState = ОК, теперь инициализация монокуляра завершена.