SLAM.PoseGraph.ModelicaPoseGraphCovariance.BuildPairs

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

function BuildPairs
    input Real nodeMask[nodeCapacity]; input Real edgeMask[edgeCapacity];
    input Integer source[edgeCapacity]; input Integer target[edgeCapacity];
    input Real Ji[edgeCapacity,poseDimension,poseDimension];
    input Real Jj[edgeCapacity,poseDimension,poseDimension];
    input Real information[edgeCapacity,poseDimension,poseDimension];
    output Pairs pairs;
  protected
    Real blocks[pairCapacity,pairDimension,pairDimension]; Real normalBlock[poseDimension,poseDimension];
    Real left[poseDimension,poseDimension]; Real right[poseDimension,poseDimension];
    Real L[pairDimension,pairDimension]; Real paddingScale; Boolean factorValid;
    Integer a; Integer b; Integer pa; Integer pb; Integer oa; Integer ob; Integer node;
  algorithm
    pairs := EmptyPairs(); pairs.valid := true;
    blocks := zeros(pairCapacity,pairDimension,pairDimension); normalBlock := zeros(poseDimension,poseDimension);
    left := normalBlock; right := normalBlock; L := zeros(pairDimension,pairDimension); factorValid := false;
    paddingScale := 1.0; a := 1; b := 1; pa := 1; pb := 1; oa := 0; ob := 0; node := 1;
    for i in 1:nodeCapacity loop
      pairs.valid := pairs.valid and (nodeMask[i] == 0.0 or nodeMask[i] == 1.0);
      pairs.active[i] := i > 1 and nodeMask[i] == 1.0;
    end for;
    for edge in 1:edgeCapacity loop
      pairs.valid := pairs.valid and (edgeMask[edge] == 0.0 or edgeMask[edge] == 1.0);
      if pairs.valid and edgeMask[edge] == 1.0 then
        a := source[edge]; b := target[edge];
        pairs.valid := a >= 1 and a <= nodeCapacity and b >= 1 and b <= nodeCapacity and a <> b;
        if pairs.valid then pairs.valid := nodeMask[a] == 1.0 and nodeMask[b] == 1.0; end if;
        if pairs.valid then
          for sideA in 1:2 loop
            for sideB in 1:2 loop
              a := if sideA == 1 then source[edge] else target[edge];
              b := if sideB == 1 then source[edge] else target[edge];
              if a > 1 and b > 1 then
                pa := div(a-2,2)+1; pb := div(b-2,2)+1;
                if pa == pb then
                  oa := mod(a-2,2)*poseDimension; ob := mod(b-2,2)*poseDimension;
                  left := if sideA == 1 then Ji[edge,:,:] else Jj[edge,:,:];
                  right := if sideB == 1 then Ji[edge,:,:] else Jj[edge,:,:];
                  normalBlock := transpose(left)*information[edge,:,:]*right;
                  for i in 1:poseDimension loop for j in 1:poseDimension loop
                    blocks[pa,oa+i,ob+j] := blocks[pa,oa+i,ob+j]+normalBlock[i,j];
                  end for; end for;
                end if;
              end if;
            end for;
          end for;
        end if;
      end if;
    end for;
    if pairs.valid then
      for pair in 1:pairCapacity loop
        paddingScale := 0.0;
        for row in 1:pairDimension loop
          node := 2+2*(pair-1)+div(row-1,poseDimension);
          if node <= nodeCapacity then
            if pairs.active[node] then paddingScale := max(paddingScale,abs(blocks[pair,row,row])); end if;
          end if;
        end for;
        if paddingScale == 0.0 then paddingScale := 1.0; end if;
        for row in 1:pairDimension loop
          node := 2+2*(pair-1)+div(row-1,poseDimension);
          // Inactive and out-of-range padding never read numerical input cells.
          if node > nodeCapacity then blocks[pair,row,row] := paddingScale;
          elseif not pairs.active[node] then blocks[pair,row,row] := paddingScale; end if;
        end for;
        (L,factorValid) := FactorPair(blocks[pair,:,:]); pairs.valid := pairs.valid and factorValid;
        pairs.factor[pair,:,:] := L;
      end for;
    end if;
    if not pairs.valid then pairs := EmptyPairs(); end if;
  end BuildPairs;