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

nodeState

Obtener o establecer el estado del nodo en el gráfico de factores

Desde R2022a

contraer todo en la página

Sintaxis

state = nodeState(fg,nodeIDs)
nodeState(fg,nodeIDs,newStates)

Descripción

state = nodeState(fg,nodeIDs) obtiene el estado de los nodos con los ID de nodo especificados en el gráfico de factores especificado.

ejemplo

nodeState(fg,nodeIDs,newStates) establece el estado de los nodos con los ID especificados nodeIDs en el objeto factorGraph especificado fg en los estados especificados newStates.

ejemplo

Ejemplos

contraer todo

Abrir script en vivo

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.

Argumentos de entrada

contraer todo

Gráfico factorial, especificado como un objeto factorGraph.

ID de los nodos que se obtendrán o establecerán, especificados como un vector fila de elementos N de números enteros no negativos. N es el número total de nodos que se obtendrán o establecerán.

Todos los ID de nodo especificados deben especificar nodos del mismo tipo.

Nuevos estados de nodo, especificados como una matriz M por N. M es el número de ID especificados y N es el número de elementos de estado para los nodos especificados. Cada fila de la matriz especifica los valores del elemento de estado para el elemento correspondiente de nodeIDs.

Estos son los tipos de nodos admitidos y la forma de sus estados correspondientes:

  • POSE_SE2 — Pose en el espacio de estados SE(2) en la forma [x y theta]. x y y son las posiciones x y y, respectivamente, y theta es la orientación.

  • POSE_SE3 — Pose en el espacio de estados SE(3) en la forma [x y z qw qx qy qz]. x, y y z son las posiciones x, y y z, respectivamente, y qw, qx, qy y qz representan la orientación como un cuaternión.

  • POSE_SE3_SCALE — Escala de pose en el espacio de estados SE(3) en forma de una variable s.

  • "TRANSFORM_SE3"— Transformación del sensor en el espacio de estados SE(3) en la forma [dx dy dz dqw dqx dqy dqz]. dx, dy y dz son el cambio de posición en x, y y z respectivamente. dqw, dqx, dqy y dqz son el cambio en la rotación del cuaternión en w, x, y y z, respectivamente.

  • VEL3 — Velocidad 3-D en la forma [vx vy vz]. vx, vy y vz son las velocidades x, y y z, respectivamente.

  • POINT_XY — Punto 2-D en la forma [x y]. x y y son las posiciones x y y, respectivamente.

  • POINT_XYZ — Punto 3D con la forma [x y z]. x, y y z son las posiciones x, y y z, respectivamente.

  • IMU_BIAS — Sesgo del giroscopio y del acelerómetro de la IMU en la forma [bias_gyro_x bias_gyro_y bias_gyro_z bias_accel_x bias_accel_y bias_accel_z], donde:

    • bias_gyro_x, bias_gyro_y y bias_gyro_z son los sesgos del giroscopio IMU x, y y z, respectivamente.

    • bias_accel_x, bias_accel_y y bias_accel_z son los sesgos del acelerómetro x, y y z, respectivamente.

Argumentos de salida

contraer todo

Estado de los nodos, devuelto como una matriz M por N. M es el número de ID y N es el número de elementos de estado para los nodos especificados.

Estos son los tipos de nodos admitidos y la forma de sus estados correspondientes:

  • POSE_SE2 — Pose en el espacio de estados SE(2) en la forma [x y theta]. x y y son las posiciones x y y, respectivamente, y theta es la orientación.

  • POSE_SE3 — Pose en el espacio de estados SE(3) en la forma [x y z qw qx qy qz]. x, y y z son las posiciones x, y y z, respectivamente, y qw, qx, qy y qz representan la orientación como un cuaternión.

  • POSE_SE3_SCALE — Escala de pose en el espacio de estados SE(3) en forma de una variable s.

  • "TRANSFORM_SE3"— Transformación del sensor en el espacio de estados SE(3) en la forma [dx dy dz dqw dqx dqy dqz]. dx, dy y dz son el cambio de posición en x, y y z respectivamente. dqw, dqx, dqy y dqz son el cambio en la rotación del cuaternión en w, x, y y z, respectivamente.

  • VEL3 — Velocidad 3-D en la forma [vx vy vz]. vx, vy y vz son las velocidades x, y y z, respectivamente.

  • POINT_XY — Punto 2-D en la forma [x y]. x y y son las posiciones x y y, respectivamente.

  • POINT_XYZ — Punto 3D con la forma [x y z]. x, y y z son las posiciones x, y y z, respectivamente.

  • IMU_BIAS — Sesgo del giroscopio y del acelerómetro de la IMU en la forma [bias_gyro_x bias_gyro_y bias_gyro_z bias_accel_x bias_accel_y bias_accel_z], donde:

    • bias_gyro_x, bias_gyro_y y bias_gyro_z son los sesgos del giroscopio IMU x, y y z, respectivamente.

    • bias_accel_x, bias_accel_y y bias_accel_z son los sesgos del acelerómetro x, y y z, respectivamente.

Capacidades ampliadas

expandir todo

Historial de versiones

Introducido en R2022a

expandir todo

Consulte también

Objetos