factorCameraSE3AndPointXYZ

Factor relating SE(3) camera pose and 3-D point

Since R2023a

Description

The factorCameraSE3AndPointXYZ object describes the visual projection factor relating the poses of a pinhole camera in the SE(3) state space and 3-D landmark points. You can add this object as a factor to a factorGraph object.

The factorCameraSE3AndPointXYZ object requires Computer Vision Toolbox™.

Creation

Syntax

F = factorCameraSE3AndPointXYZ(nodeID,cameraIntrinsicMatrix)

F = factorCameraSE3AndPointXYZ(___,Name=Value)

Description

F = factorCameraSE3AndPointXYZ(nodeID,cameraIntrinsicMatrix) creates a factorCameraSE3AndPointXYZ object, F, with the specified node ID pairs property NodeID set to nodeID, and the property K set to cameraIntrinsicMatrix. The factor object supports the construction of multiple factors with different node ID pairs at the same time.

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

Properties

NodeID — Node ID pairs

N-by-2 matrix of nonnegative integers

This property is read-only.

Node ID pairs, specified as an N-by-2 matrix of nonnegative integers. N is the number of factors. Each row represents a factor that connects to nodes of types POSE_SE3 and POINT_XYZ at the specified node IDs in the factor graph. The rows are of the form [CameraPoseID LandmarkID], where CameraPoseID is the camera pose node ID, and LandmarkID is the landmark node ID.

If a factor in this factorCameraSE3AndPointXYZ object specifies a node that does not exist in the factor graph with the specified ID, a node of the required type with that ID is automatically created and adds it to the factor graph when you add the factor to the factor graph.

You must specify this property at object creation.

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

Data Types: double

K — Camera intrinsic matrix

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

This property is read-only.

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 3-by-3 matrix has the format

$$\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 from Computer Vision Toolbox.

You must specify this property at object creation.

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

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.

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

These are the node types that the NodeID property of each factor object specifies and connects to:

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"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
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 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