//using pointmatcher.net
public ICPData ComputeICP(DataPoints reading, DataPoints reference, EuclideanTransform initialTransform)
{
// initialize your point cloud reading here
// initialize your reference point cloud here
// your initial guess at the transform from reading to reference
pointmatcher.net.ICP icp = new pointmatcher.net.ICP
{
ReadingDataPointsFilters = new RandomSamplingDataPointsFilter(prob: 1f),
ReferenceDataPointsFilters = new SamplingSurfaceNormalDataPointsFilter(SamplingMethod.RandomSampling, ratio: 1f),
OutlierFilter = new TrimmedDistOutlierFilter(ratio: 0.99f) // [0 - 1]
};
transform = icp.Compute(reading, reference, initialTransform);
return(new ICPData(ApplyTransformation(transform, reading), transform));
}
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);
}