factorCameraSE3AndPointXYZ

Factor relating SE(3) camera pose, 3-D landmark point, and optional sensor transform

Since R2023a

Description

The factorCameraSE3AndPointXYZ object contains visual projection factors that each describe the relationship between the pose of a pinhole camera in the SE(3) state space and a 3-D landmark point, or between the pose of a pinhole camera 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.

You can use factors connecting an SE(3) camera pose, a 3-D landmark point, and a sensor transform to estimate sensor transform during extrinsic calibration between a camera and another sensor. You can specify this sensor transform estimate as the SensorTransform property of a factorCameraSE3AndPointXYZ object, which you can then add to a factor graph created from the trajectory data collected after calibration.

You can also use factors connecting 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.

The factorCameraSE3AndPointXYZ object requires a Computer Vision Toolbox™ license.

Creation

Syntax

F = factorCameraSE3AndPointXYZ(nodeID,cameraIntrinsicMatrix)

F = factorCameraSE3AndPointXYZ(___,Name=Value)

Description

F = factorCameraSE3AndPointXYZ(nodeID,cameraIntrinsicMatrix) creates a factorCameraSE3AndPointXYZ object, F, with the specified node identification numbers nodeID and camera intrinsic matrix cameraIntrinsicMatrix. You can create a factorCameraSE3AndPointXYZ object that contains multiple factors.

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

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

F = factorCameraSE3AndPointXYZ(___,Name=Value) specifies properties using one or more name-value arguments in addition to the arguments from the previous syntax.

Input Arguments

nodeID — Node ID numbers

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

Node ID numbers, specified as an N-by-2 or N-by-3 matrix of nonnegative integers. N is the number of factors. This argument sets the NodeID property.

• If nodeID is an N-by-2 matrix of nonnegative integers, then each row must be of the form [CameraPoseID LandmarkID], where CameraPoseID is the ID of a camera pose node of type POSE_SE3, and LandmarkID is the ID of a landmark point node of type POINT_XYZ.

• If nodeID is an N-by-3 matrix of nonnegative integers, then each row must be of the form [CameraPoseID LandmarkID SensorTransformID], where CameraPoseID is the ID of a camera pose node of type POSE_SE3, LandmarkID is the ID of a landmark point node of type POINT_XYZ, and SensorTransformID is the ID of a sensor transform node of type TRANSFORM_SE3. In this case, factorCameraSE3AndPointXYZ ignores any specified value for the SensorTransform property, and sets it to an SE(3) transformation representing an identity rotation with no translation.

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.

cameraIntrinsicMatrix — Camera intrinsic matrix

3-by-3 matrix | 3-by-3-by-N array

Camera intrinsic matrix, specified as a 3-by-3 matrix or 3-by-3-by-N array. N is the number of factors. When specified as a 3-by-3 matrix, the same camera intrinsic matrix applies to all the factors. A valid camera intrinsic matrix has the form:

$$\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$.

The coordinates [c_x, c_y] specify the principal point of the camera, in pixels. The coordinates [f_x, f_y] specify the focal length of the camera, in pixels.

This argument sets the K property.

Note

This argument is equivalent to the property K (Computer Vision Toolbox) of the cameraIntrinsics (Computer Vision Toolbox) object.

Name-Value Arguments

Measurement — Measured image point position

[0 0] (default) | two-element row vector | N-by-2 matrix

Measured image point position, specified as a two-element row vector or N-by-2 matrix. N is the number of factors. Each row represents a 2-D image point observation [x y] of a specified 3-D point in a specified camera frame.

This argument sets the Measurement.

Data Types: single | double

Information — Information matrix associated with the uncertainty of the measurement

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

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

This argument sets the Information.

Data Types: single | double

SensorTransform — Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to the initial camera frame

se3() (default) | se3 object

Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to the initial camera sensor reference frame, specified as an se3 object.

A sensor transform is unnecessary if the connecting pose nodes contain poses in the initial camera sensor reference frame. Otherwise, you must specify the sensor transform.

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

This argument sets the SensorTransform.

Output Arguments

F — Factors relating SE(3) camera pose, 3-D landmark point, and optional sensor transform

factorCameraSE3AndPointXYZ object

Factors relating an SE(3) camera pose, 3-D point, and optional sensor transform, returned as a factorCameraSE3AndPointXYZ object.

• If nodeID is an N-by-2 matrix of nonnegative integers, then F contains factors that each relate a pose of a pinhole camera in the SE(3) state space to a 3-D landmark point.

• If nodeID is an N-by-3 matrix of nonnegative integers, then F contains factors that each relate a pose of a pinhole camera in the SE(3) state space, a 3-D landmark point, and a sensor transform.

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 factorCameraSE3AndPointXYZ function.

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

• If NodeID is an N-by-2 matrix of nonnegative integers, then each row is of the form [CameraPoseID LandmarkID], where CameraPoseID is the ID of a camera pose node of type POSE_SE3, and LandmarkID is the ID of a landmark point node of type POINT_XYZ.

• If NodeID is an N-by-3 matrix of nonnegative integers, then each row is of the form [CameraPoseID LandmarkID SensorTransformID], where CameraPoseID is the ID of a camera pose node of type POSE_SE3, LandmarkID is the ID of a landmark point node of type POINT_XYZ, and SensorTransformID is the ID of a sensor transform node of type TRANSFORM_SE3.

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.

K — Camera intrinsic matrix

3-by-3 matrix | 3-by-3-by-N array

This property is read-only after object creation. You must specify this property at object creation.

Camera intrinsic matrix, specified as a 3-by-3 matrix or 3-by-3-by-N array. N is the number of factors. When specified as a 3-by-3 matrix, the same camera intrinsic matrix applies to all the factors. The camera intrinsic matrix has the form:

$$\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$.

The coordinates [c_x, c_y] represent the principal point of the camera, in pixels. The coordinates [f_x ,f_y] represent the focal length of the camera, in pixels.

Note

This property is equivalent to the property K (Computer Vision Toolbox) of the cameraIntrinsics (Computer Vision Toolbox) object.

Data Types: double

Measurement — Measured image point position

[0 0] (default) | two-element row vector | N-by-2 matrix

Measured image point position, specified as a two-element row vector or N-by-2 matrix. N is the number of factors. Each row represents a 2-D image point observation [x y] of a specified 3-D point in a specified camera frame.

Data Types: single | double

Information — Information matrix associated with the uncertainty of the measurement

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

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

Data Types: single | double

SensorTransform — Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to the initial camera frame

se3() (default) | se3 object

Transformation consisting of 3-D translation and rotation to transform connecting pose nodes to the initial camera sensor reference frame, specified as an se3 object.

For example, if the connected pose nodes store IMU poses in the initial IMU sensor reference frame, the sensor transform rotates and translates a pose in the initial IMU sensor reference frame to the initial camera sensor reference frame. The initial sensor reference frame has the very first sensor pose at its origin.

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

Create factorCameraSE3AndPointXYZ Object and Add It to factorGraph Object

Create a factorGraph object.

G = factorGraph;

Generate a new unique node ID to represent a camera pose node.

camId = generateNodeID(G,1);

Generate two new unique IDs to represent 3-D points.

pointIds = generateNodeID(G,2);

Specify a camera intrinsic matrix.

focalLength    = [800 800]; % specified in units of pixels
principalPoint = [320 240]; % in pixels [x, y]
cameraIntrinsicMatrix = [focalLength(1) 0 principalPoint(1); ...
                         0 focalLength(2) principalPoint(2); ...
                         0 0 1];

camMeasurements = [240 115; ... % first factor measurement
                   100 315];    % second factor measurement

Create a factorCameraSE3AndPointXYZ object that specifies two factors. The first factor connects the camera pose node and the first point node. The second factor connects the camera pose node and the second point node.

fCam = factorCameraSE3AndPointXYZ([camId pointIds(1); camId pointIds(2)], ...
                                  cameraIntrinsicMatrix, ...
                                  Measurement=camMeasurements);

Adding the factor object to the factor graph adds the nodes with IDs camId and pointIds to the factor graph, connecting them as specified by the factors.

addFactor(G,fCam);

Node camId is of type "POSE_SE3". Nodes pointIds are of type "POINT_XYZ", and both connect to the camera node.

nodeType(G,camId)

ans = 
"POSE_SE3"

nodeType(G,pointIds(1))

ans = 
"POINT_XYZ"

nodeType(G,pointIds(2))

ans = 
"POINT_XYZ"

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 R2023a

R2025a: Estimate sensor transform during extrinsic calibration

You can now estimate or refine the SensorTransform property using factor graph optimization by connecting the sensor transform to an SE(3) camera pose and 3-D landmark point. To establish this connection, specify the node identification numbers for the sensor transform nodes as the third column of the nodeID.