isNodeFixed
Description
Examples
Optimize Simple Factor Graph
Create a factor graph.
fg = factorGraph;
Define two approximate pose states of the robot.
rstate = [0 0 0; 1 1 pi/2];
Define the relative pose measurement between two nodes from the odometry as the pose difference between the states with some noise. The relative measurement must be in the reference frame of the second node so you must rotate the difference in position to be in the reference frame of the second node.
posediff = diff(rstate);
rotdiffso2 = so2(posediff(3),"theta");
transformedPos = transform(inv(rotdiffso2),posediff(1:2));
odomNoise = 0.1*rand;
measure = [transformedPos posediff(3)] + odomNoise;
Create a SE(2) two-pose factor with the relative measurement. Then add the factor to the factor graph to create two nodes.
ids = generateNodeID(fg,1,"factorTwoPoseSE2");
f = factorTwoPoseSE2(ids,Measurement=measure);
addFactor(fg,f);
Get the state of both pose nodes.
stateDefault = nodeState(fg,ids)
stateDefault = 2×3
0 0 0
0 0 0
Because these nodes are new, they have default state values. Ideally before optimizing, you should assign an approximate guess of the absolute pose. This increases the possibility of the optimize
function finding the global minimum. Otherwise optimize
may become trapped in the local minimum, producing a suboptimal solution.
Keep the first node state at the origin and set the second node state to an approximate xy-position at [0.9 0.95]
and a theta rotation of pi/3
radians. In practical applications you could use sensor measurements from your odometry to determine the approximate state of each pose node.
nodeState(fg,ids(2),rstate(2,:))
ans = 1×3
1.0000 1.0000 1.5708
Before optimizing, save the node state so you can reoptimize as needed.
statePriorOpt1 = nodeState(fg,ids);
Optimize the nodes and check the node states.
optimize(fg); stateOpt1 = nodeState(fg,ids)
stateOpt1 = 2×3
-0.1038 0.8725 0.1512
1.1038 0.1275 1.8035
Note that after optimization the first node did not stay at the origin because although the graph does have the initial guess for the state, the graph does not have any constraint on the absolute position. The graph has only the relative pose measurement, which acts as a constraint for the relative pose between the two nodes. So the graph attempts to reduce the cost related to the relative pose, but not the absolute pose. To provide more information to the graph, you can fix the state of nodes or add an absolute prior measurement factor.
Reset the states and then fix the first node. Then verify that the first node is fixed.
nodeState(fg,ids,statePriorOpt1); fixNode(fg,ids(1)) isNodeFixed(fg,ids(1))
ans = logical
1
Reoptimize the factor graph and get the node states.
optimize(fg)
ans = struct with fields:
InitialCost: 1.8470
FinalCost: 1.8470e-16
NumSuccessfulSteps: 2
NumUnsuccessfulSteps: 0
TotalTime: 1.4591e-04
TerminationType: 0
IsSolutionUsable: 1
OptimizedNodeIDs: 1
FixedNodeIDs: 0
stateOpt2 = nodeState(fg,ids)
stateOpt2 = 2×3
0 0 0
1.0815 -0.9185 1.6523
Note that after optimizing this time, the first node state remained at the origin.
Input Arguments
fg
— Factor graph
factorGraph
object
Factor graph, specified as a factorGraph
object.
nodeID
— Node ID of existing node
nonnegative integer
Node ID of an existing node, specified as a nonnegative integer.
Output Arguments
isFixed
— Fix status of node
true
or 1
| false
or 0
Fix status of the node, returned as a logical 1
(true
) or 0
(false
). The
function returns true
when the node is fixed, and returns
false
when the node is free.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2022a
MATLAB 命令
您点击的链接对应于以下 MATLAB 命令:
请在 MATLAB 命令行窗口中直接输入以执行命令。Web 浏览器不支持 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)