private double[,] BuildA(CorrespondenceCollection correspondences)
{
double[,] A = new double[correspondences.Count, numUnknowns];
Vector3 crossProduct;
Correspondence correspondence;
for (int row = 0; row < correspondences.Count; row++)
{
correspondence = correspondences[row];
crossProduct = Vector3.Cross(
correspondence.ModelPoint.Position,
correspondence.StaticPoint.UnitNormal
);
A[row, 0] = crossProduct[0];
A[row, 1] = crossProduct[1];
A[row, 2] = crossProduct[2];
A[row, 3] = correspondence.StaticPoint.UnitNormal[0];
A[row, 4] = correspondence.StaticPoint.UnitNormal[1];
A[row, 5] = correspondence.StaticPoint.UnitNormal[2];
}
return(A);
}
public CorrespondenceCollection Find(ReadOnlyCollection <Point> staticPoints, ReadOnlyCollection <Point> modelPoints)
{
List <DistanceNode> distanceNodes = CreateDistanceNodeList(staticPoints, modelPoints);
CorrespondenceCollection correspondences = CreateCorrespondenceCollection(distanceNodes, Mathf.Min(staticPoints.Count, modelPoints.Count));
return(correspondences);
}
public bool Equals(CorrespondenceCollection other)
{
return(
this.correspondences.UnorderedElementsAreEqual(other.correspondences) &&
this.modelpoints.UnorderedElementsAreEqual(other.modelpoints) &&
this.staticpoints.UnorderedElementsAreEqual(other.staticpoints)
);
}
private CorrespondenceCollection FilterCorrespondences(CorrespondenceCollection correspondences)
{
foreach (ICorrespondenceFilter filter in Settings.CorrespondenceFilters)
{
correspondences = filter.Filter(correspondences);
}
return(correspondences);
}
protected override void ValidateCorrespondences(CorrespondenceCollection correspondences)
{
base.ValidateCorrespondences(correspondences);
if (correspondences.Count < numUnknowns)
{
throw new System.ArgumentException("The system is underdetermined, cannot compute the transform.");
}
}
private CorrespondenceCollection ComputeCorrespondences(List <Point> staticPoints)
{
Mesh modelMesh = modelFragment.GetComponent <MeshFilter>().mesh;
CorrespondenceCollection correspondences = Settings.CorrespondenceFinder.Find(
staticPoints.AsReadOnly(), ModelSamplingInformation
);
return(correspondences);
}
protected override Matrix4x4 FindTransformImplementation(CorrespondenceCollection correspondences)
{
// Correspondences.Count x 6 matrix
double[,] A = BuildA(correspondences);
// Correspondences.Count x 1 matrix
double[] b = BuildB(correspondences);
return(ComputeTransform(A, b));
}
private void CorrespondecesToVector3Lists(
CorrespondenceCollection correspondences,
ref List <Vector3d> modelPoints, ref List <Vector3d> staticPoints)
{
foreach (Correspondence correspondence in correspondences)
{
modelPoints.Add(new Vector3d(correspondence.ModelPoint.Position));
staticPoints.Add(new Vector3d(correspondence.StaticPoint.Position));
}
}
protected virtual void ValidateCorrespondences(CorrespondenceCollection correspondences)
{
if (correspondences == null)
{
throw new System.NullReferenceException();
}
if (correspondences.IsEmpty())
{
throw new System.NotSupportedException("Cannot compute the transform if no correspondences are given.");
}
}
public override bool Equals(object obj)
{
if (obj == null || GetType() != obj.GetType())
{
return(false);
}
CorrespondenceCollection other = (CorrespondenceCollection)obj;
return(this.Equals(other));
}
internal CorrespondenceCollectionBuilder(List <DistanceNode> distanceNodes, int numPointsSmallestFragment)
{
distanceNodes.Sort(DistanceNode.SortDescendingOnDistance());
DistanceNodes = new Stack <DistanceNode>(distanceNodes);
StaticPointsInACorrespondence = new HashSet <Point>();
ModelPointsInACorrespondence = new HashSet <Point>();
FinalCorrespondenceCount = numPointsSmallestFragment;
Correspondences = new CorrespondenceCollection();
}
private float ComputeIntialError(CorrespondenceCollection initialCorrespondences)
{
if (initialCorrespondences.Count < 6)
{
Terminate(ICPTerminatedMessage.TerminationReason.Error, "Could only find " + initialCorrespondences.Count + " correspondences to compute the initial error.");
return(float.NaN);
}
else
{
float initialError = Settings.ErrorMetric.ComputeInitialError(initialCorrespondences);
this.Error = initialError;
return(initialError);
}
}
private double[] BuildB(CorrespondenceCollection correspondences)
{
double[] b = new double[correspondences.Count];
Correspondence correspondence;
for (int row = 0; row < correspondences.Count; row++)
{
correspondence = correspondences[row];
b[row] = Vector3.Dot(correspondence.StaticPoint.UnitNormal, correspondence.StaticPoint.Position)
- Vector3.Dot(correspondence.StaticPoint.UnitNormal, correspondence.ModelPoint.Position);
}
return(b);
}
protected override Matrix4x4 FindTransformImplementation(CorrespondenceCollection correspondences)
{
List <Vector4> modelCoordinates = new List <Vector4>(correspondences.Count);
List <Vector4> staticCoordinates = new List <Vector4>(correspondences.Count);
foreach (Correspondence correspondence in correspondences)
{
modelCoordinates.Add(VectorUtils.HomogeneousCoordinate(correspondence.ModelPoint.Position));
staticCoordinates.Add(VectorUtils.HomogeneousCoordinate(correspondence.StaticPoint.Position));
}
return(new _IGDTransformFinder(
modelPoints: modelCoordinates, staticPoints: staticCoordinates,
configuration: configuration
).FindTransform());
}
/// <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);
}
/// <summary>
/// Finds the transform that should be applied to the model points to
/// reduce the sum of squard distances error.
/// </summary>
/// <returns>The transform.</returns>
/// <param name="correspondences">Correspondences.</param>
protected override Matrix4x4 FindTransformImplementation(CorrespondenceCollection correspondences)
{
List <Vector3d> modelPoints = new List <Vector3d>();
List <Vector3d> staticPoints = new List <Vector3d>();
CorrespondecesToVector3Lists(correspondences, ref modelPoints, ref staticPoints);
LandmarkTransform transformComputer = new LandmarkTransform(modelPoints, staticPoints);
bool computationSucceed = transformComputer.ComputeTransform();
if (!computationSucceed)
{
Debug.LogError(
"Could not compute the transform, should not happen, since " +
"ValidateCorrespondences should extract these issues");
}
return(transformComputer.TransformMatrix.ToUnityMatrix());
}
public void PrepareStep()
{
if (HasTerminated)
{
return;
}
Correspondences = ComputeCorrespondences(StaticPoints);
Correspondences = FilterCorrespondences(Correspondences);
if (iterationCounter.AtFirstCount())
{
float initialError = ComputeIntialError(Correspondences);
if (HasTerminated)
{
return;
}
this.errorThreshold = ComputeErrorThreshold(initialError);
SendMessageToAllListeners("OnICPStarted", new ICPStartedMessage(initialError, this.errorThreshold));
}
this.preparationStepEndNotification();
TerminateIfNeeded();
}
protected abstract Matrix4x4 FindTransformImplementation(CorrespondenceCollection correspondences);
public virtual Matrix4x4 FindTransform(CorrespondenceCollection correspondences)
{
ValidateCorrespondences(correspondences);
return(FindTransformImplementation(correspondences));
}
