factorTwoPoseSIM3

Factor relating two SIM(3) poses

Since R2025a

Description

The factorTwoPoseSIM3 object contains factors that each describe the relationship between a pair of similarity transformation poses in the SIM(3) state space. A similarity transformation pose in the SIM(3) state space, is represented using a combination of a SE(3) pose and a scalar pose scale. You can use this object to add one or more factors to a factorGraph object.

Creation

Syntax

F = factorTwoPoseSIM3(nodeID)

F = factorTwoPoseSIM3(nodeID,Name=Value)

Description

F = factorTwoPoseSIM3(nodeID) creates a factorTwoPoseSIM3 object, F, with the specified node identification numbers nodeID.

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

Input Arguments

nodeID — Node ID numbers

N-by-4 matrix of nonnegative integers

Node ID numbers, specified as N-by-4 matrix of nonnegative integers, where N is the number of factors. Each row represents a factor that connects to two similarity transformation poses in the SIM(3) state space. A similarity transformation pose in the SIM(3) state space, is represented using a combination of an SE(3) pose node (POSE_SE3) and an SE(3) pose scale node (POSE_SE3_SCALE).

Each row is of the form [PoseID1 PoseScaleID1 PoseID2 PoseScaleID2], where PoseID1 and PoseScaleID1 are the SE(3) pose node and pose scale node, respectively, of the first SIM(3) pose and PoseID2 and PoseScaleID2 are the SE(3) pose node and pose scale node, respectively, of the second SIM(3) pose.

If a factor in the nodeID argument specifies a node ID that does not exist in the factor graph, then adding the factor to the factor graph adds a node of the corresponding type with that ID to the factor graph.

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

This argument sets the NodeID.

Name-Value Arguments

Measurement — Measured SE(3) or SIM(3) relative pose

[zeros(N,3) ones(N,1) zeros(N,3) ones(N,1)] (default) | N-by-8 matrix | N-by-7 matrix

Measured SE(3) or SIM(3) relative poses, specified as an N-by-7 matrix or N-by-8 matrix. N is the number of specified factors.

• N-by-7 matrix — Each row specifies an SE(3) relative pose of the form [dx dy dz dqw dqx dqy dqz].

• N-by-8 matrix — Each row specifies an SIM(3) relative pose of the form [dx dy dz dqw dqx dqy dqz ds].

dx, dy, and dz are the changes in position in the x-, y-, and z-axes respectively. dqw, dqx, dqy, and dqz are the changes in quaternion rotation for the w, x, y, and z components, respectively. ds is the change in scale.

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.

• N-by-7 matrix — The Measurement matrix is extended by adding an 8th column representing the relative scale, with each row containing a default scalar value of 1. This updated matrix sets the Measurement.

• N-by-8 matrix — The Measurement matrix sets the Measurement.

Data Types: single | double

Information — Information matrix associated with the uncertainty of the measurement

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

Information matrix associated with the uncertainty of the measurement, specified as a 7-by-7 matrix or 7-by-7-by-N array. N is the number of factors. When specified as a 7-by-7 matrix, the same information matrix applies to all the factors.

This argument sets the Information.

Data Types: single | double

Output Arguments

F — Factors relating similarity transformation poses in SIM(3) state space

factorTwoPoseSIM3 object

Factors relating similarity transformation poses in the SIM(3) state space, returned as a factorTwoPoseSIM3 object.

Properties

NodeID — Node ID numbers

N-by-4 matrix of nonnegative numbers

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

Node ID numbers, specified as N-by-4 matrix of nonnegative integers, where N is the number of factors. Each row represents a factor that connects to two similarity transformation poses in the SIM(3) state space. A similarity transformation pose in the SIM(3) state space, is represented using a combination of an SE(3) pose node (POSE_SE3) and an SE(3) pose scale node (POSE_SE3_SCALE).

Each row is of the form [PoseID1 PoseScaleID1 PoseID2 PoseScaleID2], where PoseID1 and PoseScaleID1 are the SE(3) pose node and pose scale node, respectively, of the first SIM(3) pose and PoseID2 and PoseScaleID2 are the SE(3) pose node and pose scale node, respectively, of the second SIM(3) pose.

If a factor in the NodeID property has a node ID that does not exist in the factor graph, then adding the factor to the factor graph adds a node of the corresponding type with that 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 SE(3) or SIM(3) relative pose

[zeros(N,3) ones(N,1) zeros(N,3) ones(N,1)] (default) | N-by-8 matrix | N-by-7 matrix

Measured SE(3) or SIM(3) relative poses, specified as an N-by-7 matrix or N-by-8 matrix. N is the number of specified factors.

• N-by-7 matrix — Each row specifies an SE(3) relative pose of the form [dx dy dz dqw dqx dqy dqz].

• N-by-8 matrix — Each row specifies an SIM(3) relative pose of the form [dx dy dz dqw dqx dqy dqz ds].

dx, dy, and dz are the changes in position in the x-, y-, and z-axes respectively. dqw, dqx, dqy, and dqz are the changes in quaternion rotation for the w, x, y, and z components, respectively. ds is the change in scale.

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

Data Types: single | double

Information — Information matrix associated with the uncertainty of the measurement

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

Information matrix associated with the uncertainty of the measurement, specified as a 7-by-7 matrix or 7-by-7-by-N array. N is the number of factors. When specified as a 7-by-7 matrix, the same information matrix applies to all the factors.

Data Types: single | double

Object Functions

nodeType

Get node type of node in factor graph

Examples

Add Similarity Transformation Factor To Factor Graph Using factorTwoPoseSIM3 Object

This example shows how to create similarity transformation factor using factorTwoPoseSIM3 object and add it to a factor graph.

Specify 4 IDs representing pose nodes and pose scale nodes of two consecutive SIM(3) poses.

% SE(3)pose node ID of first SIM(3) pose
poseID1 = 0;

% pose scale node ID of first SIM(3) pose
scaleID1 = 1;

% SE(3)pose node ID of second SIM(3) pose
poseID2 = 2;

% pose scale node ID of second SIM(3) pose
scaleID2 = 3;

To create a similarity transformation factor connecting the two SIM(3) poses, create a factorTwoPoseSIM3 object by specifying the 4 node IDs.

factor1 = factorTwoPoseSIM3([poseID1,scaleID1,poseID2,scaleID2],Measurement=[0,0.1,0,1,0,0,0,0.2], Information=2*eye(7));

Create a default factor graph and add the factorTwoPoseSIM3 object to it. This will add the similarity transformation factor to the graph.

G = factorGraph;
addFactor(G,factor1);

Observe the node types.

nodeType(G,poseID1)

ans = 
"POSE_SE3"

nodeType(G,scaleID1)

ans = 
"POSE_SE3_SCALE"

generateNodeId function can be used to generate node IDs automatically.

nodeIDs = generateNodeID(G,1,"factorTwoPoseSIM3")

nodeIDs = 1×4

     4     5     6     7

Create a factorTwoPoseSIM3 object using the generated node IDs.

factor2 = factorTwoPoseSIM3(nodeIDs);

Add the factor to the factor graph.

addFactor(G,factor2);

factorTwoPoseSIM3 supports SE(3) measurement of type [dx dy dz dqw dqx dqy dqz] too.

If the specified measurement is an SE(3) measurement, the last column, which represents the relative scale, is assigned a default value of 1 for all factors in the factorTwoPoseSIM3 object.

nodeIDs = generateNodeID(G,1,"factorTwoPoseSIM3");
factor3 = factorTwoPoseSIM3(nodeIDs, "Measurement",[0,0.1,0,1,0,0,0])

factor3 = 
  factorTwoPoseSIM3 with properties:

         NodeID: [8 9 10 11]
    Measurement: [0 0.1000 0 1 0 0 0 1]
    Information: [7×7 double]

addFactor(G,factor3);

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"]

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.

Version History

Introduced in R2025a