factorPoseSE2AndPointXY
Description
The factorPoseSE2AndPointXY
object contains factors that each describe the
relationship between a position in the SE(2) state space and a 2-D landmark point. You can use
this object to add one or more factors to a factorGraph
object.
Creation
Description
creates a F
= factorPoseSE2AndPointXY(nodeID
)factorPoseSE2AndPointXY
object, F
, with
the node identification numbers property NodeID
set to
nodeID
.
specifies properties using one or more name-value arguments in addition to the argument
from the previous syntax. For example, F
= factorPoseSE2AndPointXY(___,Name=Value
)factorPoseSE2AndPointXY([1
2],Measurement=[1 5])
sets the Measurement
property of
the factorPoseSE2AndPointXY
object to [1 5]
.
Properties
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_SE2
to a node of
type POINT_XY
in the form [PoseID
PointID], where PoseID is the ID of the
POSE_SE2
node and PointID is the ID of the
POINT_XY
node in the factor graph.
If a factor in the factorPoseSE2AndPointXY
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.
For more information about the expected node types of all supported factors, see Expected Node Types of Factor Objects.
Measurement
— Measured relative position
zeros(N,2)
(default) | N-by-2 matrix
Measured relative position between the current position and landmark point, specified as an N-by-2 matrix where each row is of the form [dx dy], in meters. N is the total number of factors, and dx and dy are the change in position in x and y, respectively.
Information
— Information matrix associated with uncertainty of measurements
eye(2)
(default) | 2-by-2 matrix | 2-by-2-by-N array
Information matrix associated with the uncertainty of the measurements, specified as
a 2-by-2 matrix or a 2-by-2-by-N array. N is the
total number of factors specified by the factorPoseSE2AndPointXY
object. Each
information matrix corresponds to the measurements of the corresponding node in
NodeID
.
If you specify this property as a 2-by-2 matrix when NodeID
contains more than one row, the information matrix corresponds to all measurements in
Measurement
.
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.
Object Functions
nodeType | Get node type of node in factor graph |
Examples
Estimate Poses Using Landmark Factors
Create a matrix of positions of the landmarks to use for localization, and the real poses of the robot to compare your factor graph estimate against. Use the exampleHelperPlotGroundTruth
helper function to visualize the landmark points and the real path of the robot.
gndtruth = [0 0 0; 2 0 pi/2; 2 2 pi; 0 2 pi]; landmarks = [3 0; 0 3]; exampleHelperPlotGroundTruth(gndtruth,landmarks)
Use the exampleHelperSimpleFourPoseGraph
helper function to create a factor graph contains four poses related by three 2-D two-pose factors. For more details, see the factorTwoPoseSE2
object page.
fg = exampleHelperSimpleFourPoseGraph(gndtruth);
Create Landmark Factors
Generate node IDs to create two node IDs for two landmarks. The second and third pose nodes observe the first landmark point so they should connect to that landmark with a factor. The third and fourth pose nodes observe the second landmark.
lmIDs = generateNodeID(fg,2); lmFIDs = [1 lmIDs(1); % Pose Node 1 <-> Landmark 1 2 lmIDs(1); % Pose Node 2 <-> Landmark 1 2 lmIDs(2); % Pose Node 2 <-> Landmark 2 3 lmIDs(2)]; % Pose Node 3 <-> Landmark 2
Define the relative position measurements between the position of the poses and their landmarks in the reference frame of the pose node. Then add some noise.
lmFMeasure = [0 -1; % Landmark 1 in pose node 1 reference frame -1 2; % Landmark 1 in pose node 2 reference frame 2 -1; % Landmark 2 in pose node 2 reference frame 0 -1]; % Landmark 2 in pose node 3 reference frame lmFMeasure = lmFMeasure + 0.1*rand(4,2);
Create the landmark factors with those relative measurements and add it to the factor graph.
lmFactor = factorPoseSE2AndPointXY(lmFIDs,Measurement=lmFMeasure); addFactor(fg,lmFactor);
Set the initial state of the landmark nodes to the real position of the landmarks with some noise.
nodeState(fg,lmIDs,landmarks+0.1*rand(2));
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.
rng default
optimize(fg)
ans = struct with fields:
InitialCost: 0.0538
FinalCost: 6.2053e-04
NumSuccessfulSteps: 4
NumUnsuccessfulSteps: 0
TotalTime: 1.7595e-04
TerminationType: 0
IsSolutionUsable: 1
OptimizedNodeIDs: [1 2 3 4 5]
FixedNodeIDs: 0
Visualize and Compare Results
Get and store the updated node states for the robot and landmarks. Then 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_SE2")
poseIDs = 1×4
0 1 2 3
poseStatesOpt = nodeState(fg,poseIDs)
poseStatesOpt = 4×3
0 0 0
2.0815 0.0913 1.5986
1.9509 2.1910 -3.0651
-0.0457 2.0354 -2.9792
landmarkStatesOpt = nodeState(fg,lmIDs)
landmarkStatesOpt = 2×2
3.0031 0.1844
-0.1893 2.9547
handle = show(fg,Orientation="on",OrientationFrameSize=0.5,Legend="on"); grid on; hold on; exampleHelperPlotGroundTruth(gndtruth,landmarks,handle);
More About
Expected Node Types of Factor Objects
These are the node types that the NodeID
property of each factor object specifies and connects to:
Factor Object | Expected Node Types of Specified Node IDs |
---|---|
factorGPS | ["POSE_SE3"] |
factorIMU | ["POSE_SE3","VEL3","IMU_BIAS","POSE_SE3","VEL3","IMU_BIAS"] |
factorCameraSE3AndPointXYZ | ["POSE_SE3","POINT_XYZ"] |
factorPoseSE2AndPointXY | ["POSE_SE2","POINT_XY"] |
factorPoseSE3AndPointXYZ | ["POSE_SE3","POINT_XYZ"] |
factorTwoPoseSE2 | ["POSE_SE2","POSE_SE2"] |
factorTwoPoseSE3 | ["POSE_SE3","POSE_SE3"] |
factorIMUBiasPrior | ["IMU_BIAS"] |
factorPoseSE3Prior | ["POSE_SE3"] |
factorVelocity3Prior | ["VEL_3"] |
For example, factorPoseSE2AndPointXY([1 2])
creates a 2-D landmark factor connecting to node IDs 1 and 2. If you try to add that factor to a factor graph that already contains nodes 1 and 2, the factor expects nodes 1 and 2 to be of types "POSE_SE2"
and "POINT_XY"
, respectively.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2022bR2023a: Specify multiple factors
The NodeID
, Measurement
, and
Information
properties now accept additional rows to specify
multiple factors.
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)