isNodeFixed

Check if node is fixed

Since R2022a

Syntax

isFixed = isNodeFixed(fg,nodeIDs)

Description

isFixed = isNodeFixed(fg,nodeIDs) returns a logical flag indicating whether the nodes with the specified node ID in the factor graph is fixed or not fixed during optimization. When you fix a node, the optimize function does not change the state of that node.

Examples

Optimize Simple Factor Graph

Create a factor graph.

fg = factorGraph;

Define two pose states of the robot as the ground truth.

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 factor connecting two SE(2) pose with the relative measurment between the poses. 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),[0.9 0.95 pi/3])

ans = 1×3

    0.9000    0.9500    1.0472

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.1161    0.9026    0.0571
    1.0161    0.0474    1.7094

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.9452
               FinalCost: 1.9452e-16
      NumSuccessfulSteps: 2
    NumUnsuccessfulSteps: 0
               TotalTime: 8.7976e-05
         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

fg — Factor graph

factorGraph object

Factor graph, specified as a factorGraph object.

nodeIDs — IDs of the nodes to fix

nonnegative integer | N-element row vector of nonnegative integers

IDs of the nodes to fix, specified as a nonnegative integer or N-element row vector of nonnegative integers.

Output Arguments

isFixed — Fix status of nodes

true or 1 | false or 0 | N-element vector of logical values

Fix status of the node, returned as a logical 1 (true) or 0 (false). The function returns true when the node is fixed, and returns false when the node is free.

If nodeID is an N-element vector, then isFixed is returned as an N-element vector of logical values corresponding to each of the specified nodes.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

When generating portable C code with a C++ compiler, you must specify hierarchical packing with non-minimal headers. For more information on packaging options, see the packNGo (MATLAB Coder) function.

Version History

Introduced in R2022a

R2024b: Check multiple nodes at once

isNodeFixed now supports specifying more than one node ID at once to check multiple node IDs.