SLAM.PoseGraph.ModelicaPoseGraphCovariance.Select

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

function Select
    input Real position[nodeCapacity,3]; input Real rotation[nodeCapacity,3,3];
    input Real nodeMask[nodeCapacity]; input Real edgeMask[edgeCapacity];
    input Real fromNode[edgeCapacity]; input Real toNode[edgeCapacity];
    input Real translation[edgeCapacity,3]; input Real measuredRotation[edgeCapacity,3,3];
    input Real information[edgeCapacity,poseDimension,poseDimension] "Already scaled by PrepareProblem; no second inflation";
    input Integer currentNode; input Integer referenceNode;
    input Integer maximumPCG=48; input Real tolerance=1e-10; input Boolean requested=true;
    output Result result;
  protected
    Integer source[edgeCapacity]; Integer target[edgeCapacity]; Integer selected[selectedDimension];
    Real Ji[edgeCapacity,poseDimension,poseDimension]; Real Jj[edgeCapacity,poseDimension,poseDimension];
    Real B[nodeCapacity,poseDimension,selectedDimension]; Real X[nodeCapacity,poseDimension,selectedDimension];
    Real HX[nodeCapacity,poseDimension,selectedDimension]; Real residual[nodeCapacity,poseDimension,selectedDimension];
    Real whitened[nodeCapacity,poseDimension,selectedDimension];
    Real r[nodeCapacity,poseDimension]; Real z[nodeCapacity,poseDimension];
    Real direction[nodeCapacity,poseDimension]; Real product[nodeCapacity,poseDimension];
    Real q[nodeCapacity,poseDimension]; Real edgeResidual[poseDimension];
    Real edgeJi[poseDimension,poseDimension]; Real edgeJj[poseDimension,poseDimension];
    Real rho; Real nextRho; Real curvature; Real alpha; Real beta; Real initialNorm;
    Real norm; Real value; Real graphStatus; Real activeNodes; Real activeEdges;
    Boolean valid; Boolean chartValid; Boolean running; Boolean coarseEnabled;
    Tree tree; Coarse coarse; Pairs pairs;
  algorithm
    result:=Empty(); source:=fill(1,edgeCapacity); target:=source; selected:=fill(1,selectedDimension);
    Ji:=zeros(edgeCapacity,poseDimension,poseDimension); Jj:=Ji;
    B:=zeros(nodeCapacity,poseDimension,selectedDimension); X:=B; HX:=B; residual:=B; whitened:=B;
    r:=zeros(nodeCapacity,poseDimension); z:=r; direction:=r; product:=r; q:=r;
    edgeJi:=zeros(poseDimension,poseDimension); edgeJj:=edgeJi; edgeResidual:=zeros(poseDimension);
    rho:=0.0; nextRho:=0.0; curvature:=0.0; alpha:=0.0; beta:=0.0; initialNorm:=0.0;
    norm:=0.0; value:=0.0; graphStatus:=0.0; activeNodes:=0.0; activeEdges:=0.0;
    valid:=false; chartValid:=false; running:=false; coarseEnabled:=false; coarse:=EmptyCoarse(); pairs:=EmptyPairs();
    if requested then
      result.status:=-1;
      valid:=currentNode>=1 and currentNode<=nodeCapacity and referenceNode>=1 and referenceNode<=nodeCapacity
        and maximumPCG>=0 and maximumPCG<=maximumIterations and tolerance>=1e-14 and tolerance<=1e-2;
      if valid then valid:=nodeMask[currentNode]==1.0 and nodeMask[referenceNode]==1.0; end if;
      if valid then
        (source,target,graphStatus,activeNodes,activeEdges):=PGValidateGraph(position,rotation,nodeMask,
          edgeMask,fromNode,toNode,translation,measuredRotation,information);
        result.status:=-2; valid:=graphStatus==1.0;
      end if;
      if valid then
        result.status:=-4;
        for edge in 1:edgeCapacity loop
          if edgeMask[edge]==1.0 then
            (edgeResidual,edgeJi,edgeJj,chartValid):=PGEdge(position[source[edge],:],rotation[source[edge],:,:],
              position[target[edge],:],rotation[target[edge],:,:],translation[edge,:],measuredRotation[edge,:,:]);
            Ji[edge,:,:]:=edgeJi; Jj[edge,:,:]:=edgeJj;
            valid:=valid and chartValid;
            for row in 1:poseDimension loop
              for column in 1:poseDimension loop
                valid:=valid and information[edge,row,column]==information[edge,column,row]
                  and abs(Ji[edge,row,column])<=1e18 and abs(Jj[edge,row,column])<=1e18;
              end for;
            end for;
          end if;
        end for;
      end if;
      if valid then
        result.status:=-3; tree:=BuildTree(nodeMask,edgeMask,source,target,Ji,Jj,information); valid:=tree.valid;
      end if;
      if valid then
        // Preserve X=0/tree-only majorant when no PCG step is requested. With
        // only the root active there is no free subspace or coarse rank to test.
        coarseEnabled:=maximumPCG>0 and activeNodes>1.0;
        if coarseEnabled then
          coarse:=BuildCoarse(position,rotation,nodeMask,edgeMask,source,target,Ji,Jj,information); valid:=coarse.valid;
          if valid then pairs:=BuildPairs(nodeMask,edgeMask,source,target,Ji,Jj,information); valid:=pairs.valid; end if;
        end if;
      end if;
      if valid then
        result.status:=-5;
        for column in 1:selectedDimension loop
          selected[column]:=if column<=poseDimension then currentNode else referenceNode;
          if selected[column]>1 then B[selected[column],mod(column-1,poseDimension)+1,column]:=1.0; end if;
          r:=B[:,:,column]; z:=if coarseEnabled then BalancedPairSolve(r,pairs,coarse) else TreeSolve(r,tree); direction:=z;
          rho:=PGDot(r,z); initialNorm:=PGDot(r,r);
          valid:=valid and rho>=0.0 and rho<=1e100;
          running:=valid and initialNorm>0.0;
          for iteration in 1:maximumIterations loop
            if running and iteration<=maximumPCG then
              product:=PGNormalProduct(direction,nodeMask,edgeMask,source,target,Ji,Jj,information,zeros(nodeCapacity,poseDimension),0.0);
              curvature:=PGDot(direction,product); valid:=curvature>0.0 and curvature<=1e100 and rho>0.0;
              if valid then
                alpha:=rho/curvature; X[:,:,column]:=X[:,:,column]+alpha*direction;
                r:=r-alpha*product; norm:=PGDot(r,r); result.iterations[column]:=iteration;
                valid:=norm>=0.0 and norm<=1e100;
                running:=valid and norm>tolerance*tolerance*initialNorm;
                if running then
                  z:=if coarseEnabled then BalancedPairSolve(r,pairs,coarse) else TreeSolve(r,tree); nextRho:=PGDot(r,z); valid:=nextRho>0.0 and nextRho<=1e100;
                  if valid then beta:=nextRho/rho; direction:=z+beta*direction; rho:=nextRho; else running:=false; end if;
                end if;
              else running:=false;
              end if;
            end if;
          end for;
          HX[:,:,column]:=PGNormalProduct(X[:,:,column],nodeMask,edgeMask,source,target,Ji,Jj,information,zeros(nodeCapacity,poseDimension),0.0);
          residual[:,:,column]:=B[:,:,column]-HX[:,:,column];
          norm:=PGDot(residual[:,:,column],residual[:,:,column]);
          result.residualNorm[column]:=sqrt(max(0.0,norm));
          result.converged[column]:=norm<=tolerance*tolerance*initialNorm;
          q:=Backward(residual[:,:,column],tree);
          for node in 1:nodeCapacity loop
            if node>1 and nodeMask[node]==1.0 then
              // Only L_W^-1 q is needed for the full residual Gram upper bound.
              for axis in 1:poseDimension loop
                value:=q[node,axis];
                for k in 1:poseDimension loop
                  if k<axis then value:=value-tree.weightFactor[node,axis,k]*whitened[node,k,column]; end if;
                end for;
                whitened[node,axis,column]:=value/tree.weightFactor[node,axis,axis];
              end for;
            end if;
          end for;
        end for;
        for row in 1:selectedDimension loop
          for column in 1:selectedDimension loop
            result.lower[row,column]:=PGDot(B[:,:,row],X[:,:,column])+PGDot(X[:,:,row],B[:,:,column])
              -0.5*(PGDot(X[:,:,row],HX[:,:,column])+PGDot(HX[:,:,row],X[:,:,column]));
            result.residualUpper[row,column]:=PGDot(whitened[:,:,row],whitened[:,:,column]);
            result.upper[row,column]:=result.lower[row,column]+result.residualUpper[row,column];
            valid:=valid and abs(result.lower[row,column])<=1e100
              and abs(result.residualUpper[row,column])<=1e100 and abs(result.upper[row,column])<=1e100;
          end for;
        end for;
        if valid then
          result.accepted:=true; result.status:=1; result.treeEdges:=tree.count; result.activeNodes:=integer(activeNodes);
        end if;
      end if;
      if not valid then
        graphStatus:=result.status; result:=Empty(); result.status:=integer(graphStatus);
      end if;
    end if;
  end Select;