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

addFactor

Agregar factor al gráfico de factores

Desde R2022a

Sintaxis

factorIDs = addFactor(fg,factor)

factorIDs = addFactor(fg,factor,groupID)

Descripción

La función addFactor agrega uno o más factores a un gráfico de factores y puede agrupar los factores y nodos que se especifican mediante los factores agregados.

factorIDs = addFactor(fg,factor) agrega uno o más factores al gráfico de factores especificado y devuelve los ID de los factores agregados.

Si la suma de los factores da como resultado un nodo no válido, addFactor devuelve un error e indica los nodos no válidos.

Nota

addFactor solo admite la adición de un solo factor para los objetos factorIMU y factorGPS.

factorIDs = addFactor(fg,factor,groupID) agrega un factor al gráfico de factores con ID de grupo groupID. Los ID de nodo del mismo grupo se pueden recuperar por ID de grupo usando la función nodeIDs. Puede utilizar ID de grupo para representar marcas de tiempo o fotogramas.

Ejemplos

Optimizar gráfico de factores simples

Crea una gráfica de factores.

fg = factorGraph;

Define dos estados de pose del robot como la ground-truth.

rstate = [0 0 0;
          1 1 pi/2];

Defina la medida de pose relativa entre dos nodos de la odometría como la diferencia de pose entre los estados con algo de ruido. La medida relativa debe estar en el marco de referencia del segundo nodo, por lo que debe rotar la diferencia de posición para estar en el marco de referencia del segundo nodo.

posediff = diff(rstate);
rotdiffso2 = so2(posediff(3),"theta");
transformedPos = transform(inv(rotdiffso2),posediff(1:2));
odomNoise = 0.1*rand;
measure = [transformedPos posediff(3)] + odomNoise;

Crea un factor que conecte dos poses SE(2) con la medida relativa entre las poses. Luego agregue el factor al gráfico de factores para crear dos nodos.

ids = generateNodeID(fg,1,"factorTwoPoseSE2");
f = factorTwoPoseSE2(ids,Measurement=measure);
addFactor(fg,f);

Obtenga el estado de ambos nodos de pose.

stateDefault = nodeState(fg,ids)

stateDefault = 2×3

     0     0     0
     0     0     0

Como estos nodos son nuevos, tienen valores de estado predeterminados. Idealmente, antes de optimizar, deberías asignar una estimación aproximada de la pose absoluta. Esto aumenta la posibilidad de que la función optimize encuentre el mínimo global. De lo contrario, optimize podría quedar atrapado en el mínimo local, produciendo una solución subóptima.

Mantenga el primer estado del nodo en el origen y establezca el segundo estado del nodo en una posición xy aproximada en [0.9 0.95] y una rotación theta de pi/3 radianes. En aplicaciones prácticas, puede utilizar mediciones de sensores de su odometría para determinar el estado aproximado de cada nodo de pose.

nodeState(fg,ids(2),[0.9 0.95 pi/3])

ans = 1×3

    0.9000    0.9500    1.0472

Antes de optimizar, guarde el estado del nodo para poder volver a optimizarlo según sea necesario.

statePriorOpt1 = nodeState(fg,ids);

Optimice los nodos y verifique los estados de los nodos.

optimize(fg);
stateOpt1 = nodeState(fg,ids)

stateOpt1 = 2×3

   -0.1161    0.9026    0.0571
    1.0161    0.0474    1.7094

Tenga en cuenta que después de la optimización, el primer nodo no permaneció en el origen porque, aunque el gráfico tiene la estimación inicial del estado, no tiene ninguna restricción en la posición absoluta. El gráfico solo tiene la medida de pose relativa, que actúa como una restricción para la pose relativa entre los dos nodos. Entonces el gráfico intenta reducir el coste relacionado con la pose relativa, pero no con la pose absoluta. Para proporcionar más información al gráfico, puede corregir el estado de los nodos o agregar un factor de medición previo absoluto.

Restablezca los estados y luego arregle el primer nodo. Luego verifique que el primer nodo esté fijo.

nodeState(fg,ids,statePriorOpt1);
fixNode(fg,ids(1))
isNodeFixed(fg,ids(1))

ans = logical
   1

Vuelva a optimizar el gráfico de factores y obtenga los estados de los nodos.

optimize(fg)

ans = struct with fields:
             InitialCost: 1.9452
               FinalCost: 1.9452e-16
      NumSuccessfulSteps: 2
    NumUnsuccessfulSteps: 0
               TotalTime: 7.3910e-05
         TerminationType: 0
        IsSolutionUsable: 1
        OptimizedNodeIDs: 1
            FixedNodeIDs: 0

stateOpt2 = nodeState(fg,ids)

stateOpt2 = 2×3

         0         0         0
    1.0815   -0.9185    1.6523

Tenga en cuenta que después de optimizar este tiempo, el estado del primer nodo permaneció en el origen.

Agregar nodos a grupos

Cree un gráfico de factores, genere identificadores de nodos y cree dos factores factorTwoPoseSE2.

fg1 = factorGraph;
ids = generateNodeID(fg1,[2 2])

ids = 2×2

     0     1
     2     3

f = factorTwoPoseSE2(ids);

Agrupar todos los nodos

Suma todos los nodos de los factores al grupo 1.

addFactor(fg1,f,1);
fg1Group1 = nodeIDs(fg1,GroupID=1)

fg1Group1 = 1×4

     0     1     2     3

Agrupar nodos por columna

Especifique el ID del grupo como un vector fila para agregar los nodos de la primera columna de los ID de nodo de los factores al grupo 1 y agregue los nodos de la segunda columna al grupo 2.

fg2 = factorGraph;
addFactor(fg2,f,[1 2]);
fg2Group1 = nodeIDs(fg2,GroupID=1)

fg2Group1 = 1×2

     0     2

fg2Group2 = nodeIDs(fg2,GroupID=2)

fg2Group2 = 1×2

     1     3

Agrupar nodos por fila

Especifique el ID de grupo como vector columna para agregar los nodos de la primera fila en los ID de nodo de los factores al grupo 1 y agregue los nodos de la segunda fila al grupo 2.

fg3 = factorGraph;
addFactor(fg3,f,[1; 2]);
fg3Group1 = nodeIDs(fg3,GroupID=1)

fg3Group1 = 1×2

     0     1

fg3Group2 = nodeIDs(fg3,GroupID=2)

fg3Group2 = 1×2

     2     3

Agrupar nodos por matriz

También puede especificar el ID del grupo como una matriz del mismo tamaño que los ID de los nodos de los factores para asignar cada nodo a un grupo específico. Agregue el primer y cuarto nodo al grupo 1 y el segundo y tercer nodo a los grupos 2 y 3, respectivamente.

fg4 = factorGraph;
groups = [1 2;
          3 1];
addFactor(fg4,f,groups);
fg4Group1 = nodeIDs(fg4,GroupID=1)

fg4Group1 = 1×2

     0     3

fg4Group2 = nodeIDs(fg4,GroupID=2)

fg4Group2 = 
1

fg4Group3 = nodeIDs(fg4,GroupID=3)

fg4Group3 = 
2

Argumentos de entrada

fg — Gráfico de factores para agregar factores

objeto factorGraph

Gráfico de factores al que agregar factor, especificado como un objeto factorGraph.

factor — Factores para agregar al gráfico de factores

objeto de factor válido

Factores para agregar al gráfico de factores, especificados como un objeto de factor válido.

Un objeto de factor válido debe ser uno de estos objetos y el objeto no debe crear ningún nodo no válido cuando se agrega al gráfico de factores:

• factorGPS

• factorIMU

• factorCameraSE3AndPointXYZ

• factorPoseSE2AndPointXY

• factorPoseSE3AndPointXYZ

• factorTwoPoseSE2

• factorTwoPoseSE3

• factorTwoPoseSIM3

• factorIMUBiasPrior

• factorPoseSE3Prior

• factorVelocity3Prior

Con la excepción de factorGPS y factorIMU, puede agregar simultáneamente múltiples factores al gráfico de factores usando cualquiera de los objetos de factor enumerados. factorGPS y factorIMU solo admiten la adición de un solo factor.

Si el objeto de factor especificado crea nodos no válidos, entonces addFactor no agrega ninguno de los factores del objeto de factor.

groupID — ID de grupo para nodos de factor agregado

entero no negativo | vector fila de dos elementos de números enteros no negativos | N- vector columna de elementos enteros no negativos | Matriz N por 2 de números enteros no negativos

ID de grupo para nodos del factor agregado, especificados como cualquiera de estas opciones:

groupID Tamaño	Comportamiento de agrupación
entero no negativo	Asigna todos los nodos a un grupo. Por ejemplo, si agrega un objeto de factor que tiene un valor NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ con un valor groupID de 1, addFactor agrega los nodos 1, 2, 3 y 4 al grupo 1.
vector fila de dos elementos de números enteros no negativos	Especifique grupos para cada columna de nodos. Por ejemplo, si agrega un objeto de factor que tiene un valor NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ con un valor groupID de $$\left[\begin{array}{cc}1& 2\end{array}\right]$$, addFactor agrega los nodos 1 y 3 al grupo 1 y agrega los nodos 2 y 4 al grupo 2. El comportamiento de los factores IMU es diferente. Si agrega un factor IMU con un valor NodeID de $$\left[\begin{array}{cccccc}1& 2& 3& 4& 5& 6\end{array}\right]$$ y groupID establecido en $$\left[\begin{array}{cc}1& 2\end{array}\right]$$, addFactor agrega los nodos 1, 2 y 3 al grupo 1 y los nodos 4, 5 y 6 al grupo 2.
N- vector columna de elementos enteros no negativos	Agrupa los nodos por factor, donde N es el número total de factores especificados por la propiedad NodeID de factor. Por ejemplo, si agrega un objeto de factor que tiene un valor NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ con un valor groupID de $$\left[\begin{array}{c}1\\ 2\end{array}\right]$$, addFactor agrega los nodos 1 y 2 al grupo 1 y agrega los nodos 3 y 4 al grupo 2.
Matriz N por 2 de números enteros no negativos	Agregue nodos en NodeID al grupo especificado en su índice correspondiente en groupID, donde N es el número total de filas de la propiedad NodeID de factor. Por ejemplo, si agrega un objeto de factor que tiene un valor NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ con un valor groupID de $$\left[\begin{array}{cc}1& 2\\ 3& 1\end{array}\right]$$, addFactor agrega los nodos 1 y 4 al grupo 1, agrega el nodo 2 al grupo 2 y agrega el nodo 3 al grupo 3.

Nota

Al agregar un objeto factorIMU o factorGPS a un gráfico de factores, groupID solo acepta estos valores:

• factorIMU — Entero no negativo o un vector fila de dos elementos de enteros no negativos.

• factorGPS — Entero no negativo

Agregar nodos a grupos le permite consultar los identificadores de nodos por grupo especificando el argumento de nombre-valor GroupID de la función nodeIDs.

Argumentos de salida

factorIDs — ID de factores de factores agregados

N- vector fila de elementos enteros no negativos

Identificadores de los factores agregados, devueltos como un vector fila de elementos N de números enteros no negativos. N es el número total de factores agregados.

La función devuelve este argumento solo cuando agrega correctamente los factores al gráfico de factores. Si la adición de factores da como resultado un nodo no válido, entonces addFactor no agrega ninguno de los factores del objeto de factor.

Capacidades ampliadas

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

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

Al generar código C portable con un compilador C++, debe especificar el empaquetado jerárquico con encabezados no mínimos. Para obtener más información sobre las opciones de embalaje, consulte la función packNGo (MATLAB Coder).

Historial de versiones

Introducido en R2022a

R2025a: Añadir el factor de transformación de similitud SIM(3)

addFactor ahora admite agregar el factor de transformación de similitud SIM(3) al gráfico de factores, usando el objeto factorTwoPoseSIM3.

R2023a: Especificar ID de grupo para factores agregados

addFactor ahora admite la especificación de grupos a los cuales agregar nodos de factores agregados por ID de grupo.

Consulte también

Objetos

• factorGraph | factorTwoPoseSE2 | factorTwoPoseSE3 | factorPoseSE3Prior | factorVelocity3Prior | factorIMUBiasPrior | factorGPS | factorIMU

Funciones

• nodeIDs

Temas

• Localización de peatones basada en gráficos de factores con sensores IMU y GPS
• SLAM de punto de referencia usando marcadores AprilTag