SLAM.PoseGraph.RGBDGraphMeasurements.PrepareProblem

Parameters, inputs, outputs, defaults and equations from the source file.

function PrepareProblem
    input RGBDKeyframes.Catalog catalog;
    input State state;
    input Boolean requested = true;
    output Problem result;
  protected
    Integer oldest; Integer id; Integer slot; Boolean valid;
  algorithm
    result := EmptyProblem(); valid := false;
    if requested then valid := ValidState(catalog,state) and catalog.nextId > 1; end if;
    if valid then
      for node in 1:nodeCapacity loop
        if catalog.occupied[node] then
          valid := valid and RGBDUncertaintyProper(catalog.bodyRotations[node,:,:]);
          for axis in 1:dimension loop valid := valid and abs(catalog.bodyPositions[node,axis]) <= 1e6; end for;
        end if;
      end for;
    end if;
    if valid then
      result.nodeCount := min(catalog.nextId-1,nodeCapacity); oldest := max(1,catalog.nextId-nodeCapacity);
      for node in 1:result.nodeCount loop
        id := oldest+node-1; slot := mod(id-1,nodeCapacity)+1;
        result.nodeId[node] := id; result.catalogSlot[node] := slot; result.nodeMask[node] := 1.0;
        result.positions[node,:] := catalog.bodyPositions[slot,:]; result.rotations[node,:,:] := catalog.bodyRotations[slot,:,:];
      end for;
      for edge in 1:edgeCapacity loop result.edgeCount := result.edgeCount+(if state.edges[edge].enabled then 1 else 0); end for;
      // No edge independence is presumed. For E arbitrarily correlated
      // errors with bounded marginal covariance R_e, C <= E*diag(R_e).
      // This changes the information scale, never the stored measurement.
      // It does not establish independence from the inertial/filter prior.
      result.correlationInflation := max(1,result.edgeCount);
      for edge in 1:edgeCapacity loop
        if state.edges[edge].enabled then
          result.edgeMask[edge] := 1.0;
          result.fromNode[edge] := state.edges[edge].referenceId-oldest+1;
          result.toNode[edge] := state.edges[edge].currentId-oldest+1;
          result.measuredRotation[edge,:,:] := state.edges[edge].rotation;
          result.measuredTranslation[edge,:] := state.edges[edge].translation;
          result.information[edge,:,:] := state.edges[edge].information/result.correlationInflation;
        end if;
      end for;
      result.accepted := true;
    end if;
  end PrepareProblem;