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

optimizePoseGraph

Optimizar nodos en el gráfico de pose

Desde R2019b

Sintaxis

updatedGraph = optimizePoseGraph(poseGraph)

updatedGraph = optimizePoseGraph(poseGraph,solver)

[updatedGraph,solutionInfo] = optimizePoseGraph(___)

[___] = optimizePoseGraph(___,Name,Value)

Descripción

updatedGraph = optimizePoseGraph(poseGraph) ajusta las poses en función de las restricciones de borde definidas en el gráfico especificado para mejorar el gráfico general. Optimiza un gráfico de pose 2D o 3D. El gráfico de pose devuelto tiene la misma topología con nodos actualizados.

Esta optimización del gráfico de pose supone que todas las restricciones de borde y cierres de bucle son válidos. Para considerar recortar bordes en función de cierres de bucle incorrectos, consulte la función trimLoopClosures .

updatedGraph = optimizePoseGraph(poseGraph,solver) especifica el tipo de solver para optimizar el gráfico de pose.

[updatedGraph,solutionInfo] = optimizePoseGraph(___) devuelve estadísticas adicionales sobre el proceso de optimización en solutionInfo usando cualquiera de las sintaxis anteriores.

[___] = optimizePoseGraph(___,Name,Value) especifica opciones adicionales usando uno o más pares Name,Value . Por ejemplo, 'MaxIterations',1000 aumenta el número máximo de iteraciones a 1000.

Ejemplos

Optimizar un gráfico de pose en 3D

Optimice un gráfico de pose en función de los nodos y las restricciones de los bordes. El gráfico de pose utilizado en este ejemplo se tomó del MIT Dataset y se generó utilizando información extraída de un estacionamiento.

Cargue el gráfico de pose del conjunto de datos del MIT. Inspeccione el objeto poseGraph3D para ver el número de nodos y cierres de bucle.

load parking-garage-posegraph.mat pg
disp(pg);

  poseGraph3D with properties:

               NumNodes: 1661
               NumEdges: 6275
    NumLoopClosureEdges: 4615
     LoopClosureEdgeIDs: [128 129 130 132 133 134 135 137 138 139 140 142 143 144 146 147 148 150 151 204 205 207 208 209 211 212 213 215 216 217 218 220 221 222 223 225 226 227 228 230 231 232 233 235 236 237 238 240 241 242 243 244 ... ] (1x4615 double)
        LandmarkNodeIDs: [1x0 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.

title('Original Pose Graph')
show(pg,'IDs','off');
view(-30,45)

Optimiza el gráfico de pose. Los nodos se ajustan en función de las restricciones de los bordes y los cierres de bucles. Trace el gráfico de pose optimizado para ver el ajuste de los nodos con cierres de bucle.

updatedPG = optimizePoseGraph(pg);
figure
title('Updated Pose Graph')
show(updatedPG,'IDs','off');
view(-30,45)

Argumentos de entrada

poseGraph — Gráfico de pose en 2D o 3D

poseGraph objeto | poseGraph3D objeto | digraph objeto

Gráfico de pose 2D o 3D, especificado como un objeto poseGraph, poseGraph3D, digraph .

Para usar el objeto digraph , genere el gráfico de pose usando createPoseGraph a partir de un imageviewset o pcviewset . Debe tener Computer Vision Toolbox™ y solver debe estar configurado en "builtin-trust-region". Los pares nombre-valor 'LoopClosuresToIgnore' y 'FirstNodePose' se ignoran si se especifican.

Los bordes del objeto digraph se describen mediante objetos simtform3d (Image Processing Toolbox) o rigidtform3d (Image Processing Toolbox) .

solver — solver de gráficos de pose

"builtin-trust-region" (predeterminado) | "g2o-levenberg-marquardt"

solver de gráficos de pose, especificado como "builtin-trust-region" o "g2o-levenberg-marquardt". Para ajustar cualquiera de los solver, utilice los argumentos del par nombre-valor para ese solver.

Argumentos de par nombre-valor

Especifique pares de argumentos opcionales como Name1=Value1,...,NameN=ValueN, donde Name es el nombre del argumento y Value es el valor correspondiente. Los argumentos nombre-valor deben aparecer después de los otros argumentos, pero el orden de los pares no importa.

Antes de R2021a, utilice comas para separar cada nombre y valor, y encierre Name entre comillas.

Ejemplo: 'MaxTime', 300

Nota

Dependiendo de la entrada solver , la función admite diferentes pares nombre-valor.

Si la entrada solver se establece en "builtin-trust-region":

MaxTime — Tiempo máximo permitido

500 (predeterminado) | escalar numérico positivo

Tiempo máximo permitido, especificado como el par separado por comas que consta de 'MaxTime' y un escalar numérico positivo en segundos. El optimizador sale después de exceder este tiempo.

GradientTolerance — Límite inferior de la norma del gradiente

0.5e-8 (predeterminado) | escalar

Límite inferior de la norma del gradiente, especificado como el par separado por comas que consta de 'GradientTolerance' y un escalar. La norma del gradiente se calcula en función de la función de coste de la optimización. Si la norma cae por debajo de este valor, el optimizador sale.

FunctionTolerance — Límite inferior del cambio en la función de costes

1e-8 (predeterminado) | escalar

Límite inferior del cambio en la función de coste, especificado como el par separado por comas que consta de 'FunctionTolerance' y un escalar. Si el cambio de la función de costes cae por debajo de este valor entre los pasos de optimización, el optimizador sale.

StepTolerance — Límite inferior del tamaño del paso

1e-12 (predeterminado) | escalar

Límite inferior del tamaño del paso, especificado como el par separado por comas que consta de 'StepTolerance' y un escalar. Si la norma del paso de optimización cae por debajo de este valor, el optimizador sale.

InitialTrustRegionRadius — Radio de región de confianza inicial

100 (predeterminado) | escalar

Radio de la región de confianza inicial, especificado como escalar.

VerboseOutput — Mostrar información de iteración intermedia

'off' (predeterminado) | 'on'

Muestra información de iteración intermedia en la línea de comando MATLAB^® , especificada como el par separado por comas que consta de 'VerboseOutput' y 'off' o 'on'.

LoopClosuresToIgnore — ID de bordes de cierre de bucle en el gráfico de pose

vector

Identificadores de bordes de cierre de bucle en poseGraph, especificados como el par separado por comas que consta de 'LoopClosuresToIgnore' y un vector. Para obtener ID de borde del gráfico de pose, use findEdgeID.

FirstNodePose — Pose del primer nodo

[0 0 0] o [0 0 0 1 0 0 0] (predeterminado) | [x y theta] | [x y z qw qx qy qz]

Pose del primer nodo en poseGraph, especificado como el par separado por comas que consta de 'FirstNodePose' y un vector de pose.

Para poseGraph (2-D), la pose es un vector [x y theta] , que define la posición y el ángulo de orientación relativos de xy, theta.

Para poseGraph3D, la pose es un vector [x y z qw qx qy qz] , que define la posición relativa xyz y la orientación del cuaternión, [qw qx qy qz].

Nota

Muchas otras fuentes de gráficos de pose 3D, incluidos los formatos .g2o , especifican la orientación del cuaternión en un orden diferente, por ejemplo, [qx qy qz qw]. Verifique la fuente de los datos de su gráfico de pose antes de agregar nodos a su objeto poseGraph3D .

Si la entrada solver se establece en "g2o-levenberg-marquardt":

MaxIterations — Número máximo de iteraciones

300 (predeterminado) | número entero positivo

Número máximo de iteraciones, especificado como el par separado por comas que consta de 'MaxIterations' y un entero positivo. El optimizador sale después de exceder este número de iteraciones.

MaxTime — Tiempo máximo permitido

500 (predeterminado) | escalar numérico positivo

Tiempo máximo permitido, especificado como el par separado por comas que consta de 'MaxTime' y un escalar numérico positivo en segundos. El optimizador sale después de exceder este tiempo.

FunctionTolerance — Límite inferior del cambio en la función de costes

1e-8 (predeterminado) | escalar

Límite inferior del cambio en la función de coste, especificado como el par separado por comas que consta de 'FunctionTolerance' y un escalar. Si el cambio de la función de costes cae por debajo de este valor entre los pasos de optimización, el optimizador sale.

VerboseOutput — Mostrar información de iteración intermedia

'off' (predeterminado) | 'on'

Muestra información de iteración intermedia en la línea de comando MATLAB , especificada como el par separado por comas que consta de 'VerboseOutput' y 'off' o 'on'.

LoopClosuresToIgnore — ID de bordes de cierre de bucle en el gráfico de pose

vector

Identificadores de bordes de cierre de bucle en poseGraph, especificados como el par separado por comas que consta de 'LoopClosuresToIgnore' y un vector. Para obtener ID de borde del gráfico de pose, use findEdgeID.

FirstNodePose — Pose del primer nodo

[0 0 0] o [0 0 0 1 0 0 0] (predeterminado) | [x y theta] | [x y z qw qx qy qz]

Pose del primer nodo en poseGraph, especificado como el par separado por comas que consta de 'FirstNodePose' y un vector de pose.

Para poseGraph (2-D), la pose es un vector [x y theta] , que define la posición y el ángulo de orientación relativos de xy, theta.

Para poseGraph3D, la pose es un vector [x y z qw qx qy qz] , que define la posición relativa xyz y la orientación del cuaternión, [qw qx qy qz].

Nota

Muchas otras fuentes de gráficos de pose 3D, incluidos los formatos .g2o , especifican la orientación del cuaternión en un orden diferente, por ejemplo, [qx qy qz qw]. Verifique la fuente de los datos de su gráfico de pose antes de agregar nodos a su objeto poseGraph3D .

Argumentos de salida

updatedGraph — Gráfico de pose optimizado en 2-D o 3-D

poseGraph objeto | poseGraph3D objeto

Gráfico de pose 2D o 3D optimizado, devuelto como un objeto poseGraph o poseGraph3D .

solutionInfo — Estadísticas del proceso de optimización.

estructura

Estadísticas del proceso de optimización, devueltas como una estructura con estos campos:

• Iterations : número de iteraciones utilizadas en la optimización.

• ResidualError — Valor de la función de coste cuando sale el optimizador.

• Exit Flag — Condición de salida para el optimizador:

  • 1 — Mínimo local encontrado.

  • 2 : número máximo de iteraciones alcanzado. Consulte MaxIterations argumento de par nombre-valor.

  • 3 — Se agotó el tiempo de espera del algoritmo durante la operación.

  • 4 — Tamaño mínimo del paso. El tamaño del paso está por debajo del argumento del par nombre-valor StepTolerance .

  • 5 : el cambio en error está por debajo del mínimo.

  • 8 : el radio de la región de confianza está por debajo del mínimo establecido en InitialTrustRegionRadius.

Referencias

[1] Grisetti, G., R. Kummerle, C. Stachniss, and W. Burgard. "A Tutorial on Graph-Based SLAM." IEEE Intelligent Transportation Systems Magazine. Vol. 2, No. 4, 2010, pp. 31–43. doi:10.1109/mits.2010.939925.

[2] Carlone, Luca, Roberto Tron, Kostas Daniilidis, and Frank Dellaert. "Initialization Techniques for 3D SLAM: a Survey on Rotation Estimation and its Use in Pose Graph Optimization." 2015 IEEE International Conference on Robotics and Automation (ICRA). 2015, pp. 4597–4604.

Capacidades ampliadas

Generación de código C/C++

Genere código C y C++ mediante MATLAB® Coder™.

• Utilice esta sintaxis al construir objetos poseGraph o poseGraph3D para la generación de código:

  poseGraph = poseGraph('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) especifica un límite superior en el número de bordes y nodos permitidos en el gráfico de pose al generar código. Este límite solo es necesario al generar código.

• El argumento de entrada "g2o-levenberg-marquardt" solver no es compatible con la generación de código.

Historial de versiones

Introducido en R2019b

Consulte también

Funciones

• trimLoopClosures | addRelativePose | removeEdges | edgeNodePairs | edgeConstraints | findEdgeID | nodeEstimates

Objetos

• poseGraph | poseGraph3D | lidarSLAM

Temas

• Implementar localización y mapeo simultáneos (SLAM) con escaneos Lidar
• Implemente localización y mapeo simultáneos en línea (SLAM) con escaneos Lidar