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);
        }
Exemple #3
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);
        }