Main Content

nodeState

Get or set node state in factor graph

Since R2022a

Description

example

state = nodeState(fg,nodeIDs) gets the state of the nodes with the specified node IDs in the specified factor graph.

example

nodeState(fg,nodeIDs,newStates) sets the state of the nodes with the specified IDs nodeIDs in the specified factorGraph object fg to the specified states newStates.

Examples

collapse all

Create a factor graph.

fg = factorGraph;

Define two approximate pose states of the robot.

rstate = [0 0 0;
          1 1 pi/2];

Define the relative pose measurement between two nodes from the odometry as the pose difference between the states with some noise. The relative measurement must be in the reference frame of the second node so you must rotate the difference in position to be in the reference frame of the second node.

posediff = diff(rstate);
rotdiffso2 = so2(posediff(3),"theta");
transformedPos = transform(inv(rotdiffso2),posediff(1:2));
odomNoise = 0.1*rand;
measure = [transformedPos posediff(3)] + odomNoise;

Create a SE(2) two-pose factor with the relative measurement. Then add the factor to the factor graph to create two nodes.

ids = generateNodeID(fg,1,"factorTwoPoseSE2");
f = factorTwoPoseSE2(ids,Measurement=measure);
addFactor(fg,f);

Get the state of both pose nodes.

stateDefault = nodeState(fg,ids)
stateDefault = 2×3

     0     0     0
     0     0     0

Because these nodes are new, they have default state values. Ideally before optimizing, you should assign an approximate guess of the absolute pose. This increases the possibility of the optimize function finding the global minimum. Otherwise optimize may become trapped in the local minimum, producing a suboptimal solution.

Keep the first node state at the origin and set the second node state to an approximate xy-position at [0.9 0.95] and a theta rotation of pi/3 radians. In practical applications you could use sensor measurements from your odometry to determine the approximate state of each pose node.

nodeState(fg,ids(2),rstate(2,:))
ans = 1×3

    1.0000    1.0000    1.5708

Before optimizing, save the node state so you can reoptimize as needed.

statePriorOpt1 = nodeState(fg,ids);

Optimize the nodes and check the node states.

optimize(fg);
stateOpt1 = nodeState(fg,ids)
stateOpt1 = 2×3

   -0.1038    0.8725    0.1512
    1.1038    0.1275    1.8035

Note that after optimization the first node did not stay at the origin because although the graph does have the initial guess for the state, the graph does not have any constraint on the absolute position. The graph has only the relative pose measurement, which acts as a constraint for the relative pose between the two nodes. So the graph attempts to reduce the cost related to the relative pose, but not the absolute pose. To provide more information to the graph, you can fix the state of nodes or add an absolute prior measurement factor.

Reset the states and then fix the first node. Then verify that the first node is fixed.

nodeState(fg,ids,statePriorOpt1);
fixNode(fg,ids(1))
isNodeFixed(fg,ids(1))
ans = logical
   1

Reoptimize the factor graph and get the node states.

optimize(fg)
ans = struct with fields:
             InitialCost: 1.8470
               FinalCost: 1.8470e-16
      NumSuccessfulSteps: 2
    NumUnsuccessfulSteps: 0
               TotalTime: 1.4591e-04
         TerminationType: 0
        IsSolutionUsable: 1
        OptimizedNodeIDs: 1
            FixedNodeIDs: 0

stateOpt2 = nodeState(fg,ids)
stateOpt2 = 2×3

         0         0         0
    1.0815   -0.9185    1.6523

Note that after optimizing this time, the first node state remained at the origin.

Input Arguments

collapse all

Factor graph, specified as a factorGraph object.

IDs of the nodes to get or set, specified as an N-element row vector of nonnegative integers. N is the total number of nodes to get or set.

All specified node IDs must specify nodes of the same type.

New node states, specified as an M-by-N matrix. M is the number of specified IDs and N is the number of state elements for the specified nodes. Each row of the matrix specifies the state element values for the corresponding element of nodeIDs.

These are the supported node types and the form of their corresponding states:

  • POSE_SE2 — Pose in SE(2) state space in the form [x y theta]. x and y are the x- and y-positions, respectively, and theta is the orientation.

  • POSE_SE3 — Pose in SE(3) state space in the form [x y z qw qx qy qz]. x, y, and z are the x-, y-, and z-positions, respectively, and qw, qx, qy, and qz represent the orientation as a quaternion.

  • VEL3 — 3-D velocity in the form [vx vy vz]. vx, vy, and vz are the x-, y-, and z-velocities, respectively.

  • POINT_XY — 2-D point in the form [x y]. x and y are the x- and y-positions, respectively.

  • POINT_XYZ — 3-D point in the form [x y z]. x, y, and z are the x-, y-, and z-positions, respectively.

  • IMU_BIAS — IMU gyroscope and accelerometer bias in the form [bias_gyro_x bias_gyro_y bias_gyro_z bias_accel_x bias_accel_y bias_accel_z], where:

    • bias_gyro_x, bias_gyro_y, and bias_gyro_z are the x, y, and z IMU gyroscope biases, respectively.

    • bias_accel_x, bias_accel_y, and bias_accel_z are the x, y, and z accelerometer biases, respectively.

Output Arguments

collapse all

State of the nodes, returned as an M-by-N matrix. M is the number of IDs and N is the number of state elements for the specified nodes.

These are the supported node types and the form of their corresponding states:

  • POSE_SE2 — Pose in SE(2) state space in the form [x y theta]. x and y are the x- and y-positions, respectively, and theta is the orientation.

  • POSE_SE3 — Pose in SE(3) state space in the form [x y z qw qx qy qz]. x, y, and z are the x-, y-, and z-positions, respectively, and qw, qx, qy, and qz represent the orientation as a quaternion.

  • VEL3 — 3-D velocity in the form [vx vy vz]. vx, vy, and vz are the x-, y-, and z-velocities, respectively.

  • POINT_XY — 2-D point in the form [x y]. x and y are the x- and y-positions, respectively.

  • POINT_XYZ — 3-D point in the form [x y z]. x, y, and z are the x-, y-, and z-positions, respectively.

  • IMU_BIAS — IMU gyroscope and accelerometer bias in the form [bias_gyro_x bias_gyro_y bias_gyro_z bias_accel_x bias_accel_y bias_accel_z], where:

    • bias_gyro_x, bias_gyro_y, and bias_gyro_z are the x, y, and z IMU gyroscope biases, respectively.

    • bias_accel_x, bias_accel_y, and bias_accel_z are the x, y, and z accelerometer biases, respectively.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a

expand all

See Also

Objects