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

optimize

Optimizar gráfico de factores

Desde R2022a

Sintaxis

solnInfo = optimize(graph)

solnInfo = optimize(graph,poseNodeIDs)

solnInfo = optimize(___,solverOptions)

Descripción

La función optimize optimiza un gráfico de factores para encontrar una solución que minimice el coste del problema de mínimos cuadrados no lineal formulado por el gráfico de factores.

solnInfo = optimize(graph) optimiza el gráfico de factores utilizando las opciones predeterminadas del solver , establece los estados de los nodos en los estados optimizados y devuelve la información de la solución resultante.

solnInfo = optimize(graph,poseNodeIDs) optimiza los nodos de pose especificados y cualquier nodo relacionado, excepto los nodos de pose no especificados.

Nota

Los ID de nodo especificados forman un gráfico de factores conectados. Para obtener más información, consulte Conectividad del gráfico de factores.

solnInfo = optimize(___,solverOptions) optimiza el gráfico de factores utilizando las opciones de solver de gráficos de factores especificadas, además de cualquier combinación de argumentos de entrada de sintaxis anteriores.

Ejemplos

Optimizar gráfico de factores simples

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.

Optimice incrementalmente el gráfico de factores mediante la ventana deslizante

Al crear gráficos de factores grandes, es ineficiente volver a optimizar un gráfico de factores completo cada vez que agrega nuevos factores y nodos al gráfico en un nuevo paso de tiempo. Este ejemplo muestra un enfoque de optimización alternativo. En lugar de optimizar un gráfico de factores completo en cada paso de tiempo, optimice un subconjunto o ventana de los nodos de pose más recientes. Luego, en el siguiente paso, deslice la ventana al siguiente conjunto de nodos de pose más recientes y optimice esos nodos de pose. Este enfoque minimiza la cantidad de veces que necesita volver a optimizar partes más antiguas del gráfico de factores.

Cree un gráfico de factores y cargue el archivo sensorData MAT. Este archivo MAT contiene datos del sensor para diez pasos de tiempo.

fg = factorGraph;
load sensorData.mat

Establezca el tamaño de la ventana en cinco nodos de pose. Cuanto mayor sea el tamaño de la ventana deslizante, más precisa será la optimización. Si el tamaño de la ventana deslizante es pequeño, es posible que no haya suficientes medidas para que la optimización produzca una solución precisa.

windowSize = 5;

Para cada paso de tiempo:

1. Genere una nueva ID de nodo para el nodo de pose del paso de tiempo actual.

2. Obtenga los datos de la pose actual para el paso de tiempo actual.

3. Genere una ID de nodo para un nodo de punto de referencia recién detectado. Conecte el nodo de pose actual a dos puntos de referencia. Supongamos que el primer punto de referencia que el robot detecta en el paso de tiempo actual es el mismo que el segundo punto de referencia que el robot detectó en el paso de tiempo anterior. Por primera vez, ambos puntos de referencia son nuevos, por lo que debe generar dos ID de puntos de referencia.

4. Agregue un factor de dos poses que conecte el nodo de pose en el paso de tiempo actual con el nodo de pose del paso de tiempo anterior.

5. Agregue factores de punto de referencia que creen los ID de nodo punto de referencia especificados al gráfico.

6. Establezca el estado inicial de los nodos de puntos de referencia y el nodo de pose actual como datos del sensor para el paso de tiempo actual.

7. Si el gráfico contiene al menos cinco nodos de pose, corrija el nodo de pose más antiguo en la ventana deslizante y luego optimice los nodos de pose en la ventana deslizante. Debido a que tanto las mediciones entre las poses como las mediciones entre las poses y los puntos de referencia son relativas, debe corregir el nodo más antiguo o la solución de optimización puede ser incorrecta. Tenga en cuenta que cuando especifica los nodos de pose, incluye factores que relacionan los nodos de pose especificados con otros nodos de pose especificados o con cualquier nodo que no sea de pose.

for t = 1:10
    % 1. Generate node ID for pose at this time step
    currPoseID = generateNodeID(fg,1);
    % 2. Get current pose data at this time step
    currPose = poseInitStateData{t};

    % 3. On the first time step, create pose node and two landmarks
    if t == 1
        lmID = generateNodeID(fg,2);
        lmFactorIDs = [currPoseID lmID(1); 
                       currPoseID lmID(2)];
    else % On subsequent time steps, connect to previous landmark and create new landmark
        lmIDNew = generateNodeID(fg,1);
        allLandmarkIDs = nodeIDs(fg,NodeType="POINT_XY");
        lmIDPrior = allLandmarkIDs(end);
        lmID = [lmIDPrior lmIDNew];
        lmFactorIDs = [currPoseID lmIDPrior; 
                       currPoseID lmIDNew];
    end

    % 4. After first time step, connect current pose with previous node
    if t > 1
        allPoseIDs = nodeIDs(fg,NodeType="POSE_SE2");
        prevPoseID = allPoseIDs(end);
        poseFactor = factorTwoPoseSE2([prevPoseID currPoseID],Measurement=poseSensorData{t});
        addFactor(fg,poseFactor);
    end

    % 5. Create landmark factors with sensor observation data and add to graph
    lmFactor = factorPoseSE2AndPointXY(lmFactorIDs,Measurement=lmSensorData{t});
    addFactor(fg,lmFactor);

    % 6. Set initial guess for states of the pose node and observed landmarks nodes
    nodeState(fg,lmID,lmInitStateData{t});
    nodeState(fg,currPoseID,currPose);
    
    % 7. Optimize nodes in sliding window when there are enough poses
    if t >= windowSize
        allPoseIDs = nodeIDs(fg,NodeType="POSE_SE2");
        poseIDsInWindow = allPoseIDs(end-(windowSize-1):end);
        fixNode(fg,poseIDsInWindow(1));
        optimize(fg,poseIDsInWindow);
    end
end

Obtenga todas las identificaciones de poses y todas las identificaciones de puntos de referencia. Utilice esos ID para obtener el estado optimizado de los nodos de pose y los nodos de puntos de referencia.

allPoseIDs = nodeIDs(fg,NodeType="POSE_SE2");
allLandmarkIDs = nodeIDs(fg,NodeType="POINT_XY");
optPoseStates = nodeState(fg,allPoseIDs);
optLandmarkStates = nodeState(fg,allLandmarkIDs);

Utilice la función auxiliar exampleHelperPlotPositionsAndLandmarks para trazar tanto ground-truth de las poses como los puntos de referencia.

initPoseStates = cat(1,poseInitStateData{:});
initLandmarkStates = cat(1,lmInitStateData{:});
exampleHelperPlotPositionsAndLandmarks(initPoseStates, ...
                                       initLandmarkStates)

Trace ground-truth de las poses y los puntos de referencia junto con la solución de optimización.

exampleHelperPlotPositionsAndLandmarks(initPoseStates, ...
                                       initLandmarkStates, ...
                                       optPoseStates, ...
                                       optLandmarkStates)

Tenga en cuenta que puede mejorar la precisión de la optimización aumentando el tamaño de la ventana deslizante o utilizando opciones personalizadas de solver de gráficos de factores.

Argumentos de entrada

graph — Gráfico de factores

factorGraph objeto

Gráfico de factores, especificado como un objeto factorGraph .

poseNodeIDs — ID de nodos de pose para optimizar

N vector fila de elementos de enteros no negativos

ID de los nodos de pose para optimizar 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 de pose a optimizar.

Los nodos de pose especificados por poseNodeIDs deben ser todos del tipo "POSE_SE2" o deben ser todos del tipo "POSE_SE3". Los nodos de pose especificados también deben ser únicos. Por ejemplo, poseNodeIDs no puede ser [1 2 1] porque el ID de nodo 1 no es único en este vector.

Los nodos de pose especificados en el gráfico de factores deben formar un gráfico de factores conectado. Para obtener más información, consulte Conectividad del gráfico de factores.

solverOptions — Opciones de solución para el gráfico de factores

factorGraphSolverOptions objeto

Opciones de solución para el gráfico de factores, especificadas como un objeto factorGraphSolverOptions .

Argumentos de salida

solnInfo — Resultados de la optimización

estructura

Resultados de la optimización, devueltos como una estructura que contiene:

• InitialCost — Coste inicial del problema de mínimos cuadrados no lineal formulado por el gráfico de factores antes de la optimización.

• FinalCost — Coste final del problema de mínimos cuadrados no lineal formulado por el gráfico de factores después de la optimización.

  Nota

  El coste es la suma de términos de error, conocidos como residuos, donde cada residual es una función de un subconjunto de mediciones de factores.

• NumSuccessfulSteps — Número de iteraciones en las que el solver reduce el coste. Este valor incluye la iteración de inicialización en 0 además de las iteraciones del minimizador.

• NumUnsuccessfulSteps : número de iteraciones en las que la iteración no es numéricamente válida o el solver no reduce el coste.

• TotalTime : tiempo total de optimización del solver en segundos.

• TerminationType — Tipo de terminación como un número entero en el rango [0, 2]:

  • 0 : Solver encontró una solución que cumple con el criterio de convergencia y reduce el coste después de la optimización.

  • 1 : Solver no pudo encontrar una solución que cumpla con el criterio de convergencia después de ejecutar el número máximo de iteraciones.

  • 2 : el solucionador finalizó debido a un error.

• IsSolutionUsable : la solución se puede utilizar si la solución ha convergido o optimize ha alcanzado el número máximo de iteraciones. El valor es 1 (true) si la solución es utilizable y el valor es 0 (false) si la solución no es utilizable.

• OptimizedNodeIDs : ID de nodos que se optimizaron durante la optimización.

• FixedNodeIDs : ID de nodos que se corrigieron durante la optimización en el gráfico de factores o en el gráfico de factores parciales. Tenga en cuenta que estos nodos fijos aún contribuyen a la optimización de otros nodos especificados.

Más acerca de

Conectividad del gráfico de factores

Un gráfico de factores se considera conectado si hay una ruta entre cada par de nodos. Por ejemplo, para un gráfico de factores que contiene cuatro nodos de pose, conectados consecutivamente por tres factores, hay rutas en el gráfico de factores desde un nodo del gráfico a cualquier otro nodo del gráfico.

connected = isConnected(fg,[1 2 3 4])

connected =

     1

Si el gráfico no contiene el nodo 3, aunque todavía hay una ruta del nodo 1 al nodo 2, no hay ninguna ruta del nodo 1 o del nodo 2 al nodo 4.

connected = isConnected(fg,[1 2 4])

connected =

     0

Un gráfico de factores completamente conectado es importante para la optimización. Si el gráfico de factores no está completamente conectado, entonces la optimización se produce por separado para cada uno de los gráficos desconectados, lo que puede producir resultados no deseados. La conectividad de los gráficos puede volverse más compleja cuando especifica ciertos subconjuntos de ID de nodos de pose para optimizar. Esto se debe a que la función optimize optimiza partes del gráfico de factores utilizando los ID especificados para identificar qué factores usar para crear un gráfico de factores parcial. optimize agrega un factor al gráfico de factor parcial si ese factor se conecta a cualquiera de los nodos de pose especificados y no se conecta a ningún nodo de pose no especificado. La función también agrega los nodos que no son de pose a los que se conectan los factores agregados, pero no agrega otros factores conectados a esos nodos. Por ejemplo, para este gráfico de factores hay tres nodos de pose, dos nodos que no son de pose y los factores que conectan los nodos.

Si especifica los nodos 1 y 2, entonces los factores 1, 3, 4 y 5 forman un gráfico de factores para la optimización porque se conectan para posar los nodos 1 y 2. La optimización incluye los nodos 4 y 5 porque se conectan a factores que se relacionan con los ID de nodo de pose especificados.

Si especifica poseNodeIDs como [1 3], entonces la función optimize optimiza cada gráfico separado por separado porque el gráfico de factor formado no contiene una ruta entre los nodos 1 y 3.

Sugerencias

• Antes de optimizar el gráfico de factores o un subconjunto de nodos, use la función nodeState para guardar los estados de los nodos en el espacio de trabajo. Si, después de ejecutar la optimización, desea realizar ajustes, puede restablecer los estados del nodo a los estados guardados.

• Para depurar una optimización parcial de un gráfico de factores, verifique los campos OptimizedNodeIDs y FixedNodeIDs del argumento de salida solnInfo para ver cuál de los ID de nodos optimizados y cuáles de los nodos fijos contribuyeron a la optimización.

• Para verificar si poseNodeIDs forma un gráfico de factores conectados, use la función isConnected .

Capacidades ampliadas

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

Historial de versiones

Introducido en R2022a

R2023b: Optimizar nodos especificados

La función optimize ahora admite la especificación de ID de nodo para optimizar un subconjunto de nodos a partir de un gráfico de factores. Esto es útil para aplicaciones como visual SLAM, donde debes optimizar los nodos en una ventana deslizante.

Consulte también

Objetos

• factorGraph | factorGraphSolverOptions

Temas

• SLAM de punto de referencia usando marcadores AprilTag