public EuclideanTransform Compute(
DataPoints readingIn,
DataPoints referenceIn,
EuclideanTransform T_refIn_dataIn)
{
// Apply reference filters
// reference is express in frame <refIn>
referenceIn = this.ReferenceDataPointsFilters.Filter(referenceIn);
// Create intermediate frame at the center of mass of reference pts cloud
// this help to solve for rotations
var meanReference = VectorHelpers.Mean(referenceIn.points);
EuclideanTransform T_refIn_refMean = new EuclideanTransform
{
translation = meanReference,
rotation = System.Numerics.Quaternion.Identity
};
// Reajust reference position:
// from here reference is express in frame <refMean>
// Shortcut to do T_refIn_refMean.inverse() * reference
for (int i = 0; i < referenceIn.points.Length; i++)
{
referenceIn.points[i].point -= meanReference;
}
// Init matcher with reference points center on its mean
var matcher = this.MatcherFactory.ConstructMatcher(referenceIn);
return(computeWithTransformedReference(readingIn, referenceIn, matcher, T_refIn_refMean, T_refIn_dataIn));
}
public DataPoints Filter(DataPoints input)
{
int pointsCount = input.points.Length;
var buildData = new BuildData
{
points = input.points,
indices = Enumerable.Range(0, pointsCount).ToArray(),
pointsToKeep = new List <DataPoint>(),
unfitPointsCount = 0
};
// build the new point cloud
buildNew(
buildData,
0,
pointsCount,
MinBound(input.points.Select(p => p.point)),
MaxBound(input.points.Select(p => p.point))
);
return(new DataPoints
{
points = buildData.pointsToKeep.ToArray(),
contiansNormals = true
});
}
public KdTreeMatcher(DataPoints cloud, int knn, float epsilon, float maxDist)
{
this.knn = knn;
this.epsilon = epsilon;
this.maxDist = maxDist;
this.kdtree = new KdTreeNearestNeighborSearch(CreateCloudMatrix(cloud));
}
public Matrix <float> ComputeOutlierWeights(DataPoints filteredReading, DataPoints filteredReference, Matches matches)
{
float limit = matches.GetDistsQuantile(this.ratio);
Matrix <float> dists = new DenseMatrix(matches.Dists);
var result = dists.Clone();
result.MapInplace(d => (d <= limit) ? 1 : 0);
return(result);
}
private static ErrorElements GetMatchedPoints(
DataPoints requestedPts,
DataPoints sourcePts,
Matches matches,
Matrix <float> outlierWeights)
{
Debug.Assert(matches.Ids.RowCount > 0);
Debug.Assert(matches.Ids.ColumnCount > 0);
Debug.Assert(matches.Ids.ColumnCount == requestedPts.points.Length); //nbpts
Debug.Assert(outlierWeights.RowCount == matches.Ids.RowCount); // knn
int knn = outlierWeights.RowCount;
int maxPointsCount = matches.Ids.RowCount * matches.Ids.ColumnCount;
var keptPoints = new List <DataPoint>(maxPointsCount);
var matchedPoints = new List <DataPoint>(maxPointsCount);
List <float> keptWeights = new List <float>(maxPointsCount);
//float weightedPointUsedRatio = 0;
for (int k = 0; k < knn; k++) // knn
{
for (int i = 0; i < requestedPts.points.Length; ++i) //nb pts
{
float weight = outlierWeights.At(k, i);
if (weight != 0.0f)
{
keptPoints.Add(requestedPts.points[i]);
int matchIdx = matches.Ids.At(k, i);
matchedPoints.Add(sourcePts.points[matchIdx]);
keptWeights.Add(weight);
//weightedPointUsedRatio += weight;
}
}
}
var result = new ErrorElements
{
reading = new DataPoints
{
points = keptPoints.ToArray(),
contiansNormals = requestedPts.contiansNormals,
},
reference = new DataPoints
{
points = matchedPoints.ToArray(),
contiansNormals = sourcePts.contiansNormals,
},
weights = keptWeights.ToArray(),
};
return(result);
}
public DataPoints Filter(DataPoints input)
{
var result = input;
foreach (var filter in this.Filters)
{
result = filter.Filter(result);
}
return(result);
}
public static float AverageSqDistance(DataPoints points, DataPoints points2)
{
float sum = 0;
for (int i = 0; i < points.points.Length; i++)
{
sum += (points.points[i].point - points2.points[i].point).LengthSquared();
}
return(sum / points.points.Length);
}
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));
}
private static DenseColumnMajorMatrixStorage <float> CreateCloudMatrix(DataPoints points)
{
var pts = points.points;
var result = new DenseMatrix(3, pts.Length);
for (int i = 0; i < pts.Length; i++)
{
result.At(0, i, pts[i].point.X);
result.At(1, i, pts[i].point.Y);
result.At(2, i, pts[i].point.Z);
}
return((DenseColumnMajorMatrixStorage <float>)result.Storage);
}
public Matches FindClosests(DataPoints filteredReading)
{
int n = this.reference.points.Length;
var indexes = DenseColumnMajorMatrixStorage <int> .OfInit(1, n, (i, j) => j);
var distances = DenseColumnMajorMatrixStorage <float> .OfInit(
1,
n,
(i, j) => (filteredReading.points[j].point - this.reference.points[j].point).LengthSquared());
return(new Matches
{
Dists = distances,
Ids = indexes,
});
}
public DataPoints Filter(DataPoints input)
{
var result = new List <DataPoint>();
foreach (var p in input.points)
{
if (r.NextDouble() < this.Prob)
{
result.Add(p);
}
}
return(new DataPoints
{
points = result.ToArray(),
});
}
public Matches FindClosests(DataPoints filteredReading)
{
int n = filteredReading.points.Length;
var results = DenseColumnMajorMatrixStorage <int> .OfInit(1, n, (i, j) => 0);
var resultDistances = DenseColumnMajorMatrixStorage <float> .OfInit(1, n, (i, j) => 0);
Vector <float> maxRadii = DenseVector.Create(n, i => this.maxDist);
var query = CreateCloudMatrix(filteredReading);
kdtree.knn(query, results, resultDistances, maxRadii, this.knn, this.epsilon, SearchOptionFlags.AllowSelfMatch);
return(new Matches
{
Ids = results,
Dists = resultDistances
});
}
public static DataPoints ApplyTransformation(DataPoints points, EuclideanTransform transform)
{
var resultPoints = new DataPoint[points.points.Length];
for (int i = 0; i < points.points.Length; i++)
{
var x = points.points[i];
resultPoints[i] = new DataPoint
{
point = transform.Apply(x.point),
normal = System.Numerics.Vector3.Transform(x.point, transform.rotation)
};
}
return(new DataPoints
{
points = resultPoints,
contiansNormals = points.contiansNormals
});
}
EuclideanTransform computeWithTransformedReference(
DataPoints readingIn,
DataPoints reference,
IMatcher matcher,
EuclideanTransform T_refIn_refMean,
EuclideanTransform T_refIn_dataIn)
{
// Apply readings filters
// reading is express in frame <dataIn>
//const int nbPtsReading = reading.features.cols();
readingIn = this.ReadingDataPointsFilters.Filter(readingIn);
// Reajust reading position:
// from here reading is express in frame <refMean>
EuclideanTransform
T_refMean_dataIn = T_refIn_refMean.Inverse() * T_refIn_dataIn;
var reading = ApplyTransformation(readingIn, T_refMean_dataIn);
this.Inspector.Inspect(reference, "reference");
// Since reading and reference are express in <refMean>
// the frame <refMean> is equivalent to the frame <iter(0)>
EuclideanTransform T_iter = EuclideanTransform.Identity;
int iterationCount = 0;
bool iterate = true;
var transformationChecker = this.TransformationCheckerFactory.CreateTransformationChecker();
// iterations
while (iterate)
{
//-----------------------------
// Transform Readings
var stepReading = ApplyTransformation(reading, T_iter);
this.Inspector.Inspect(stepReading, "i" + iterationCount.ToString());
//-----------------------------
// Match to closest point in Reference
Matches matches = matcher.FindClosests(stepReading);
//-----------------------------
// Detect outliers
var outlierWeights = this.OutlierFilter.ComputeOutlierWeights(stepReading, reference, matches);
System.Diagnostics.Debug.Assert(outlierWeights.RowCount == matches.Ids.RowCount);
System.Diagnostics.Debug.Assert(outlierWeights.ColumnCount == matches.Ids.ColumnCount);
// the error minimizer's result gets tacked on to what we had before
T_iter = ErrorMinimizerHelper.Compute(stepReading, reference, outlierWeights, matches, this.ErrorMinimizer) * T_iter;
// Old version
//T_iter = T_iter * this->errorMinimizer->compute(
// stepReading, reference, outlierWeights, matches);
// in test
iterate = transformationChecker.ShouldContinue(T_iter);
++iterationCount;
}
// Move transformation back to original coordinate (without center of mass)
// T_iter is equivalent to: T_iter(i+1)_iter(0)
// the frame <iter(0)> equals <refMean>
// so we have:
// T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_refMean_dataIn
// T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_iter(0)_dataIn
// T_refIn_refMean remove the temperary frame added during initialization
return(T_refIn_refMean * T_iter * T_refMean_dataIn);
}
public KnownCorrespondenceMatcher(DataPoints reference)
{
this.reference = reference;
}
public IMatcher ConstructMatcher(DataPoints reference)
{
return(new KnownCorrespondenceMatcher(reference));
}
public void Inspect(DataPoints pointSet, string name)
{
}
public DataPoints Filter(DataPoints input)
{
return(input);
}
public IMatcher ConstructMatcher(DataPoints reference)
{
return(new KdTreeMatcher(reference, this.knn, this.epsilon, this.maxDist));
}
