hog.multiscale利用meanshift融合

//
#include"stdafx.h"
#include<stdio.h>
#include "opencv2\opencv.hpp"

#include "windows.h"

#include "fstream"

#include <iostream>

#include<iterator>
#include<string>
#include<objdetect\objdetect.hpp>

#include<opencv2\core\core.hpp>

#ifdef HAVE_IPP
#include "ipp.h"
#endif

 using namespace std;

 using namespace cv;



 class HOGInvoker : public ParallelLoopBody
{
public:
    HOGInvoker( const HOGDescriptor* _hog, const Mat& _img,
                double _hitThreshold, Size _winStride, Size _padding,
                const double* _levelScale, std::vector<Rect> * _vec, Mutex* _mtx,
                std::vector<double>* _weights=0, std::vector<double>* _scales=0 )
    {
        hog = _hog;
        img = _img;
        hitThreshold = _hitThreshold;
        winStride = _winStride;
        padding = _padding;
        levelScale = _levelScale;
        vec = _vec;
        weights = _weights;
        scales = _scales;
        mtx = _mtx;
    }

    void operator()( const Range& range ) const
    {
        
		int i, i1 = range.start, i2 = range.end;
        double minScale = i1 > 0 ? levelScale[i1] : i2 > 1 ? levelScale[i1+1] : max(img.cols, img.rows);
		printf("%d , %d \n",i1,i2);
		//double minScale=levelScale[i1];
        Size maxSz(cvCeil(img.cols/minScale), cvCeil(img.rows/minScale));
        Mat smallerImgBuf(maxSz, img.type());
        vector<Point> locations;
        vector<double> hitsWeights;

        for( i = i1; i < i2; i++ )
        {
            double scale = levelScale[i];
            Size sz(cvRound(img.cols/scale), cvRound(img.rows/scale));
            Mat smallerImg(sz, img.type(), smallerImgBuf.data);
            if( sz == img.size() )
                smallerImg = Mat(sz, img.type(), img.data, img.step);
            else
                resize(img, smallerImg, sz);
            hog->detect(smallerImg, locations, hitsWeights, hitThreshold/scale, winStride, padding);
            Size scaledWinSize = Size(cvRound(hog->winSize.width*scale), cvRound(hog->winSize.height*scale));

            mtx->lock();
            for( size_t j = 0; j < locations.size(); j++ )
            {
                vec->push_back(Rect(cvRound(locations[j].x*scale),
                                    cvRound(locations[j].y*scale),
                                    scaledWinSize.width, scaledWinSize.height));
                if (scales)
                {
                    scales->push_back(scale);
                }
            }
            mtx->unlock();

            if (weights && (!hitsWeights.empty()))
            {
                mtx->lock();
                for (size_t j = 0; j < locations.size(); j++)
                {
                    weights->push_back(hitsWeights[j]);
                }
                mtx->unlock();
            }
        }
    }

    const HOGDescriptor* hog;
    Mat img;
    double hitThreshold;
    Size winStride;
    Size padding;
    const double* levelScale;
    std::vector<Rect>* vec;
    std::vector<double>* weights;
    std::vector<double>* scales;
    Mutex* mtx;
};


 
struct MyHog:public HOGDescriptor
{
	public:


	void get(const Mat& img, vector<Rect>& foundLocations,
                                     double hitThreshold, Size winStride, Size padding,
                                     double scale0, double finalThreshold, bool useMeanshiftGrouping)
	{
		vector<double> foundWeights;
		this->detectMultiScale2(img, foundLocations, foundWeights, hitThreshold, winStride,
                     padding, scale0, finalThreshold, useMeanshiftGrouping);
	}

	void get1(const Mat& img, vector<Rect>& foundLocations,
                                     double hitThreshold, Size winStride, Size padding,
                                     double scale0, double finalThreshold, bool useMeanshiftGrouping)
	{
		vector<double> foundWeights;
		this->detectMultiScale3(img, foundLocations, foundWeights, hitThreshold, winStride,
                     padding, scale0, finalThreshold, useMeanshiftGrouping);
	}

void detectMultiScale2(
    const Mat& img, vector<Rect>& foundLocations, vector<double>& foundWeights,
    double hitThreshold, Size winStride, Size padding,
    double scale0, double finalThreshold, bool useMeanshiftGrouping) const
{
    double scale = 1.;
    int levels = 0;
	
    vector<double> levelScale;
    for( levels = 0; levels < nlevels; levels++ )
    {
        levelScale.push_back(scale);
        if( cvRound(img.cols/scale) < img.cols*0.55 ||// 可以自己制定图像的终止大小
            cvRound(img.rows/scale) < img.rows*0.55 ||
            scale0 <= 1 )
            break;
        scale *= scale0;
    }

	//修改后
	 //   for( levels = 0; levels < 4; levels++ )
  //  {
  // levelScale.push_back(scale__[levels]);

  //}

    levels = max(levels, 1);
    levelScale.resize(levels);
	
    std::vector<Rect> allCandidates;
    std::vector<double> tempScales;
    std::vector<double> tempWeights;
    std::vector<double> foundScales;
    Mutex mtx;

    parallel_for_(Range(0, (int)levelScale.size()),
                 HOGInvoker(this, img, hitThreshold, winStride, padding, &levelScale[0], &allCandidates, &mtx, &tempWeights, &tempScales));//并行寻找候选区域

    std::copy(tempScales.begin(), tempScales.end(), back_inserter(foundScales));
    foundLocations.clear();
    std::copy(allCandidates.begin(), allCandidates.end(), back_inserter(foundLocations));
    foundWeights.clear();
    std::copy(tempWeights.begin(), tempWeights.end(), back_inserter(foundWeights));

    if ( useMeanshiftGrouping )
    {
        groupRectangles_meanshift1(foundLocations, foundWeights, foundScales, finalThreshold, winSize);
    }
    else
    {
        groupRectangles(foundLocations, foundWeights, (int)finalThreshold, 0.2);
    }
}


void detectMultiScale3(
    const Mat& img, vector<Rect>& foundLocations, vector<double>& foundWeights,
    double hitThreshold, Size winStride, Size padding,
    double scale0, double finalThreshold, bool useMeanshiftGrouping) const
{
    double scale = 1.;
    int levels = 0;

    vector<double> levelScale;
    for( levels = 0; levels < nlevels; levels++ )
    {
        levelScale.push_back(scale);
        if( cvRound(img.cols/scale) < img.cols ||// 可以自己制定图像的终止大小
            cvRound(img.rows/scale) < img.rows ||
            scale0 <= 1 )
            break;
        scale *= scale0;
    }

    levels = max(levels, 1);
    levelScale.resize(levels);
	
    std::vector<Rect> allCandidates;
    std::vector<double> tempScales;
    std::vector<double> tempWeights;
    std::vector<double> foundScales;
    Mutex mtx;

    parallel_for_(Range(0, (int)levelScale.size()),
                 HOGInvoker(this, img, hitThreshold, winStride, padding, &levelScale[0], &allCandidates, &mtx, &tempWeights, &tempScales));//并行寻找候选区域

    std::copy(tempScales.begin(), tempScales.end(), back_inserter(foundScales));
    foundLocations.clear();
    std::copy(allCandidates.begin(), allCandidates.end(), back_inserter(foundLocations));
    foundWeights.clear();
    std::copy(tempWeights.begin(), tempWeights.end(), back_inserter(foundWeights));

    if ( useMeanshiftGrouping )
    {
        groupRectangles_meanshift1(foundLocations, foundWeights, foundScales, finalThreshold, winSize);
    }
    else
    {
        groupRectangles(foundLocations, foundWeights, (int)finalThreshold, 0.2);
    }
}

class MeanshiftGrouping
{
public:
    MeanshiftGrouping(const Point3d& densKer, const vector<Point3d>& posV,
        const vector<double>& wV, double eps, int maxIter = 20)
    {
        densityKernel = densKer;
        weightsV = wV;
        positionsV = posV;
        positionsCount = (int)posV.size();
        meanshiftV.resize(positionsCount);
        distanceV.resize(positionsCount);
        iterMax = maxIter;
        modeEps = eps;

        for (unsigned i = 0; i<positionsV.size(); i++)
        {
            meanshiftV[i] = getNewValue(positionsV[i]);
            distanceV[i] = moveToMode(meanshiftV[i]);//做最大为iterMax次循环//均值漂移后的符合要求的距离
            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;
			///映射到对应尺度的图片的坐标/////////sPt为scale//
			////////////////////////////////////////////


            double w = (weightsV[i])*std::exp(-((aPt-bPt).dot(aPt-bPt))/2)/std::sqrt(sPt.dot(Point3d(1,1,1)));
			//重新计算的权重，原权重为线性SVM的得分
            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;
		int num=0;
		size_t aa=positionsV.size();
		int len = int(aa);
        for (size_t i=0; i<aa; 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);
    }
};

void groupRectangles_meanshift1(vector<Rect>& rectList, vector<double>& foundWeights,
                               vector<double>& foundScales, double detectThreshold, Size winDetSize)const
{
    groupRectangles_meanshift(rectList, detectThreshold, &foundWeights, foundScales, winDetSize);
}

//new grouping function with using meanshift
static void groupRectangles_meanshift(vector<Rect>& rectList, double detectThreshold, vector<double>* foundWeights,
                                      vector<double>& scales, Size winDetSize)
{
    int detectionCount = (int)rectList.size();
    vector<Point3d> hits(detectionCount), resultHits;
    vector<double> hitWeights(detectionCount), resultWeights;
    Point2d hitCenter;

    for (int i=0; i < detectionCount; i++)
    {
        hitWeights[i] = (*foundWeights)[i];
        hitCenter = (rectList[i].tl() + rectList[i].br())*(0.5); //center of rectangles
        hits[i] = Point3d(hitCenter.x, hitCenter.y, std::log(scales[i]));
    }

    rectList.clear();
    if (foundWeights)
        foundWeights->clear();

    double logZ = std::log(1.3);
    Point3d smothing(8, 16, logZ);

    MeanshiftGrouping msGrouping(smothing, hits, hitWeights, 1e-5, 100);

    msGrouping.getModes(resultHits, resultWeights, 1);

    for (unsigned i=0; i < resultHits.size(); ++i)
    {

        double scale = exp(resultHits[i].z);
        hitCenter.x = resultHits[i].x;
        hitCenter.y = resultHits[i].y;
        Size s( int(winDetSize.width * scale), int(winDetSize.height * scale) );
        Rect resultRect( int(hitCenter.x-s.width/2), int(hitCenter.y-s.height/2),
            int(s.width), int(s.height) );

        if (resultWeights[i] > detectThreshold)//detectThreshold
        {
            rectList.push_back(resultRect);
            foundWeights->push_back(resultWeights[i]);
        }
    }
}
};


int _tmain(int argc, _TCHAR* argv[])
{
	 vector<float> x;
	  vector<float> x1;
	ifstream fileIn1("1.txt", ios::in);
	ifstream fileIn("2.txt", ios::in);
	
	 float val = 0.0f;

     while(!fileIn.eof())

     {

        fileIn>>val;

         x.push_back(val);

     }

     fileIn.close();

	      while(!fileIn1.eof())

     {

        fileIn1>>val;

         x1.push_back(val);

     }

     fileIn1.close();
	 

	MyHog hog_;
	hog_.gammaCorrection=false;
	hog_.blockSize=cv::Size(16,16);
	hog_.blockStride=cv::Size(8,8);
	hog_.cellSize=cv::Size(8,8);
	hog_.winSize=cv::Size(64,64);
	hog_.setSVMDetector(x);


	MyHog hog_1;
	hog_1.gammaCorrection=false;
	hog_1.blockSize=cv::Size(16,16);
	hog_1.blockStride=cv::Size(8,8);
	hog_1.cellSize=cv::Size(8,8);
	hog_1.winSize=cv::Size(64,64);
	hog_1.setSVMDetector(x1);


	vector<Rect> found, found_filtered;
	vector<Rect> found1;
	vector<double> weigths;
	 Mat img;
	

	 int n = 0;
	std::stringstream ss;
	std::string str;
	
	str=str+str;
	  vector<string> img_path;//输入文件名变量 
	  string buf;    
	  ifstream filename( "list.txt" );
	   while( filename )//将训练样本文件依次读取进来    
    {    
        if( getline( filename, buf ) )    
        {        
            img_path.push_back( buf );//图像路径    
        }    
    }    
	 img = imread("D:/1.png");
	
	double scale=1.3;//将原图放大1.3倍   

	int wid = (int)img.size().width*scale;
	int hei = (int)img.size().height*scale;
	Mat imgResize=Mat(wid,hei,CV_8U);
	Size dsize = Size(wid,hei);

	resize(img,imgResize,dsize);

	double t = (double)getTickCount();
	hog_.get(imgResize, found,0.0, Size(2, 2), Size(1, 1), 1.15, 5,true);//1
	hog_1.get(imgResize, found1,0.0, Size(2, 2), Size(1, 1), 1.15, 5,true);//2
	double t1 =(double)getTickCount();
	double tt=(t1 - t)/getTickFrequency();
	cout << "Times passed in seconds: " << tt << endl;

	copy(found1.begin(),found1.end(),back_inserter(found));
	int i=0,j=0;
	  for( i = 0; i < found.size(); i++ )

    {
		//进行了重合去除
         Rect r = found[i];

         for( j = 0; j < found.size(); j++ )

             if( j != i && (r & found[j]) == r)

                 break;

         if( j == found.size() )

             found_filtered.push_back(r);

    }

    for( i = 0; i < found_filtered.size(); i++ )

     {

         Rect r = found_filtered[i];

        // the HOG detector returns slightly larger rectangles than the real objects.

        // so we slightly shrink the rectangles to get a nicer output.


         //r.x += cvRound(r.width*0.1);

         r.width = cvRound(r.width*1.15);

         //r.y += cvRound(r.height*0.1);

         r.height = cvRound(r.height*1.15);



         rectangle(imgResize, r.tl(), r.br(), cv::Scalar(0,255,0), 3);

     }
	found.clear();
	found_filtered.clear();
	 
	 Size dsize1 = Size(img.size().width,img.size().height);
	 Mat img2=Mat(dsize1,CV_8U);
	 resize(imgResize,img2,dsize1);
	 string name=str;
	name+= "aa.jpg";//保存图片
	 imwrite(name,imgResize);
    	 imshow("people detector", img2);

     waitKey();
	 
	return 0;
}