Create a matrix of positions of the landmarks to use for localization, and the real positions of the robot to compare your factor graph estimate against. Use the exampleHelperPlotPositionsAndLandmarks
helper function to visualize the landmark points and the real path of the robot..
Use Landmarks and Other Data as Factors
Create a factor graph, and add a prior factor to loosely fix the start pose of the robot by providing an estimate pose.
Create factorPoseSE3AndXYZ
landmark factor objects that relate the first and second pose nodes to the first landmark point, and then add the landmark factors to the factor graph. The landmark factors used here are for SE(3) state space but the process is identical for landmark factors for SE(2) state space. Add some random number to the relative position between the landmark and the ground truth position to simulate real sensor measurements.
Create landmark factors for the second and third landmark points, as well, relating them to the second and third pose nodes and third and fourth pose nodes, respectively. Use the exampleHelperAddNoiseAndAddToFactorGraph
helper function to add noise to the measurement for each landmark factor and add the factors to the factor graph. Once you have added all landmark factors to the factor graph, the IDs of the pose nodes are 1, 2, 3, and 4, and the IDs of the landmark nodes are 5, 6, and 7.
Use relative pose factors to relate consecutive poses, and add the factors to the factor graph. To simulate sensor readings for the measurements of each factor, take the difference between a consecutive pair of ground truth positions, append a quaternion of zero, and add noise.
Optimize Factor Graph
Optimize the factor graph with the default solver options. The optimization updates the states of all nodes in the factor graph, so the positions of vehicle and the landmarks update.
ans = struct with fields:
InitialCost: 71.6462
FinalCost: 0.0140
NumSuccessfulSteps: 5
NumUnsuccessfulSteps: 0
TotalTime: 0.0328
TerminationType: 0
IsSolutionUsable: 1
Visualize and Compare Results
Get and store the updated node states for the vehicle and landmarks and plot the results, comparing the factor graph estimate of the robot path to the known ground truth of the robot.
fgposopt = 4×7
-0.0000 0.0000 -0.0000 1.0000 -0.0000 0.0000 0.0000
2.0529 -2.0006 0.0528 0.9991 0.0415 0.0115 0.0053
5.0501 3.0537 0.3775 0.9980 0.0569 0.0210 0.0197
10.0939 2.3060 0.0984 0.9883 0.1423 0.0465 0.0274
fglmopt = 3×3
0.0753 -2.9889 0.0527
3.0294 4.0345 0.5962
7.1825 1.2102 0.1122