Main Content

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

    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.

    example

    Properties

    expand all

    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

    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

    [fx0cx0fycy001].

    The coordinates [cx, cy] represent the principal point of the camera, in pixels. The coordinates [fx ,fy] 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

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

    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

    nodeTypeGet node type of node in factor graph

    Examples

    collapse all

    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

    expand all

    Version History

    Introduced in R2023a