SLAM.PoseGraph.ModelicaPoseGraphCovariance.Inverse6

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

function Inverse6
    input Real A[poseDimension,poseDimension];
    output Real inverse[poseDimension,poseDimension];
    output Boolean valid;
  protected
    Real work[poseDimension,poseDimension]; Real scale; Real pivot; Real swap;
    Real largest; Real factor; Real closure; Integer selected;
  algorithm
    inverse:=identity(poseDimension); work:=A; scale:=0.0; valid:=true;
    pivot:=0.0; swap:=0.0; largest:=0.0; factor:=0.0; closure:=0.0; selected:=1;
    for row in 1:poseDimension loop
      for column in 1:poseDimension loop
        valid:=valid and abs(A[row,column])<=1e18;
        scale:=max(scale,abs(A[row,column]));
      end for;
    end for;
    valid:=valid and scale>1e-12;
    for column in 1:poseDimension loop
      if valid then
        selected:=column; largest:=abs(work[column,column]);
        for row in 1:poseDimension loop
          if row>column and abs(work[row,column])>largest then
            selected:=row; largest:=abs(work[row,column]);
          end if;
        end for;
        valid:=largest>1e-12*scale and largest<=1e18;
        if valid then
          for k in 1:poseDimension loop
            swap:=work[column,k]; work[column,k]:=work[selected,k]; work[selected,k]:=swap;
            swap:=inverse[column,k]; inverse[column,k]:=inverse[selected,k]; inverse[selected,k]:=swap;
          end for;
          pivot:=work[column,column];
          for k in 1:poseDimension loop
            work[column,k]:=work[column,k]/pivot; inverse[column,k]:=inverse[column,k]/pivot;
          end for;
          for row in 1:poseDimension loop
            if row<>column then
              factor:=work[row,column];
              for k in 1:poseDimension loop
                work[row,k]:=work[row,k]-factor*work[column,k];
                inverse[row,k]:=inverse[row,k]-factor*inverse[column,k];
              end for;
            end if;
          end for;
        end if;
      end if;
    end for;
    for row in 1:poseDimension loop
      for column in 1:poseDimension loop
        closure:=sum(inverse[row,k]*A[k,column] for k in 1:poseDimension)
          -(if row==column then 1.0 else 0.0);
        valid:=valid and abs(closure)<1e-8 and abs(inverse[row,column])<=1e18;
      end for;
    end for;
    if not valid then inverse:=zeros(poseDimension,poseDimension); end if;
  end Inverse6;