Contenido principal

Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Odometría visual-inercial monocular (VIO) mediante grafo de factores

Desde R2023b

La odometría visual-inercial monocular estima la posición y la orientación del robot (por ejemplo, un UAV en este ejemplo) mediante el uso de datos de una cámara y un sensor de unidad de medición inercial (IMU). La estimación del estado basada en cámara es precisa durante la navegación a baja velocidad, pero 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 cámara monocular solo puede determinar poses hasta una escala arbitraria, lo que significa que la escala real del entorno no se puede medir directamente. Por el contrario, la navegación inercial puede manejar fácilmente la navegación de alta velocidad y proporciona estimaciones de posición a escala mundial. Al combinar las ventajas de los datos de la cámara y de la IMU a través de la optimización de grafos de factores estrechamente acoplados, se puede lograr una mayor precisión. Para lograr un rendimiento eficiente en el tiempo de ejecución, es posible optimizar solo una pequeña parte de todo el grafo de factores, que contiene solo las mediciones más recientes, en cada paso de optimización. Este enfoque se conoce popularmente como optimización de ventana deslizante u optimización de gráficos parciales. Esta figura muestra un UAV equipado con una IMU y una cámara monocular.

Visión general

El sistema visual-inercial implementado en este ejemplo consta de una versión simplificada del front-end de odometría visual monocular del algoritmo VINS [1] y un back-end de grafo de factores.

La interfaz de odometría visual funciona de manera similar a los algoritmos de estructura a partir del movimiento (SfM) estándar, como el orientado FAST y el rotado BRIEF (ORB) y la localización y mapeo simultáneos (SLAM). La interfaz de odometría visual detecta y rastrea puntos clave de imágenes en múltiples cuadros, estima las poses de la cámara y triangula puntos 3D usando geometría de múltiples vistas. El back-end del grafo de factores optimiza 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, el algoritmo debe alinear la cámara y la IMU para calcular el factor de escala de posición de la cámara con respecto a la medición de la IMU y calcular la rotación de la gravedad. Estos pasos de alineación son cruciales ya que transforman las posiciones de la cámara en el marco de referencia de navegación local de la IMU y brindan estimaciones de 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. El conjunto de datos tiene un tamaño de 628 MB e incluye lecturas de IMU, imágenes y parámetros intrínsecos de la cámara.

data = helperDownloadVIOData();

Corrige la semilla aleatoria para que sea repetible.

rng(0)

Inicializar parámetros del algoritmo

Utilice la función helperVIOParameters para inicializar los parámetros que debe utilizar para el algoritmo de odometría visual-inercial. Consulte la función auxiliar para ver más detalles.

params = helperVIOParameters;
% Create a struct that stores status information
status = struct("isFirstFrame",true,"isMapInitialized",false,"isIMUAligned",false,"isMedianDepthEstimated",false);
% Set initial scene median depth.
initialSceneMedianDepth = 4;
viewID = 0;
removedFrameIDs = [];
allCameraTracks = cell(1,5000);

Configurar un grafo de factores para la optimización del grafo de factores estrechamente acoplado del back-end.

fg = factorGraph;
maxFrames = 10000;
maxLandmarks = 100000;
ids = helperGenerateNodeID(fg,maxFrames,maxLandmarks);

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);

Utilice la clase auxiliar helperFeaturePointManager para mantener el seguimiento de puntos clave.

fpManager = helperFeaturePointManager(data.intrinsics,params,maxFrames,maxLandmarks);
% Set up the key point detector.
fpManager.DetectorFunc = @(I)helperDetectKeyPoints(I);

Cree una vista de imagen configurada para mantener las poses del marco.

viewSet = imageviewset;

Especifique el primer y último fotograma a procesar a partir del conjunto de datos. Luego, procese el primer fotograma.

% IMU data is available starting 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;

Estructura a partir del movimiento (SfM)

Las lecturas del acelerómetro y del giroscopio de los datos de la IMU contienen inherentemente cierto nivel de sesgo y ruido. Para estimar con precisión estos valores de sesgo, es necesario obtener estimaciones de pose precisas a partir de los primeros fotogramas. Esto se puede lograr utilizando Estructura a partir del movimiento (SfM), una técnica que estima la estructura 3D de una escena utilizando una secuencia de imágenes 2D. El proceso SfM implica varios pasos críticos:

  • Inicialización del mapa: estima la pose relativa entre el primer fotograma clave y el fotograma actual cuando hay suficiente paralaje. Esto se logra utilizando correspondencias 2D-2D, que son pistas de puntos clave a lo largo de múltiples fotogramas.

  • Triangulación: crea puntos 3D mediante la triangulación utilizando las poses mundiales de los fotogramas clave y las correspondencias 2D-2D.

  • Estimación de pose: continúe rastreando estos puntos 3D en cuadros subsiguientes y determine la pose del cuadro actual utilizando correspondencias 3D-2D.

Preprocesamiento de imágenes

Antes de extraer características de las imágenes para usarlas en el algoritmo, debe preprocesar las imágenes para garantizar la precisión en la detección y el seguimiento de las características. El preprocesamiento de imágenes generalmente implica estos pasos:

  • Ecualizar: aplica ecualización de histograma para mejorar el contraste de la imagen. Este paso es particularmente importante en condiciones de poca iluminación, ya que puede mejorar significativamente la visibilidad de las características, lo que permite una extracción y un seguimiento más sólidos.

  • Desdistorsionar: corrige las distorsiones de la lente, incluidas las distorsiones radiales y tangenciales, que pueden deformar la imagen. La corrección de la distorsión es esencial para una estimación precisa del estado porque garantiza que las características extraídas representen con precisión la geometría de la escena.

Iniciar el proceso de lectura y procesamiento de imágenes para odometría visual. Recorre los fotogramas comenzando desde el startFrameIdx especificado y continúa hasta que se inicializa el mapa o hasta que se alcanza el endFrameIdx. Para cada cuadro, lea y preprocese la imagen para mejorar las características y corregir distorsiones para la detección y el seguimiento de características. En el primer cuadro, configure el rastreador de características e inicialice el mapa. En los cuadros siguientes, continúe el proceso de seguimiento de características y actualización de mapas. Detecta y rastrea puntos clave en la imagen y asigna a cada cuadro un ID de vista único para realizar un seguimiento de la pose de la cámara.

% Assign start frame index to current frame index and start reading images
currFrameIdx = startFrameIdx;
while ~status.isMapInitialized && currFrameIdx < endFrameIdx
    % Read image data captured by the camera for the current frame.
    I = data.images{currFrameIdx};
    % Assign a unique view ID for each camera frame.
    viewID = viewID + 1;
    % Equalize and undistort the images
    I = helperProcessImage(I,params,data.intrinsics);
    if status.isFirstFrame
        % In the first frame, detect new key points and initialize the tracker for
        % future tracking.
        currPoints = createNewFeaturePoints(fpManager,I);
        updateSlidingWindow(fpManager,I,currPoints,true(size(currPoints,1),1),viewID);
        initialize(tracker, currPoints, I);
        firstI = I;
        viewSet = addView(viewSet,viewID,rigidtform3d);
        status.isFirstFrame = false;
    else

Seguimiento de funciones

Para calcular con precisión la pose del cuadro de la cámara, debe calcular correspondencias 2D-2D, que son pistas de puntos de imagen 2D en varios cuadros que corresponden al mismo punto de referencia físico en la escena. Si bien existen varios métodos para estimar estos puntos característicos, este ejemplo utiliza un rastreador Kalman por su eficacia en el seguimiento de puntos característicos en una secuencia de imágenes.

El rendimiento del proceso de seguimiento depende en gran medida de la precisión de estos puntos de seguimiento, que son susceptibles a errores, incluidos valores atípicos. Los valores atípicos pueden surgir de varios factores, como estructuras repetitivas dentro de la escena, y pueden afectar significativamente la precisión del seguimiento. Por lo tanto, el rechazo de valores atípicos es una tarea crítica en el seguimiento de características. 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.

Comience a rastrear puntos característicos de un cuadro al siguiente, actualice la ventana deslizante con estos puntos y administre el rechazo de valores atípicos utilizando la descomposición de la matriz fundamental. Inicialmente, rastree los puntos característicos del cuadro anterior en el cuadro actual utilizando el rastreador Kalman. Luego actualice la ventana deslizante, agregando un marco válido y eliminando el marco más antiguo. Durante este proceso, si detecta suficiente paralaje, calcule la pose relativa entre los fotogramas clave. Realice el rechazo de valores atípicos de forma iterativa para refinar la matriz fundamental y extraer las correspondencias internas, que son cruciales para una estimación precisa de la pose.

        % Track previous frame points in the current frame.
        [currPoints,validIdx] = tracker(I);
        % 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(fpManager,I,currPoints,validIdx,viewID);
        if (removedFrameID > fpManager.slidingWindowViewIDs(1))
            % Store non-key frames or removed frame IDs.
            removedFrameIDs(end + 1) = removedFrameID; %#ok<SAGROW>
        end

        if windowState.isFirstFewViews
            % Accept the first few camera views.
            viewSet = addView(viewSet,viewID,rigidtform3d);
        elseif windowState.isEnoughParallax
            % Estimate relative pose between the first key frame in the
            % window and the current frame.
            swIDs = getSlidingWindowIDs(fpManager);
            [matches1,matches2] = get2DCorrespondensesBetweenViews(fpManager,swIDs(end-1),swIDs(end));

            inliersIndex = false(size(matches1,1),1);
            for k = 1:10
                [f,inliers] = estimateFundamentalMatrix( ...
                    matches1,matches2,Method="RANSAC", ...
                    NumTrials=params.F_Iterations,DistanceThreshold=params.F_Threshold, ...
                    Confidence=params.F_Confidence);
                if length(find(inliersIndex)) < length(find(inliers))
                    inliersIndex = inliers;
                    fundamentalMatrix = f;
                end
            end

            inlierPrePoints = matches1(inliersIndex,:);
            inlierCurrPoints = matches2(inliersIndex,:);
            relPose = estrelpose(fundamentalMatrix,data.intrinsics,inlierPrePoints,inlierCurrPoints);
            viewSet = addView(viewSet,viewID,relPose);

            % Map is initialized now
            status.isMapInitialized = true;
            break
        end

Agregue un nuevo punto característico al rastreador de Kalman en caso de que la cantidad de puntos descienda por debajo del umbral de seguimiento de características.

        createNewFeaturePoints(fpManager,I);
        currPoints = getKeyPointsInView(fpManager,viewID);
        setPoints(tracker,currPoints);
    end
    currFrameIdx = currFrameIdx + 1;
end

if status.isMapInitialized
    % Show matched features
    hFeature = showMatchedFeatures(firstI,I,matches1,matches2);
    title(hFeature.Parent,"Enough Parallax Between Key Frames");
else
    error('Unable to initialize the map.')
end

Figure contains an axes object. The hidden axes object with title Enough Parallax Between Key Frames contains 4 objects of type image, line. One or more of the lines displays its values using only markers

Alineación cámara-IMU

La alineación de la cámara y la IMU es crucial para la fusión de sensores en los algoritmos de odometría visual-inercial o SLAM, ya que garantiza que las mediciones tanto de la cámara como de la IMU sean coherentes y se puedan combinar y optimizar de manera efectiva. Debe realizar estas tareas para alinear la cámara y la IMU:

  • Cálculo de rotación de gravedad: calcule la rotación necesaria para alinear el marco de navegación local de la IMU con el marco inicial de la cámara rotando el vector de gravedad medido por la IMU para que coincida con la orientación del eje z de la cámara. Puede aplicar la inversa de esta matriz de rotación al marco de coordenadas de la cámara, garantizando que los ejes z de ambos sistemas estén alineados, lo que es esencial para un seguimiento de movimiento y una estimación de pose precisos.

  • Cálculo de la escala de pose de la cámara: dado que la cámara proporciona mediciones de escala relativas, debe calcular un factor de escala que alinee la escala de pose de la cámara con la de la IMU o la escala mundial. Este paso es necesario para integrar los datos de movimiento relativo de la cámara con la escala absoluta proporcionada por la IMU.

  • Estimación inicial del sesgo de la IMU: la fusión precisa de sensores requiere el conocimiento de cualquier sesgo inherente en las mediciones de la IMU. Debe estimar los valores de sesgo iniciales tanto para el acelerómetro como para el giroscopio para corregir el flujo de datos de la IMU. Esta corrección permite que el algoritmo logre una alineación y una integración más precisas de los datos de la IMU con los datos de la cámara.

% Initialize camera and IMU parameters
% 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");
readyToAlignCameraAndIMU = false;

Después de inicializar el mapa, lea y procese las imágenes hasta que la IMU esté alineada con la cámara.

% Camera-IMU alignment loop
while ~status.isIMUAligned

Triangulació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.

    % Triangulate new 3D points based on the latest 2D-2D correspondences stored in 
    % the feature point manager and view set.
    [newXYZ,newXYZID,newPointViews,newPointObs] = triangulateNew3DPoints(fpManager,viewSet);

Refinamiento de pose estimado mediante optimización del grafo de factores

La optimización del grafo de factores minimiza los errores en la trayectoria o la estimación de la pose de la cámara de varias fuentes, como el seguimiento de características inexacto o la presencia de valores atípicos.

La optimización de gráficos ajusta las estimaciones de la posición del nodo de la cámara para que satisfagan las restricciones impuestas por las mediciones del sensor. Estas mediciones de sensores incluyen observaciones de cámara, donde se proyectan puntos 3D sobre el plano de la imagen para generar observaciones de puntos de imagen 2D, así como restricciones de datos IMU, como poses relativas y cambios en la velocidad. Puede categorizar la optimización según el tipo de factores utilizados. Dependiendo de los tipos de mediciones del sensor, la optimización puede ser de dos tipos:

  • Ajuste de paquete: esta optimización utiliza solo las medidas de la cámara para refinar las poses de la cámara. El grafo de factores utiliza el objeto factorCameraSE3AndPointXYZ para representar las restricciones de las observaciones de la cámara.

  • Optimización visual-inercial: esta optimización extiende el ajuste del paquete al incorporar mediciones de IMU, como datos de giroscopio y acelerómetro, en el grafo de factores. El grafo de factores utiliza el objeto factorIMU para representar restricciones basadas en IMU en el proceso de optimización.

Inicialmente, antes de que la cámara y la IMU estén completamente alineadas, el grafo de factores se basará únicamente en factores basados en la cámara para el ajuste de paquete. A medida que el algoritmo avanza y establece la alineación, utiliza mediciones de la cámara y de la IMU para una optimización visual-inercial más robusta.

    % Update the sliding window with the latest 3-D points and camera view pose.
    newPointsTriangulated = false;
    if ~isempty(newXYZ)
        newPointsTriangulated = true;
        % Store all new 3D-2D correspondences.
        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(fg,fCam);
    end

    % Set current camera pose node state guess.
    slidingWindowViewIDs = getSlidingWindowIDs(fpManager);
    currentCameraPose = poses(viewSet,slidingWindowViewIDs(end));
    nodeState(fg, ...
        ids.pose(slidingWindowViewIDs(end)), ...
        helperCameraPoseTableToSE3Vector(currentCameraPose));

Debido a la naturaleza computacionalmente intensiva de este proceso de optimización, no es posible realizarlo después de la estimación de cada fotograma de la cámara. En cambio, para equilibrar la precisión con la eficiencia computacional, puede controlar la frecuencia de optimización utilizando un parámetro. Utilice este parámetro para determinar el intervalo óptimo en el que se debe ejecutar la optimización, en función de la cantidad de cuadros procesados, para satisfacer las necesidades de optimización de la aplicación.

    if helperDecideToRunGraphOptimization(currFrameIdx,newPointsTriangulated,params)

La optimización del grafo 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 grafo 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.

        % Use partial factor graph optimization with only the latest key
        % frames, for performance. 
        nodeIdsToOptimize = ids.pose(slidingWindowViewIDs);
        xyzIds = getPointIdsInViews(fpManager,slidingWindowViewIDs);

        % Add guess for newly triangulated 3-D point node states.
        xyz = getXYZPoints(fpManager,xyzIds);
        fg.nodeState(ids.point3(xyzIds),xyz);

        % Fix a few pose nodes during graph optimization
        helperFixNodes(fg,ids.pose,slidingWindowViewIDs,windowState.isWindowFull);
        % Optimize the sliding window.
        optiInfo = optimize(fg,nodeIdsToOptimize,params.SolverOpts);
        % Unfix nodes after optimization
        helperUnFixNodes(fg,ids.pose,slidingWindowViewIDs,windowState.isWindowFull);

Actualice el administrador de funciones y el conjunto de vistas con los resultados de la optimización.

        if ~status.isMedianDepthEstimated
            status.isMedianDepthEstimated = true;
            xyz = fg.nodeState(ids.point3(xyzIds));
            EstimatedMedianDepth = median(vecnorm(xyz.'));
            [posesUpdated,xyz] = helperTransformToNavigationFrame(helperUpdateCameraPoseTable(poses(viewSet,slidingWindowViewIDs), ...
                fg.nodeState(ids.pose(slidingWindowViewIDs))), ...
                xyz,rigidtform3d,initialSceneMedianDepth/EstimatedMedianDepth);
            % Set current camera pose node state guess.
            fg.nodeState(ids.pose(slidingWindowViewIDs), ...
                helperCameraPoseTableToSE3Vector(posesUpdated));
            % Add guess for newly triangulated 3-D points node states.
            fg.nodeState(ids.point3(xyzIds),xyz);
        else
            posesUpdated = helperUpdateCameraPoseTable(poses(viewSet,slidingWindowViewIDs), ...
                fg.nodeState(ids.pose(slidingWindowViewIDs)));

            xyz = fg.nodeState(ids.point3(xyzIds));
        end
        % Update the view set after visual-inertial optimization.
        viewSet = updateView(viewSet,posesUpdated);
        setXYZPoints(fpManager,xyz,xyzIds);
    end

Agregue nuevos puntos de características al rastreador de Kalman, en caso de que la cantidad de puntos descienda por debajo del umbral de seguimiento de características.

    createNewFeaturePoints(fpManager,I);
    currPoints = getKeyPointsInView(fpManager,viewID);
    setPoints(tracker,currPoints);

    if windowState.isWindowFull && ~readyToAlignCameraAndIMU
        % The sliding window is full and the camera and IMU are not yet aligned.
        readyToAlignCameraAndIMU = true;
        % Plot the map created by Initial SfM. The figure shows the
        % estimated camera pose and observed landmarks
        helperPlotCameraPosesAndLandmarks(fpManager,viewSet,removedFrameIDs,true);
    end
    % Keep processing new images.
    currFrameIdx = currFrameIdx + 1;
    if currFrameIdx > endFrameIdx
        break
    else
        I = data.images{currFrameIdx};
        I = helperProcessImage(I,params,data.intrinsics);
        viewID = viewID + 1;
        % Track previous frame points in the current frame.
        [currPoints,validIdx] = tracker(I);
        [removedFrameID,windowState] = updateSlidingWindow(fpManager,I,currPoints,validIdx,viewID);
        if (removedFrameID > fpManager.slidingWindowViewIDs(1))
            % Store non-key frames or removed frame IDs.
            removedFrameIDs(end + 1) = removedFrameID; %#ok<SAGROW>
        end
        swIDs = getSlidingWindowIDs(fpManager);
        if length(swIDs) > 2
            [matches1,matches2] = get2DCorrespondensesBetweenViews(fpManager,swIDs(end-2),viewID);
        end

        if readyToAlignCameraAndIMU
            swIDs = getSlidingWindowIDs(fpManager);
            % Because we have not yet computed the latest frame pose,
            % So use only the past few frames for alignment.
            swIDs = swIDs(1:end-1);
            [gyro,accel] = helperExtractIMUDataBetweenViews( ...
                data.gyroReadings,data.accelReadings,data.timeStamps,allFrameIDs(swIDs));
            [xyz] = getXYZPoints(fpManager,xyzIds);
            % Align camera with IMU.
            camPoses = poses(viewSet,swIDs);
            [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(viewSet);
                % Transform camera poses to navigation frame using
                % computed gravity rotation and pose scale.
                [posesUpdated,xyz] = helperTransformToNavigationFrame(posesUpdated,xyz,gRot,scale);
                viewSet = updateView(viewSet,posesUpdated);
                % Plot the scaled and unscaled estimated trajectory against
                % ground truth.
                helperPlotCameraIMUAlignment(data,camPoses,scale,allFrameIDs);
                % After alignment, add IMU factors to factor graph.
                for k = 1:length(gyro)
                    nId = [ids.pose(swIDs(k)),ids.vel(swIDs(k)),ids.bias(swIDs(k)), ...
                        ids.pose(swIDs(k+1)),ids.vel(swIDs(k+1)),ids.bias(swIDs(k+1))];
                    fIMU = factorIMU(nId,gyro{k},accel{k},imuParams,SensorTransform=data.camToIMUTransform);
                    fg.addFactor(fIMU);
                end

                % Set camera pose node guesses and 3-D point guesses
                % after alignment.
                fg.nodeState(ids.pose(swIDs), ...
                    helperCameraPoseTableToSE3Vector(poses(viewSet,swIDs)));
                fg.nodeState(ids.point3(xyzIds),xyz);

Calcule una estimación inicial del sesgo de IMU utilizando la optimización del grafo 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(fg,ids.pose(swIDs));
                fixNode(fg,ids.point3(xyzIds));
                % Add velocity prior to first IMU velocity node.
                fVelPrior = factorVelocity3Prior(ids.vel(swIDs(1)));
                addFactor(fg,fVelPrior);

                % Add bias prior to first bias node.
                fBiasPrior = factorIMUBiasPrior(ids.bias(swIDs(1)));
                addFactor(fg,fBiasPrior);

                % Perform visual-inertial optimization after alignment to estimate
                % initial IMU bias values.
                soll1 = optimize(fg,params.SolverOpts);
                fixNode(fg,ids.pose(swIDs),false);
                fixNode(fg,ids.point3(xyzIds),false);

                fixNode(fg,ids.pose(swIDs(1)));
                soll = optimize(fg,params.SolverOpts);
                fixNode(fg,ids.pose(swIDs(1)),false);

                % Update feature manager and view set after optimization.
                viewSet = updateView(viewSet,helperUpdateCameraPoseTable(poses(viewSet,swIDs), ...
                    fg.nodeState(ids.pose(swIDs))));
                xyz = fg.nodeState(ids.point3(xyzIds));
                setXYZPoints(fpManager,xyz,xyzIds);
            end
        else
            [currPoints,pointIds,isTriangulated] = getKeyPointsInView(fpManager,viewID);
            hasTriangulatedPoints = true(size(currPoints,1),1);
            triangulatedPoints = find(isTriangulated);

No hay ninguna predicción IMU disponible, por lo que se utilizan correspondencias 3D-2D para estimar la posición de la vista actual.

            x3D = getXYZPoints(fpManager,pointIds(isTriangulated));
            c2D = currPoints(isTriangulated,:);
            ii = false(size(x3D,1),1);
            currPose = rigidtform3d;
            [currPose,ii] = helperEstimateCameraPose(params,c2D,x3D,data.intrinsics,currPose,ii);
            hasTriangulatedPoints(triangulatedPoints(~ii)) = false;

            setKeyPointValidityInView(fpManager,viewID,hasTriangulatedPoints);
            viewSet = addView(viewSet,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];
            fg.addFactor(fCam);
        end
    end
end

Figure contains an axes object. The axes object with title Initial Structure from Motion contains 211 objects of type line, text, patch, scatter.

Estimated scale: 1.8529

Figure contains an axes object. The axes object with title Camera-IMU Alignment contains 3 objects of type line. These objects represent Ground Truth, Estimated trajectory before alignment, Estimated trajectory after alignment.

Optimización visual-inercial

Una vez que la alineación de la cámara y la IMU es exitosa, el grafo de factores puede aprovechar los factores de la cámara y la IMU para optimizar las estimaciones de estado del sistema. El grafo de factores visual-inercial consta de varios tipos de nodos, cada uno de los cuales representa diferentes aspectos del estado del sistema en distintas marcas de tiempo:

  • Nodos de pose de cámara: estos nodos representan la posición y orientación estimadas de la cámara en diferentes puntos del tiempo. El algoritmo SfM estima estos nodos de pose y vincula los nodos de pose con otros nodos de pose a través de factores de proyección de cámara y factores IMU, que integran las mediciones visuales e inerciales.

  • Nodos de referencia de puntos 3D: estos nodos corresponden a las entidades mapeadas en el entorno. SfM estima estos nodos y los conecta a los nodos de posición de la cámara utilizando factores de proyección de la cámara, que ayudan a refinar las posiciones de los puntos de referencia en función de los puntos de imagen observados.

  • Nodos de velocidad IMU: estos nodos capturan la velocidad estimada de la cámara en diferentes marcas de tiempo. La preintegración de IMU calcula estos nodos y los conecta con otros nodos utilizando factores IMU que reflejan cambios en el movimiento entre marcas de tiempo.

  • Nodos de sesgo de IMU: representan los sesgos estimados en las mediciones de IMU; estos nodos son inicialmente desconocidos y se refinan a través del proceso de optimización. Los factores IMU conectan estos nodos con otros nodos.

Cada paso de tiempo contiene un nodo de pose, un nodo de velocidad y un nodo de sesgo. Cada uno de estos nodos en un paso de tiempo se conecta a los nodos de pose, velocidad y sesgo de otro paso de tiempo mediante un objeto factorIMU. Cuando el UAV observa puntos característicos usando 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 UAV, y cada nuevo paso de tiempo introduce nodos y factores adicionales al gráfico.

Factor_Graph_With_IMU_And_Camera_Factors.png

Habilitar visualización de trayectoria. Deseleccione para omitir la visualización de la trayectoria.

visualizeTrajectory = true;
if visualizeTrajectory
    axTraj = [];
    axFactorGraph = axes(figure);
end

Inicie el ciclo de optimización visual-inercial. Extraiga las lecturas del giroscopio y del acelerómetro entre el fotograma de la imagen actual y el último fotograma de la imagen. Luego, cree la transformación para transformar una pose de cámara en el marco base de la IMU para el cálculo residual de la IMU. Y luego crea y agrega el factor IMU al gráfico.

while currFrameIdx < endFrameIdx + 1
    % Extract gyro and accel reading between current image frame
    % and last acquired image frame to create IMU factor.
    swIDs = getSlidingWindowIDs(fpManager);
    svs = swIDs((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 transform 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.
    fg.addFactor(fIMU);

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 grafo de factores para refinar aún más esta predicción.

    % Set velocity and bias guess.
    prevP = nodeState(fg,ids.pose(svs(1)));
    prevVel = nodeState(fg,ids.vel(svs(1)));
    prevBias = nodeState(fg,ids.bias(svs(1)));
    [pp,pv] = fIMU.predict(prevP,prevVel,prevBias);

    [currPoints,pointIds,isTriangulated] = getKeyPointsInView(fpManager,viewID);
    hasTriangulatedPoints = true(size(currPoints,1),1);
    triangulatedPoints = find(isTriangulated);

Utilice la pose prevista de IMU como estimación inicial para el ajuste del paquete de solo movimiento.

    x3D = getXYZPoints(fpManager,pointIds(isTriangulated));
    c2D = currPoints(isTriangulated,:);
    [currPose,velRefined,biasRefined,ii] = helperBundleAdjustmentMotion( ...
        x3D,c2D,data.intrinsics,size(I),pp,pv,prevP,prevVel,prevBias,fIMU);
    fg.nodeState(ids.vel(viewID),velRefined);
    fg.nodeState(ids.bias(viewID),biasRefined);
    hasTriangulatedPoints(triangulatedPoints(~ii)) = false;

    setKeyPointValidityInView(fpManager,viewID,hasTriangulatedPoints);
    viewSet = addView(viewSet,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];
    fg.addFactor(fCam);
    [newXYZ,newXYZID,newPointViews,newPointObs] = triangulateNew3DPoints(fpManager,viewSet);
    newPointsTriangulated = false;
    if ~isempty(newXYZ)
        newPointsTriangulated = true;
        % Store all new 3D-2D correspondences.
        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(fg,fCam);
    end

    % Set current camera pose node state guess.
    slidingWindowViewIDs = getSlidingWindowIDs(fpManager);
    currentCameraPose = poses(viewSet,slidingWindowViewIDs(end));
    nodeState(fg, ...
        ids.pose(slidingWindowViewIDs(end)), ...
        helperCameraPoseTableToSE3Vector(currentCameraPose));
    
    if helperDecideToRunGraphOptimization(currFrameIdx,newPointsTriangulated,params)
        nodeIdsToOptimize = ids.pose(slidingWindowViewIDs);
        xyzIds = getPointIdsInViews(fpManager,slidingWindowViewIDs);
        xyz = getXYZPoints(fpManager,xyzIds);
        fg.nodeState(ids.point3(xyzIds),xyz);

        % Fix a few pose nodes during graph optimization
        helperFixNodes(fg,ids.pose,slidingWindowViewIDs,windowState.isWindowFull);

        % Fix the first velocity and bias nodes in the sliding
        % window.
        fixNode(fg,ids.vel(slidingWindowViewIDs(1)));
        fixNode(fg,ids.bias(slidingWindowViewIDs(1)));

        % Optimize the sliding window.
        optiInfo = optimize(fg,nodeIdsToOptimize,params.SolverOpts);

        % Unfix nodes after optimization
        helperUnFixNodes(fg,ids.pose,slidingWindowViewIDs,windowState.isWindowFull);

        posesUpdated = helperUpdateCameraPoseTable(poses(viewSet,slidingWindowViewIDs), ...
            fg.nodeState(ids.pose(slidingWindowViewIDs)));

        xyz = fg.nodeState(ids.point3(xyzIds));

        % Update the view set after visual-inertial optimization.
        viewSet = updateView(viewSet,posesUpdated);
        setXYZPoints(fpManager,xyz,xyzIds);

        % Visualize the estimated trajectory.
        if visualizeTrajectory && mod(currFrameIdx,params.optimizationFrequency*4)==0
            axTraj = helperVisualizeTrajectory(axTraj,fpManager,viewSet,removedFrameIDs);
            show(fg,"Landmark","on","Parent",axFactorGraph,"Legend","on");
            xlim(axFactorGraph,[-4 7]);
            ylim(axFactorGraph,[-8 3]);
            zlim(axFactorGraph,[-3 1]);
            view(axFactorGraph,36,40);
            title(axFactorGraph,"Estimated Trajectory and Landmarks");
            drawnow;
        end
    end

Agregue nuevos puntos de características al rastreador de Kalman, en caso de que la cantidad de puntos descienda por debajo del umbral de seguimiento de características.

    createNewFeaturePoints(fpManager,I);
    currPoints = getKeyPointsInView(fpManager,viewID);
    setPoints(tracker,currPoints);

    % Process new images.
    currFrameIdx = currFrameIdx + 1;
    if currFrameIdx > endFrameIdx
        break
    end
    I = data.images{currFrameIdx};
    I = helperProcessImage(I,params,data.intrinsics);
    viewID = viewID + 1;
    % Track previous frame points in the current frame.
    [currPoints,validIdx] = tracker(I);
    [removedFrameID,windowState] = updateSlidingWindow(fpManager,I,currPoints,validIdx,viewID);
    if (removedFrameID > fpManager.slidingWindowViewIDs(1))
        % Store non-key frames or removed frame IDs.
        removedFrameIDs(end+1) = removedFrameID;
    end
    swIDs = getSlidingWindowIDs(fpManager);
end

Figure contains an axes object. The axes object with title Estimated Trajectory contains 2 objects of type line. These objects represent Estimated Trajectory, Sliding Window Frames, Most Recent Frame.

Figure contains an axes object. The axes object with title Estimated Trajectory and Landmarks contains 3 objects of type line, scatter. One or more of the lines displays its values using only markers These objects represent Pose Node, Pose Edge, Landmark Node.

Esta es una imagen de muestra de la escena tomada por la cámara UAV en el cuadro de inicio.

sample.png

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.

axFG = show(fg);
xlim(axFG,[-10 10]);
ylim(axFG,[-10 10]);
zlim(axFG,[-5 4]);
view(axFG,10.46,-4.28);
title(axFG,"Estimated Trajectory and Landmarks");

Figure contains an axes object. The axes object with title Estimated Trajectory and Landmarks contains 3 objects of type line, scatter. One or more of the lines displays its values using only markers

Comparar la trayectoria estimada con ground-truth

Como medida de precisión, calcule estas métricas:

  • Error de trayectoria absoluto (ATE) — Error cuadrático medio (RMSE) entre las ubicaciones de las cámaras calculadas y las ubicaciones de las cámaras 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.

axf = axes(figure);
helperPlotAgainstGroundTruth(viewSet,data.gTruth,data.camToIMUTransform, ...
            allFrameIDs(viewSet.Views.ViewId),axf,removedFrameIDs);

Figure contains an axes object. The axes object contains 2 objects of type line. These objects represent Ground Truth Trajectory, Estimated Trajectory.

Evalúe la precisión de seguimiento, basándose en el error cuadrático medio (RMSE) y el error de escala mediano.

helperComputeErrorAgainstGroundTruth(data.gTruth,viewSet,allFrameIDs,removedFrameIDs,data.camToIMUTransform);
    "Absolute RMSE for key frame trajectory (m): "    "0.19021"

    "Percentage of median scale error: "    "1.9709"

Funciones auxiliares

Esta sección detalla las breves funciones auxiliares incluidas en este ejemplo. Se han incluido funciones auxiliares más amplias en archivos separados.

helperDownloadVIOData descarga el conjunto de datos de odometría visual-inercial desde la URL especificada a la carpeta de salida especificada.

helperFeaturePointManager administra pistas de puntos clave.

helperVIOParameters inicializa los parámetros ajustables del algoritmo de odometría visual-inercial.

helperBundleAdjustmentMotion refina la pose del cuadro actual usando un ajuste de paquete de solo movimiento.

helperCameraPoseTableToSE3Vector convierte la tabla de poses en una matriz de poses SE(3) de N por 7.

helperDetectKeyPoints detecta puntos clave a través de detectores ajustables.

helperExtractIMUDataBetweenViews extrae datos de IMU entre vistas especificadas.

helperProcessImage ecualiza y desinforma las imágenes si es necesario.

helperEstimateCameraPose estima la pose actual de la cámara usando correspondencias 3D-2D.

helperTransformToNavigationFrame transforma y escala las poses de entrada y los puntos 3D XYZ al marco de referencia de navegación local de la IMU usando la rotación de gravedad y la escala de pose.

helperUpdateCameraPoseTable actualiza la tabla de poses con las últimas poses SE(3) estimadas de N por 7.

helperPlotCameraIMUAlignment traza el resultado de la alineación de la cámara-IMU.

helperVisualizeTrajectory traza la trayectoria con los últimos datos almacenados en el conjunto de vistas y el administrador de características.

helperPlotCameraPosesAndLandmarks traza la trayectoria estimada y puntos de referencia en 3D.

helperPlotAgainstGroundTruth traza la trayectoria estimada y la trayectoria ground-truth para comparación visual.

helperComputeErrorAgainstGroundTruth calcula el error de trayectoria absoluto y el error de escala en comparación con la ground-truth conocida.

helperGenerateNodeID genera identificadores de nodos de grafos de factores únicos para un número fijo de poses de vista de cámara, velocidades de IMU, sesgos de IMU y nodos de puntos 3D.

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

helperDecideToRunGraphOptimization decide si ejecutar u omitir la optimización del gráfico en el cuadro actual.

function shouldOptimize = helperDecideToRunGraphOptimization(currFrameIdx,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 = (currFrameIdx < numberOfInitialFrames) || (mod(currFrameIdx,params.optimizationFrequency) == 0) || newPointsTriangulated;
end

helperFixNodes corrige los nodos de pose durante la optimización del gráfico.

function helperFixNodes(factorGraph,poseIds,slidingWindowIds,isWindowFull)
% helperFixNodes fixes pose nodes during graph optimization
if isWindowFull
    fixNode(factorGraph,poseIds(slidingWindowIds(1:11)));
else
    fixNode(factorGraph,poseIds(slidingWindowIds(1)));
end
end

helperUnFixNodes desarregla los nodos después de la optimización del gráfico.

function helperUnFixNodes(factorGraph,poseIds,slidingWindowIds,isWindowFull)
% helperUnFixNodes unfixes nodes after graph optimization
if isWindowFull
    fixNode(factorGraph,poseIds(slidingWindowIds(1:11)),false);
else
    fixNode(factorGraph,poseIds(slidingWindowIds(1)),false);
end
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

Consulte también

| | | (Computer Vision Toolbox)

Temas