Визуальная одометрия - процесс определения местоположения и ориентации камеры путём анализа последовательности изображений. Визуальная одометрия используется в различных приложениях, таких как мобильные роботы, самоуправляемые автомобили и беспилотные летательные аппараты. В этом примере показано, как оценить траекторию одной откалиброванной камеры по последовательности изображений.
В этом примере показано, как оценить траекторию калиброванной камеры по последовательности 2-D видов. В этом примере используются изображения из набора данных New Tsukuba Stereo, созданного в CVLAB Университета Tsukuba. (https://cvlab.cs.tsukuba.ac.jp). Набор данных состоит из синтетических изображений, генерируемых с помощью компьютерной графики, и включает в себя позы наземной камеры истинности.
Без дополнительной информации траектория монокулярной камеры может быть восстановлена только до неизвестного масштабного коэффициента. Системы монокулярной визуальной одометрии, используемые на мобильных роботах или автономных транспортных средствах, обычно получают масштабный коэффициент от другого датчика (например, колесного одометра или GPS) или от объекта известного размера в сцене. В этом примере вычисляется масштабный коэффициент на основе истинного состояния земли.
Пример разделен на три части:
Оценка позы второго вида относительно первого вида. Оцените положение второго вида, оценив основную матрицу и разложив ее на расположение и ориентацию камеры.
Начальная оценка траектории камеры с помощью глобальной регулировки пучка. Устранение отклонений с помощью эпиполярного ограничения. Поиск 3D-to-2D соответствий между точками, триангулированными из двух предыдущих видов, и текущим видом. Вычислите позу мировой камеры для текущего вида, решив задачу «перспектива-n-точка» (PnP). Оценка камеры неизбежно приводит к ошибкам, которые накапливаются с течением времени. Этот эффект называется дрейфом. Для уменьшения дрейфа в примере уточняются все позиции, оцененные до сих пор с помощью регулировки пучка.
Оценка оставшейся траектории камеры с помощью подстройки пучка с помощью окна. С каждым новым видом увеличивается время, необходимое для уточнения всех поз. Корректировка пакета с окнами - это способ сократить время вычислений, оптимизируя только последние n видов, а не всю траекторию. Время вычисления дополнительно сокращается за счет того, что не вызывается регулировка пучка для каждого вида.
В этом примере используются изображения из набора данных New Tsukuba Stereo, созданного в CVLAB Университета Tsukuba. Если вы используете эти изображения в своей работе или публикациях, пожалуйста, приведите следующие документы:
[1] Мартин Перис Марторелл, Ацуто Маки, Сара Мартулл, Ясухиро Охкава, Кадзухиро Фукуи, «К симуляционной управляемой системе стереовидения». Материалы МКПП, стр. 1038-1042, 2012.
[2] Сара Мартулл, Мартин Перис Марторелл, Кадзухиро Фукуи, «Реалистичный набор данных CG Stereo Image with Ground Truth Disparity Maps», Материалы семинара ICPR TrakMark2012, стр. 40-42, 2012.
images = imageDatastore(fullfile(toolboxdir('vision'), 'visiondata', 'NewTsukuba')); % Load ground truth camera poses. load(fullfile(toolboxdir('vision'), 'visiondata', 'visualOdometryGroundTruth.mat'));
Использовать imageviewset объект для хранения и управления точками изображения и позицией камеры, связанной с каждым видом, а также совпадениями точек между парами видов. После заполнения imageviewset объект, его можно использовать для поиска дорожек точек на нескольких видах и извлечения поз камеры, которые будут использоваться triangulateMultiview и bundleAdjustment функции.
% Create an empty imageviewset object to manage the data associated with each view. vSet = imageviewset; % Read and display the first image. Irgb = readimage(images, 1); player = vision.VideoPlayer('Position', [20, 400, 650, 510]); step(player, Irgb);
% Create the camera intrinsics object using camera intrinsics from the % New Tsukuba dataset. focalLength = [615 615]; % specified in units of pixels principalPoint = [320 240]; % in pixels [x, y] imageSize = size(Irgb,[1,2]); % in pixels [mrows, ncols] intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);
Преобразование в серый масштаб и без искажения. В этом примере отсутствие искажения не имеет эффекта, поскольку изображения являются синтетическими, без искажения объектива. Однако для реальных образов необходимо неискажение.
prevI = undistortImage(im2gray(Irgb), intrinsics); % Detect features. prevPoints = detectSURFFeatures(prevI, 'MetricThreshold', 500); % Select a subset of features, uniformly distributed throughout the image. numPoints = 200; 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, rigid3d(eye(3), [0 0 0]), 'Points', prevPoints);
Создайте два графических объекта камеры, представляющих предполагаемые и фактические позы камеры на основе данных о истинности земли из набора данных New Tsukuba.
% 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; camPose = poses(vSet); camEstimated = plotCamera(camPose, 'Size', cameraSize,... 'Color', 'g', 'Opacity', 0); % Plot actual camera pose. camActual = plotCamera('Size', cameraSize, 'AbsolutePose', ... rigid3d(groundTruthPoses.Orientation{1}, groundTruthPoses.Location{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и добавьте его в imageviewset.
% Read and display the image.
viewId = 2;
Irgb = readimage(images, viewId);
step(player, Irgb);
% Convert to gray scale and undistort. I = undistortImage(im2gray(Irgb), intrinsics); % 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)), intrinsics); % Exclude epipolar outliers. indexPairs = indexPairs(inlierIdx, :); % Add the current view to the view set. vSet = addView(vSet, viewId, rigid3d(orient, loc), 'Points', currPoints); % 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-to-2D соответствий между точками мира, триангулированными из двух предыдущих видов, и точками изображения из текущего вида. Использовать helperFindEpipolarInliers найти совпадения, удовлетворяющие эпиполярному ограничению, а затем использовать helperFind3Dto2DCorrespondences для триангуляции 3-D точек из двух предыдущих видов и поиска соответствующих 2-D точек на текущем виде.
Вычислите позу мировой камеры для текущего вида, решив задачу «перспектива-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(im2gray(Irgb), intrinsics); % 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)), intrinsics); indexPairs = indexPairs(inlierIdx, :); % Triangulate points from the previous two views, and find the % corresponding points in the current view. [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet,... intrinsics, 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, intrinsics); % Restore the original warning state warning(warningstate) % Add the current view to the view set. vSet = addView(vSet, viewId, rigid3d(orient, loc), 'Points', currPoints); % 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, intrinsics); % Refine camera poses using bundle adjustment. [~, camPoses] = bundleAdjustment(xyzPoints, tracks, camPoses, ... intrinsics, 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-12,... 'RelativeTolerance', 1e-12, 'MaxIterations', 200, 'FixedViewID', 1); 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(im2gray(Irgb), intrinsics); % 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, ... intrinsics, 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, intrinsics); % Restore the original warning state warning(warningstate) % Add the current view and connection to the view set. vSet = addView(vSet, viewId, rigid3d(orient, loc), 'Points', currPoints); 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, intrinsics); % 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, intrinsics, 'FixedViewIDs', fixedIds, ... 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-12,... 'RelativeTolerance', 1e-12, 'MaxIterations', 200); 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] Мартин Перис Марторелл, Ацуто Маки, Сара Мартулл, Ясухиро Охкава, Кадзухиро Фукуи, «К симуляционной управляемой системе стереовидения». Материалы МКПП, стр. 1038-1042, 2012.
[2] Сара Мартулл, Мартин Перис Марторелл, Кадзухиро Фукуи, «Реалистичный набор данных CG Stereo Image with Ground Truth Disparity Maps», Материалы семинара ICPR TrakMark2012, стр. 40-42, 2012.
[3] M.I.A. Луракис и А. А. Аргирос (2009). «Преимущества для малого бизнеса: пакет программного обеспечения для настройки общего разреженного пакета». Транзакции ACM по математическому программному обеспечению (ACM) 36 (1): 1-30.
[4] Р. Хартли, А. Зиссерман, «Геометрия множественного вида в компьютерном зрении», Cambridge University Press, 2003.
[5] B. Триггеры; П. Маклаухлан; Р. Хартли; А. Фицгиббон (1999). «Корректировка пучка: современный синтез». Материалы Международного рабочего совещания по алгоритмам видения. Спрингер-Верлаг. стр. 298-372.
[6] X.-S. Гао, X.-R. Хоу, Дж. Тан и Х.-Ф. Cheng, «Полная классификация решений для проблемы перспективы - три точки», IEEE Trans. Pattern Analysis and Machine Intelligence, том 25, № 8, стр. 930-943, 2003.