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 position in the SE(3) state space and a 3-D landmark point. You can use this object to add one or more factors to a `factorGraph` object.

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

example

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

expand all

This property is read-only.

Node ID numbers, specified as an N-by-2 matrix of nonnegative integers, where N is the total number of desired factors. Each row represents a factor connecting a node of type, `POSE_SE3` to a node of type `POINT_XYZ` in the form [PoseID PointID], where PoseID is the ID of the `POSE_SE3` node and PointID is the ID of the `POINT_XYZ` node in the factor graph.

If a factor in the `factorPoseSE3AndPointXYZ` object specifies an ID that does not correspond to a node in the factor graph, the factor graph automatically creates a node of the required type with that ID and adds it to the factor graph when adding 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.

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.

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 \left(x,x\right)& \sigma \left(x,y\right)& \sigma \left(x,z\right)\\ \sigma \left(y,x\right)& \sigma \left(y,y\right)& \sigma \left(y,z\right)\\ \sigma \left(z,x\right)& \sigma \left(y,x\right)& \sigma \left(z,z\right)\end{array}\right]$`

Each element indicates the covariance between two variables. For example, σ(x,y) is the covariance between x and y.

Object Functions

 `nodeType` Get node type of node in factor graph

Examples

collapse all

Create a matrix of positions of the landmarks to use for localization, and the real poses of the robot to compare your factor graph estimate against. Use the `exampleHelperPlotPositionsAndLandmarks` helper function to visualize the landmark points and the real path of the robot.

```gndtruth = [0 0 0; 2 0 pi/2; 2 2 pi; 0 2 pi]; landmarks = [3 0; 0 3]; exampleHelperPlotPositionsAndLandmarks(gndtruth,landmarks)```

Use the `exampleHelperSimpleFourPoseGraph` helper function to create a factor graph contains four poses related by three 2-D two-pose factors. For more details, see the `factorTwoPoseSE2` object page.

`fg = exampleHelperSimpleFourPoseGraph(gndtruth);`

Create Landmark Factors

Generate node IDs to create two node IDs for two landmarks. The second and third pose nodes observe the first landmark point so they should connect to that landmark with a factor. The third and fourth pose nodes observe the second landmark.

```lmIDs = generateNodeID(fg,2); lmFIDs = [1 lmIDs(1); % Pose Node 1 <-> Landmark 1 2 lmIDs(1); % Pose Node 2 <-> Landmark 1 2 lmIDs(2); % Pose Node 2 <-> Landmark 2 3 lmIDs(2)]; % Pose Node 3 <-> Landmark 2```

Define the relative position measurements between the position of the poses and their landmarks in the reference frame of the pose node. Then add some noise.

```lmFMeasure = [0 -1; % Landmark 1 in pose node 1 reference frame -1 2; % Landmark 1 in pose node 2 reference frame 2 -1; % Landmark 2 in pose node 2 reference frame 0 -1]; % Landmark 2 in pose node 3 reference frame lmFMeasure = lmFMeasure + 0.1*rand(4,2);```

Create the landmark factors with those relative measurements and add it to the factor graph.

```lmFactor = factorPoseSE2AndPointXY(lmFIDs,Measurement=lmFMeasure); addFactor(fg,lmFactor);```

Set the initial state of the landmark nodes to the real position of the landmarks with some noise.

`nodeState(fg,lmIDs,landmarks+0.1*rand(2));`

Optimize Factor Graph

Optimize the factor graph with the default solver options. The optimization updates the states of all nodes in the factor graph, so the positions of vehicle and the landmarks update.

```rng default optimize(fg)```
```ans = struct with fields: InitialCost: 0.0538 FinalCost: 6.2053e-04 NumSuccessfulSteps: 4 NumUnsuccessfulSteps: 0 TotalTime: 9.4008e-04 TerminationType: 0 IsSolutionUsable: 1 OptimizedNodeIDs: [1 2 3 4 5] FixedNodeIDs: 0 ```

Visualize and Compare Results

Get and store the updated node states for the robot and landmarks. Then plot the results, comparing the factor graph estimate of the robot path to the known ground truth of the robot.

`poseIDs = nodeIDs(fg,NodeType="POSE_SE2")`
```poseIDs = 1×4 0 1 2 3 ```
`poseStatesOpt = nodeState(fg,poseIDs)`
```poseStatesOpt = 4×3 0 0 0 2.0815 0.0913 1.5986 1.9509 2.1910 -3.0651 -0.0457 2.0354 -2.9792 ```
`landmarkStatesOpt = nodeState(fg,lmIDs)`
```landmarkStatesOpt = 2×2 3.0031 0.1844 -0.1893 2.9547 ```
`exampleHelperPlotPositionsAndLandmarks(gndtruth,landmarks,poseStatesOpt,landmarkStatesOpt);`