factorTwoPoseSE3

Relative pose factor connecting two SE(3) poses

Since R2022a

Description

The factorTwoPoseSE3 object contains factors that each relate a pair of poses in the SE(3) state space, or relate a pair of poses in the SE(3) state space and a sensor transform. You can use this object to add one or more factors to a factorGraph object.

During extrinsic calibration of a multi-sensor setup, you can use factors that relate a pair of poses in the SE(3) state space and a sensor transform to estimate the sensor transform between your sensors. Extrinsic calibration, which is the process of estimating the sensor transform from a base sensor to a target sensor is a one-time step. Once you have estimated this sensor transformation, you can specify the estimate to the SensorTransform property of a factorTwoPoseSE3 object and use the object to perform trajectory refinement using multi-sensor SLAM.

You can also use factors that relate a pair of poses in the SE(3) state space and a sensor transform to refine sensor transform values during optimization. To perform this refinement, free the sensor transform node by using the fixNode function with the flag argument specified as false.

For more details about sensor transform estimation and refinement, see Multi-Sensor Extrinsic Calibration using Factor Graph.

Creation

Syntax

F = factorTwoPoseSE3(nodeID)

F = factorTwoPoseSE3(nodeID,Name=Value)

Description

F = factorTwoPoseSE3(nodeID) creates a factorTwoPoseSE3 object, F, with the node identification numbers property NodeID set to nodeID. You can create a factorTwoPoseSE3 object that contains multiple factors.

• If nodeID is an N-by-2 matrix of nonnegative integers, then factorTwoPoseSE3(nodeID) creates a factorTwoPoseSE3 object that contains N factors, each of which relate a pair of poses in the SE(3) state space

• If nodeID is an N-by-3 matrix of nonnegative integers, then factorTwoPoseSE3(nodeID) creates a factorTwoPoseSE3 object that contains N factors, each of which relate a pair of poses in the SE(3) state space and a sensor transform. In this case, factorTwoPoseSE3 ignores any specified value for the SensorTransform property and sets it to an SE(3) transformation representing an identity rotation with no translation.

F = factorTwoPoseSE3(nodeID,Name=Value) specifies properties using one or more name-value arguments. For example, factorTwoPoseSE3([1 2],Measurement=[1 2 3 4 5 6 7]) sets the Measurement property of the factorTwoPoseSE3 object to [1 2 3 4 5 6 7].

Properties

NodeID — Node ID numbers

N-by-2 matrix of nonnegative integers | N-by-3 matrix of nonnegative integers

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

Node ID numbers, represented as an N-by-2 or N-by-3 matrix of nonnegative integers, where N is the total number of desired factors.

• If NodeID is an N-by-2 matrix of nonnegative integers, then each row is of the form [PoseID1 PoseID2]. PoseID1 and PoseID2 are nodes of type POSE_SE3. The factorTwoPoseSE3 object connects these nodes in the factor graph using the specified node IDs. The corresponding row of the Measurement property stores the relative pose from the node with ID PoseID1 to the node with ID PoseID2.

• If NodeID is an N-by-3 matrix of nonnegative integers, then each row is of the form [PoseID1 PoseID2 SensorTransformID]. PoseID1 and PoseID2 are nodes of type POSE_SE3, and SensorTransformID is a node of type TRANSFORM_SE3. The factorTwoPoseSE3 object connects these nodes in the factor graph using the specified node IDs. The corresponding row of the Measurement property stores the relative pose from the node with ID PoseID1 to the node with ID PoseID2.

When you use the addFactor function to add a factorTwoPoseSE3 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 corresponding type as explained previously. 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 node of the corresponding type as explained previously with specified 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 relative pose

N-by-7 matrix

Measured relative pose, 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 are the change in position in x, y, and z respectively. dqw, dqx, dqy, and dqz are the change in quaternion rotation in w, x, y, and z, 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].

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

Note

If the relative pose measurement is not in the same coordinate frame as the connecting pose nodes, you must specify a sensor transform to the SensorTransform property. If the sensor transform is unknown, you can perform extrinsic calibration to estimate it. If the sensor transform is known but needs refinement, you can refine the estimate by connecting factorTwoPoseSE3 object to a sensor transform node. For more details about the sensor transform estimation and refinement workflows, see Multi-Sensor Extrinsic Calibration using Factor Graph.

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

Information — Information matrices associated with measurements

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

Information matrices associated with the measurements, specified as a 6-by-6 matrix or a 6-by-6-by-N array. N is the total number of factors specified by the factorTwoPoseSE3 object. Each information matrix corresponds to the measurements of the corresponding node in NodeID.

Each information matrix is a diagonal matrix. The six diagonal values correspond, in order, to the information for translation along the x,y, and z axes (Ix, Iy, Iz) followed by rotation about the x,y and z axes (Iroll, Ipitch, Iyaw).

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

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

SensorTransform — Transformation from base sensor reference frame to target sensor reference frame

se3() (default) | se3 object

Transformation from base sensor reference frame to target sensor reference frame, specified as an se3 object.

For example, in a camera-lidar multi-sensor setup, if the connected pose nodes store camera poses in the base camera sensor reference frame, the sensor transform rotates and translates a pose in the base camera sensor reference frame to the target lidar sensor reference frame.

Note

If the nodeID argument is an N-by-3 matrix of nonnegative integers, then factorTwoPoseSE3 ignores any specified value for the SensorTransform property and sets it to an SE(3) transformation representing an identity rotation with no translation.

Object Functions

nodeType

Get node type of node in factor graph

Examples

Add factorTwoPoseSE3 Object to Factor Graph

Create a two pose SE(3) state space factor with the connected node IDs set to 1 and 2.

f = factorTwoPoseSE3([1 2]);

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

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 R2022a

R2026a: Perform multi-sensor extrinsic calibration

You can now perform multi-sensor extrinsic calibration using the new SensorTransform property. You can also estimate the sensor transform using factor graph optimization by connecting a sensor transform node of type TRANSFORM_SE3 to the factorTwoPoseSE3 factor. To establish this connection, specify a sensor transform node identification number as the third element in each row of the nodeID argument.

You can also refine the sensor transform estimate using factor graph optimization by freeing the sensor transform node during optimization.

R2023a: Specify multiple factors

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