fixNode
Descripción
Ejemplos
Crea una gráfica de factores.
fg = factorGraph;
Define dos estados de pose del robot como la ground-truth.
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;
Crea un factor que conecte dos poses SE(2) con la medida relativa entre las poses. 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
podría quedar atrapado en el mínimo local, produciendo una solución subóptima.
Mantenga el primer estado del nodo en el origen y establezca el segundo estado del 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),[0.9 0.95 pi/3])
ans = 1×3
0.9000 0.9500 1.0472
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.1161 0.9026 0.0571
1.0161 0.0474 1.7094
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.9452
FinalCost: 1.9452e-16
NumSuccessfulSteps: 2
NumUnsuccessfulSteps: 0
TotalTime: 7.3910e-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
Gráfico factorial que contiene nodos a reparar, especificado como un objeto factorGraph
.
Identificadores de los nodos que se repararán o liberarán dentro del gráfico factorial, especificados como un vector fila de elementos N de números enteros no negativos. N es el número total de nodos que se repararán o liberarán.
Nuevo estado de corrección del nodo, especificado como un 1
(true
) o 0
(false
) lógico. 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
Al generar código C portable con un compilador C++, debe especificar el empaquetado jerárquico con encabezados no mínimos. Para obtener más información sobre las opciones de embalaje, consulte la función packNGo
(MATLAB Coder).
Historial de versiones
Introducido en R2022afixNode
ahora admite la reparación o liberación de más de un nodo a la vez especificando nodeIDs
como un vector de identificaciones de nodo.
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Seleccione un país/idioma
Seleccione un país/idioma para obtener contenido traducido, si está disponible, y ver eventos y ofertas de productos y servicios locales. Según su ubicación geográfica, recomendamos que seleccione: .
También puede seleccionar uno de estos países/idiomas:
Cómo obtener el mejor rendimiento
Seleccione China (en idioma chino o inglés) para obtener el mejor rendimiento. Los sitios web de otros países no están optimizados para ser accedidos desde su ubicación geográfica.
América
- América Latina (Español)
- Canada (English)
- United States (English)
Europa
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)