public static EuclideanTransform Compute(
DataPoints filteredReading,
DataPoints filteredReference,
Matrix <float> outlierWeights,
Matches matches,
IErrorMinimizer minimizer)
{
Debug.Assert(matches.Ids.RowCount > 0);
ErrorElements mPts = ErrorMinimizerHelper.GetMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
return(minimizer.SolveForTransform(mPts));
}
public static EuclideanTransform IterativeSolveForTransform(ErrorElements errorElements, IErrorMinimizer minimizer)
{
var match = new ErrorElements();
match.reference = errorElements.reference;
match.reading = errorElements.reading;
match.weights = errorElements.weights;
var t = EuclideanTransform.Identity;
for (int i = 0; i < 100; i++)
{
t = minimizer.SolveForTransform(match) * t;
match.reading = ICP.ApplyTransformation(errorElements.reading, t);
}
return(t);
}