Vision.Registration.RegistrationWeighted.WeightedNormal

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

function WeightedNormal
    input Real sourcePoint[:,3]; input Real targetPoint[size(sourcePoint,1),3];
    input Real enabled[size(sourcePoint,1)];
    input Real sourceCovariance[size(sourcePoint,1),3,3]; input Real targetCovariance[size(sourcePoint,1),3,3];
    input Real rotation[3,3]; input Real translation[3]; input Real minimumPivot;
    output Boolean valid; output Real cost; output Real H[6,6]; output Real gradient[6]; output Real rms;
  protected
    Boolean pointValid; Real q[3]; Real r[3]; Real J[3,6]; Real wr[3]; Real wj[3,6];
    Real covariance[3,3]; Real squared; Integer count;
  algorithm
    valid := true; cost := 0; H := zeros(6,6); gradient := zeros(6); squared := 0; count := 0;
    for point in 1:size(sourcePoint,1) loop
      if enabled[point] == 1.0 then
        q := rotation*sourcePoint[point,:]; r := q+translation-targetPoint[point,:];
        J := cat(2,identity(3),-WeightedSkew(q));
        covariance := targetCovariance[point,:,:]+rotation*sourceCovariance[point,:,:]*transpose(rotation);
        (pointValid,wr,wj) := WeightedWhiten(covariance,r,J,minimumPivot);
        valid := valid and pointValid;
        H := H+transpose(wj)*wj; gradient := gradient+transpose(wj)*wr;
        cost := cost+wr*wr; squared := squared+r*r; count := count+1;
      end if;
    end for;
    rms := if count > 0 then sqrt(squared/count) else 0;
    valid := valid and count >= 3 and cost >= 0 and cost < 1e100;
  end WeightedNormal;