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.

factorGraph

Gráfico bipartito de factores y nodos.

Desde R2022a

expandir todo en la página

Descripción

Un objeto factorGraph almacena un gráfico bipartito que consta de factores conectados a nodos variables. Los nodos representan las variables aleatorias desconocidas en un problema de estimación, como las poses de los robots, y los factores representan restricciones probabilísticas en esos nodos, derivadas de mediciones o conocimientos previos. Durante la optimización, el gráfico de factores utiliza todos los factores y los estados actuales de los nodos para actualizar los estados de los nodos.

Para utilizar el gráfico de factores:

  1. Cree un objeto factorGraph vacío.

  2. Para cada tipo de factor deseado:

    1. Genere ID de nodo utilizando la función de objeto generateNodeID .

    2. Defina factores con los ID de nodo deseados, utilizando cualquiera de los objetos de factor admitidos:

    3. Agregue factores al gráfico de factores usando la función de objeto addFactor . Si el gráfico de factores no contiene un nodo con el ID especificado, la función crea automáticamente un nodo con ese ID y lo agrega al gráfico de factores al agregar el factor al gráfico de factores. Si el gráfico de factores contiene un nodo con el ID especificado, asegúrese de que agregar el nuevo factor no provoque una discrepancia en el tipo de nodo. Para obtener más información, consulte Sugerencias. Para obtener una lista de los tipos de nodos esperados para cada factor, consulte Tipos de nodos esperados de objetos de factores.

  3. Compruebe si todos los nodos en el gráfico de factores están conectados a al menos otro nodo usando la función de objeto isConnected .

  4. Cree un objeto factorGraphSolverOptions para especificar las opciones del solver de gráficos de factores.

  5. Optimice el gráfico de factores utilizando la función de objeto optimize con las opciones deseadas de solver del gráfico de factores.

  6. Extraiga datos de nodos del gráfico de factores, como ID de nodos y estados de nodos, utilizando las funciones de objeto nodeIDs y nodeState .

Creación

Sintaxis

graph = factorGraph

Descripción

ejemplo

graph = factorGraph crea un objeto factorGraph vacío.

Propiedades

expandir todo

Esta propiedad o parámetro es de solo lectura.

Número de nodos en el gráfico de factores, especificado como un entero positivo. NumNodes tiene un valor de 0 cuando el gráfico de factores está vacío y NumNodes aumenta cada vez que agrega un factor que especifica nuevos ID de nodo al gráfico de factores.

Los nodos en el gráfico de factores pueden ser cualquiera de estos tipos:

  • "POSE_SE2" — Pose en el espacio de estados SE(2)

  • "POSE_SE3" — Pose en el espacio de estados SE(3)

  • "VEL3" — Velocidad 3-D

  • "POINT_XY" — Punto 2-D

  • "POINT_XYZ" — Punto 3-D

  • "IMU_BIAS" — Giroscopio IMU y polarización del acelerómetro

Para verificar el tipo de nodo de un nodo en el gráfico, use la función nodeType .

Nota

El gráfico de factores establece el tipo de nodo cuando agrega el objeto de factor que especifica ese nodo al gráfico de factores. No puede cambiar el tipo de nodo de un nodo después de agregarlo al gráfico.

Esta propiedad o parámetro es de solo lectura.

Número de factores en el gráfico de factores, especificado como un número entero positivo. NumFactors tiene un valor de 0 cuando el gráfico de factores está vacío y NumFactors aumenta cada vez que agrega un factor al gráfico de factores.

Puedes usar addfactor para agregar cualquiera de estos objetos de factores al gráfico de factores:

Relacionar poses con mediciones de sensores

  • factorGPS — Conecte el nodo de pose SE(3) ("POSE_SE3") a una medición GPS.

  • factorIMU — Conecta dos nodos de pose SE(3) ("POSE_SE3"), dos nodos de velocidad 3-D ("VEL3") y dos Nodos de polarización de IMU ("IMU_BIAS") utilizando una medición de IMU.

Relacionar poses con posiciones de punto de referencia

  • factorCameraSE3AndPointXYZ — Conecte el nodo de pose SE(3) de una cámara estenopeica ("POSE_SE3") a nodos de referencia tridimensionales ("Point_XYZ") utilizando medidas de pose relativa.

  • factorPoseSE2AndPointXY — Conecte un nodo de pose SE(2) ("POSE_SE2") a nodos de referencia 2-D ("Point_XY") usando mediciones de pose relativas .

  • factorPoseSE3AndPointXYZ — Conecte un nodo de pose SE(3) ("POSE_SE3") a nodos de referencia 3-D ("Point_XYZ") usando mediciones de pose relativas .

Relacionar poses entre sí

  • factorTwoPoseSE2 — Conecte pares de nodos de pose SE(2) ("POSE_SE2") con poses relativas utilizando medidas de pose relativas.

  • factorTwoPoseSE3 — Conecte pares de nodos de pose SE(3) ("POSE_SE3") con poses relativas utilizando medidas de pose relativas.

Relacionar poses o velocidades con medidas conocidas anteriormente

  • factorIMUBiasPrior — Conecta nodos de pose SE(3) ("POSE_SE3"), nodos de velocidad 3-D ("VEL3") y nodos de polarización IMU ("IMU_BIAS") a mediciones IMU conocidas anteriormente.

  • factorPoseSE3Prior — Conecte los nodos de pose SE(3) ("POSE_SE3") a mediciones de pose SE(3) conocidas anteriormente.

  • factorVelocity3Prior — Conecte el nodo de velocidad 3-D ("VEL_3") a mediciones de velocidad SE(3) conocidas anteriormente.

Funciones del objeto

addFactorAgregar factor al gráfico de factores
fixNodeArreglar o liberar nodos en el gráfico de factores
generateNodeIDGenerar nuevos ID de nodo
hasNodeCompruebe si el ID del nodo existe en el gráfico de factores
isConnectedCompruebe si el gráfico de factores está conectado
isNodeFixedCompruebe si el nodo está arreglado
nodeIDsObtener ID de nodo en el gráfico de factores
nodeStateObtener o establecer el estado del nodo en el gráfico de factores
nodeTypeObtener el tipo de nodo en el gráfico de factores
optimizeOptimizar gráfico de factores

Ejemplos

contraer todo

Abrir script en vivo

Crea una gráfica de factores.

fg = factorGraph;

Defina dos estados de pose aproximados del robot. 

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;

Cree un factor de dos poses SE(2) con la medida relativa. 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 puede quedar atrapado en el mínimo local, produciendo una solución subóptima.

Mantenga el estado del primer nodo en el origen y establezca el estado del segundo 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),rstate(2,:))
ans = 1×3

    1.0000    1.0000    1.5708

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.1038    0.8725    0.1512
    1.1038    0.1275    1.8035

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.8470
               FinalCost: 1.8470e-16
      NumSuccessfulSteps: 2
    NumUnsuccessfulSteps: 0
               TotalTime: 8.2970e-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.

Más acerca de

expandir todo

Sugerencias

  • Para especificar múltiples factores y nodos a la vez para un tipo de factor específico, use la función generateNodeID y especifique el número de factores y el tipo de factor. Consulte la función generateNodeID para obtener más detalles.

    poseIDPairs = generateNodeID(fg,3,"factorTwoPoseSE2");
ftpse2 = factorTwoPoseSE2(poseIDPairs);

  • Puede obtener los estados de todos los nodos de pose usando primero la función nodeIDs y especificando el tipo de nodo como "POSE_SE2" para las poses del robot SE(2) y "POSE_SE3" para poses de robot SE(3). Luego, use la función nodeState con esos ID de nodo para obtener los estados de los nodos de pose del robot.

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

  • Verifique los tipos de nodos que cada factor crea o a los que se conecta antes de agregar factores al gráfico de factores para evitar errores de discrepancia de tipos de nodos. Para obtener una lista de los tipos de nodos esperados para cada factor, consulte Tipos de nodos esperados de objetos de factores.

Referencias

[1] Dellaert, Frank. Factor graphs and GTSAM: A Hands-On Introduction. Georgia: Georgia Tech, September, 2012.

Capacidades ampliadas

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

Historial de versiones

Introducido en R2022a

Consulte también

Objetos

Funciones

Temas