コード例 #1
0
ファイル: MomentsFeatureMatcher.cs プロジェクト: KFlaga/Cam3D
        public override void Match()
        {
            int r21 = 2 * WindowRadius + 1;
            _leftMoments = new List<Vector<double>>();
            _rightMoments = new List<Vector<double>>();

            // Find circural mask bounds:
            // for x = [-r,r] -> y = [ -sqrt(r^2 - x^2), sqrt(r^2 - x^2) ]
            _ybounds = new int[r21];
            for(int x = -WindowRadius; x <= WindowRadius; ++x)
            {
                _ybounds[x + WindowRadius] = (int)Math.Sqrt(WindowRadius * WindowRadius - x * x);
            }

            // Find moment vectors for each feature point
            for(int i = 0; i < LeftFeaturePoints.Count; ++i)
            {
                IntVector2 pixel = LeftFeaturePoints[i];
                if(pixel.X < WindowRadius || pixel.Y < WindowRadius ||
                    pixel.X >= LeftImage.ColumnCount - WindowRadius ||
                    pixel.Y >= LeftImage.RowCount - WindowRadius)
                {
                    _leftMoments.Add(null);
                }
                else
                {
                    Vector<double> mom = ComputeMomentVectorForPatch(pixel, LeftImage);
                    if(UseCenteredMoments)
                        mom = ComputeCenteredMomentVector(mom);
                    if(UseScaledMoments)
                        ScaleMomentVector(mom);
                    Vector<double> invMom = ComputeInvariantMomentVector(mom);
                    _leftMoments.Add(invMom);
                }
            }

            for(int i = 0; i < RightFeaturePoints.Count; ++i)
            {
                IntVector2 pixel = RightFeaturePoints[i];
                if(pixel.X < WindowRadius || pixel.Y < WindowRadius ||
                    pixel.X >= RightImage.ColumnCount - WindowRadius ||
                    pixel.Y >= RightImage.RowCount - WindowRadius)
                {
                    _rightMoments.Add(null);
                }
                else
                {
                    Vector<double> mom = ComputeMomentVectorForPatch(pixel, RightImage);
                    if(UseCenteredMoments)
                        mom = ComputeCenteredMomentVector(mom);
                    if(UseScaledMoments)
                        ScaleMomentVector(mom);
                    Vector<double> invMom = ComputeInvariantMomentVector(mom);
                    _rightMoments.Add(invMom);
                }
            }

            // We need to find covariance matrix of invariants as they have
            // different magnitudes, so simple ||Il - Ir|| may not be best choice
            // E = 1/(n-1) sum( (xi-m)(xi-m)^T ) (x is column vector, m = 1/n sum(xi)
            // 1) Find mean
            int n = 0;
            Vector<double> meanInv = new DenseVector(_invCount);
            for(int i = 0; i < _leftMoments.Count; ++i)
            {
                if(_leftMoments[i] != null)
                {
                    meanInv.PointwiseAddThis(_leftMoments[i]);
                    ++n;
                }
            }
            for(int i = 0; i < _rightMoments.Count; ++i)
            {
                if(_rightMoments[i] != null)
                {
                    meanInv.PointwiseAddThis(_rightMoments[i]);
                    ++n;
                }
            }
            meanInv.MultiplyThis(1.0 / n);
            // 2) Find E
            Matrix<double> cov = new DenseMatrix(_invCount, _invCount);
            for(int i = 0; i < _leftMoments.Count; ++i)
            {
                if(_leftMoments[i] != null)
                {
                    cov.PointwiseAddThis(CamCore.MatrixExtensions.FromVectorProduct(_leftMoments[i] - meanInv));
                }
            }
            for(int i = 0; i < _rightMoments.Count; ++i)
            {
                if(_rightMoments[i] != null)
                {
                    cov.PointwiseAddThis(CamCore.MatrixExtensions.FromVectorProduct(_rightMoments[i] - meanInv));
                }
            }
            cov.MultiplyThis(1.0 / (n - 1));
            var covInv = cov.Inverse();

            // Match each point pair and find ||Il - Ir||E
            List<MatchedPair> costs;
            var matchLeft = new List<MatchedPair>();
            var matchRight = new List<MatchedPair>();
            for(int l = 0; l < LeftFeaturePoints.Count; ++l)
            {
                costs = new List<MatchedPair>(LeftFeaturePoints.Count);
                if(_leftMoments[l] != null)
                {
                    for(int r = 0; r < RightFeaturePoints.Count; ++r)
                    {
                        if(_rightMoments[r] != null)
                        {
                            var d = _leftMoments[l] - _rightMoments[r];
                            costs.Add(new MatchedPair()
                            {
                                LeftPoint = new Vector2(LeftFeaturePoints[l]),
                                RightPoint = new Vector2(RightFeaturePoints[r]),
                                Cost = UseMahalanobis ? d * covInv * d : // cost = d^T * E^-1 * d
                                    d.DotProduct(d)
                            });
                        }
                    }
                    costs.Sort((c1, c2) => { return c1.Cost > c2.Cost ? 1 : (c1.Cost < c2.Cost ? -1 : 0); });
                    // Confidence will be (c2-c1)/(c1+c2)
                    MatchedPair match = costs[0];
                    match.Confidence = (costs[1].Cost - costs[0].Cost) / (costs[1].Cost + costs[0].Cost);
                    matchLeft.Add(match);
                }
            }

            for(int r = 0; r < RightFeaturePoints.Count; ++r)
            {
                costs = new List<MatchedPair>(RightFeaturePoints.Count);
                if(_rightMoments[r] != null)
                {
                    for(int l = 0; l < LeftFeaturePoints.Count; ++l)
                    {
                        if(_leftMoments[l] != null)
                        {
                            var d = _leftMoments[l] - _rightMoments[r];
                            costs.Add(new MatchedPair()
                            {
                                LeftPoint = new Vector2(LeftFeaturePoints[l]),
                                RightPoint = new Vector2(RightFeaturePoints[r]),
                                Cost = UseMahalanobis ? d * covInv * d : // cost = d^T * E^-1 * d
                                    d.DotProduct(d)
                            });
                        }
                    }
                    costs.Sort((c1, c2) => { return c1.Cost > c2.Cost ? 1 : (c1.Cost < c2.Cost ? -1 : 0); });
                    // Confidence will be (c2-c1)/(c1+c2)
                    MatchedPair match = costs[0];
                    match.Confidence = (costs[1].Cost - costs[0].Cost) / (costs[1].Cost + costs[0].Cost);
                    matchRight.Add(match);
                }
            }

            Matches = new List<MatchedPair>();
            foreach(var ml in matchLeft)
            {
                MatchedPair mr = matchRight.Find((m) => { return ml.LeftPoint.DistanceTo(m.LeftPoint) < 0.01; });
                // We have both sides matches
                if(mr != null && ml.RightPoint.DistanceTo(mr.RightPoint) < 0.01)
                {
                    // Cross check sucessful
                    Matches.Add(mr);
                }
            }
        }