예제 #1
0
        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));
        }
예제 #2
0
        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
            });
        }
예제 #3
0
            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);
        }
예제 #7
0
        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));
        }
예제 #9
0
            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);
            }
예제 #10
0
            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,
                });
            }
예제 #11
0
        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(),
            });
        }
예제 #12
0
            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
                });
            }
예제 #13
0
        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
            });
        }
예제 #14
0
        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);
        }
예제 #15
0
 public KnownCorrespondenceMatcher(DataPoints reference)
 {
     this.reference = reference;
 }
예제 #16
0
 public IMatcher ConstructMatcher(DataPoints reference)
 {
     return(new KnownCorrespondenceMatcher(reference));
 }
 public void Inspect(DataPoints pointSet, string name)
 {
 }
 public DataPoints Filter(DataPoints input)
 {
     return(input);
 }
예제 #19
0
 public IMatcher ConstructMatcher(DataPoints reference)
 {
     return(new KdTreeMatcher(reference, this.knn, this.epsilon, this.maxDist));
 }