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 pose in the SE(3) state space and a 3-D landmark point, or between a pose in the SE(3) state space, a 3-D landmark point, 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 pose in the SE(3) state space, a 3-D landmark point, 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 factorPoseSE3AndPointXYZ object and use the object to perform trajectory refinement using multi-sensor SLAM.

You can also use factors that relate an SE(3) camera pose, 3-D landmark point, and 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 = 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. You can create a factorPoseSE3AndPointXYZ object that contains multiple factors.

• If nodeID is an N-by-2 matrix of nonnegative integers, then factorPoseSE3AndPointXYZ(nodeID) creates a factorPoseSE3AndPointXYZ object that contains N factors, each of which relate a pose in the SE(3) state space and a 3-D landmark point.

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

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 | 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 factorPoseSE3AndPointXYZ 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 [PoseID PointID]. PoseID and PointID are nodes of type, POSE_SE3 and POINT_XYZ, respectively. The factorPoseSE3AndPointXYZ object connects these nodes in the factor graph using the specified node IDs. The corresponding row of the Measurement property stores the relative position between current position node with ID PoseID and landmark point node with ID PointID.

• If NodeID is an N-by-3 matrix of nonnegative integers, then each row is of the form [PoseID PointID SensorTransformID]. PoseID and PointID are nodes of type, POSE_SE3 and POINT_XYZ respectively, and SensorTransformID is a node of type TRANSFORM_SE3. The factorPoseSE3AndPointXYZ object connects those nodes in the factor graph using the specified node IDs. The corresponding row of the Measurement property stores the relative position between current position node with ID PoseID and landmark point node with ID PointID.

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

• If a node ID represented by the NodeID correspond 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 position

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.

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

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.

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. The base sensor reference frame has the very first sensor pose at its origin.

Note

If the nodeID argument is an N-by-3 matrix of nonnegative integers, then factorPoseSE3AndPointXYZ 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

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 R2022b

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