optimize
Sintaxis
Descripción
La función optimize optimiza un gráfico de factores para encontrar una solución que minimice el costo del problema de mínimos cuadrados no lineales formulado por el gráfico de factores. La optimización del gráfico factorial utiliza el solucionador Ceres para la estimación de la covarianza del estado de los nodos, un proceso que implica mayores costos de cálculo y tiempos de estimación más largos a medida que aumenta el número de nodos. Para obtener más información sobre la estimación de covarianza de Ceres-Solver, consulte http://ceres-solver.org/nnls_covariance.html.
Para estimar la covarianza del estado del nodo, optimice con opciones de solucionador personalizadas utilizando el objeto factorGraphSolverOptions que tiene la propiedad StateCovarianceType establecida en uno o más tipos de nodo. Al optimizar gráficos factoriales que requieren estimar la covarianza del estado para una gran cantidad de nodos, considere usar la optimización de ventana deslizante para aumentar la velocidad de optimización al estimar la covarianza del estado para menos nodos a la vez. Para obtener más información sobre la optimización de ventanas deslizantes, consulte Optimice incrementalmente el gráfico de factores mediante la ventana deslizante.
solnInfo = optimize(fg,poseNodeIDs)
Nota
Al menos uno de los nodos de pose especificados debe cumplir uno o ambos de estos requisitos:
Tenga un estado de pose fijo utilizando la función
fixNode.Relacionarse con uno o más factores que proporcionan información absoluta sobre el estado:
solnInfo = optimize(___,solverOptions)optimize(fg,factorGraphSolverOptions(StateCovarianceType="POSE_SE2")) especifica que optimize debe estimar y almacenar la covarianza del estado del nodo para los nodos de pose SE(2) en el gráfico de factores durante la optimización.
Ejemplos
Cree una matriz de posiciones de los puntos de referencia para usar en la localización y las poses reales del robot para comparar la estimación del gráfico de factores. Utilice la función auxiliar exampleHelperPlotGroundTruth para visualizar los puntos de referencia y la trayectoria real del robot.
gndtruth = [0 0 0;
2 0 pi/2;
2 2 pi;
0 2 pi];
landmarks = [3 0; 0 3];
exampleHelperPlotGroundTruth(gndtruth,landmarks)
Utilice la función auxiliar exampleHelperSimpleFourPoseGraph para crear un gráfico de factores que contenga cuatro poses relacionadas por tres factores de dos poses en 2D. Para obtener más detalles, consulte la página del objeto factorTwoPoseSE2.
fg = exampleHelperSimpleFourPoseGraph(gndtruth);
Crear factores punto de referencia
Genere ID de nodo para crear dos ID de nodo para dos puntos de referencia. Los nodos de pose segunda y tercera observan el primer punto de referencia, por lo que deben conectarse a ese punto de referencia con un factor. Los nodos de pose tercero y cuarto observan el segundo punto de referencia.
lmIDs = generateNodeID(fg,2); lmFIDs = [1 lmIDs(1); % Pose Node 1 <-> Landmark 1 2 lmIDs(1); % Pose Node 2 <-> Landmark 1 2 lmIDs(2); % Pose Node 2 <-> Landmark 2 3 lmIDs(2)]; % Pose Node 3 <-> Landmark 2
Defina las medidas de posición relativa entre la posición de las poses y sus puntos de referencia en el marco de referencia del nodo de pose. Luego agrega algo de ruido.
lmFMeasure = [0 -1; % Landmark 1 in pose node 1 reference frame -1 2; % Landmark 1 in pose node 2 reference frame 2 -1; % Landmark 2 in pose node 2 reference frame 0 -1]; % Landmark 2 in pose node 3 reference frame lmFMeasure = lmFMeasure + 0.1*rand(4,2);
Cree los factores de referencia con esas medidas relativas y agréguelos al gráfico de factores.
lmFactor = factorPoseSE2AndPointXY(lmFIDs,Measurement=lmFMeasure); addFactor(fg,lmFactor);
Establezca el estado inicial de los nodos de puntos de referencia en la posición real de los puntos de referencia con algo de ruido.
nodeState(fg,lmIDs,landmarks+0.1*rand(2));
Optimizar gráfico de factores
Optimice el gráfico de factores con las opciones de solver predeterminadas. La optimización actualiza los estados de todos los nodos en el gráfico de factores, por lo que se actualizan las posiciones del vehículo y los puntos de referencia.
rng default
optimize(fg)ans = struct with fields:
InitialCost: 0.0538
FinalCost: 6.2053e-04
NumSuccessfulSteps: 4
NumUnsuccessfulSteps: 0
TotalTime: 1.9908e-04
TerminationType: 0
IsSolutionUsable: 1
OptimizedNodeIDs: [1 2 3 4 5]
FixedNodeIDs: 0
Visualice y compare resultados
Obtenga y almacene los estados de nodo actualizados para el robot y los puntos de referencia. Luego, trace los resultados, comparando la estimación del gráfico de factores de la trayectoria del robot con ground-truth del robot.
poseIDs = nodeIDs(fg,NodeType="POSE_SE2")poseIDs = 1×4
0 1 2 3
poseStatesOpt = nodeState(fg,poseIDs)
poseStatesOpt = 4×3
0 0 0
2.0815 0.0913 1.5986
1.9509 2.1910 -3.0651
-0.0457 2.0354 -2.9792
landmarkStatesOpt = nodeState(fg,lmIDs)
landmarkStatesOpt = 2×2
3.0031 0.1844
-0.1893 2.9547
handle = show(fg,Orientation="on",OrientationFrameSize=0.5,Legend="on"); grid on; hold on; exampleHelperPlotGroundTruth(gndtruth,landmarks,handle);
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 MAT sensorData. Este archivo MAT contiene datos del sensor para diez pasos de tiempo.
fg = factorGraph;
load sensorData.matPara estimar y almacenar también información de covarianza durante la optimización de la ventana deslizante, cree opciones de solucionador personalizadas que especifiquen que se debe estimar la covarianza de estado para los nodos de pose SE(2).
options = factorGraphSolverOptions(StateCovarianceType="POSE_SE2")options =
factorGraphSolverOptions with properties:
MaxIterations: 200
FunctionTolerance: 1.0000e-06
GradientTolerance: 1.0000e-10
StepTolerance: 1.0000e-08
VerbosityLevel: 0
TrustRegionStrategyType: 1
StateCovarianceType: Pose_SE2
InitialTrustRegionRadius: 10000
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:
Genere una nueva ID de nodo para el nodo de pose del paso de tiempo actual.
Obtenga los datos de la pose actual para el paso de tiempo actual.
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.
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.
Agregue factores de punto de referencia que creen los ID de nodo punto de referencia especificados al gráfico.
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.
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,options); end end
Obtenga todas las identificaciones de poses y todas las identificaciones de puntos de referencia. Utilice esos identificadores para obtener el estado optimizado de los nodos de pose y de los nodos de puntos de referencia, y obtener la covarianza del estado de los nodos de pose.
allPoseIDs = nodeIDs(fg,NodeType="POSE_SE2"); allLandmarkIDs = nodeIDs(fg,NodeType="POINT_XY"); optPoseStates = nodeState(fg,allPoseIDs); optLandmarkStates = nodeState(fg,allLandmarkIDs); poseCov = nodeCovariance(fg,allPoseIDs);
Utilice la función auxiliar exampleHelperPlotPositionsAndLandmarks para trazar la ground-truth de las poses y 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
Argumentos de salida
Más acerca de
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 al usar los ID especificados para identificar qué factores usar para crear un gráfico de factores parciales. optimize agrega un factor al gráfico de factores parciales 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, utilice la función
nodeStatepara 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.Si su optimización no tuvo éxito o dio como resultado estados de nodo inexactos, considere reevaluar los estados de estimación iniciales de los nodos y refinarlos según sea necesario utilizando la función
nodeState.Para depurar una optimización parcial de un gráfico de factores, verifique los campos
OptimizedNodeIDsyFixedNodeIDsdel argumento de salidasolnInfopara ver cuáles de los ID de nodo optimizados y cuáles de los nodos fijos contribuyeron a la optimización.Para comprobar si
poseNodeIDsforma un gráfico factorial conectado, utilice la funciónisConnected.
