factorPoseSE3AndPointXYZ

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

Since R2022b

Description

The 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 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

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 type 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.

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,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 NodeID.

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.

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.

Object Functions

nodeType

Get node type of node in factor graph

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 R2022b

R2023a: Specify multiple factors

The NodeID, Measurement, and Information properties now accept additional rows to specify multiple factors.