addFactor
Descripción
La función addFactor
agrega uno o más factores a un gráfico de factores y puede agrupar los factores y nodos especificados por los factores agregados.
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.
Agregar nodos a grupos
Cree un gráfico de factores, genere ID de nodo y cree dos factores factorTwoPoseSE2
.
fg1 = factorGraph; ids = generateNodeID(fg1,[2 2])
ids = 2×2
0 1
2 3
f = factorTwoPoseSE2(ids);
Agrupar todos los nodos
Suma todos los nodos de los factores al grupo 1.
addFactor(fg1,f,1); fg1Group1 = nodeIDs(fg1,GroupID=1)
fg1Group1 = 1×4
0 1 2 3
Agrupar nodos por columna
Especifique el ID del grupo como un vector fila para agregar los nodos de la primera columna de los ID de nodo de los factores al grupo 1 y agregue los nodos de la segunda columna al grupo 2.
fg2 = factorGraph; addFactor(fg2,f,[1 2]); fg2Group1 = nodeIDs(fg2,GroupID=1)
fg2Group1 = 1×2
0 2
fg2Group2 = nodeIDs(fg2,GroupID=2)
fg2Group2 = 1×2
1 3
Agrupar nodos por fila
Especifique el ID de grupo como vector columna para agregar los nodos de la primera fila en los ID de nodo de los factores al grupo 1 y agregue los nodos de la segunda fila al grupo 2.
fg3 = factorGraph; addFactor(fg3,f,[1; 2]); fg3Group1 = nodeIDs(fg3,GroupID=1)
fg3Group1 = 1×2
0 1
fg3Group2 = nodeIDs(fg3,GroupID=2)
fg3Group2 = 1×2
2 3
Agrupar nodos por matriz
También puede especificar el ID del grupo como una matriz del mismo tamaño que los ID de los nodos de los factores para asignar cada nodo a un grupo específico. Agregue el primer y cuarto nodo al grupo 1 y el segundo y tercer nodo a los grupos 2 y 3, respectivamente.
fg4 = factorGraph; groups = [1 2; 3 1]; addFactor(fg4,f,groups); fg4Group1 = nodeIDs(fg4,GroupID=1)
fg4Group1 = 1×2
0 3
fg4Group2 = nodeIDs(fg4,GroupID=2)
fg4Group2 = 1
fg4Group3 = nodeIDs(fg4,GroupID=3)
fg4Group3 = 2
Argumentos de entrada
graph
— Gráfico de factores para agregar factores
factorGraph
objeto
Gráfico de factores al que agregar factores, especificado como un objeto factorGraph
.
factor
— Factores para agregar al gráfico de factores
objeto de factor válido
Factores para agregar al gráfico de factores, especificados como un objeto de factor válido.
Un objeto de factor válido debe ser uno de estos objetos y el objeto no debe crear ningún nodo no válido cuando se agrega al gráfico de factores:
Con la excepción de factorGPS
y factorIMU
, puede agregar simultáneamente varios factores al gráfico de factores utilizando cualquiera de los objetos de factores enumerados. factorGPS
y factorIMU
solo admiten la suma de un solo factor.
Si el objeto de factor especificado crea nodos no válidos, entonces addFactor
no agrega ninguno de los factores del objeto de factor.
groupID
— ID de grupo para nodos de factor agregado
entero no negativo | vector fila de dos elementos de números enteros no negativos | N-elemento vector columna de enteros no negativos | N-por-2 matriz de enteros no negativos
ID de grupo para nodos del factor agregado, especificados como cualquiera de estas opciones:
groupID Tamaño | Comportamiento de agrupación |
---|---|
entero no negativo | Asigna todos los nodos a un grupo. Por ejemplo, si agrega un objeto factor que tiene un valor |
vector fila de dos elementos de números enteros no negativos | Especifique grupos para cada columna de nodos. Por ejemplo, si agrega un objeto factor que tiene un valor El comportamiento de los factores IMU es diferente. Si agrega un factor IMU con un valor |
N-elemento vector columna de enteros no negativos | Agrupe los nodos por factor, donde N es el número total de factores especificados por la propiedad Por ejemplo, si agrega un objeto de factor que tiene un valor |
N-por-2 matriz de enteros no negativos | Agregue nodos en Por ejemplo, si agrega un objeto factor que tiene un valor |
Nota
Al agregar un objeto factorIMU
o factorGPS
a un gráfico de factores, groupID
acepta solo estos valores:
factorIMU
: entero no negativo o vector fila de dos elementos de enteros no negativos.factorGPS
— Entero no negativo
Agregar nodos a grupos le permite consultar los ID de nodo por grupo especificando el argumento nombre-valor GroupID
de la función nodeIDs
.
Argumentos de salida
factorIDs
— ID de factores de factores agregados
N vector fila de elementos de enteros no negativos
ID de factor de los factores agregados, devueltos como un vector fila de elemento N de enteros no negativos. N es el número total de factores agregados.
La función devuelve este argumento solo cuando agrega correctamente los factores al gráfico de factores. Si la suma de los factores da como resultado un nodo no válido, entonces addFactor
no agrega ninguno de los factores del objeto factor.
Capacidades ampliadas
Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.
Historial de versiones
Introducido en R2022aR2023a: Especificar ID de grupo para factores agregados
addFactor
ahora admite la especificación de grupos para agregar nodos de factores agregados por ID de grupo.
Consulte también
Objetos
factorGraph
|factorTwoPoseSE2
|factorTwoPoseSE3
|factorPoseSE3Prior
|factorVelocity3Prior
|factorIMUBiasPrior
|factorGPS
|factorIMU
Funciones
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- 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)