/// <summary>
/// Processes the point list pair.
/// </summary>
/// <param name="estimatedLocationPoints">The estimated location points (from AR Toolkit).</param>
/// <param name="axisStartPoints">The axis start point (from the camera).</param>
/// <param name="axisEndPoints">The axis end point (from the camera).</param>
/// <param name="myNumPtsCalculated">Nnumber of points calculated.</param>
/// <returns>The average point of all processed points if successful, otherwise a zero length vector.</returns>
/// <remarks>estimatedLocationPoints and seenFromCameraPoints must be matched lists.</remarks>
/// <exception cref="ArgumentNullException"></exception>
/// <exception cref="ArgumentException">estimatedLocationPoints and seenFromCameraPoints should be matched lists.</exception>
private clsPoint3d ProcessPointListPair(List <clsPoint3d> estimatedLocationPoints, List <clsPoint3d> seenFromCameraPoints, ref int myNumPtsCalculated)
{
if (estimatedLocationPoints == null || seenFromCameraPoints == null)
{
throw new ArgumentNullException();
}
if (estimatedLocationPoints.Count != seenFromCameraPoints.Count)
{
throw new ArgumentException("estimatedLocationPoints and seenFromCameraPoints should be matched lists.");
}
myNumPtsCalculated = 0;
// Take copies of the list, such as not to affect the passed list
estimatedLocationPoints = estimatedLocationPoints.ToList();
seenFromCameraPoints = seenFromCameraPoints.ToList();
// Get the points within 12.5mm of the average
var points = GetPointsWithinDistanceOfAveragePoint(estimatedLocationPoints, 12.5);
// Align the matched lists by removing the axis points matched to the estimated location points that have been removed
var removedPoints = estimatedLocationPoints.Where(p => !points.Contains(p)).ToList();
removedPoints.ForEach(p => {
var index = estimatedLocationPoints.IndexOf(p);
estimatedLocationPoints.RemoveAt(index);
seenFromCameraPoints.RemoveAt(index);
});
// Don't process anything if we have less than 3 usable points
if (points.Count <= 3)
{
return(new clsPoint3d(0, 0, 0));
}
var intersectionPoints = new List <clsPoint3d>();
// Get the first line - don't loop through all as we compare to the next line
for (var i = 0; i <= estimatedLocationPoints.Count - 2; i++)
{
var line1 = new clsLine3d(estimatedLocationPoints[i], seenFromCameraPoints[i]);
// Compare to the next line
for (var j = i + 1; j <= estimatedLocationPoints.Count - 1; j++)
{
var line2 = new clsLine3d(estimatedLocationPoints[j].Copy(), seenFromCameraPoints[j].Copy());
// Get midpoint of each line
var p1 = line1.Point;
p1.Normalise();
var p2 = line2.Point;
p2.Normalise();
// Determine angle between lines
var a = Acos(p1.Dot(p2)) * 180 / PI;
if (a > myPGAngleTol)
{
// Angle is above minimum threshold, so determine the point on each line which is closest to the other line
var p3 = line1.ClosestPointToLine(line2);
var p4 = line2.ClosestPointToLine(line1);
// Determine the distance between the closest points
var d = p3.Dist(p4);
if (d < myPGPointTol)
{
// Distance is below threshold so add the intersection point as the midpoint between them
intersectionPoints.Add(new clsPoint3d((p3.X + p4.X) / 2, (p3.Y + p4.Y) / 2, (p3.Z + p4.Z) / 2));
}
}
}
}
var intersectionPointsToFind = 10;
if (intersectionPoints.Count >= intersectionPointsToFind)
{
myNumPtsCalculated = intersectionPoints.Count;
return(GetAveragePoint(intersectionPoints));
}
else
{
DebugStringList.Add($"{intersectionPoints.Count} / {intersectionPointsToFind}");
}
return(new clsPoint3d(0, 0, 0));
}
