factorTwoPoseSE2
Descripción
El objeto factorTwoPoseSE2 contiene factores que relacionan pares de poses en el espacio de estados SE(2) para un objeto factorGraph.
Creación
Descripción
F = factorTwoPoseSE2(nodeID)factorTwoPoseSE2, F, con la propiedad de números de identificación de nodo NodeID establecida en nodeID.
F = factorTwoPoseSE2(nodeID,Name=Value)factorTwoPoseSE2([1 2],Measurement=[1 5 7]) establece la propiedad Measurement del objeto factorTwoPoseSE2 en [1 5 7].
Propiedades
Funciones del objeto
nodeType | Obtener el tipo de nodo en el gráfico de factores |
Ejemplos
Defina ground-truth para cinco poses de robots como un bucle y cree un gráfico de factores.
gndtruth = [0 0 0;
2 0 pi/2;
2 2 pi;
0 2 3*pi
0 0 0];
fg = factorGraph;Genere los ID de nodo necesarios para crear tres factores factorTwoPoseSE2. Como el nodo 4 coincidiría directamente sobre el nodo 0, en lugar de especificar un factor que conecte el nodo 3 con un nuevo nodo 4, cree un cierre de bucle agregando otro factor que relacione el nodo 3 con el nodo 0.
poseFIDs = generateNodeID(fg,3,"factorTwoPoseSE2");
poseFIDs = [poseFIDs; 3 0]poseFIDs = 4×2
0 1
1 2
2 3
3 0
Defina la medida relativa entre cada pose consecutiva y agregue un poco de ruido para que la medida se parezca más a la lectura de un sensor.
relMeasure = [2 0 pi/2; 2 0 pi/2; 2 0 pi/2; 2 0 pi/2] + 0.1*rand(4,3);
Cree los factores factorTwoPoseSE2 con las medidas relativas definidas y luego agregue los factores al gráfico de factores.
poseFactor = factorTwoPoseSE2(poseFIDs,Measurement=relMeasure); addFactor(fg,poseFactor);
Obtenga los ID de nodo de todos los nodos de pose SE2 en el gráfico de factores.
poseIDs = nodeIDs(fg,NodeType="POSE_SE2");Debido a que los nodos de tipo POSE_SE2 tienen un estado predeterminado de [0 0 0], debe proporcionar una estimación inicial del estado. Normalmente esto proviene de un sensor de odometría en el robot. Pero para este ejemplo, utilice ground-truth con algo de ruido.
predictedState = gndtruth(1:4,:); predictedState(2:4,:) = predictedState(2:4,:) + 0.1*rand(3,3);
Luego, establezca los estados de los nodos de pose en los estados estimados previstos.
nodeState(fg,poseIDs,predictedState);
Arregle el primer nodo de pose. Debido a que todos los nodos son relativos entre sí, necesitan un estado conocido para ser un ancla.
fixNode(fg,0);
Optimice el gráfico de factores y los resultados visuales
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 para que las poses del vehículo se actualicen.
rng default
optimize(fg)ans = struct with fields:
InitialCost: 6.1614
FinalCost: 0.0118
NumSuccessfulSteps: 5
NumUnsuccessfulSteps: 0
TotalTime: 1.5998e-04
TerminationType: 0
IsSolutionUsable: 1
OptimizedNodeIDs: [1 2 3]
FixedNodeIDs: 0
Obtenga y almacene los estados de nodo actualizados para el robot. Luego, trace los resultados, comparando la estimación del gráfico de factores de la trayectoria del robot con ground-truth del robot.
poseStatesOpt = nodeState(fg,poseIDs)
poseStatesOpt = 4×3
0 0 0
2.0777 0.0689 1.5881
2.0280 2.1646 -3.1137
0.0132 2.0864 -1.6014
figure plot(gndtruth(:,1),gndtruth(:,2),Marker="*",LineWidth=1.5) hold on plot([poseStatesOpt(:,1); 0],[poseStatesOpt(:,2); 0],Marker="*",LineStyle="--",LineWidth=1); legend(["Ground Truth","Opt. Position"]); s2 = se2(poseStatesOpt,"xytheta"); plotTransforms(s2,FrameSize=0.5,FrameAxisLabels="on"); axis padded hold off
Tenga en cuenta que las poses no coinciden perfectamente con la ground-truth porque no hay muchos factores en este gráfico que la función optimize pueda usar para proporcionar una solución más precisa. La precisión se puede mejorar utilizando mediciones más precisas, conjeturas precisas del estado inicial y agregando factores adicionales para agregar más información para que la use el optimizador.
