factorPoseSE2AndPointXY
Descripción
El objeto factorPoseSE2AndPointXY contiene factores que describen la relación entre una posición en el espacio de estados SE(2) y un punto de referencia 2-D. Puede utilizar este objeto para agregar uno o más factores a un objeto factorGraph.
Creación
Descripción
F = factorPoseSE2AndPointXY(nodeID)factorPoseSE2AndPointXY, F, con la propiedad de números de identificación de nodo NodeID establecida en nodeID.
F = factorPoseSE2AndPointXY(___,Name=Value)factorPoseSE2AndPointXY([1 2],Measurement=[1 5]) establece la propiedad Measurement del objeto factorPoseSE2AndPointXY en [1 5].
Propiedades
Funciones del objeto
nodeType | Obtener el tipo de nodo en el gráfico de factores |
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);
