public EuclideanTransform Compute(
DataPoints readingIn,
DataPoints referenceIn,
EuclideanTransform T_refIn_dataIn)
{
// Apply reference filters
// reference is express in frame <refIn>
referenceIn = this.ReferenceDataPointsFilters.Filter(referenceIn);
// Create intermediate frame at the center of mass of reference pts cloud
// this help to solve for rotations
var meanReference = VectorHelpers.Mean(referenceIn.points);
EuclideanTransform T_refIn_refMean = new EuclideanTransform
{
translation = meanReference,
rotation = System.Numerics.Quaternion.Identity
};
// Reajust reference position:
// from here reference is express in frame <refMean>
// Shortcut to do T_refIn_refMean.inverse() * reference
for (int i = 0; i < referenceIn.points.Length; i++)
{
referenceIn.points[i].point -= meanReference;
}
// Init matcher with reference points center on its mean
var matcher = this.MatcherFactory.ConstructMatcher(referenceIn);
return(computeWithTransformedReference(readingIn, referenceIn, matcher, T_refIn_refMean, T_refIn_dataIn));
}
private float minDiffTransErr = 1.0f; //0.001f;
// TODO: make params settable by constructor?
public bool ShouldContinue(EuclideanTransform transform)
{
transforms.Add(transform);
float rotErr = 0, transErr = 0;
if (this.transforms.Count > smoothLength)
{
for (int i = transforms.Count - 1; i >= transforms.Count - smoothLength; i--)
{
//Compute the mean derivative
rotErr += Math.Abs(VectorHelpers.AngularDistance(transforms[i].rotation, transforms[i - 1].rotation));
transErr += (transforms[i].translation - transforms[i - 1].translation).Length();
}
if (rotErr / smoothLength < this.minDiffRotErr && transErr / smoothLength < this.minDiffTransErr)
{
return(false);
}
}
if (float.IsNaN(rotErr))
{
throw new ArithmeticException("abs rotation norm not a number");
}
if (float.IsNaN(transErr))
{
throw new ArithmeticException("abs translation norm not a number");
}
return(true);
}
