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

addRelativePose

Agregar pose relativa al gráfico de pose

Sintaxis

addRelativePose(poseGraph,measurement)

addRelativePose(poseGraph,measurement,infoMat)

addRelativePose(poseGraph,measurement,infoMat,fromNodeID)

addRelativePose(poseGraph,measurement,infoMat,fromNodeID,toNodeID)

[nodePair,edgeID] = addRelativePose(___)

Descripción

addRelativePose(poseGraph,measurement) crea un nodo basado en la entrada measurement que se conecta al último nodo de pose en el gráfico de pose. Para agregar nodos de referencia, consulte la función addPointLandmark.

addRelativePose(poseGraph,measurement,infoMat) también especifica la matriz de información como parte de la restricción del borde, que representa la incertidumbre de la medición de la pose.

addRelativePose(poseGraph,measurement,infoMat,fromNodeID) crea un nuevo nodo de pose y lo conecta al nodo específico especificado por fromNodeID.

addRelativePose(poseGraph,measurement,infoMat,fromNodeID,toNodeID) crea un borde al especificar una medida de pose relativa entre los nodos existentes especificados por fromNodeID y toNodeID. Este borde se llama cierre de bucle. Si ya existe un cierre de bucle, la función agrega la nueva medición. Al llamar a la función optimizePoseGraph se combinan múltiples mediciones adjuntas en un único borde. Esta sintaxis no admite la adición de aristas a un nodo de referencia.

[nodePair,edgeID] = addRelativePose(___) devuelve el borde recién agregado y el ID del borde utilizando cualquiera de las sintaxis anteriores.

Ejemplos

Identificar y eliminar cierres de bucles falsos de Pose Graph

Este ejemplo muestra cómo identificar y eliminar cierres de bucles espurios del gráfico de pose. Para hacer esto, puede modificar la pose relativa de un borde de cierre de bucle e intentar optimizar el gráfico de pose con y sin eliminar el cierre de bucle espurio automático y comparar los resultados.

Cargue el Conjunto de datos de Intel Research Lab que contiene un gráfico de pose 2-D. Optimiza el gráfico de pose. Traza el gráfico de pose con las identificaciones desactivadas. Las líneas rojas indican cierres de bucle identificados en el conjunto de datos.

load intel-2d-posegraph.mat pg
optimizedPG = optimizePoseGraph(pg);
show(optimizedPG,IDs="off");
title("Optimized Pose Graph")

Modifique la postura relativa del borde de cierre del bucle 1386 a algunos valores aleatorios.

loopclosureId = 1386;
nodePair = edgeNodePairs(optimizedPG,loopclosureId);
[relPose,infoMat] = edgeConstraints(optimizedPG,loopclosureId);
relPose(2) = -5;
relPose(3) = 1.5;
addRelativePose(optimizedPG,relPose,infoMat,nodePair(1),nodePair(2));

Optimice el gráfico de pose sin recortar el cierre automático del bucle. Trace el gráfico de pose optimizado para ver el ajuste deficiente de los nodos con cierres de bucle.

[updatedPG,solutionInfo] = optimizePoseGraph(optimizedPG);
show(updatedPG,IDs="off");
title("Updated Pose Graph")

Ciertos cierres de bucle deben recortarse del gráfico de pose. Utilice la función trimLoopClosures para recortar estos cierres de bucle defectuosos. Establezca el umbral de truncamiento y las iteraciones máximas para los parámetros del recortador.

trimParams = struct("TruncationThreshold",0.5,"MaxIterations",100);

Generar opciones de solver .

solverOptions = poseGraphSolverOptions("g2o-levenberg-marquardt");

Utilice la función trimLoopClosures con los parámetros del recortador y las opciones del solucionador. Trace el nuevo gráfico de pose para ver que se eliminaron los cierres de bucle incorrectos.

[newPG,trimInfo] = trimLoopClosures(updatedPG,trimParams,solverOptions);
show(newPG,IDs="off");
title("New Pose Graph")

Argumentos de entrada

poseGraph — Gráfico de pose

objeto poseGraph | objeto poseGraph3D

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

measurement — Pose relativa entre nodos.

vector [x y theta] | vector [x y z qw qx qy qz]

Pose relativa entre nodos, especificada como una de las siguientes:

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

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

Nota

Muchas otras fuentes de gráficos de poses 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.

infoMat — Matriz de información

vector de seis elementos | vector de 21 elementos

Matrices de información, especificadas en forma compacta como un vector de seis elementos o un vector de 21 elementos.

Cada vector es la forma compacta del triángulo superior de la matriz de información cuadrada. Una matriz de información representa la incertidumbre de la medición. La matriz se calcula como la inversa de la covarianza. Si la medición es un vector [x y theta], la matriz de covarianza es un 3 por 3 de cálculos de covarianza por pares. Normalmente, la incertidumbre está determinada por el modelo del sensor.

Para poseGraph (2-D), cada matriz de información es un vector de seis elementos. El valor predeterminado es [1 0 0 1 0 1]. Para los nodos de referencia, los últimos tres elementos se devuelven como NaN.

Para poseGraph3D, cada matriz de información es un vector de 21 elementos. El valor predeterminado es [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1].

fromNodeID — Nodo desde el que conectarse

número entero positivo

Nodo desde el que conectarse, especificado como un entero positivo. Este entero corresponde al ID de nodo de un nodo en poseGraph. Cuando se especifica sin toNodeID, addRelativePose crea un nuevo nodo y agrega un borde entre el nuevo nodo y el nodo fromNodeID.

toNodeID — Nodo al que adjuntar

número entero positivo

Nodo al que conectarse, especificado como un entero positivo. Este entero corresponde al ID de nodo de un nodo en poseGraph. addRelativePose agrega un borde entre este nodo y el nodo fromNodeID.

Argumentos de salida

nodePair — Par de nodos de borde en gráfico de pose

vector de dos elementos

Pares de nodos de borde en un gráfico de pose, devueltos como un vector de dos elementos que enumera los ID de los dos nodos que conecta cada borde. Pueden existir múltiples aristas entre el mismo par de nodos.

edgeID — ID del borde agregado

número entero positivo

ID del borde agregado, devuelto como un entero positivo.

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 la cantidad 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.

poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)

Historial de versiones

Introducido en R2019b

Consulte también

Funciones

• optimizePoseGraph | findEdgeID | edgeNodePairs | edgeConstraints | nodeEstimates | removeEdges

Objetos

• poseGraph | poseGraph3D | lidarSLAM

Temas

• Implementar localización y mapeo simultáneos (SLAM) con escaneos Lidar
• SLAM de punto de referencia usando marcadores AprilTag