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

trimLoopClosures

Optimice el gráfico de pose y elimine cierres de bucle incorrectos

Sintaxis

poseGraphUpdated = trimLoopClosures(poseGraphObj,trimParams,solverOptions)

[poseGraphUpdated,trimInfo] = trimLoopClosures(poseGraphObj,trimParams,solverOptions)

Descripción

poseGraphUpdated = trimLoopClosures(poseGraphObj,trimParams,solverOptions) optimiza el gráfico de pose para satisfacer mejor las restricciones de borde y elimina cualquier borde de cierre de bucle defectuoso en función de los parámetros de error residual especificados en trimParams. Crea la entrada solverOptions utilizando la función poseGraphSolverOptions.

La función implementa el método de no convexidad graduada (GNC) con costo robusto de mínimos cuadrados truncados (TLS) en combinación con el solucionador de gráficos de pose no mínimos [1].

[poseGraphUpdated,trimInfo] = trimLoopClosures(poseGraphObj,trimParams,solverOptions) devuelve información adicional relacionada con el proceso de recorte.

Ejemplos

Optimizar y recortar cierres de bucle para gráficos de poses 2D

Optimice un gráfico de pose en función de los nodos y las restricciones de los bordes. Recorte el bucle cerrado en función de los errores residuales de sus bordes.

Cargue el conjunto de datos que contiene un gráfico de pose 2D. Inspeccione el objeto poseGraph para ver la cantidad de nodos y cierres de bucle.

load grid-2d-posegraph.mat pg
disp(pg)

  poseGraph with properties:

               NumNodes: 120
               NumEdges: 193
    NumLoopClosureEdges: 74
     LoopClosureEdgeIDs: [120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 … ] (1×74 double)
        LandmarkNodeIDs: [1×0 double]

Traza el gráfico de pose con las identificaciones desactivadas. Las líneas rojas indican cierres de bucle identificados en el conjunto de datos. Las poses en el gráfico deben seguir un patrón de cuadrícula, pero muestran evidencia de variación con el tiempo.

show(pg,'IDs','off');
title('Original Pose Graph')

Optimice el gráfico de pose usando la función optimizePoseGraph. De forma predeterminada, esta función utiliza el solucionador "builtin-trust-region". Debido a que el gráfico de pose contiene algunos cierres de bucle incorrectos, el gráfico de pose resultante en realidad no es deseable.

pgOptim = optimizePoseGraph(pg);
figure;
show(pgOptim);

Observe los errores residuales de los bordes del gráfico de pose original. Los valores de error atípicos grandes al final indican cierres de bucle incorrectos.

resErrorVec = edgeResidualErrors(pg);
plot(resErrorVec);
title('Edge Residual Errors by Edge ID')

Ciertos cierres de bucle deben recortarse del gráfico de pose en función de su error residual. Utilice la función trimLoopClosures para recortar estos cierres de bucle defectuosos. Establezca el umbral máximo y de truncamiento para los parámetros del recortador. Este umbral se establece en función de la precisión de la medición y debe ajustarse a su sistema.

trimParams.MaxIterations = 100;
trimParams.TruncationThreshold = 25;

solverOptions = poseGraphSolverOptions; 

Utilice la función trimLoopClosures con los parámetros del recortador y las opciones del solucionador.

[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);

A partir de la salida trimInfo, grafique los cierres de bucle eliminados del gráfico de pose optimizado. Al trazar con el gráfico de errores residuales anterior, puede ver que se eliminaron los grandes cierres de bucles de errores.

removedLCs = trimInfo.LoopClosuresToRemove;

hold on
plot(removedLCs,zeros(length(removedLCs)),'or')
title('Edge Residual Errors and Removed Loop Closures')
legend('Residual Errors', 'Removed Loop Closures')
xlabel('Edge IDs')
ylabel('Edge Residual Error')
hold off

Muestre el nuevo gráfico de pose con los cierres de bucle defectuosos recortados.

show(pgNew,"IDs","off");

Argumentos de entrada

poseGraphObj — Gráfico de pose

objeto poseGraph | objeto poseGraph3D

Gráfico de pose, especificado como un objeto poseGraph o poseGraph3D.

trimParams — Parámetros de error residual para el recorte.

estructura

Parámetros de error residual para recortar cierres de bucle, especificados como una estructura con campos:

• MaxIterations — Número máximo de iteraciones permitidas para el recorte de cierre de bucle, especificado como un entero positivo. En una iteración de recorte, el gráfico de pose se optimiza en función de las opciones del solucionador y se recortan todos los bordes fuera de TruncationThreshold.

• TruncationThreshold — Error residual máximo permitido para un borde. Este valor depende en gran medida del gráfico de pose que especifique en poseGraphObj. Para encontrar un umbral adecuado en función de todos los errores, utilice la función edgeResidualErrors para el gráfico de pose.

Ejemplo: struct('MaxIterations',10,'TruncationThreshold',20)

Tipos de datos: struct

solverOptions — Opciones de solver de gráficos de pose

Parámetros poseGraphSolverOptions

Opciones del solucionador de gráficos de poses, especificadas como un conjunto de parámetros generados al llamar a la función poseGraphSolverOptions. La función genera un conjunto de opciones de solver con valores predeterminados para el tipo de solver de gráfico de pose especificado:

pgSolverTrustRegion = poseGraphSolverOptions('builtin-trust-region')

pgSolverTrustRegion = 

TrustRegion (builtin-trust-region-dogleg) options:

               MaxIterations: 300
                     MaxTime: 10
           FunctionTolerance: 1.0000e-08
           GradientTolerance: 5.0000e-09
               StepTolerance: 1.0000e-12
    InitialTrustRegionRadius: 100
               VerboseOutput: 'off'

pgSolverG2o = poseGraphSolverOptions('g2o-levenberg-marquardt')

pgSolverG2o = 

G2oLevenbergMarquardt (g2o-levenberg-marquardt) options:

        MaxIterations: 300
              MaxTime: 10
    FunctionTolerance: 1.0000e-09
        VerboseOutput: 'off'

Modifique las opciones para ajustar los parámetros del solver utilizando la notación de puntos.

pgSolverG2o.MaxIterations = 200;

Argumentos de salida

poseGraphUpdated — Pose gráfico con cierres de bucle recortados

objeto poseGraph | objeto poseGraph3D

Gráfico de pose con cierres de bucle recortados, especificado como un objeto poseGraph o poseGraph3D.

trimInfo — Información del proceso de recorte.

estructura

Información del proceso de recorte, devuelta como una estructura con campos:

• LoopClosuresToRemove — Identificadores de borde de cierre de bucle que se eliminarán de la entrada poseGraphObj. Estos cierres de bucle se eliminan en la salida poseGraphUpdated.

• Iterations — Número de iteraciones de recorte realizadas.

Referencias

[1] Yang, Heng, et al. “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection.” IEEE Robotics and Automation Letters, vol. 5, no. 2, Apr. 2020, pp. 1127–34. DOI.org (Crossref), doi:10.1109/LRA.2020.2965893.

Historial de versiones

Introducido en R2020b

Consulte también

Funciones

• poseGraphSolverOptions | edgeResidualErrors | removeEdges | edgeNodePairs | edgeConstraints

Objetos

• poseGraph | poseGraph3D | lidarSLAM