Визуальная одометрия - это процесс определения местоположения и ориентации камеры путем анализа последовательности изображений. Визуальная одометрия используется в различных приложениях, таких как мобильные роботы, беспилотные автомобили и беспилотные летательные транспортные средства. В этом примере показано, как оценить траекторию одной калиброванной камеры из последовательности изображений.
В этом примере показано, как оценить траекторию калиброванной камеры из последовательности 2-D представлений. Этот пример использует изображения из стерео- Набор данных New Tsukuba, созданного в CVLAB Университета Цукуба. (https://cvlab.cs.tsukuba.ac.jp). Набор данных состоит из синтетических изображений, сгенерированных с помощью компьютерной графики, и включает основную истину положений камеры.
Без дополнительной информации траектория монокулярной камеры может быть восстановлена только до неизвестного масштабного коэффициента. Монокулярные системы визуальной одометрии, используемые на мобильных роботах или автономных транспортных средствах, обычно получают масштабный коэффициент от другого датчика (например, колесного одометра или GPS) или от объекта известного размера в сцене. Этот пример вычисляет коэффициент шкалы из основной истины.
Пример разделен на три части:
Оценка положения второго вида относительно первого вида. Оцените положение второго представления путем оценки существенной матрицы и разложения ее на местоположение и ориентацию камеры.
Загрузка, оценивающая траекторию камеры с помощью глобальной регулировки пучка. Устраните выбросы с помощью эпиполярного ограничения. Найдите 3D-to-2D соответствия между точками, триангулированными из двух предыдущих видов, и текущим видом. Вычислите положение мировой камеры для текущего вида, решив задачу perspective-n-point (PnP). Оценка положения камеры неизбежно приводит к ошибкам, которые накапливаются со временем. Этот эффект называется дрейфом. Чтобы уменьшить дрейф, пример уточняет все положения, оцененные до сих пор, используя регулировку пучка.
Оценка оставшейся траектории камеры с помощью настройки пакета с окном. С каждым новым представлением время, необходимое для уточнения всех положений, увеличивается. Корректировка пакета с окном является способом уменьшить время расчета только путем оптимизации последних n представлений, а не всей траектории. Время расчета дополнительно уменьшается, не вызывая регулировку пучка для каждого представления.
Этот пример использует изображения из стерео- Набор данных New Tsukuba, созданного в CVLAB Университета Цукуба. Если вы используете эти изображения в своих собственных работах или публикациях, пожалуйста, приведите следующие статьи:
[1] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui, «To a Simulation Driven Stereo Vision System». Материалы ICPR, стр. 1038-1042, 2012.
[2] Sarah Martull, Martin Peris Martorell, Kazuhiro Fukui, «Realistic CG Stereo Image Dataset with Основная Истина 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);
Преобразуйте в серую шкалу и undistort. В этом примере неискажение не имеет никакого эффекта, потому что изображения синтетические, без искажений объектива. Однако для реальных изображений необходимо неискажение.
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] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui, «To a Simulation Driven Stereo Vision System». Материалы ICPR, стр. 1038-1042, 2012.
[2] Sarah Martull, Martin Peris Martorell, Kazuhiro Fukui, «Realistic CG Stereo Image Dataset with Основная Истина Disparity Maps», Труды семинара ICPR TrakMark2012, стр. 40-42, 2012.
[3] M.I.A. Луракис и А. А. Аргирос (2009). «SBA: программный пакет для типовой регулировки разреженного пакета». Транзакции ACM на математическом программном обеспечении (ACM) 36 (1): 1-30.
[4] R. Hartley, A. Zisserman, «Multiple View Geometry in Компьютерное Зрение», Cambridge University Press, 2003.
[5] Б. Триггеры; П. Маклаучлан; Р. Хартли; А. Фицгиббон (1999). Bundle Adjustment: A Modern Synthesis (неопр.) (недоступная ссылка). Материалы Международного практикума по алгоритмам видения. Springer-Verlag. с. 298-372.
[6] X.-S. Гао, X.-R. Хоу, Ж. Тан и Х.-Ф. Cheng, «Complete Solution Classification for the Perspective-Three-Point Problem», IEEE Trans. Pattern Analysis and Machine Intelligence, vol. 25, no. 8, pp. 930-943, 2003.