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.

fixNode

Arreglar o liberar nodos en el gráfico de factores

Desde R2022a

contraer todo en la página

Sintaxis

fixNode(graph,nodeIDs)
fixNode(graph,nodeIDs,flag)

Descripción

ejemplo

fixNode(graph,nodeIDs) corrige los nodos con los ID de nodo especificados durante la optimización. Cuando arreglas un nodo, la función optimize no cambia el estado de ese nodo.

fixNode(graph,nodeIDs,flag) especifica si se repara o libera el nodo con los ID especificados, nodeIDs.

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.

Argumentos de entrada

contraer todo

Gráfico de factores que contiene nodos para arreglar, especificado como un objeto factorGraph .

ID de los nodos que se van a fijar o liberar dentro del gráfico de factores, especificados como un vector fila de elementos N de enteros no negativos. N es el número total de nodos a reparar o liberar.

Nuevo estado de corrección del nodo, especificado como 1 (true) o 0 (false Si se especifica como true, la función repara el nodo para que la función optimize no cambie el estado del nodo. Si se especifica como false, la función libera el nodo durante la optimización.

Capacidades ampliadas

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

Historial de versiones

Introducido en R2022a

expandir todo

Consulte también

Objetos