public static Matrix TransfromBack3dPoints(Matrix R, Matrix t, Matrix pts23, double scale)
{
var backprojected = Utils.PutRTo4x4(R.T()).Multiply(pts23);
var C = ComputeMatrix.Center(t, R);
for (int i = 0; i < backprojected.Cols; ++i)
{
backprojected[0, i] = backprojected[0, i] * scale + C[0, 0];
backprojected[1, i] = backprojected[1, i] * scale + C[1, 0];
backprojected[2, i] = backprojected[2, i] * scale + C[2, 0];
}
return(backprojected);
}
public static void FindBestScale(Matrix R12, Matrix t12, Matrix R23, Matrix t23, Matrix K,
List <PointF> pts1, List <PointF> pts2, List <PointF> pts3, int minSampleSize,
out double scale, out double confidence, out List <int> inliers)
{
FindTransformation.TriangulateChieral(pts1, pts2, K, R12, t12, out var est3d_12);
FindTransformation.TriangulateChieral(pts2, pts3, K, R23, t23, out var est3d_23);
// Find best scale, so that both sets will be closest
RansacScaleEstimation ransacModel = new RansacScaleEstimation(est3d_12, est3d_23, R12, ComputeMatrix.Center(t12, R12));
int sampleSize = Math.Max(minSampleSize, (int)(0.05 * pts1.Count));
int minGoodPoints = (int)(0.4 * pts1.Count);
int maxIterations = 100;
double meanRefPointSize = GetMeanSize(est3d_12);
double threshold = meanRefPointSize * meanRefPointSize * 0.08;
var result = RANSAC.ProcessMostInliers(ransacModel, maxIterations, sampleSize, minGoodPoints, threshold, 1.0);
scale = (double)result.BestModel;
inliers = result.Inliers;
var backprojected23to12 = TransfromBack3dPoints(R12, t12, est3d_23, scale);
Image <Arthmetic, double> inliersOnly12 = new Image <Arthmetic, double>(result.Inliers.Count, 4);
Image <Arthmetic, double> inliersOnly23 = new Image <Arthmetic, double>(result.Inliers.Count, 4);
for (int i = 0; i < result.Inliers.Count; ++i)
{
int k = result.Inliers[i];
for (int j = 0; j < 4; ++j)
{
inliersOnly12[j, i] = est3d_12[j, k];
inliersOnly23[j, i] = backprojected23to12[j, k];
}
}
// Find errors
var distances = inliersOnly12.Sub(inliersOnly23);
double meanDistance = distances.Norm / distances.Cols;
double meanSize = GetMeanSize(inliersOnly12, inliersOnly23);
double relativeMeanDistance = meanDistance / meanSize;
double error = result.BestError;
double relativeError = error / (meanSize * meanSize);
Errors.TraingulationError(inliersOnly12, inliersOnly23, out double mean, out double median, out List <double> errs);
confidence = (double)inliers.Count / (double)pts1.Count;
}
