Factor relating SE(3) position and 3-D point
factorPoseSE3AndPointXYZ object contains factors that each describe the
relationship between a position in the SE(3) state space and a 3-D landmark point. You can use
this object to add one or more factors to a
F = factorPoseSE3AndPointXYZ(
the node identification numbers property,
NodeID, set to
specifies properties using one or more name-value arguments in addition to the argument
from the previous syntax. For example,
F = factorPoseSE3AndPointXYZ(___,
2],Measurement=[1 2 3]) sets the
factorPoseSE3AndPointXYZ object to
NodeID — Node ID numbers
N-by-2 matrix of nonnegative integers
This property is read-only.
Node ID numbers, specified as an N-by-2 matrix of nonnegative
integers, where N is the total number of desired factors. Each row
represents a factor connecting a node of type,
POSE_SE3 to a node of
POINT_XYZ in the form [PoseID
PointID], where PoseID is the ID of the
POSE_SE3 node and PointID is the ID of the
POINT_XYZ node in the factor graph.
If a factor in the
factorPoseSE3AndPointXYZ object specifies an ID that does not
correspond to a node in the factor graph, the factor graph automatically creates a node
of the required type with that ID and adds it to the factor graph when adding the factor
to the factor graph.
You must specify this property at object creation.
Measurement — Measured relative position
zeros(N,3) (default) | N-by-3 matrix
Measured relative position between current position and landmark point, specified as an N-by-3 matrix where each row is of the form [dx dy dz], in meters. N is the total number of factors, and dx, dy, and dz are the change in position in x, y, and z, respectively.
Information — Information matrix associated with uncertainty of measurements
eye(3) (default) | 3-by-3 matrix | 3-by-3-by-N array
Information matrix associated with the uncertainty of the measurements, specified as
a 3-by-3 matrix or a 3-by-3-by-N array. N is the
total number of factors specified by the
factorPoseSE3AndPointXYZ object. Each
information matrix corresponds to the measurements of the corresponding node in
If you specify this property as a 3-by-3 matrix when
contains more than one row, the information matrix corresponds to all measurements in
This information matrix is the inverse of the covariance matrix, where the covariance matrix is of the form:
Each element indicates the covariance between two variables. For example, σ(x,y) is the covariance between x and y.
|Get node type of node in factor graph|
Estimate Position Using Landmark Factors
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..
landmarks = [0 -3 0; 3 4 0; 7 1 0]; realpos = [0 0 0; 2 -2 0; 5 3 0; 10 2 0]; exampleHelperPlotPositionsAndLandmarks(realpos,landmarks)
Create Robot Pose Nodes
Create a factor graph, and add a prior factor to loosely fix the start pose of the robot by providing an estimate pose.
fg = factorGraph; rng(1) pf = factorPoseSE3Prior(0);
Generate node IDs to use to create three
factorTwoPoseSE3 relative pose factors that relate four robot poses. To simulate sensor readings for the measurements of each factor, take the difference between a consecutive pair of ground truth positions, add noise, and append a quaternion of zero to provide a rotation of zero. Then add the prior factor and the pose factors to the factor graph.
zeroQuat = [1 0 0 0]; rpfIDs = generateNodeID(fg,3,"factorTwoPoseSE3")
rpfIDs = 3×2 0 1 1 2 2 3
rpfmeasure = [(diff(realpos) + 0.1*rand(3)) repmat(zeroQuat,3,1)]; rpf = factorTwoPoseSE3(rpfIDs,Measurement=rpfmeasure); addFactor(fg,pf); addFactor(fg,rpf);
Create Landmark Factors
Generate node IDs to create three
factorPoseSE3AndXYZ landmark factor objects that relate to the pose nodes. The first and second pose nodes observe the first landmark point so they should connect to that landmark with a factor. The second and third pose nodes observe the second landmark. The third and fourth pose nodes observe the third landmark.
landmarkIDs = generateNodeID(fg,3)'
landmarkIDs = 3×1 4 5 6
The landmark factors used here are for 3-D state space but the process is identical for landmark factors for 2-D state space. Add some random number to the relative position between the landmark and the ground truth position to simulate real sensor measurements. Then create the landmark factors and add them to the factor graph.
lmf1measure = [landmarks(1,:) - realpos(1:2,:)] + 0.5*rand(1,3); lmf2measure = [landmarks(2,:) - realpos(2:3,:)] + 0.5*rand(1,3); lmf3measure = [landmarks(3,:) - realpos(3:4,:)] + 0.5*rand(1,3); lmf1 = factorPoseSE3AndPointXYZ([[0 1]' repmat(landmarkIDs(1),2,1)],Measurement=lmf1measure); lmf2 = factorPoseSE3AndPointXYZ([[1 2]' repmat(landmarkIDs(2),2,1)],Measurement=lmf2measure); lmf3 = factorPoseSE3AndPointXYZ([[2 3]' repmat(landmarkIDs(3),2,1)],Measurement=lmf3measure); addFactor(fg,lmf1); addFactor(fg,lmf2); addFactor(fg,lmf3);
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.
fgso = factorGraphSolverOptions; optimize(fg,fgso)
ans = struct with fields: InitialCost: 72.6129 FinalCost: 0.0011 NumSuccessfulSteps: 4 NumUnsuccessfulSteps: 0 TotalTime: 4.2391e-04 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.
poseIDs = nodeIDs(fg,NodeType="POSE_SE3"); fgposopt = nodeState(fg,poseIDs)
fgposopt = 4×7 0.0000 0.0000 0.0000 1.0000 0.0000 -0.0000 0.0000 2.0278 -1.9778 0.0173 1.0000 0.0018 -0.0034 0.0014 5.0684 3.0500 0.0871 0.9999 -0.0010 -0.0072 0.0089 10.0844 2.1475 0.1972 0.9999 0.0006 -0.0121 0.0100
fglmopt = nodeState(fg,landmarkIDs); exampleHelperPlotPositionsAndLandmarks(realpos,landmarks,fgposopt,fglmopt)
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version HistoryIntroduced in R2022b
R2023a: Specify multiple factors
Information properties now accept additional rows to specify