factorPoseSE3AndPointXYZ

Factor relating SE(3) position and 3-D point

Description

The factorPoseSE3AndPointXYZ object describes the relationship between a position in the SE(3) state space and a 3-D landmark point. You can create this object as a factor to a factorGraph object.

Creation

Syntax

F = factorPoseSE3AndPointXYZ(nodeID)

F = factorPoseSE3AndPointXYZ(___,Name=Value)

Description

F = factorPoseSE3AndPointXYZ(nodeID) creates a factorPoseSE3AndPointXYZ object, F, with the node identification numbers property, NodeID, set to nodeID.

F = factorPoseSE3AndPointXYZ(___,Name=Value) specifies properties using one or more name-value arguments in addition to the argument from the previous syntax. For example, factorPoseSE3AndPointXYZ([1 2],Measurement=[1 2 3]) sets the Measurement property of the factorPoseSE3AndPointXYZ object to [1 2 3].

Properties

NodeID — Node ID numbers

two-element row vector of integers

This property is read-only.

Node ID numbers, specified as a two-element row vector of integers in the form [PoseNodeID XYZPointNodeID]. PoseNodeID must be a node of type "POSE_SE3" , and XYZPointNodeID must be a node of type "POINT_XYZ".

If the factor graph does not contain a node with a specified ID, then, when you add the factor to the factorGraph object, the addFactor function creates the specified node and adds it to the factorGraph.

You must specify this property at object creation.

Data Types: int

Measurement — Measured relative position

[0,0,0] (default) | three-element row vector

Measured relative position between current position and landmark point, specified as a three-element row vector of the form [dx dy dz], in meters. dx, dy, and dz are the change in position in x, y, and z respectively.

Information — Information matrix associated with measurement

eye(3) (default) | 3-by-3 matrix

Information matrix associated with the uncertainty of the measurement, specified as a 3-by-3 matrix.

This information matrix is the inverse of the covariance matrix, where the covariance matrix is of the form:

$$\left[\begin{array}{ccc}\sigma (x,x)& \sigma (x,y)& \sigma (x,z)\\ \sigma (y,x)& \sigma (y,y)& \sigma (y,z)\\ \sigma (z,x)& \sigma (y,x)& \sigma (z,z)\end{array}\right]$$

Each element indicates the covariance between two variables. For example, σ(x,y) is the covariance between x and y.

Data Types: double

Object Functions

nodeType

Get node type of node in factor graph

Examples

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)

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.

fg = factorGraph;
rng(2)
pf = factorPoseSE3Prior(1,Measurement=[0 0 0 1 0 0 0]);
addFactor(fg,pf);

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.

% Landmark 1 Factors
measurementlmf1 = landmarks(1,:) - realpos(1,:) + 0.1*rand(1,3);
measurementlmf2 = landmarks(1,:) - realpos(2,:) + 0.1*rand(1,3);
lmf1 = factorPoseSE3AndPointXYZ([1 5],Measurement=measurementlmf1);
lmf2 = factorPoseSE3AndPointXYZ([2 5],Measurement=measurementlmf2);
addFactor(fg,lmf1);
addFactor(fg,lmf2);

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.

% Landmark 2 Factors
lmf3 = factorPoseSE3AndPointXYZ([2 6],Measurement=landmarks(2,:)-realpos(2,:));
lmf4 = factorPoseSE3AndPointXYZ([3 6],Measurement=landmarks(2,:)-realpos(3,:));

% Landmark 3 Factors
lmf5 = factorPoseSE3AndPointXYZ([3 7],Measurement=landmarks(3,:)-realpos(3,:));
lmf6 = factorPoseSE3AndPointXYZ([4 7],Measurement=landmarks(3,:)-realpos(4,:));

exampleHelperAddNoiseAndAddToFactorGraph(fg,[lmf3 lmf4 lmf5 lmf6])

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.

zeroQuat = [1 0 0 0];
rp1 = factorTwoPoseSE3([1 2],Measurement=[realpos(2,:)-realpos(1,:) zeroQuat]);
rp2 = factorTwoPoseSE3([2 3],Measurement=[realpos(3,:)-realpos(2,:) zeroQuat]);
rp3 = factorTwoPoseSE3([3 4],Measurement=[realpos(4,:)-realpos(3,:) zeroQuat]);
exampleHelperAddNoiseAndAddToFactorGraph(fg,[rp1 rp2 rp3])

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: 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 = [fg.nodeState(1); fg.nodeState(2); fg.nodeState(3); fg.nodeState(4)]

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 = [fg.nodeState(5); fg.nodeState(6); fg.nodeState(7)]

fglmopt = 3×3

    0.0753   -2.9889    0.0527
    3.0294    4.0345    0.5962
    7.1825    1.2102    0.1122

exampleHelperPlotPositionsAndLandmarks(realpos,landmarks,fgposopt,fglmopt)

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022b