Odometría visual-inercial monocular mediante gráfico de factores
La odometría visual-inercial monocular estima la posición y orientación del robot utilizando datos de la cámara y del sensor de la unidad de medición inercial (IMU). La estimación del estado basada en la cámara es precisa durante la navegación a baja velocidad. Sin embargo, la estimación basada en cámaras enfrenta desafíos como el desenfoque de movimiento y la pérdida de seguimiento a velocidades más altas. Además, la estimación basada en una cámara monocular puede estimar poses a una escala arbitraria. Por otro lado, la navegación inercial puede manejar fácilmente la navegación de alta velocidad y estimar poses a escala mundial. Puede combinar las ventajas de ambos tipos de datos de sensores para lograr una mayor precisión mediante la optimización del gráfico de factores estrechamente acoplados. Para lograr un buen rendimiento en el tiempo de ejecución, en cada paso de optimización solo se optimiza una pequeña parte del gráfico de factores completo que contiene solo las mediciones más recientes. Esta variante de optimización de gráficos de factores se conoce popularmente como ventana deslizante u optimización de gráficos parciales.
Visión general
El sistema visual-inercial implementado en este ejemplo consta de una versión simplificada de la interfaz de odometría visual monocular del algoritmo VINS [1] y una interfaz de gráfico de factores.
La interfaz de odometría visual tiene responsabilidades similares a los algoritmos de estructura estándar a partir de movimiento (SfM), como FAST orientado y BRIEF rotado (ORB) y localización y mapeo simultáneos (SLAM). La interfaz de odometría visual detecta y rastrea puntos clave en múltiples fotogramas, estima las poses de la cámara y triangula puntos 3D utilizando geometría de vista múltiple. El back-end del gráfico de factores optimiza conjuntamente las poses estimadas de la cámara, los puntos 3D, la velocidad de la IMU y el sesgo. Antes de fusionar las mediciones de la cámara y la IMU, debe alinear la cámara y la IMU para calcular la escala de pose de la cámara, la rotación de la gravedad y la velocidad y el sesgo iniciales de la IMU.
Configuración
Este ejemplo utiliza el conjunto de datos Blackbird (NYC Subway Winter) para demostrar el flujo de trabajo de odometría visual-inercial. Descargue el conjunto de datos.
data = helperDownloadData();
Corrige la semilla aleatoria para que sea repetible.
rng(0)
Inicializar parámetros del algoritmo
Utilice la función helperVIOParameters para inicializar y ajustar estos parámetros:
Parámetros visuales del front-end
Umbral de consenso de muestra aleatoria (RANSAC) (
F_Threshold), confianza (F_Confidence)e iteraciones (F_Iterations)Error bidireccional del rastreador Kanade-Lucas-Tomasi (KLT) (
KLT_BiErr), número de niveles (KLT_Levels) y tamaño de bloque (KLT_Block)Paralaje mínimo para la selección de fotogramas clave (
keyFrameParallax) y la triangulación de nuevos puntos 3-D (triangulateParallax)Número mínimo de puntos clave para rastrear en cada cuadro (
numTrackedThresh)Número máximo de puntos clave para rastrear en cada cuadro (
maxPointsToTrack)
Parámetros de back-end de optimización del gráfico de factores
Opciones de solver de gráficos de factores (
SolverOpts)Tamaño de ventana deslizante (
windowSize): número máximo de fotogramas recientes o nodos de pose para optimizar en el gráfico de factores. Por lo general, la estimación del estado se realiza de forma incremental. Lo que significa que el sistema procesa secuencialmente los datos del cuadro, como imágenes de la cámara y mediciones de IMU, en cada paso de tiempo para estimar la pose o el estado del robot en ese paso de tiempo en particular. La optimización del gráfico de factores puede utilizar todas las mediciones y estimaciones del sensor hasta el marco actual para refinar las estimaciones del estado. El uso de todos los datos del marco en el gráfico de factores en cada paso de optimización produce soluciones más precisas, pero puede ser muy extenso desde el punto de vista computacional. Para mejorar el rendimiento del tiempo de ejecución, puede considerar solo algunas mediciones de fotogramas recientes para la optimización. Este tipo de optimización se conoce como optimización de ventana deslizante u optimización de gráfico parcial.Velocidad de fotogramas a la que ejecutar la optimización del gráfico de factores (
optimizationFrequency). Después de procesar un número fijo de cuadros especificados por optimizaciónFrequency, la optimización del gráfico de factores se utiliza para refinar las estimaciones de pose. Llamar a la optimización del gráfico después de cada cuadro produce estimaciones más precisas pero aumenta el tiempo de ejecución.
params = helperVIOParameters();
% Set to true if IMU data is available.
useIMU = true;Inicializar variables.
status = struct("firstFrame",true,"isMapInitialized",false,"isIMUAligned",false,"Mediandepth",false); % Set to true to attempt camera-IMU alignment. readyToAlignCameraAndIMU = false; % Set initial scene median depth. initialSceneMedianDepth = 4; viewId = 0; removedFrameIds = []; allCameraTracks = cell(1,5000); % Enable visualization. vis = true; showMatches = false; if vis % Figure variables. axMatches = []; axTraj = []; axMap = []; end
Configure el gráfico de factores para la optimización del gráfico de factores estrechamente acoplados de back-end.
% Set up factor graph for fusion. slidingWindowFactorGraph = factorGraph; maxFrames = 10000; maxLandmarks = 100000; ids = helperGenerateNodeID(slidingWindowFactorGraph,maxFrames,maxLandmarks); % Information matrix (measure of accuracy) associated with the camera % projection factors that relate 3-D points and estimated camera poses. cameraInformation = ((data.intrinsics.K(1,1)/1.5)^2)*eye(2); % Initialize IMU parameters. The fusion accuracy depends on these parameters. imuParams = factorIMUParameters(SampleRate=100,GyroscopeNoise=0.1, ... GyroscopeBiasNoise=3e-3,AccelerometerNoise=0.3, ... AccelerometerBiasNoise=1e-3,ReferenceFrame="ENU");
Cree el rastreador de puntos para realizar un seguimiento de puntos clave en varios fotogramas.
tracker = vision.PointTracker(MaxBidirectionalError=params.KLT_BiErr, ...
NumPyramidLevels=params.KLT_Levels,BlockSize=params.KLT_Block);Configure el administrador de funciones para mantener seguimientos de puntos clave.
fManager = helperFeaturePointManager(data.intrinsics,params,maxFrames,maxLandmarks);
% Set up the key point detector.
fManager.DetectorFunc = @(I)helperDetectKeyPoints(I);Cree una vista de imagen configurada para mantener las poses del marco.
vSet = imageviewset;
Especifique el primer y último fotograma a procesar a partir del conjunto de datos. Luego, procese el primer fotograma.
% IMU data is available from frame number 40 in the data set. startFrameIdx = 40; % Index of the last frame to process in this example. For reasonable % example execution time, process up to only frame 1000 of the data set. endFrameIdx = 1000; allFrameIds = startFrameIdx:endFrameIdx; % In the first frame, detect new key points and initialize the tracker for % future tracking. status.firstFrame = false; I = data.images{startFrameIdx}; if params.Equalize % Enhance contrast if images are dark. I = adapthisteq(I,NumTiles=params.NumTiles,ClipLimit=params.ClipLimit); end if params.Undistort % Undistort if images contain perspective distortion. I = undistortImage(I,data.intrinsics); end % Assign a unique view ID for each processed camera frame or image. viewId = viewId + 1; currPoints = createNewFeaturePoints(fManager,I); updateSlidingWindow(fManager,I,currPoints,true(size(currPoints,1),1),viewId); initialize(tracker, currPoints, I); prevI = I; firstI = I; vSet = addView(vSet,viewId,rigidtform3d);
Comience un bucle a través de todo el conjunto de datos.
for curIdx = allFrameIds(2:end)Preprocesamiento de imágenes
El preprocesamiento de imágenes implica estos pasos:
Ecualizar: mejora el contraste de una imagen para corregir la iluminación tenue, lo que puede afectar la extracción y el seguimiento de funciones.
Undistort: corrige las distorsiones radiales y tangenciales que pueden afectar la estimación del estado.
% Read image data. I = data.images{curIdx}; if params.Equalize % Enhance contrast if images are dark. I = adapthisteq(I,NumTiles=params.NumTiles,ClipLimit=params.ClipLimit); end if params.Undistort % Undistort if images contain perspective distortion. I = undistortImage(I,data.intrinsics); end % Assign a unique view ID for each processed camera frame or image. viewId = viewId + 1;
Seguimiento de funciones
Para calcular la pose del fotograma de una cámara, debe calcular las correspondencias 2D-2D (pistas de puntos de imagen 2D en varios fotogramas). Hay varias formas de estimar puntos característicos 2-D que ven el mismo punto de referencia (pistas de puntos clave), pero este ejemplo utiliza un rastreador Kalman para rastrear puntos característicos en múltiples imágenes.
No todas las pistas son precisas y pueden contener valores atípicos. El rendimiento del seguimiento también depende de los parámetros del rastreador de Kalman, como el error bidireccional. Incluso en un caso ideal, se pueden esperar algunas pistas no válidas, como las debidas a estructuras repetitivas. Como tal, el rechazo de valores atípicos es una tarea fundamental en el seguimiento de funciones. Para rechazar valores atípicos de las pistas, utilice la descomposición matricial fundamental en el administrador de puntos característicos mientras actualiza la ventana deslizante con los últimos seguimientos de puntos característicos.
% Track previous frame points in the current frame.
[currPoints,validIdx] = tracker(I);
if status.isMapInitialized
[prevPoints,pointIds,isTriangulated] = getKeyPointsInView(fManager,viewId-1);
end
% Update the sliding window after tracking features in the current
% frame. If the sliding window already contains maximum number of
% frames specified using windowSize, one frame with id
% removeFrameId will be removed from the window to accommodate
% space for the current frame.
[removedFrameId,windowState] = updateSlidingWindow(fManager,I,currPoints,validIdx,viewId);
if (removedFrameId > fManager.slidingWindowViewIds(1))
% Store non-key frames or removed frame IDs.
removedFrameIds(end + 1) = removedFrameId; %#ok
end
Visualice las pistas de puntos característicos entre el último fotograma clave y el fotograma actual.
if status.isMapInitialized svIds = getSlidingWindowIds(fManager); if length(svIds) > 2 [matches1,matches2] = get2DCorrespondensesBetweenViews(fManager,svIds(end-2),viewId); if vis && showMatches if isempty(axMatches) %#ok axMatches = axes(figure); %#ok end % Visualize matches between the last key frame and the % current view. showMatchedFeatures(data.images{allFrameIds(svIds(end-2))},I,matches1,matches2, ... Parent=axMatches); end end end
Estructura inicial a partir del movimiento (SfM)
Las lecturas del acelerómetro y giroscopio de los datos de la IMU contienen algo de sesgo y ruido. Para estimar los valores de sesgo, debe obtener estimaciones de pose precisas entre los primeros fotogramas. Puede lograr esto utilizando SfM. La SFM implica estos pasos principales:
Cuando haya suficiente paralaje entre el primer fotograma clave y el fotograma actual, estime la pose relativa entre los dos, utilizando correspondencias 2D-2D (pistas de puntos clave en varios fotogramas).
Triangule puntos tridimensionales utilizando las poses mundiales de fotogramas clave y correspondencias 2D-2D.
Realice un seguimiento de los puntos 3D en el fotograma actual y calcule la pose del fotograma actual utilizando correspondencias 3D-2D.
if ~status.isMapInitialized if windowState.FirstFewViews % Accept the first few camera views. vSet = addView(vSet,viewId,rigidtform3d); elseif windowState.EnoughParallax % Estimate relative pose between the first key frame in the % window and the current frame. svIds = getSlidingWindowIds(fManager); [matches1,matches2] = get2DCorrespondensesBetweenViews(fManager,svIds(end-1),svIds(end-0)); valRel = false(size(matches1,1),1); for k = 1:10 [F1,valRel1] = estimateFundamentalMatrix( ... matches1,matches2,Method="RANSAC", ... NumTrials=params.F_Iterations,DistanceThreshold=params.F_Threshold, ... Confidence=params.F_Confidence); if length(find(valRel)) < length(find(valRel1)) valRel = valRel1; F = F1; end end inlierPrePoints = matches1(valRel,:); inlierCurrPoints = matches2(valRel,:); relPose = estrelpose(F,data.intrinsics, ... inlierPrePoints,inlierCurrPoints); % Get the table containing the previous camera pose. prevPose = rigidtform3d; % Compute the current camera pose in the global coordinate % system relative to the first view. currPose = relPose; %vSet = addView(vSet,svIds(end-1),currPose); vSet = addView(vSet,viewId,currPose); status.isMapInitialized = true; if vis axisSFM = axes(figure); %#ok showMatchedFeatures(firstI,I,matches1,matches2, ... Parent=axisSFM); title(axisSFM,"Enough Parallax Between Key Frames"); end end else
Alineación cámara-IMU
Para optimizar las mediciones de la cámara y la IMU, debe alinearlas llevándolas al mismo marco de coordenadas base y escala. La alineación consta principalmente de estas tareas principales:
Calcule la escala de pose de la cámara para que sea similar a la IMU o escala mundial.
Calcule la rotación de gravedad necesaria para rotar el vector de gravedad desde el marco de referencia de navegación local de la IMU hasta el marco de referencia inicial de la cámara. La inversa de esta rotación alinea el eje z de la cámara con el marco de referencia de navegación local.
Calcule el sesgo inicial de IMU.
if ~status.isIMUAligned && readyToAlignCameraAndIMU svIds = getSlidingWindowIds(fManager); % Because you have not yet computed the latest frame pose, % So use only the past few frames for alignment. svIds = svIds(1:end-1); [gyro,accel] = helperExtractIMUDataBetweenViews( ... data.gyroReadings,data.accelReadings,data.timeStamps,allFrameIds(svIds)); [xyz] = getXYZPoints(fManager,xyzIds); % Align camera with IMU. camPoses = poses(vSet,svIds); [gRot,scale,info] = ... estimateGravityRotationAndPoseScale(camPoses,gyro,accel, ... SensorTransform=data.camToIMUTransform,IMUParameters=imuParams); disp("Estimated scale: " + scale);
Si la alineación es exitosa, actualice las poses de la cámara, los puntos 3D y agregue factores IMU entre los fotogramas iniciales en la ventana deslizante actual.
if info.IsSolutionUsable && scale > 1e-3 status.isIMUAligned = true; posesUpdated = poses(vSet); % Transform camera poses to navigation frame using % computed gravity rotation and pose scale. [posesUpdated,xyz] = helperTransformToNavigationFrame(posesUpdated,xyz,gRot,scale); vSet = updateView(vSet,posesUpdated); % Plot the scaled and unscaled estimated trajectory against % ground truth. if vis p1 = data.camToIMUTransform.transform(vertcat(camPoses.AbsolutePose.Translation)); axAlign = axes(figure); %#ok g1 = data.gTruth(allFrameIds(camPoses.ViewId),1:3); plot3(g1(:,1),g1(:,2),g1(:,3),"g",Parent=axAlign); hold(axAlign,"on") plot3(scale*p1(:,1),scale*p1(:,2),scale*p1(:,3),"r",Parent=axAlign); plot3(p1(:,1),p1(:,2),p1(:,3),"b",Parent=axAlign); hold(axAlign,"off") legend(axAlign,"Ground Truth","Estimated scaled trajectory","Estimated trajectory") title("Camera-IMU Alignment") drawnow end if status.isIMUAligned % After alignment, add IMU factors to factor graph. for k = 1:length(gyro) nId = [ids.pose(svIds(k)),ids.vel(svIds(k)),ids.bias(svIds(k)), ... ids.pose(svIds(k+1)),ids.vel(svIds(k+1)),ids.bias(svIds(k+1))]; fIMU = factorIMU(nId,gyro{k},accel{k},imuParams,SensorTransform=data.camToIMUTransform); slidingWindowFactorGraph.addFactor(fIMU); end end % Set camera pose node guesses and 3-D point guesses % after alignment. slidingWindowFactorGraph.nodeState( ... ids.pose(svIds), ... helperCameraPoseTableToSE3Vector( ... poses(vSet,svIds))); slidingWindowFactorGraph.nodeState( ... ids.point3(xyzIds),xyz);
Calcule una estimación inicial del sesgo de IMU utilizando la optimización del gráfico de factores con la proyección de la cámara y los factores de IMU.
% Add prior to first camera pose to fix it softly during % optimization. fixNode(slidingWindowFactorGraph,ids.pose(svIds)); fixNode(slidingWindowFactorGraph,ids.point3(xyzIds)); % Add velocity prior to first IMU velocity node. fVelPrior = factorVelocity3Prior(ids.vel(svIds(1))); addFactor(slidingWindowFactorGraph,fVelPrior); % Add bias prior to first bias node. fBiasPrior = factorIMUBiasPrior(ids.bias(svIds(1))); addFactor(slidingWindowFactorGraph,fBiasPrior); % Perform visual-inertial optimization after alignment to estimate % initial IMU bias values. soll1 = optimize(slidingWindowFactorGraph, ... params.SolverOpts); fixNode(slidingWindowFactorGraph,ids.pose(svIds),false); fixNode(slidingWindowFactorGraph,ids.point3(xyzIds),false); fixNode(slidingWindowFactorGraph,ids.pose(svIds(1))); soll = optimize(slidingWindowFactorGraph, ... params.SolverOpts); fixNode(slidingWindowFactorGraph,ids.pose(svIds(1)),false); % Update feature manager and view set after optimization. vSet = updateView(vSet,helperUpdateCameraPoseTable(poses(vSet,svIds), ... slidingWindowFactorGraph.nodeState( ... ids.pose(svIds)))); xyz = slidingWindowFactorGraph.nodeState( ... ids.point3(xyzIds)); setXYZPoints(fManager,xyz,xyzIds); end end
Estimated scale: 1.8529
Predicción de la pose IMU
Cuando los datos de IMU están disponibles, puede predecir la pose mundial de la cámara integrando lecturas de acelerómetro y giroscopio. Utilice la optimización del gráfico de factores para refinar aún más esta predicción.
imuGuess = false;
if status.isIMUAligned
% Extract gyro and accel reading between current image frame
% and last acquired image frame to create IMU factor.
svIds = getSlidingWindowIds(fManager);
svs = svIds((end-1):end);
[gyro,accel] = helperExtractIMUDataBetweenViews(data.gyroReadings, ...
data.accelReadings,data.timeStamps,allFrameIds(svs));
nodeID = [ids.pose(svs(1)) ...
ids.vel(svs(1)) ...
ids.bias(svs(1)) ...
ids.pose(svs(2)) ...
ids.vel(svs(2)) ...
ids.bias(svs(2))];
% Create the transformation required to trasform a camera pose
% to IMU base frame for the IMU residual computation.
fIMU = factorIMU(nodeID,gyro{1},accel{1},imuParams, ...
SensorTransform=data.camToIMUTransform);
% Add camera pose and IMU factor to graph.
slidingWindowFactorGraph.addFactor(fIMU);
% Set velocity and bias guess.
prevP = nodeState(slidingWindowFactorGraph,ids.pose(svs(1)));
prevVel = nodeState(slidingWindowFactorGraph,ids.vel(svs(1)));
prevBias = nodeState(slidingWindowFactorGraph,ids.bias(svs(1)));
[pp,pv] = fIMU.predict(prevP,prevVel,prevBias);
imuGuess = true;
end
[currPoints,pointIds,isTriangulated] = getKeyPointsInView(fManager,viewId);
cVal = true(size(currPoints,1),1);
cTrf = find(isTriangulated);Si no hay ninguna predicción de IMU disponible, utilice correspondencias 3D-2D para estimar la pose de la vista actual.
if ~imuGuess x3D = getXYZPoints(fManager,pointIds(isTriangulated)); c2D = currPoints(isTriangulated,:); ii = false(size(x3D,1),1); currPose = rigidtform3d; for k = 1:params.F_loop [currPosel,iil] = estworldpose( ... currPoints(isTriangulated,:),x3D, ... data.intrinsics,MaxReprojectionError=params.F_Threshold,Confidence=params.F_Confidence, ... MaxNumTrials=params.F_Iterations); if length(find(ii)) < length(find(iil)) ii = iil; currPose = currPosel; end end cVal(cTrf(~ii)) = false; else
Utilice la pose prevista de IMU como estimación inicial para el ajuste del paquete de solo movimiento.
x3D = getXYZPoints(fManager,pointIds(isTriangulated));
c2D = currPoints(isTriangulated,:);
[currPose,velRefined,biasRefined,ii] = helperBundleAdjustmentMotion( ...
x3D,c2D,data.intrinsics,size(I),pp,pv,prevP,prevVel,prevBias,fIMU);
slidingWindowFactorGraph.nodeState( ...
ids.vel(viewId),velRefined);
slidingWindowFactorGraph.nodeState( ...
ids.bias(viewId),biasRefined);
cVal(cTrf(~ii)) = false;
end
setKeyPointValidityInView(fManager,viewId,cVal);
vSet = addView(vSet,viewId,currPose);Agregue factores de proyección de la cámara relacionados con las pistas de puntos 3D de la vista actual.
obs2 = pointIds(isTriangulated);
obs2 = obs2(ii);
fCam = factorCameraSE3AndPointXYZ( ...
[ids.pose(viewId*ones(size(obs2))) ids.point3(obs2)], ...
data.intrinsics.K,Measurement=c2D(ii,:), ...
Information=cameraInformation);
allCameraTracks{viewId} = [viewId*ones(size(obs2)) obs2 fCam.Measurement];
slidingWindowFactorGraph.addFactor(fCam);
endTriangulación de puntos tridimensionales
Cuando se utilizan las últimas correspondencias 2D-2D para la estimación de la pose del mundo de la cámara, con frecuencia se deben crear nuevos puntos 3D.
if status.isMapInitialized [newXYZ,newXYZID,newPointViews,newPointObs] = triangulateNew3DPoints(fManager,vSet); if vis && isempty(axMap) && windowState.WindowFull axMap = axes(figure); %#ok % Plot the map created by Initial SfM helperPlotCameraPosesAndLandmarks(axMap,fManager,vSet,removedFrameIds,true); end
Refinamiento de pose estimado mediante optimización del gráfico de factores
La optimización del gráfico de factores reduce el error en la estimación de la trayectoria o la pose de la cámara. Varios factores, como el seguimiento inexacto y los valores atípicos, pueden contribuir a errores de estimación.
La optimización de gráficos ajusta las estimaciones de los nodos de pose de la cámara para satisfacer diversas restricciones de medición del sensor, como observaciones de la cámara (proyección de puntos 3D sobre una observación de puntos de imagen 2D que genera un marco de imagen), poses relativas de IMU y cambio de velocidad relativa. Puede categorizar la optimización según el tipo de factores utilizados. Las dos categorías importantes son las siguientes.
Ajuste de paquete: utiliza únicamente mediciones de la cámara.
factorCameraSE3AndPointXYZes útil para agregar restricciones de medición de la cámara al gráfico.Optimización visual-inercial: junto con las mediciones de la cámara, agregue mediciones de IMU, como lecturas de giroscopio y acelerómetro, al gráfico usando
factorIMU.
El sistema gráfico de factor visual-inercial consta de los siguientes tipos de nodos conectados mediante diferentes factores:
Nodos de pose de cámara en diferentes marcas de tiempo: están conectados a otros nodos mediante la proyección de la cámara y factores IMU. Las estimaciones de los nodos de pose de la cámara se calculan utilizando SfM.
Nodos de puntos de referencia tridimensionales: están conectados a nodos de pose de cámara mediante factores de proyección de cámara. Las estimaciones de los nodos de punto de referencia se calculan utilizando SfM.
Nodos de velocidad IMU en diferentes marcas de tiempo: están conectados a otros nodos mediante factores IMU. Las estimaciones del nodo de velocidad se calculan mediante la preintegración de IMU.
Nodos de polarización IMU en diferentes marcas de tiempo: están conectados a otros nodos mediante factores IMU. Las estimaciones de los nodos de sesgo suelen ser el resultado de la optimización del gráfico de factores. Estos son desconocidos antes de la optimización.
Cada paso de tiempo contiene un nodo de pose, un nodo de velocidad y un nodo de polarización. Cada uno de estos nodos en un paso de tiempo está conectado a los nodos de pose, velocidad y polarización de otro paso de tiempo utilizando un objeto factorIMU. Cuando el VANT observa puntos característicos utilizando la cámara, un objeto factorCameraSE3AndPointXYZ relaciona los puntos característicos observados con el nodo de pose del paso de tiempo observado. Este proceso se repite para cada nuevo paso de tiempo del vuelo del VANT .
Actualice la ventana deslizante con los últimos puntos 3D y poses de vista de cámara.
newPointsTriangulated = false;
if ~isempty(newXYZ)
newPointsTriangulated = true;
% Store all new 3D-2D correspondenses.
for pIte = 1:length(newPointViews)
allCameraTracks{newPointViews(pIte)} = [allCameraTracks{newPointViews(pIte)}; newPointObs{pIte}];
end
obs = vertcat(newPointObs{:});
% Create camera projection factors with the latest
% 3-D point landmark observations in the current image.
fCam = factorCameraSE3AndPointXYZ([ids.pose(obs(:,1)) ...
ids.point3(obs(:,2))],data.intrinsics.K, ...
Measurement=obs(:,3:4), ...
Information=cameraInformation);
addFactor(slidingWindowFactorGraph,fCam);
end
% Set current camera pose node state guess.
slidingWindowViewIds = getSlidingWindowIds(fManager);
currentCameraPose = poses(vSet,slidingWindowViewIds(end));
nodeState(slidingWindowFactorGraph, ...
ids.pose(slidingWindowViewIds(end)), ...
helperCameraPoseTableToSE3Vector(currentCameraPose));Refine las poses estimadas del fotograma de la cámara y los puntos 3D mediante la optimización del gráfico de factores. La optimización requiere mucho tiempo. Por lo tanto, la optimización no se ejecuta después de estimar la pose de cada cuadro. La frecuencia de fotogramas a la que se ejecuta la optimización se puede controlar mediante un parámetro.
if helperDecideToRunGraphOptimization(curIdx,newPointsTriangulated,params) % Use partial factor graph optimization with only the latest key % frames, for performance. nodeIdsToOptimize = ids.pose(slidingWindowViewIds); xyzIds = getPointIdsInViews(fManager,slidingWindowViewIds); % Add guess for newly triangulated 3-D point node states. xyz = getXYZPoints(fManager,xyzIds); slidingWindowFactorGraph.nodeState( ... ids.point3(xyzIds),xyz); % Fix a few nodes during graph optimization % to fix the camera pose scale. Unfix them after optimization. if windowState.WindowFull fixNode(slidingWindowFactorGraph, ... ids.pose(slidingWindowViewIds(1:11))); else fixNode(slidingWindowFactorGraph, ... ids.pose(slidingWindowViewIds(1))); end if status.isIMUAligned % Fix the first velocity and bias nodes in the sliding % window. fixNode(slidingWindowFactorGraph, ... ids.vel(slidingWindowViewIds(1))); fixNode(slidingWindowFactorGraph, ... ids.bias(slidingWindowViewIds(1))); end % Optimize the sliding window. optiInfo = optimize(slidingWindowFactorGraph,nodeIdsToOptimize,params.SolverOpts); if windowState.WindowFull fixNode(slidingWindowFactorGraph, ... ids.pose(slidingWindowViewIds(1:11)),false); else fixNode(slidingWindowFactorGraph, ... ids.pose(slidingWindowViewIds(1)),false); end
Actualice el administrador de funciones y vea el conjunto con sus resultados de optimización.
if ~status.Mediandepth status.Mediandepth = true; xyz = slidingWindowFactorGraph.nodeState( ... ids.point3(xyzIds)); medianDepth = median(vecnorm(xyz.')); [posesUpdated,xyz] = helperTransformToNavigationFrame(helperUpdateCameraPoseTable(poses(vSet,slidingWindowViewIds), ... slidingWindowFactorGraph.nodeState(ids.pose(slidingWindowViewIds))), ... xyz,rigidtform3d,initialSceneMedianDepth/medianDepth); % Set current camera pose node state guess. slidingWindowFactorGraph.nodeState(ids.pose(slidingWindowViewIds), ... helperCameraPoseTableToSE3Vector(posesUpdated)); % Add guess for newly triangulated 3-D points node states. slidingWindowFactorGraph.nodeState( ... ids.point3(xyzIds),xyz); else posesUpdated = helperUpdateCameraPoseTable(poses(vSet,slidingWindowViewIds), ... slidingWindowFactorGraph.nodeState( ... ids.pose(slidingWindowViewIds))); xyz = slidingWindowFactorGraph.nodeState( ... ids.point3(xyzIds)); end % Update the view set after visual-inertial optimization. vSet = updateView(vSet,posesUpdated); setXYZPoints(fManager,xyz,xyzIds); end end
Agregue un nuevo punto de característica al rastreador de Kalman, en caso de que la cantidad de puntos descienda por debajo del umbral de seguimiento de características.
createNewFeaturePoints(fManager,I);
currPoints = getKeyPointsInView(fManager,viewId);
setPoints(tracker,currPoints);
if ~status.isIMUAligned && useIMU && status.isMapInitialized && windowState.WindowFull
% The sliding window is full and the camera and IMU are not yet aligned.
readyToAlignCameraAndIMU = true;
end
prevPrevI = prevI;
prevI = I;Visualice la trayectoria estimada.
if status.isMapInitialized && (mod(curIdx,10)==0) if vis if isempty(axTraj) axTraj = helperCreateTrajectoryVisualization([-4 7 -8 3 -3 1]); end % Visualize the estimated trajectory. helperVisualizeTrajectory(axTraj,fManager,vSet,removedFrameIds); end end end
Imagen de muestra de la escena.
Traza todas las poses de cámara de fotogramas clave y puntos 3D. Observe los puntos de referencia en elementos como el techo, el piso y los pilares.
helperPlotCameraPosesAndLandmarks(axMap,fManager,vSet,removedFrameIds);
Comparar la trayectoria estimada con ground-truth
Como medida de precisión, calcule estas métricas:
Error de trayectoria absoluta (ATE): error cuadrático medio (RMSE) entre las ubicaciones de las cámaras calculadas y las ubicaciones de las cámaras reales ground-truth.
Error de escala: porcentaje de la distancia entre la escala mediana calculada y la escala original.
Trazar la trayectoria estimada contra ground-truth.
addedFrameIds = allFrameIds(vSet.Views.ViewId);
axf = axes(figure);
helperPlotAgainstGroundTruth(vSet,data.gTruth,data.camToIMUTransform, ...
addedFrameIds,axf,removedFrameIds);
Evalúe la precisión del seguimiento, basándose en el error cuadrático medio (RMSE) y el error de escala mediana.
helperComputeErrorAgainstGroundTruth(data.gTruth,vSet,allFrameIds,removedFrameIds,data.camToIMUTransform);
"Absolute RMSE for key frame trajectory (m): " "0.19021"
"Percentage of median scale error: " "1.9709"
Funciones de apoyo
Esta sección detalla las breves funciones auxiliares incluidas en este ejemplo. Se han incluido funciones auxiliares más amplias en archivos separados.
helperFeaturePointManager administra pistas de puntos clave.
helperVIOParameters inicializa los parámetros ajustables del algoritmo de odometría visual-inercial.
helperBundleAdjustmentMotion refina la pose del fotograma actual usando el ajuste de conjunto de solo movimiento.
helperSelectNewKeyPointsUniformly selecciona un número específico de puntos clave recién creados a una distancia específica de los puntos rastreados en el fotograma clave actual.
helperCreateTrajectoryVisualization crea un gráfico de trayectoria con una ventana deslizante resaltada.
helperVisualizeTrajectory actualiza el trazado de trayectoria con los datos más recientes almacenados en el conjunto de vistas y el administrador de funciones.
helperPlotAgainstGroundTruth traza la trayectoria estimada y la trayectoria ground-truth para comparación visual.
helperGenerateNodeID genera ID de nodo de gráfico de factor únicos para un número fijo de poses de vista de cámara, velocidades de IMU, sesgos de IMU y nodos de puntos 3-D.
function ids = helperGenerateNodeID(fg,maxFrames,maxLandmarks) % helperGenerateNodeID ids.pose = generateNodeID(fg,[maxFrames 1]); ids.vel = generateNodeID(fg,[maxFrames 1]); ids.bias = generateNodeID(fg,[maxFrames 1]); ids.point3 = generateNodeID(fg,[maxLandmarks 1]); end
helperCameraPoseTableToSE3Vector convierte la tabla de poses en una matriz de poses N por 7 SE(3).
function cameraPoses = helperCameraPoseTableToSE3Vector(cameraPoseTable) % helperCameraPoseTableToSE3Vector converts camera pose table returned by % poses method of imageviewset to N-by-7 SE3 pose vector format. cameraPoses = [cat(1,cameraPoseTable.AbsolutePose.Translation) rotm2quat(cat(3,cameraPoseTable.AbsolutePose.R))]; end
helperUpdateCameraPoseTable actualiza la tabla de poses con las últimas poses estimadas N por 7 SE(3).
function cameraPoseTableUpdated = helperUpdateCameraPoseTable(cameraPoseTable,cameraPoses) % helperUpdateCameraPoseTable updates camera pose table with specified % N-by-7 SE(3) camera poses. cameraPoseTableUpdated = cameraPoseTable; R = quat2rotm(cameraPoses(:,4:7)); for k = 1:size(cameraPoses,1) cameraPoseTableUpdated.AbsolutePose(k).Translation = cameraPoses(k,1:3); cameraPoseTableUpdated.AbsolutePose(k).R = R(:,:,k); end end
helperDetectKeyPoints detecta puntos clave.
function keyPoints = helperDetectKeyPoints(grayImage) %helperDetectKeyPoints % Detect multi-scale FAST corners. keyPoints = detectORBFeatures(grayImage,ScaleFactor=1.2,NumLevels=4); % Uncomment any of the following or try different detectors to tune % keyPoints = detectFASTFeatures(grayImage,MinQuality=0.0786); % keyPoints = detectMinEigenFeatures(grayImage,MinQuality=0.01,FilterSize=3); end
helperDecideToRunGraphOptimization decide si ejecutar u omitir la optimización del gráfico en el fotograma actual.
function shouldOptimize = helperDecideToRunGraphOptimization(curIdx,newPointsTriangulated,params) % helperDecideToRunGraphOptimization % If the current frame belongs to the initial set of frames, then run graph % optimization every frame, because the initial SfM is still running. % Otherwise, after a number of frames specified by optimization frequency, % run graph optimization. Lower frequency can result in a more accurate % estimation, but can increase execution time. numberOfInitialFrames = 250; shouldOptimize = (curIdx < numberOfInitialFrames) || (mod(curIdx,params.optimizationFrequency) == 0) || newPointsTriangulated; end
helperTransformToNavigationFrame transforma y escala las poses de entrada y los puntos 3-D XYZ al marco de referencia de navegación local de IMU utilizando la rotación por gravedad y la escala de pose.
function [posesUpdated,xyzUpdated] = helperTransformToNavigationFrame(poses,xyz,gRot,poseScale) % helperTransformToNavigationFrame transforms and scales the input poses and XYZ points % using specified gravity rotation and pose scale. posesUpdated = poses; % Input gravity rotation transforms the gravity vector from local % navigation reference frame to initial camera pose reference frame. % The inverse of this transforms the poses from camera reference frame % to local navigation reference frame. Ai = gRot.A'; for k = 1:length(poses.AbsolutePose) T = Ai*poses.AbsolutePose(k).A; T(1:3,4) = poseScale*T(1:3,4); posesUpdated.AbsolutePose(k) = rigidtform3d(T); end % Transform points from initial camera pose reference frame to % local navigation reference frame of IMU. xyzUpdated = poseScale*gRot.transformPointsInverse(xyz); end
helperExtractIMUDataBetweenViews extrae datos IMU entre vistas especificadas.
function [gyro,accel] = helperExtractIMUDataBetweenViews(gyroReadings,accelReadings,timeStamps,frameIds) % helperExtractIMUDataBetweenViews extracts IMU Data (accelerometer and % gyroscope readings) between specified consecutive frames. len = length(frameIds); gyro = cell(1,len-1); accel = cell(1,len-1); for k = 2:len % Assumes the IMU data is time-synchorized with the camera data. Compute % indices of accelerometer readings between consecutive view IDs. [~,ind1] = min(abs(timeStamps.imuTimeStamps - timeStamps.imageTimeStamps(frameIds(k-1)))); [~,ind2] = min(abs(timeStamps.imuTimeStamps - timeStamps.imageTimeStamps(frameIds(k)))); imuIndBetweenFrames = ind1:(ind2-1); % Extract the data at the computed indices and store in a cell. gyro{k-1} = gyroReadings(imuIndBetweenFrames,:); accel{k-1} = accelReadings(imuIndBetweenFrames,:); end end
helperPlotCameraPosesAndLandmarks traza la trayectoria estimada y puntos de referencia en 3-D.
function helperPlotCameraPosesAndLandmarks(axisHandle,fManager,vSet,removedFrameIds,plotCams) % helperPlotCameraPosesAndLandmarks plots the key frame camera poses and % triangulated 3-D point landmarks. if nargin < 5 % By deafult plot trajectory as a line plot. If plotCams is true the % function uses the plotCamera utility to draw trajectory. plotCams = false; end % Extract key frame camera poses from view set vId = vSet.Views.ViewId; kfInd = true(length(vId),1); [~,ind] = intersect(vId,removedFrameIds); kfInd(ind) = false; camPoses = poses(vSet,vId(kfInd)); % Extract triangulated 3-D point landmarks xyzPoints = getXYZPoints(fManager); % Compute indices of nearby points indToPlot = vecnorm(xyzPoints,2,2) < 10; pcshow(xyzPoints(indToPlot,:),Parent=axisHandle,Projection="orthographic"); hold(axisHandle,"on") if plotCams c = table(camPoses.AbsolutePose,VariableNames={'AbsolutePose'}); plotCamera(c,Parent=axisHandle,Size=0.25); title(axisHandle,"Initial Structure from Motion") else traj = vertcat(camPoses.AbsolutePose.Translation); plot3(traj(:,1),traj(:,2),traj(:,3),"r-",Parent=axisHandle); view(axisHandle,27.28,-2.81) title(axisHandle,"Estimated Trajectory and Landmarks") end hold off drawnow end
helperComputeErrorAgainstGroundTruth calcula el error absoluto de trayectoria y el error de escala en comparación con ground-truth.
function [rmse,scaleError] = helperComputeErrorAgainstGroundTruth(gTruth,vSet,allFrameIds,removedFrameIds,camToIMUTransform) % helperComputeErrorAgainstGroundTruth computes the absolute trajectory % error and scale error. % Extract key frame camera poses from view set vId = vSet.Views.ViewId; kfInd = true(length(vId),1); [~,ind] = intersect(vId,removedFrameIds); kfInd(ind) = false; camPoses = poses(vSet,vId(kfInd)); locations = vertcat(camPoses.AbsolutePose.Translation); % Convert camera positions to first IMU reference frame T = se3(camPoses.AbsolutePose(1).R*(camToIMUTransform.rotm')).inv; locations = T.transform(locations); % Convert ground truth to first IMU reference frame g1 = se3(quat2rotm(gTruth(:,4:7)),gTruth(:,1:3)); g11 = (g1(1).inv)*g1; gl = vertcat(g11.trvec); gLocations = gl(allFrameIds(vId(kfInd)),1:3); scale = median(vecnorm(gLocations,2,2))/median(vecnorm(locations,2,2)); rmse = sqrt(mean(sum((locations - gLocations).^2,2))); scaleError = abs(scale-1)*100; disp(["Absolute RMSE for key frame trajectory (m): ",num2str(rmse)]) disp(["Percentage of median scale error: ",num2str(scaleError)]) end
helperDownloadData descarga el conjunto de datos desde la URL especificada a la carpeta de salida especificada.
function vioData = helperDownloadData() % helperDownloadData downloads the data set from the specified URL to the % specified output folder. vioDataTarFile = matlab.internal.examples.downloadSupportFile(... 'shared_nav_vision/data','BlackbirdVIOData.tar'); % Extract the file. outputFolder = fileparts(vioDataTarFile); if (~exist(fullfile(outputFolder,"BlackbirdVIOData"),"dir")) untar(vioDataTarFile,outputFolder); end vioData = load(fullfile(outputFolder,"BlackbirdVIOData","data.mat")); end
Referencias
[1] Qin, Tong, Peiliang Li y Shaojie Shen. “VINS-Mono: Un estimador de estado visual-inercial monocular robusto y versátil”. Transacciones IEEE sobre Robótica 34, no. 4 (agosto de 2018): 1004–20. https://doi.org/10.1109/TRO.2018.2853729
[2] Antonini, Amado, Winter Guerra, Varun Murali, Thomas Sayre-McCord y Sertac Karaman. “El conjunto de datos de Blackbird: Un conjunto de datos a gran escala para la percepción de VANT en vuelos agresivos”. En Actas del Simposio Internacional sobre Robótica Experimental de 2018, editado por Jing Xiao, Torsten Kröger y Oussama Khatib, 11:130–39. Cham: Publicaciones internacionales Springer, 2020. https://doi.org/10.1007/978-3-030-33950-0_12
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)