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

factorPoseSE2AndPointXY

Factor que relaciona la posición SE(2) y el punto 2-D

Desde R2022b

Descripción

El objeto factorPoseSE2AndPointXY contiene factores, cada uno de los cuales describe la relación entre una posición en el espacio de estados SE(2) y un punto de referencia 2-D. Puede utilizar este objeto para agregar uno o más factores a un objeto factorGraph .

Creación

Sintaxis

F = factorPoseSE2AndPointXY(nodeID)

F = factorPoseSE2AndPointXY(___,Name=Value)

Descripción

F = factorPoseSE2AndPointXY(nodeID) crea un objeto factorPoseSE2AndPointXY , F, con la propiedad de números de identificación de nodo NodeID establecida en nodeID.

F = factorPoseSE2AndPointXY(___,Name=Value) especifica propiedades utilizando uno o más argumentos de nombre-valor además del argumento de la sintaxis anterior. Por ejemplo, factorPoseSE2AndPointXY([1 2],Measurement=[1 5]) establece la propiedad Measurement del objeto factorPoseSE2AndPointXY en [1 5].

Propiedades

NodeID — Números de identificación de nodo

N-por-2 matriz de enteros no negativos

Esta propiedad o parámetro es de solo lectura.

Números de ID de nodo, especificados como una matriz N-por 2 de enteros no negativos, donde N es el número total de factores deseados. Cada fila representa un factor que conecta un nodo de tipo POSE_SE2 a un nodo de tipo POINT_XY en la forma [PoseID PointID], donde PoseID es el ID del nodo POSE_SE2 y PointID es el ID del POINT_XY nodo en el gráfico de factores.

Si un factor en el objeto factorPoseSE2AndPointXY especifica un ID que no corresponde a un nodo en el gráfico de factores, el gráfico de factores crea automáticamente un nodo del tipo requerido con ese ID y lo agrega al factor. gráfico al sumar el factor al gráfico de factores.

Debe especificar esta propiedad en la creación del objeto.

Para obtener más información sobre los tipos de nodos esperados de todos los factores admitidos, consulte Tipos de nodos esperados de objetos de factor.

Measurement — Posición relativa medida

zeros(N,2) (predeterminado) | N-por-2 matriz

Posición relativa medida entre la posición actual y el punto de referencia, especificada como una matriz N-por 2 donde cada fila tiene la forma [dx dy], en metros. N es el número total de factores, y dx y dy son el cambio de posición en x y y, respectivamente.

Information — Matriz de información asociada a la incertidumbre de las medidas.

eye(2) (predeterminado) | matriz de 2 por 2 | arreglo de 2 por 2 por N

arreglo de información asociada a la incertidumbre de las mediciones, especificada como una matriz de 2 por 2 o un arreglo de 2 por 2 por N . N es el número total de factores especificados por el objeto factorPoseSE2AndPointXY . Cada matriz de información corresponde a las medidas del nodo correspondiente en NodeID.

Si especifica esta propiedad como una matriz de 2 por 2 cuando NodeID contiene más de una fila, la matriz de información corresponde a todas las mediciones en Measurement.

Esta matriz de información es la inversa de la matriz de covarianza, donde la matriz de covarianza tiene la forma:

$$\left[\begin{array}{cc}\sigma (x,x)& \sigma (x,y)\\ \sigma (y,x)& \sigma (y,y)\end{array}\right]$$

Cada elemento indica la covarianza entre dos variables. Por ejemplo, σ(x,y) es la covarianza entre x y y.

Funciones del objeto

nodeType

Obtener el tipo de nodo en el gráfico de factores

Ejemplos

Estimar poses utilizando factores de pose 2-D

Defina ground-truth para cinco poses de robots como un bucle y cree un gráfico de factores.

gndtruth = [0 0 0; 
            2 0 pi/2; 
            2 2 pi; 
            0 2 3*pi
            0 0 0];
fg = factorGraph;

Genere los ID de nodo necesarios para crear tres factores factorTwoPoseSE2 y luego agregue manualmente Porque el nodo 4 coincidiría directamente encima del nodo 0, en lugar de especificar un factor que conecte el nodo 3 con un nuevo nodo 4. , cree un cierre de bucle agregando otro factor que relacione el nodo 3 con el nodo 0.

poseFIDs = generateNodeID(fg,3,"factorTwoPoseSE2");
poseFIDs = [poseFIDs; 3 0]

poseFIDs = 4×2

     0     1
     1     2
     2     3
     3     0

Defina la medida relativa entre cada pose consecutiva y agregue un poco de ruido para que la medida se parezca más a la lectura de un sensor.

relMeasure = [2 0 pi/2; 2 0 pi/2; 2 0 pi/2; 2 0 pi/2] + 0.1*rand(4,3);

Cree los factores factorTwoPoseSE2 con las medidas relativas definidas y luego agregue los factores al gráfico de factores.

poseFactor = factorTwoPoseSE2(poseFIDs,Measurement=relMeasure);
addFactor(fg,poseFactor);

Obtenga los ID de nodo de todos los nodos de pose SE2 en el gráfico de factores.

poseIDs = nodeIDs(fg,NodeType="POSE_SE2");

Debido a que los nodos de tipo POSE_SE2 tienen un estado predeterminado de [0 0 0], debe proporcionar una estimación inicial del estado. Normalmente esto proviene de un sensor de odometría en el robot. Pero para este ejemplo, utilice ground-truth con algo de ruido.

predictedState = gndtruth(1:4,:);
predictedState(2:4,:) = predictedState(2:4,:) + 0.1*rand(3,3);

Luego, establezca los estados de los nodos de pose en los estados estimados previstos.

nodeState(fg,poseIDs,predictedState);

Arregle el primer nodo de pose. Debido a que todos los nodos son relativos entre sí, necesitan un estado conocido para ser un ancla.

fixNode(fg,0);

Optimice el gráfico de factores y los resultados visuales

Optimice el gráfico de factores con las opciones de solver predeterminadas. La optimización actualiza los estados de todos los nodos en el gráfico de factores para que las poses del vehículo se actualicen.

rng default
optimize(fg)

ans = struct with fields:
             InitialCost: 6.1614
               FinalCost: 0.0118
      NumSuccessfulSteps: 5
    NumUnsuccessfulSteps: 0
               TotalTime: 0.0230
         TerminationType: 0
        IsSolutionUsable: 1
        OptimizedNodeIDs: [1 2 3]
            FixedNodeIDs: 0

Obtenga y almacene los estados de nodo actualizados para el robot. Luego, trace los resultados, comparando la estimación del gráfico de factores de la trayectoria del robot con ground-truth del robot.

poseStatesOpt = nodeState(fg,poseIDs)

poseStatesOpt = 4×3

         0         0         0
    2.0777    0.0689    1.5881
    2.0280    2.1646   -3.1137
    0.0132    2.0864   -1.6014

figure
plot(gndtruth(:,1),gndtruth(:,2),Marker="*",LineWidth=1.5)
hold on
plot([poseStatesOpt(:,1); 0],[poseStatesOpt(:,2); 0],Marker="*",LineStyle="--",LineWidth=1);
legend(["Ground Truth","Opt. Position"]);
s2 = se2(poseStatesOpt,"xytheta");
plotTransforms(s2,FrameSize=0.5,FrameAxisLabels="on");
axis padded
hold off

Tenga en cuenta que las poses no coinciden perfectamente con ground-truth porque no hay muchos factores en este gráfico que la función optimize pueda usar para proporcionar una solución más precisa. La precisión se puede mejorar utilizando mediciones más precisas, conjeturas precisas del estado inicial y agregando factores adicionales para agregar más información para que la use el optimizador.

Más acerca de

Tipos de nodos esperados de objetos de factor

Estos son los tipos de nodos que la propiedad NodeID de cada objeto de factor especifica y a los que se conecta:

Objeto de factor	Tipos de nodos esperados de ID de nodos especificados
factorGPS	["POSE_SE3"]
factorIMU	["POSE_SE3","VEL3","IMU_BIAS","POSE_SE3","VEL3","IMU_BIAS"]
factorCameraSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
factorIMUBiasPrior	["IMU_BIAS"]
factorPoseSE3Prior	["POSE_SE3"]
factorVelocity3Prior	["VEL_3"]

Por ejemplo, factorPoseSE2AndPointXY([1 2]) crea un factor de referencia 2-D que se conecta a los ID de nodo 1 y 2. Si intenta agregar ese factor a un gráfico de factores que ya contiene los nodos 1 y 2, el factor espera que los nodos 1 y 2 sean de los tipos "POSE_SE2" y "POINT_XY", respectivamente.

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2022b

R2023a: Especifique múltiples factores

Las propiedades NodeID, Measurement y Information ahora aceptan filas adicionales para especificar múltiples factores.

Consulte también

Objetos

• factorGraph | factorTwoPoseSE2 | factorPoseSE3AndPointXYZ