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 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 static EuclideanTransform IterativeSolveForTransform(ErrorElements errorElements, IErrorMinimizer minimizer) { var match = new ErrorElements(); match.reference = errorElements.reference; match.reading = errorElements.reading; match.weights = errorElements.weights; var t = EuclideanTransform.Identity; for (int i = 0; i < 100; i++) { t = minimizer.SolveForTransform(match) * t; match.reading = ICP.ApplyTransformation(errorElements.reading, t); } return(t); }
public EuclideanTransform SolveForTransform(ErrorElements mPts) { if (!mPts.reference.contiansNormals) { throw new ArgumentException("Reference points must have computed normals. Use appropriate input filter."); } var readingPts = mPts.reading.points; var refPts = mPts.reference.points; // Compute cross product of cross = cross(reading X normalRef) // wF = [ weights*cross ] // [ weights*normals ] // // F = [ cross ] // [ normals ] var wF = new DenseMatrix(6, readingPts.Length); var F = new DenseMatrix(6, readingPts.Length); for (int i = 0; i < readingPts.Length; i++) { var cross = Vector3.Cross(readingPts[i].point, refPts[i].normal); var wCross = mPts.weights[i] * cross; var wNormal = mPts.weights[i] * refPts[i].normal; wF.At(0, i, wCross.X); wF.At(1, i, wCross.Y); wF.At(2, i, wCross.Z); wF.At(3, i, wNormal.X); wF.At(4, i, wNormal.Y); wF.At(5, i, wNormal.Z); F.At(0, i, cross.X); F.At(1, i, cross.Y); F.At(2, i, cross.Z); F.At(3, i, refPts[i].normal.X); F.At(4, i, refPts[i].normal.Y); F.At(5, i, refPts[i].normal.Z); } // Unadjust covariance A = wF * F' var A = wF.TransposeAndMultiply(F); // dot product of dot = dot(deltas, normals) var dotProd = new DenseMatrix(1, mPts.reading.points.Length); for (int i = 0; i < readingPts.Length; i++) { var delta = readingPts[i].point - refPts[i].point; dotProd.At(0, i, Vector3.Dot(delta, refPts[i].normal)); } // b = -(wF' * dot) var b = -(wF.TransposeAndMultiply(dotProd)); // Cholesky decomposition var x = A.Cholesky().Solve(b); EuclideanTransform transform; Vector3 axis = new Vector3(x.At(0, 0), x.At(1, 0), x.At(2, 0)); float len = axis.Length(); transform.rotation = Quaternion.Normalize(Quaternion.CreateFromAxisAngle(axis / len, len)); transform.translation = new Vector3(x.At(3, 0), x.At(4, 0), x.At(5, 0)); return(transform); }