class MeanshiftGrouping replaced from objdetect.hpp to cascadedetect.cpp

modules/objdetect/src/cascadedetect.cpp

@@ -164,6 +164,154 @@ static void groupRectangles(vector<Rect>& rectList, int groupThreshold, double e
     }
 }
 
+class MeanshiftGrouping
+{
+public:
+	MeanshiftGrouping(const Point3d& densKer, const vector<Point3d>& posV, 
+		const vector<double>& wV, double modeEps = 1e-4, int maxIter = 20)
+    {
+	    densityKernel = densKer;
+        weightsV = wV;
+        positionsV = posV;
+        positionsCount = posV.size();
+	    meanshiftV.resize(positionsCount);
+        distanceV.resize(positionsCount);
+        modeEps = modeEps;
+	    iterMax = maxIter;
+        
+	    for (unsigned i = 0; i<positionsV.size(); i++)
+	    {
+		    meanshiftV[i] = getNewValue(positionsV[i]);
+
+		    distanceV[i] = moveToMode(meanshiftV[i]);
+
+		    meanshiftV[i] -= positionsV[i];
+	    }
+    }
+	
+	void getModes(vector<Point3d>& modesV, vector<double>& resWeightsV, const double eps)
+    {
+	    for (size_t i=0; i <distanceV.size(); i++)
+	    {
+		    bool is_found = false;
+		    for(size_t j=0; j<modesV.size(); j++)
+		    {
+			    if ( getDistance(distanceV[i], modesV[j]) < eps)
+			    {
+				    is_found=true;
+				    break;
+			    }
+		    }
+		    if (!is_found)
+		    {
+			    modesV.push_back(distanceV[i]);
+		    }
+	    }
+    	
+	    resWeightsV.resize(modesV.size());
+
+	    for (size_t i=0; i<modesV.size(); i++)
+	    {
+		    resWeightsV[i] = getResultWeight(modesV[i]);
+	    }
+    }
+
+protected:
+	vector<Point3d> positionsV;
+	vector<double> weightsV;
+
+	Point3d densityKernel;
+	int positionsCount;
+
+	vector<Point3d> meanshiftV;
+	vector<Point3d> distanceV;
+	int iterMax;
+	double modeEps;
+
+	Point3d getNewValue(const Point3d& inPt) const
+    {
+	    Point3d resPoint(.0);
+	    Point3d ratPoint(.0);
+	    for (size_t i=0; i<positionsV.size(); i++)
+	    {
+		    Point3d aPt= positionsV[i];
+		    Point3d bPt = inPt;
+		    Point3d sPt = densityKernel;
+    		
+		    sPt.x *= exp(aPt.z);
+		    sPt.y *= exp(aPt.z);
+    		
+		    aPt.x /= sPt.x;
+		    aPt.y /= sPt.y;
+		    aPt.z /= sPt.z;
+
+		    bPt.x /= sPt.x;
+		    bPt.y /= sPt.y;
+		    bPt.z /= sPt.z;
+    		
+		    double w = (weightsV[i])*std::exp(-((aPt-bPt).dot(aPt-bPt))/2)/std::sqrt(sPt.dot(Point3d(1,1,1)));
+    		
+		    resPoint += w*aPt;
+
+		    ratPoint.x += w/sPt.x;
+		    ratPoint.y += w/sPt.y;
+		    ratPoint.z += w/sPt.z;
+	    }
+	    resPoint.x /= ratPoint.x;
+	    resPoint.y /= ratPoint.y;
+	    resPoint.z /= ratPoint.z;
+	    return resPoint;
+    }
+
+	double getResultWeight(const Point3d& inPt) const
+    {
+	    double sumW=0;
+	    for (size_t i=0; i<positionsV.size(); i++)
+	    {
+		    Point3d aPt = positionsV[i];
+		    Point3d sPt = densityKernel;
+
+		    sPt.x *= exp(aPt.z);
+		    sPt.y *= exp(aPt.z);
+
+		    aPt -= inPt;
+    		
+		    aPt.x /= sPt.x;
+		    aPt.y /= sPt.y;
+		    aPt.z /= sPt.z;
+    		
+		    sumW+=(weightsV[i])*std::exp(-(aPt.dot(aPt))/2)/std::sqrt(sPt.dot(Point3d(1,1,1)));
+	    }
+	    return sumW;
+    }
+    
+	Point3d moveToMode(Point3d aPt) const
+    {
+	    Point3d bPt;
+	    for (int i = 0; i<iterMax; i++)
+	    {
+		    bPt = aPt;
+		    aPt = getNewValue(bPt);
+		    if ( getDistance(aPt, bPt) <= modeEps )
+		    {
+			    break;
+		    }
+	    }
+	    return aPt;
+    }
+
+    double getDistance(Point3d p1, Point3d p2) const
+    {
+	    Point3d ns = densityKernel;
+	    ns.x *= exp(p2.z);
+	    ns.y *= exp(p2.z);
+	    p2 -= p1;
+	    p2.x /= ns.x;
+	    p2.y /= ns.y;
+	    p2.z /= ns.z;
+	    return p2.dot(p2);
+    }
+};
 //new grouping function with using meanshift
 static void groupRectangles_meanshift(vector<Rect>& rectList, double detectThreshold, vector<double>* foundWeights, 
 									  vector<double>& scales, Size winDetSize)