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); }