# factorTwoPoseSE2

## Description

The `factorTwoPoseSE2`

object contains factors that relate pairs of
poses in the SE(2) state space for a `factorGraph`

object.

## Creation

### Description

creates a `F`

= factorTwoPoseSE2(`nodeID`

)`factorTwoPoseSE2`

object, `F`

, with the
node identification numbers property `NodeID`

set to
`nodeID`

.

specifies properties using one or more name-value arguments. For example,
`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

`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 that connects to two nodes of type `POSE_SE2`

at
the specified node IDs in the factor graph. The rows are of the form
[*PoseID1*
*PoseID2*].

If a factor in the `factorTwoPoseSE2`

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.

For more information about the expected node types of all supported factors, see Expected Node Types of Factor Objects.

`Measurement`

— Measured relative pose

`zeros(`*N*,3)

(default) | *N*-by-3 matrix

*N*,3)

Measured relative pose, specified as a *N*-by-3 matrix, where each
row is of the form [*dx*
*dy*
*dtheta*]. *N* is the total number of factors.
*dx* and *dy* are the change in position in
*x* and *y*, respectively, and
*dtheta* is the angle between the two positions.

`Information`

— Information matrix associated with measurements

`eye(3)`

(default) | 3-by-3 matrix | 3-by-3-*N* array

Information matrix associated with the measurement, specified as a 3-by-3 matrix or
a 3-by-3-*N* matrix. *N* is the total number of
factors specified by this `factorTwoPoseSE2`

object. Each information matrix
corresponds to the measurements of the specified nodes in
`NodeIDs`

.

If you specify this property as a 3-by-3 matrix when `NodeID`

contains more than one row, the information matrix corresponds to all measurements in
`Measurement`

.

## Object Functions

`nodeType` | Get node type of node in factor graph |

## Examples

### Estimate Poses Using 2-D Pose Factors

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 and then manually add the 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: 0.0216
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.

## 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 R2022a**

### R2023a: Specify multiple factors

The `NodeID`

, `Measurement`

, and
`Information`

properties now accept additional rows to specify
multiple factors.

## MATLAB Command

You clicked a link that corresponds to this MATLAB command:

Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.

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)