public ICPRegisterer(
GameObject staticFragment, GameObject modelFragment,
Settings settings,
Action callBack = null
)
{
StaticFragment = staticFragment;
ModelFragment = modelFragment;
Settings = settings;
FinishedCallBack = callBack;
iterationCounter = new Counter(Settings.MaxNumIterations);
stabilization = new StabilizationTermiationCondition();
setNotifcationFunctions();
hasTerminated = false;
//The static fragment does not change during ICP, consequently its points need only be sampled once.
StaticPoints = SamplePoints(StaticFragment);
ModelSamplingInformation = new SamplingInformation(ModelFragment);
settings.ErrorMetric.Set(staticFragment, settings.ReferenceTransform);
}
public List <Point> Sample(SamplingInformation samplingInfo)
{
List <Point> sample = new List <Point>(ApproximateSampleSize(samplingInfo.VertexCount));
Dictionary <int, List <Point> > bins = normalBinner.Bin(samplingInfo);
foreach (List <Point> bin in bins.Values)
{
sample.AddRange(randomSubSampling.Sample(bin));
}
return(sample);
}
/// <summary>
/// Finds the intersection of the ray with the model fragment. Returns
/// null if no intersection is found.
/// </summary>
/// <returns>The intersection.</returns>
/// <param name="ray">Ray.</param>
/// <param name="model">Model.</param>
private Point FindIntersection(Ray ray, SamplingInformation model)
{
RaycastHit hit;
bool collided = model.Collider.Raycast(ray, out hit, MaxDistance);
if (!collided)
{
return(null);
}
return(hit.ToPoint(ReferenceTransform));
}
/// <summary>
/// Finds the correspondence for static point.
/// </summary>
/// <returns>The correspondence.</returns>
/// <param name="staticPoint">Static point.</param>
/// <param name="model">Model.</param>
private Correspondence FindCorrespondence(Point staticPoint, SamplingInformation model)
{
Point intersectionInReferenceTransform = FindIntersection(staticPoint, model);
if (intersectionInReferenceTransform == null)
{
return(null);
}
return(new Correspondence(
staticPoint: staticPoint,
modelPoint: intersectionInReferenceTransform
));
}
/// <summary>
/// Finds the intersection of the (reversed) normal of the static point
/// with the model fragment. Returns null if no intersection is found.
/// </summary>
/// <returns>The intersection.</returns>
/// <param name="staticPoint">Static point.</param>
/// <param name="model">Model.</param>
private Point FindIntersection(Point staticPoint, SamplingInformation model)
{
Point hit;
Ray forwardRay = staticPoint.ToForwardWorldSpaceRay(model.Transform, epsilon);
hit = FindIntersection(forwardRay, model);
if (hit == null)
{
Ray backwardRay = staticPoint.ToBackwardWorldSpaceRay(model.Transform, epsilon);
hit = FindIntersection(backwardRay, model);
}
return(hit);
}
private List <Point> VertexNormals(SamplingInformation info)
{
Mesh fragment = info.Mesh;
List <Point> points = new List <Point>();
for (int i = 0; i < fragment.vertices.Length; i++)
{
points.Add(
new Point(
position: fragment.vertices[i].ChangeTransformOfPosition(
info.Transform, ReferenceTransform),
normal: fragment.normals[i].ChangeTransformOfDirection(
info.Transform, ReferenceTransform)
)
);
}
return(points);
}
private List <Point> NoNormals(SamplingInformation info)
{
ReadOnlyCollection <Vertex> vertices = info.DCEL.Vertices;
List <Point> points = new List <Point>(vertices.Count);
Vector3 position;
foreach (Vertex vertex in vertices)
{
position = vertex.Position;
points.Add(
new Point(
position.ChangeTransformOfPosition(info.Transform, ReferenceTransform)
)
);
}
return(points);
}
public Dictionary <int, List <Point> > Bin(SamplingInformation config)
{
Mesh fragment = config.Mesh;
Dictionary <int, List <Point> > bins = InitializeBins(fragment.vertexCount);
Vector3 position, normal;
int bin;
for (int i = 0; i < fragment.vertices.Length; i++)
{
position = fragment.vertices[i];
normal = fragment.normals[i];
bin = polyhedron.DetermineFaceIdx(normal);
bins[bin].Add(
new Point(
position: position.ChangeTransformOfPosition(config.Transform, referenceTransform),
normal: normal.ChangeTransformOfDirection(config.Transform, referenceTransform)
)
);
}
return(bins);
}
public List <Point> Sample(SamplingInformation samplingInfo)
{
List <Point> points = new AllPointsSampler(this.config).Sample(samplingInfo);
return(Sample(points));
}
public CorrespondenceCollection Find(ReadOnlyCollection <Point> staticPoints, SamplingInformation modelSamplingInformation)
{
List <Point> modelPoints = sampler.Sample(modelSamplingInformation);
return(Find(staticPoints, modelPoints.AsReadOnly()));
}
/// <summary>
/// Sample the specified samplingInfo, and returns the position of the
/// sampled points in the reference transform.
/// </summary>
/// <returns>The sample.</returns>
/// <param name="samplingInfo">Sampling info.</param>
public List <Point> Sample(SamplingInformation samplingInfo)
{
return(samplingFunction(samplingInfo));
}
/// <summary>
/// Find the point where for the specified staticPoints their normal,
/// hits the model fragment. If no intersection is found for the normal
/// shooting outward of the object the normal is reversed.
/// </summary>
/// <returns>A new correspondence collection.</returns>
/// <param name="staticPoints">Static points.</param>
/// <param name="modelSamplingInformation">Model sampling information.</param>
public CorrespondenceCollection Find(ReadOnlyCollection <Point> staticPoints, SamplingInformation modelSamplingInformation)
{
Correspondence correspondence;
CorrespondenceCollection correspondences = new CorrespondenceCollection();
foreach (Point staticPoint in staticPoints)
{
correspondence = FindCorrespondence(staticPoint, modelSamplingInformation);
if (correspondence == null)
{
continue;
}
correspondences.Add(correspondence);
}
return(correspondences);
}
