Main Content

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

Desde R2019b

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 punto 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 de 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.

ejemplo

addRelativePose(poseGraph,measurement,infoMat,fromNodeID,toNodeID) crea un borde especificando 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. Llamar a la función optimizePoseGraph combina múltiples medidas adjuntas en un solo 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

contraer todo

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 Intel Research Lab Dataset 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")

Figure contains an axes object. The axes object with title Optimized Pose Graph, xlabel X, ylabel Y contains 3 objects of type line. One or more of the lines displays its values using only markers

Modifique la pose 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")

Figure contains an axes object. The axes object with title Updated Pose Graph, xlabel X, ylabel Y contains 3 objects of type line. One or more of the lines displays its values using only markers

Ciertos cierres de bucle deben recortarse del gráfico de pose. Utilice la función trimLoopClosures para recortar estos cierres de bucle incorrectos. 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 solver . 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")

Figure contains an axes object. The axes object with title New Pose Graph, xlabel X, ylabel Y contains 3 objects of type line. One or more of the lines displays its values using only markers

Argumentos de entrada

contraer todo

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

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

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

Para poseGraph3D, la pose es un vector [x y z qw qx qy qz] , que se define mediante una posición xyz y una 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 .

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 medida es un vector [x y theta] , la matriz de covarianza es un cálculo de covarianza de 3 por 3 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 punto 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].

Nodo desde el que conectarse, especificado como un entero positivo. Este número 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 .

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

Argumentos de salida

contraer todo

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.

ID del borde agregado, devuelto como un entero positivo.

Capacidades ampliadas

Historial de versiones

Introducido en R2019b