factorTwoPoseSE2
Description
The factorTwoPoseSE2 object contains factors that relate pairs of
poses in the SE(2) state space for a factorGraph
object.
Creation
Description
F = factorTwoPoseSE2(nodeID)factorTwoPoseSE2 object, F, with the
node identification numbers property NodeID set to
nodeID.
F = factorTwoPoseSE2(nodeID,Name=Value)factorTwoPoseSE2([1 2],Measurement=[1 5 7]) sets the
Measurement property of the factorTwoPoseSE2
object to [1 5 7].
Properties
Object Functions
nodeType | Get node type of node in factor graph |
Examples
Define the ground truth for five robot poses as a loop and create a factor graph.
gndtruth = [0 0 0;
2 0 pi/2;
2 2 pi;
0 2 3*pi
0 0 0];
fg = factorGraph;Generate the node IDs needed to create three factorTwoPoseSE2 factors. Because node 4 would coincide directly on top of the node 0, instead of specifying a factor that connects node 3 to a new node 4, create a loop closure by adding another factor that relates node 3 to node 0.
poseFIDs = generateNodeID(fg,3,"factorTwoPoseSE2");
poseFIDs = [poseFIDs; 3 0]poseFIDs = 4×2
0 1
1 2
2 3
3 0
Define the relative measurement between each consecutive pose and add a little noise so the measurement is more like a sensor reading.
relMeasure = [2 0 pi/2; 2 0 pi/2; 2 0 pi/2; 2 0 pi/2] + 0.1*rand(4,3);
Create the factorTwoPoseSE2 factors with the defined relative measurements and then add the factors to the factor graph.
poseFactor = factorTwoPoseSE2(poseFIDs,Measurement=relMeasure); addFactor(fg,poseFactor);
Get the node IDs of all of the SE2 pose nodes in the factor graph.
poseIDs = nodeIDs(fg,NodeType="POSE_SE2");Because the POSE_SE2 type nodes have a default state of [0 0 0], you should provide an initial guess for the state. Normally this is from an odometry sensor on the robot. But for this example, use the ground truth with some noise.
predictedState = gndtruth(1:4,:); predictedState(2:4,:) = predictedState(2:4,:) + 0.1*rand(3,3);
Then set the states of the pose nodes to the predicted guess states.
nodeState(fg,poseIDs,predictedState);
Fix the first pose node. Because the nodes are all relative to each other, they need a known state to be an anchor.
fixNode(fg,0);
Optimize Factor Graph and Visual Results
Optimize the factor graph with the default solver options. The optimization updates the states of all nodes in the factor graph so the poses of vehicle update.
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
Get and store the updated node states for the robot. Then plot the results, comparing the factor graph estimate of the robot path to the known ground truth of the 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
Note that the poses do not match perfectly with the ground truth because there are not many factors in this graph that the optimize function can use to provide a more accurate solution. The accuracy can be improved by using more accurate measurements, accurate initial state guesses, and adding additional factors to add more information for the optimizer to use.
