SLAM.PoseGraph.ModelicaPoseGraphCovariance.BuildCoarse

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

function BuildCoarse
    input Real position[nodeCapacity,3]; input Real rotation[nodeCapacity,3,3];
    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 Coarse coarse;
  protected
    Real skew[3,3]; Real square; Real E[poseDimension,poseDimension]; Real value;
    Boolean factorValid;
  algorithm
    coarse:=EmptyCoarse(); skew:=zeros(3,3); square:=0.0;
    E:=zeros(poseDimension,poseDimension); value:=0.0; factorValid:=false; coarse.valid:=true;
    for node in 1:nodeCapacity loop
      if node>1 and nodeMask[node]==1.0 then
        skew:=PGSkew(position[node,:]-position[1,:]);
        for axis in 1:3 loop
          coarse.basis[node,axis,axis]:=1.0;
          for column in 1:3 loop
            coarse.basis[node,axis,column+3]:=-skew[axis,column];
            coarse.basis[node,axis+3,column+3]:=rotation[node,column,axis];
          end for;
        end for;
      end if;
    end for;
    for column in 1:poseDimension loop
      square:=PGDot(coarse.basis[:,:,column],coarse.basis[:,:,column]);
      coarse.valid:=coarse.valid and square>1e-24 and square<=1e100;
      if coarse.valid then coarse.basis[:,:,column]:=coarse.basis[:,:,column]/sqrt(square); end if;
    end for;
    if coarse.valid then
      for column in 1:poseDimension loop
        coarse.product[:,:,column]:=PGNormalProduct(coarse.basis[:,:,column],nodeMask,edgeMask,
          source,target,Ji,Jj,information,zeros(nodeCapacity,poseDimension),0.0);
      end for;
      for row in 1:poseDimension loop
        for column in 1:poseDimension loop
          value:=0.5*(PGDot(coarse.basis[:,:,row],coarse.product[:,:,column])
            +PGDot(coarse.basis[:,:,column],coarse.product[:,:,row]));
          E[row,column]:=value; coarse.valid:=coarse.valid and abs(value)<=1e100;
        end for;
      end for;
      if coarse.valid then
        (coarse.factor,factorValid):=PGCholesky(E,1e-12); coarse.valid:=factorValid;
      end if;
    end if;
    if not coarse.valid then coarse:=EmptyCoarse(); end if;
  end BuildCoarse;