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));
}
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);
}
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);
}