factorTwoPoseSE2

Factor relating two SE(2) poses

Since R2022a

Description

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

Creation

Syntax

F = factorTwoPoseSE2(nodeID)

F = factorTwoPoseSE2(nodeID,Name=Value)

Description

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

F = factorTwoPoseSE2(nodeID,Name=Value) specifies properties using one or more name-value arguments. For example, 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 [PoseID PoseID].

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.

You must specify this property at object creation.

Measurement — Measured relative pose

zeros(N,3) (default) | N-by-3 matrix

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

Add FactorTwoPoseSE2 Object to Factor Graph

Create measurement vector, information matrix, and a node ID number vector. Create a factorTwoPoseSE2 object, specifying the node IDs, measurement, and information as arguments.

nodeID = [1 2];
measure = [5 5 pi/2];
info = rand(3,3);
f = factorTwoPoseSE2(nodeID,Measurement=measure,Information=info);

Create a default factor graph and add the factor to the graph using the addFactor function.

g = factorGraph;
addFactor(g,f);

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.