Монокулярная визуальная одометрия
Визуальная одометрия является процессом определения местоположения и ориентации камеры путем анализа последовательности изображений. Визуальная одометрия используется во множестве приложений, таких как мобильные роботы, беспилотные автомобили и беспилотные воздушные автомобили. Этот пример показывает вам, как оценить траекторию одной калиброванной камеры от последовательности изображений.
Обзор
Этот пример показывает, как оценить траекторию калиброванной камеры от последовательности 2D представлений. Этот пример использует изображения от Нового Набора данных Стерео Цукубы, созданного в CVLAB Университета Цукубы. (https://cvlab.cs.tsukuba.ac.jp). Набор данных состоит из синтетических изображений, сгенерированной компьютерной графики использования, и включает наземные положения камеры истины.
Без дополнительной информации траектория монокулярной камеры может только быть восстановлена до неизвестного масштабного коэффициента. Монокулярные визуальные системы одометрии, используемые на мобильных роботах или автономных автомобилях обычно, получают масштабный коэффициент из другого датчика (например, одометр колеса или GPS), или от объекта известного размера в сцене. Этот пример вычисляет масштабный коэффициент из наземной истины.
Пример разделен на три части:
Оценка положения второго представления относительно первого представления. Оцените положение второго представления путем оценки существенной матрицы и разложения его на местоположение камеры и ориентацию.
Начальная загрузка оценки траектории камеры с помощью глобальной переменной связывает корректировку. Устраните выбросы с помощью epipolar ограничения. Найдите 3D-к-2D соответствия между точками триангулированными от предыдущих двух представлений и текущего представления. Вычислите мировое положение камеры для текущего представления путем решения проблемы перспективного n значений (PnP). Оценка положений камеры неизбежно приводит к ошибкам, которые накапливаются в зависимости от времени. Этот эффект называется дрейфом. Чтобы уменьшать дрейф, пример совершенствовал все положения, оцененные до сих пор с помощью корректировки пакета.
Оценка остающейся траектории камеры с помощью оконной корректировки пакета. С каждым новым представлением время это берет, чтобы совершенствовать все увеличения положений. Оконная корректировка пакета является способом уменьшать время вычисления, только оптимизируя последние представления n, а не целую траекторию. Время вычисления далее уменьшается, не вызывая корректировку пакета к каждому представлению.
Считайте входную истину последовательности изображений и земли
Этот пример использует изображения от Нового Набора данных Стерео Цукубы, созданного в CVLAB Университета Цукубы. (https://cvlab.cs.tsukuba.ac.jp). Если вы используете эти изображения в своей собственной работе или публикациях, процитируйте следующие бумаги:
[1] Мартин Перис Марторелл, Atsuto Maki, Сара Марталл, Yasuhiro Ohkawa, Kazuhiro Фукуи, "К Симуляции Управляемая Система Видения Стерео". Продолжения ICPR, pp.1038-1042, 2012.
[2] Сара Марталл, Мартин Перис Марторелл, Kazuhiro Фукуи, "Реалистический Набор данных Стереоизображения CG с Наземными Картами Несоизмеримости Истины", Продолжения семинара ICPR TrakMark2012, pp.40-42, 2012.
images = imageDatastore(fullfile(toolboxdir('vision'), 'visiondata', ... 'NewTsukuba')); % Create the camera parameters object using camera intrinsics from the % New Tsukuba dataset. K = [615 0 320; 0 615 240; 0 0 1]'; cameraParams = cameraParameters('IntrinsicMatrix', K); % Load ground truth camera poses. load(fullfile(toolboxdir('vision'), 'visiondata', ... 'visualOdometryGroundTruth.mat'));
Создайте набор представления, содержащий первое представление последовательности
Используйте объект viewSet сохранить и управлять точками изображений и положением камеры, сопоставленным с каждым представлением, а также соответствиями точки между парами представлений. Если вы заполняете объект viewSet, можно использовать его, чтобы найти дорожки точки через несколько представлений и получить положения камеры, которые будут использоваться функциями bundleAdjustment и triangulateMultiview.
% Create an empty viewSet object to manage the data associated with each view. vSet = viewSet; % Read and display the first image. Irgb = readimage(images, 1); player = vision.VideoPlayer('Position', [20, 400, 650, 510]); step(player, Irgb);
Преобразуйте в шкалу полутонов и не исказите. В этом примере неискажение не имеет никакого эффекта, потому что изображения являются синтетическим продуктом без искажения объектива. Однако для действительных изображений, неискажение необходимо.
prevI = undistortImage(rgb2gray(Irgb), cameraParams); % Detect features. prevPoints = detectSURFFeatures(prevI, 'MetricThreshold', 500); % Select a subset of features, uniformly distributed throughout the image. numPoints = 150; prevPoints = selectUniform(prevPoints, numPoints, size(prevI)); % Extract features. Using 'Upright' features improves matching quality if % the camera motion involves little or no in-plane rotation. prevFeatures = extractFeatures(prevI, prevPoints, 'Upright', true); % Add the first view. Place the camera associated with the first view % at the origin, oriented along the Z-axis. viewId = 1; vSet = addView(vSet, viewId, 'Points', prevPoints, 'Orientation', eye(3),... 'Location', [0 0 0]);
Постройте начальное положение камеры
Создайте два графических объекта камеры, представляющие предполагаемое и фактические положения камеры на основе наземных данных об истине из Нового набора данных Цукубы.
% Setup axes. figure axis([-220, 50, -140, 20, -50, 300]); % Set Y-axis to be vertical pointing down. view(gca, 3); set(gca, 'CameraUpVector', [0, -1, 0]); camorbit(gca, -120, 0, 'data', [0, 1, 0]); grid on xlabel('X (cm)'); ylabel('Y (cm)'); zlabel('Z (cm)'); hold on % Plot estimated camera pose. cameraSize = 7; camEstimated = plotCamera('Size', cameraSize, 'Location',... vSet.Views.Location{1}, 'Orientation', vSet.Views.Orientation{1},... 'Color', 'g', 'Opacity', 0); % Plot actual camera pose. camActual = plotCamera('Size', cameraSize, 'Location', ... groundTruthPoses.Location{1}, 'Orientation', ... groundTruthPoses.Orientation{1}, 'Color', 'b', 'Opacity', 0); % Initialize camera trajectories. trajectoryEstimated = plot3(0, 0, 0, 'g-'); trajectoryActual = plot3(0, 0, 0, 'b-'); legend('Estimated Trajectory', 'Actual Trajectory'); title('Camera Trajectory');
Оцените положение второго представления
Обнаружьте и извлеките функции от второго представления и совпадайте с ними к первому представлению с помощью helperDetectAndMatchFeatures. Оцените положение второго представления относительно первого представления с помощью helperEstimateRelativePose и добавьте его в viewSet.
% Read and display the image.
viewId = 2;
Irgb = readimage(images, viewId);
step(player, Irgb);
% Convert to gray scale and undistort. I = undistortImage(rgb2gray(Irgb), cameraParams); % Match features between the previous and the current image. [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(... prevFeatures, I); % Estimate the pose of the current view relative to the previous view. [orient, loc, inlierIdx] = helperEstimateRelativePose(... prevPoints(indexPairs(:,1)), currPoints(indexPairs(:,2)), cameraParams); % Exclude epipolar outliers. indexPairs = indexPairs(inlierIdx, :); % Add the current view to the view set. vSet = addView(vSet, viewId, 'Points', currPoints, 'Orientation', orient, ... 'Location', loc); % Store the point matches between the previous and the current views. vSet = addConnection(vSet, viewId-1, viewId, 'Matches', indexPairs);
Местоположение второго представления относительно первого представления может только быть восстановлено до неизвестного масштабного коэффициента. Вычислите масштабный коэффициент из наземной истины с помощью helperNormalizeViewSet, моделировав внешний датчик, который использовался бы в типичной монокулярной визуальной системе одометрии.
vSet = helperNormalizeViewSet(vSet, groundTruthPoses);
Обновите графики траектории камеры с помощью helperUpdateCameraPlots и helperUpdateCameraTrajectories.
helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ... groundTruthPoses); helperUpdateCameraTrajectories(viewId, trajectoryEstimated, trajectoryActual,... poses(vSet), groundTruthPoses);
prevI = I; prevFeatures = currFeatures; prevPoints = currPoints;
Загрузите оценку траектории камеры Используя глобальную корректировку пакета
Найдите 3D-к-2D соответствия между мировыми точками триангулированными от предыдущих двух представлений и отобразите точки от текущего представления. Используйте helperFindEpipolarInliers, чтобы найти соответствия, которые удовлетворяют epipolar ограничение, и затем используют helperFind3Dto2DCorrespondences, чтобы триангулировать 3-D точки от предыдущих двух представлений и найти соответствующие 2D точки в текущем представлении.
Вычислите мировое положение камеры для текущего представления путем решения проблемы перспективного n значений (PnP) с помощью estimateWorldCameraPose. Для первых 15 представлений используйте глобальную корректировку пакета, чтобы совершенствовать целую траекторию. Используя глобальную корректировку пакета к ограниченному количеству начальных загрузок представлений, оценивающих остальную часть траектории камеры, и это не предельно дорого.
for viewId = 3:15 % Read and display the next image Irgb = readimage(images, viewId); step(player, Irgb); % Convert to gray scale and undistort. I = undistortImage(rgb2gray(Irgb), cameraParams); % Match points between the previous and the current image. [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(... prevFeatures, I); % Eliminate outliers from feature matches. inlierIdx = helperFindEpipolarInliers(prevPoints(indexPairs(:,1)),... currPoints(indexPairs(:, 2)), cameraParams); indexPairs = indexPairs(inlierIdx, :); % Triangulate points from the previous two views, and find the % corresponding points in the current view. [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet,... cameraParams, indexPairs, currPoints); % Since RANSAC involves a stochastic process, it may sometimes not % reach the desired confidence level and exceed maximum number of % trials. Disable the warning when that happens since the outcomes are % still valid. warningstate = warning('off','vision:ransac:maxTrialsReached'); % Estimate the world camera pose for the current view. [orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, ... cameraParams, 'Confidence', 99.99, 'MaxReprojectionError', 0.8); % Restore the original warning state warning(warningstate) % Add the current view to the view set. vSet = addView(vSet, viewId, 'Points', currPoints, 'Orientation', orient, ... 'Location', loc); % Store the point matches between the previous and the current views. vSet = addConnection(vSet, viewId-1, viewId, 'Matches', indexPairs); tracks = findTracks(vSet); % Find point tracks spanning multiple views. camPoses = poses(vSet); % Get camera poses for all views. % Triangulate initial locations for the 3-D world points. xyzPoints = triangulateMultiview(tracks, camPoses, cameraParams); % Refine camera poses using bundle adjustment. [~, camPoses] = bundleAdjustment(xyzPoints, tracks, camPoses, ... cameraParams, 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-9,... 'RelativeTolerance', 1e-9, 'MaxIterations', 300); vSet = updateView(vSet, camPoses); % Update view set. % Bundle adjustment can move the entire set of cameras. Normalize the % view set to place the first camera at the origin looking along the % Z-axes and adjust the scale to match that of the ground truth. vSet = helperNormalizeViewSet(vSet, groundTruthPoses); % Update camera trajectory plot. helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ... groundTruthPoses); helperUpdateCameraTrajectories(viewId, trajectoryEstimated, ... trajectoryActual, poses(vSet), groundTruthPoses); prevI = I; prevFeatures = currFeatures; prevPoints = currPoints; end
Оцените остающуюся траекторию камеры Используя оконную корректировку пакета
Оцените, что остающаяся траектория камеры при помощи оконной корректировки пакета только совершенствовала последние 15 представлений в порядке ограничить объем вычисления. Кроме того, свяжитесь, корректировка не должна быть названа для каждого представления, потому что estimateWorldCameraPose вычисляет положение в тех же модулях как 3-D точки. Этот раздел вызывает корректировку пакета к каждому 7-му представлению. Размер окна и частота вызова корректировки пакета были выбраны экспериментально.
for viewId = 16:numel(images.Files) % Read and display the next image Irgb = readimage(images, viewId); step(player, Irgb); % Convert to gray scale and undistort. I = undistortImage(rgb2gray(Irgb), cameraParams); % Match points between the previous and the current image. [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(... prevFeatures, I); % Triangulate points from the previous two views, and find the % corresponding points in the current view. [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet, ... cameraParams, indexPairs, currPoints); % Since RANSAC involves a stochastic process, it may sometimes not % reach the desired confidence level and exceed maximum number of % trials. Disable the warning when that happens since the outcomes are % still valid. warningstate = warning('off','vision:ransac:maxTrialsReached'); % Estimate the world camera pose for the current view. [orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, ... cameraParams, 'MaxNumTrials', 5000, 'Confidence', 99.99, ... 'MaxReprojectionError', 0.8); % Restore the original warning state warning(warningstate) % Add the current view and connection to the view set. vSet = addView(vSet, viewId, 'Points', currPoints, 'Orientation', orient, ... 'Location', loc); vSet = addConnection(vSet, viewId-1, viewId, 'Matches', indexPairs); % Refine estimated camera poses using windowed bundle adjustment. Run % the optimization every 7th view. if mod(viewId, 7) == 0 % Find point tracks in the last 15 views and triangulate. windowSize = 15; startFrame = max(1, viewId - windowSize); tracks = findTracks(vSet, startFrame:viewId); camPoses = poses(vSet, startFrame:viewId); [xyzPoints, reprojErrors] = triangulateMultiview(tracks, camPoses, ... cameraParams); % Hold the first two poses fixed, to keep the same scale. fixedIds = [startFrame, startFrame+1]; % Exclude points and tracks with high reprojection errors. idx = reprojErrors < 2; [~, camPoses] = bundleAdjustment(xyzPoints(idx, :), tracks(idx), ... camPoses, cameraParams, 'FixedViewIDs', fixedIds, ... 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-9,... 'RelativeTolerance', 1e-9, 'MaxIterations', 300); vSet = updateView(vSet, camPoses); % Update view set. end % Update camera trajectory plot. helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ... groundTruthPoses); helperUpdateCameraTrajectories(viewId, trajectoryEstimated, ... trajectoryActual, poses(vSet), groundTruthPoses); prevI = I; prevFeatures = currFeatures; prevPoints = currPoints; end
hold off
Сводные данные
Этот пример показал, как оценить траекторию калиброванной монокулярной камеры от последовательности представлений. Заметьте, что предполагаемая траектория точно не совпадает с наземной истиной. Несмотря на нелинейное улучшение положений камеры, ошибки при закрытых дверях позируют, оценка накапливаются, приводя к дрейфу. В визуальных системах одометрии эта проблема обычно решается путем плавления информации от нескольких датчиков, и путем выполнения закрытия цикла.
Ссылки
[1] Мартин Перис Марторелл, Atsuto Maki, Сара Марталл, Yasuhiro Ohkawa, Kazuhiro Фукуи, "К Симуляции Управляемая Система Видения Стерео". Продолжения ICPR, pp.1038-1042, 2012.
[2] Сара Марталл, Мартин Перис Марторелл, Kazuhiro Фукуи, "Реалистический Набор данных Стереоизображения CG с Наземными Картами Несоизмеримости Истины", Продолжения семинара ICPR TrakMark2012, pp.40-42, 2012.
[3] M.I.A. Луракис и А.А. Аргирос (2009). "SBA: пакет программного обеспечения для типичной разреженной корректировки пакета". Транзакции ACM на математическом программном обеспечении (ACM) 36 (1): 1-30.
[4] Р. Хартли, А. Зиссермен, "Несколько просматривают геометрию в компьютерном зрении", издательство Кембриджского университета, 2003.
[5] Б. Триггс; П. Маклочлан; Р. Хартли; А. Фицджиббон (1999). "Свяжите Корректировку: современный Синтез". Продолжения Международного семинара на Алгоритмах визуализации. Springer-Verlag. стр 298-372.
[6] X.-S. Гао, X.-R. Как, Дж. Тан и H.-F. Ченг, "Классификация полных решений для Проблемы "Перспектива Три Точки"", Анализ Шаблона Сделки IEEE и Искусственный интеллект, издание 25, № 8, стр 930-943, 2003.