factorTransformSE3Prior

Full-state prior factor for SE(3) transform

Since R2026a

Description

The factorTransformSE3Prior object contains one or more full-state prior factors for SE(3) state space transforms.

You can link a prior factor to a sensor transform node ("TRANSFORM_SE3") to softly fix its rotation, translation, or both to a known prior value. Using a prior factor differs from fixNode because the factor allows the optimizer to make minor adjustments to the sensor transform, preventing significant deviations during optimization. The resulting soft constraint is useful for avoiding local minima, especially when optimizing with noisy calibration data or in online calibration scenarios where sensor motion is insufficient.

Creation

Syntax

F = factorTransformSE3Prior(nodeID)

F = factorTransformSE3Prior(nodeID,Name=Value)

Description

F = factorTransformSE3Prior(nodeID) creates full-state prior factors to connect to the SE(3) state space transform nodes with the specified node identification numbers nodeID in a factor graph.

F = factorTransformSE3Prior(nodeID,Name=Value) sets writable properties using one or more name-value arguments. For example, Measurement=[0.1 0.2 0.3 eul2quat([0.3 0.1 0.22])] sets the Measurement property of the factorTransformSE3Prior object to [0.1 0.2 0.3 eul2quat([0.3 0.1 0.22])].

Input Arguments

nodeID — Node identification numbers

N-element column vector of nonnegative integers

Node identification numbers, specified as an N-element column vector of nonnegative integers, where N is the total number of factors.

Each element of nodeID specifies the ID in the factor graph of the TRANSFORM_SE3 node connected to the factor specified by the corresponding row of Measurement.

When you use the addFactor function to add a factorTransformSE3Prior object to a factor graph:

• If a node ID specified to nodeID corresponds to a node in the factor graph, that node must be of the TRANSFORM_SE3 type. If that node is of a different type, the addFactor function returns an error.

• If a node ID specified to nodeID does not correspond to any node in the factor graph, the addFactor function adds a TRANSFORM_SE3 node with the specified node ID to the factor graph. The node is initialized with identity transformation value by default. After you add the factorTransformSE3Prior object to a factor graph, you must initialize the TRANSFORM_SE3 nodes connected to the prior factors in the factorTransformSE3Prior object with proper values to avoid local minima.

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

This argument sets the NodeID property.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Example: factorTransformSE3Prior(1,Measurement=[0.1 0.2 0.3 eul2quat([0.3 0.1 0.22])]]) specifies the measured absolute SE(3) prior transform of a node as [0.1 0.2 0.3 eul2quat([0.3 0.1 0.22])]].

Measurement — Measured absolute SE(3) prior transform

N-by-7 matrix

Measured absolute SE(3) prior transform, specified as an N-by-7 matrix, where each row is of the form [dx dy dz dqw dqx dqy dqz]. N is the total number of factors. dx, dy, and dz represent the translation from the base sensor along the x-, y-, and z-axes, respectively. dqw, dqx, dqy, and dqz represent the rotation (change in orientation) from the base sensor as a quaternion in w, x, y, and z parts, respectively.

By default, Measurement is an N-by-7 matrix in which the value of each row is [0 0 0 1 0 0 0].

Each measurement provides the known prior value to softly fix the transformation of the corresponding node specified by nodeID during optimization.

When you optimize the factor graph that this factor belongs to, the optimize function normalizes the quaternion measurement of the factor before the optimization.

This argument sets the Measurement property as a double matrix.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Information — Information matrices representing uncertainty of measurements

eye(6) (default) | 6-by-6 matrix | 6-by-6-by-N array

Information matrices representing the uncertainty of measurements, specified as a 6-by-6 matrix or a 6-by-6-by-N array. N is the total number of factors. Each information matrix corresponds to the measurement of the respective node in nodeID argument.

If you specify this argument as a 6-by-6 matrix when nodeID argument contains more than one element, the information matrix corresponds to all measurements in Measurement argument.

The 6-by-6 information matrix is a diagonal matrix corresponding to the 6 degrees of freedom of the SE(3) transform. Although the Measurement argument uses a quaternion with four elements for rotation, the information matrix models rotational uncertainty in three Euler angles (roll, pitch, and yaw).

The six diagonal values correspond, in order, to:

• Information about translation along the x,y, and z axes (Ix, Iy, Iz)

• Information about rotation along the x,y, and z axes (Iroll, Ipitch, Iyaw)

During factor graph optimization, the diagonal values act as weights on the error between the current value of sensor transform node and the measurement specified by the factorTransformSE3Prior object. Larger values penalize deviation more heavily, forcing the optimizer to find a solution closer to the measurement values.

You must specify higher values for the information matrix (high confidence) to lower the change in the sensor transform node state after optimization. For example, to focus on estimating rotation when translation is known accurately, use high Ix, Iy, Iz values and low Iroll, Ipitch, Iyaw values.

This argument sets the Information property.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Output Arguments

F — Full-state prior factors for SE(3) state space transforms

factorTransformSE3Prior object

Full-state prior factors for SE(3) state space transforms, returned as a factorTransformSE3Prior object.

Properties

NodeID — Node identification numbers

N-element column vector of nonnegative integers

This property is read-only after object creation. To set this property, use the nodeID argument when calling the factorTransformSE3Prior function.

Node identification numbers, represented as an N-element column vector of nonnegative integers, where N is the total number of factors. Each element of NodeID represents the ID in the factor graph of the TRANSFORM_SE3 node connected to the factor represented by the corresponding row of Measurement.

When you use the addFactor function to add a factorTransformSE3Prior object to a factor graph:

• If a node ID represented by the NodeID corresponds to a node in the factor graph, that node must be of the TRANSFORM_SE3 type. If that node is of a different type, the addFactor function returns an error.

• If a node ID represented by the NodeID does not correspond to any node in the factor graph, the addFactor function adds a TRANSFORM_SE3 node with the corresponding node ID 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 absolute SE(3) prior transform

N-by-7 matrix

Measured absolute SE(3) prior transform, specified as a numeric N-by-7 matrix and stored as a double N-by-7 matrix. Each row is of the form [dx dy dz dqw dqx dqy dqz]. N is the total number of factors. dx, dy, and dz represent the translation from the base sensor along the x-, y-, and z-axes, respectively. dqw, dqx, dqy, and dqz represent the rotation (change in orientation) from the base sensor as a quaternion in w, x, y, and z parts, respectively.

By default, Measurement is an N-by-7 matrix in which the value of each row is [0 0 0 1 0 0 0].

Each measurement provides the known prior value to softly fix the transformation of the corresponding node specified by nodeID during optimization.

When you optimize the factor graph that this factor belongs to, the optimize function normalizes the quaternion measurement of the factor before the optimization.

To set this property, use the Measurement argument when calling the factorTransformSE3Prior function.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Information — Information matrices representing the uncertainty of measurements

eye(6) (default) | 6-by-6 matrix | 6-by-6-by-N array

Information matrices representing the uncertainty of measurements, represented as a 6-by-6 matrix or a 6-by-6-by-N array. N is the total number of factors. Each information matrix corresponds to the measurement of the respective node in NodeID property.

If this property is a 6-by-6 matrix when NodeID property contains more than one element, the information matrix corresponds to all measurements in Measurement property.

The 6-by-6 information matrix is a diagonal matrix corresponding to the 6 degrees of freedom of the SE(3) transform. Although the Measurement property uses a quaternion with four elements for rotation, the information matrix models rotational uncertainty in three Euler angles (roll, pitch, and yaw).

The six diagonal values correspond, in order, to:

• Information about translation along the x,y, and z axes (Ix, Iy, Iz)

• Information about rotation along the x,y, and z axes (Iroll, Ipitch, Iyaw)

During factor graph optimization, the diagonal values act as weights on the error between the current value of sensor transform node and the measurement specified by the factorTransformSE3Prior object. Larger values penalize deviation more heavily, forcing the optimizer to find a solution closer to the measurement values.

You must specify higher values for the information matrix (high confidence) to lower the change in the sensor transform node state after optimization. For example, to focus on estimating rotation when translation is known accurately, use high Ix, Iy, Iz values and low Iroll, Ipitch, Iyaw values.

To set this property, use the Information argument when calling the factorTransformSE3Prior function.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Object Functions

nodeType

Get node type of node in factor graph

Examples

Add factorTransformSE3Prior Object to Factor Graph

Create a prior SE(3) transform factor with a node ID of 1.

f = factorTransformSE3Prior(1);

Create a default factor graph, and add the factor to the factor graph.

g = factorGraph;
addFactor(g,f);

More About

Expected Node Types of Factor Objects

The NodeID property of each factor object specifies and connects to these node types:

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"] or ["POSE_SE3","POINT_XYZ","TRANSFORM_SE3"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
factorTwoPoseSIM3	["POSE_SE3","POSE_SE3_SCALE","POSE_SE3","POSE_SE3_SCALE"]
factorIMUBiasPrior	["IMU_BIAS"]
factorPoseSE3Prior	["POSE_SE3"]
factorVelocity3Prior	["VEL_3"]
factorTransformSE3Prior	["TRANSFORM_SE3"]

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 with the IDs 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 R2026a