C++ Mat::ptr方法代码示例

// Adapted from cv_show_angles
cv::Mat
displayQuantized (const cv::Mat& quantized)
{
  cv::Mat color (quantized.size (), CV_8UC3);
  for (int r = 0; r < quantized.rows; ++r)
  {
    const uchar* quant_r = quantized.ptr (r);
    cv::Vec3b* color_r = color.ptr<cv::Vec3b> (r);

    for (int c = 0; c < quantized.cols; ++c)
    {
      cv::Vec3b& bgr = color_r[c];
      switch (quant_r[c])
      {
        case 0:
          bgr[0] = 0;
          bgr[1] = 0;
          bgr[2] = 0;
          break;
        case 1:
          bgr[0] = 55;
          bgr[1] = 55;
          bgr[2] = 55;
          break;
        case 2:
          bgr[0] = 80;
          bgr[1] = 80;
          bgr[2] = 80;
          break;
        case 4:
          bgr[0] = 105;
          bgr[1] = 105;
          bgr[2] = 105;
          break;
        case 8:
          bgr[0] = 130;
          bgr[1] = 130;
          bgr[2] = 130;
          break;
        case 16:
          bgr[0] = 155;
          bgr[1] = 155;
          bgr[2] = 155;
          break;
        case 32:
          bgr[0] = 180;
          bgr[1] = 180;
          bgr[2] = 180;
          break;
        case 64:
          bgr[0] = 205;
          bgr[1] = 205;
          bgr[2] = 205;
          break;
        case 128:
          bgr[0] = 230;
          bgr[1] = 230;
          bgr[2] = 230;
          break;
        case 255:
          bgr[0] = 0;
          bgr[1] = 0;
          bgr[2] = 255;
          break;
        default:
          bgr[0] = 0;
          bgr[1] = 255;
          bgr[2] = 0;
          break;
      }
    }
  }

  return color;
}

/**
 * @function displayCam
 * Draws the webcam image in window + detection info
 */
void displayCam(cv::Mat camImage)
{
    //-- Save matrix
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0.0, windowWidth, 0.0, windowHeight);
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    //-- Display Coordinates
    if(bDisplayDetection)
    {
        //-- Coord text
        std::stringstream sstm;
        sstm << "(x,y,z) = (" << (int)glCamX << "," << (int)glCamY << "," << (int)glCamZ << ")";
        std::string s = sstm.str();
        //std::cout<<s<<std::endl;

        //-- Display text
        glColor3f(1.0, 1.0, 1.0);
        glRasterPos2i(10,  windowHeight-(camRatio*camImage.size().height)-20);
        void * font = GLUT_BITMAP_9_BY_15;
        for (std::string::iterator i = s.begin(); i != s.end(); ++i)
        {
          char c = *i;
          glutBitmapCharacter(font, c);
        }
    }

    //-- Display image
    glRasterPos2i(0, windowHeight-(camRatio*camImage.size().height));
    cv::flip(camImage, camImage, 0);
    cv::resize(camImage, camImage, cv::Size(camRatio*camWidth, camRatio*camHeight), 0, 0, cv::INTER_CUBIC);
    glDrawPixels( camImage.size().width, camImage.size().height, GL_BGR, GL_UNSIGNED_BYTE, camImage.ptr() );

    //-- Load matrix
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();
}

static void WaveExtract(TILE &t, cv::Mat &Integral, cv::Mat &IntegtralRotated, cv::Mat &FeatMat)
{
	t.status = CInspect::ERR_INSP::OK;

	// m_ImgRun already transformed by wavelet xfrm
	float* pFeat = (float*)FeatMat.ptr(t.Index);
	cv::Mat temp;
	// row pointers for integral rectangles
	int Sx = t.tilewidth/4;
	int Sy = Sx;
	// pointers for integral rectangles
	double *pTL,*pTML,*pTC,*pTMR,*pTR;
	double      *pTLM,     *pTRM;
	double *pLC;
	double      *pBLM,     *pBRM;
	double *pBL,*pBML,*pBC,*pBMR,*pBR;

	double right,bottom,columb,middle,center;
	int w = t.tilewidth;
	int h = t.tileheight;
	int row = t.row;
	int col = t.col;
	double TotalBox;
	if(WAVEDATASIZE == 10)
	{
		// take 10 Haar like features from integral images
		pTL  = ((double*)Integral.ptr(row))+col;      pTML = pTL + Sx; pTC = pTL + Sx*2; pTMR = pTL + Sx*3; pTR  = pTL  + w;
		pTLM = ((double*)Integral.ptr(row+Sy))+col;                                                         pTRM = pTLM + w;
		pLC  = ((double*)Integral.ptr(row+Sy*2))+col;
		pBLM = ((double*)Integral.ptr(row+Sy*3))+col;  								                        pBRM = pBLM + w;
		pBL  = ((double*)Integral.ptr(row+h))+col;    pBML = pTL + Sx; pBC = pBL + Sx*2; pBMR = pBL + Sx*3; pBR  = pBL  + w;

		TotalBox = TOTAL;
		right  = RIGHT;
		bottom = BOTTOM;
		columb = COLUMB;
		middle = MIDDLE;
		center = CENTER;
		// left-right
		*pFeat++ = (float)(TotalBox - 2 * RIGHT); 
		// top-bottom
		*pFeat++ = (float)(TotalBox - 2 * BOTTOM); 
		// left+right-middle
		*pFeat++ = (float)(TotalBox - 2 * COLUMB); 
		// top+bottom-middle
		*pFeat++ = (float)(TotalBox - 2 * MIDDLE); 
		// total-center
		*pFeat++ = (float)(TotalBox - 4 * CENTER); 

		// take 10 Haar like features from IntegralRotated images
		pTL  = ((double*)IntegtralRotated.ptr(row))+col;      pTML = pTL + Sx; pTC = pTL + Sx*2; pTMR = pTL + Sx*3; pTR  = pTL  + w;
		pTLM = ((double*)IntegtralRotated.ptr(row+Sy))+col;                                                         pTRM = pTLM + w;
		pLC  = ((double*)IntegtralRotated.ptr(row+Sy*2))+col;
		pBLM = ((double*)IntegtralRotated.ptr(row+Sy*3))+col;  								                        pBRM = pBLM + w;
		pBL  = ((double*)IntegtralRotated.ptr(row+h))+col;    pBML = pTL + Sx; pBC = pBL + Sx*2; pBMR = pBL + Sx*3; pBR  = pBL  + w;

		TotalBox = TOTAL;
		// left-right
		*pFeat++ = (float)(TotalBox - 2 * RIGHT); 
		// top-bottom
		*pFeat++ = (float)(TotalBox - 2 * BOTTOM); 
		// left+right-middle
		*pFeat++ = (float)(TotalBox - 2 * COLUMB); 
		// top+bottom-middle
		*pFeat++ = (float)(TotalBox - 2 * MIDDLE); 
		// total-center
		*pFeat++ = (float)(TotalBox - 4 * CENTER); 

	}
	t.datasize = WAVEDATASIZE;
}

// showDebugScores() is a convenience debug function to call if you are having issues....
// This codew WILL BE OUT OF DATE -- Modify to show what you need to view.
static void	showDebugScores(TILE &T, KNOWLEDGE &K,cv::Mat &FeatMat)
{
	char msg[500];
	sprintf_s(msg,500,"Tile Inspect Wavelet (%4d, %4d) = \n",T.col,T.row);
	OutputDebugStringA(msg);
	float *pF = (float*)FeatMat.ptr(T.Index);

	char temp[100];
	cv::Mat CvInv = K.covar.InverseMatrix();
	if(CvInv.empty())
		OutputDebugStringA("Knowledge Empty\n");
	else
	{
		sprintf_s(msg,500,"Tile Knowledge  \n");
		OutputDebugStringA(msg);
		sprintf_s(msg,500,"Tile Feature = \n");
		OutputDebugStringA(msg);

		sprintf_s(msg,500,"%6.2f",pF[0]);
		for( int j=1; j<4; j++)
		{
			sprintf_s(temp,100,", %6.2f",pF[j]);
			strncat_s(msg,500,temp,100);
		}
		strncat_s(msg,500,"\n",1);
		OutputDebugStringA(msg);
		pF += 4;
		sprintf_s(msg,500,"%6.2f",pF[0]);
		for( int j=1; j<3; j++)
		{
			sprintf_s(temp,100,", %6.2f",pF[j]);
			strncat_s(msg,500,temp,100);
		}
		strncat_s(msg,500,"\n",1);
		OutputDebugStringA(msg);
		pF += 3;
		sprintf_s(msg,500,"%6.2f",pF[0]);
		for( int j=1; j<2; j++)
		{
			sprintf_s(temp,100,", %6.2f",pF[j]);
			strncat_s(msg,500,temp,100);
		}
		strncat_s(msg,500,"\n",1);
		OutputDebugStringA(msg);
		pF += 2;
		sprintf_s(msg,500,"%6.2f\n",pF[0]);
		OutputDebugStringA(msg);

		for(int i=0; i<CvInv.rows; i++)
		{
			pF = (float*)CvInv.ptr(i);
			sprintf_s(msg,500,"%11.2f",pF[0]);
			for( int j=1; j<CvInv.cols; j++)
			{
				sprintf_s(temp,100,", %11.2f",pF[j]);
				strncat_s(msg,500,temp,100);
			}
			strncat_s(msg,500,"\n",1);
			OutputDebugStringA(msg);
		}
					pF = (float*)CvInv.ptr(0);
					if(pF[0] == 0.0F)
						pF = (float*)CvInv.ptr(1) + 1;
	}
}

bool cnn_master::extract_blob(  const string &blob_name,                     /* in : the name of the blob */
                                const std::vector<cv::Mat> &input_images,    /* in : input img */
                                cv::Mat &cnn_feature                         /* out: output cnn feature */
                                )
{
    /*  check check check */
    if(!is_model_ready())
    {
        cout<<"error, Model not ready yet"<<endl;
        return false;
    }

    if(input_images.empty())
    {
        cout<<"error, input_images is empty, return"<<endl;
        return false;
    }
    
    if( !m_network->has_blob(blob_name))
    {
        cout<<"error, Net does not have the blob "<<blob_name<<endl;
        return false;
    }

    /*  get the infos about every blob */
    const std::vector<string> blob_names = m_network->blob_names();
    const std::vector<string> layer_names = m_network->layer_names();

    /* lock and load */
    const shared_ptr<Blob<float> > input_blob = m_network->blob_by_name( blob_names[0] );

    /* make sure the image'size is the same with the input layer */
    for(unsigned int c=0;c<input_images.size();c++)
    {
        if( input_images[c].channels() != m_input_channels)
        {
            cout<<"error, inpur image should have "<<m_input_channels<<" channels, instead of "<<input_images[c].channels()<<endl;
            return false;
        }
        if( input_images[c].cols != m_input_width || input_images[c].rows != m_input_height)
        {
            cout<<"error, input image should have width "<<m_input_width<<" and height "<<m_input_height<<endl;
            return false;
        }
    }
    
    shared_ptr<caffe::MemoryDataLayer<float> > md_layer = boost::dynamic_pointer_cast <caffe::MemoryDataLayer<float> >(m_network->layers()[0]);
    if( !md_layer)
    {
        cout<<"error, The first layer is not momory data layer"<<endl;
        return false;
    }

    /*  prepare the output blob */
    const shared_ptr<Blob<float> > output_blob = m_network->blob_by_name( blob_name );

    /* prepare the output memory */
    int output_dimension = output_blob->count() / output_blob->num();
    cnn_feature = cv::Mat::zeros( input_images.size(), output_dimension, CV_32F);

    /* once a m_batch_size */
    std::vector<cv::Mat>::const_iterator start_iter = input_images.begin();
    std::vector<cv::Mat>::const_iterator end_iter   = input_images.begin() + 
                                                     ( m_batch_size > input_images.size()? input_images.size():m_batch_size);
    while(1)
    {
        if( start_iter >= input_images.end())
            break;
        if( end_iter >= input_images.end()) /* adjust the input batch if it is too short */
            end_iter = input_images.end();

        std::vector<cv::Mat> mat_batch( start_iter, end_iter );

        /* extract the features and store */
        std::vector<int> no_use_labels( mat_batch.size(), 0 );
        md_layer->set_batch_size( mat_batch.size() );       /* sometimes it may less than m_batch_size */
        md_layer->AddMatVector( mat_batch, no_use_labels);

        /* fire the network */
        float no_use_loss=0;
        m_network->ForwardPrefilled(&no_use_loss);
        
        /* store the blob to Mat */
        const float *feature_blob_data = NULL;
        for( unsigned int c=0;c<output_blob->num();c++)
        {
            feature_blob_data = output_blob->cpu_data() + output_blob->offset(c);
            memcpy( cnn_feature.ptr( start_iter - input_images.begin() + c ), feature_blob_data, sizeof(float)*output_dimension);
        }

        /* update the iterator */
        start_iter = end_iter;
        if(m_batch_size > input_images.end() - start_iter)
            end_iter = input_images.end();
        else
            end_iter = start_iter + m_batch_size;
    }
    return true;
}

static void computeLdbDescriptor(const cv::KeyPoint& kpt,
                                 const cv::Mat& img,
                                 const cv::Mat& sum,
								 unsigned char * desc, 
								 const int& patch_size,
                                 const std::vector<std::vector<int> >& coordinates2by2_,
                                 const std::vector<std::vector<int> >& coordinates3by3_,
                                 const std::vector<std::vector<int> >& coordinates4by4_,
                                 const std::vector<std::vector<int> >& coordinates5by5_,
                                 const std::vector<int>& randSequence,
								 bool flag)
{
#ifdef TRAINING
	for(int i = 0; i < randSequence.size(); i++)
		bit_pattern_256_[i] = randSequence[i];
#endif

	// Compute the pointer to the center of the feature
	int img_y = (int)(kpt.pt.y + 0.5);
	int img_x = (int)(kpt.pt.x + 0.5);
	const int * center = reinterpret_cast<const int *> (sum.ptr(img_y)) + img_x;

	int sum2by2[4], sum3by3[9], sum4by4[16], sum5by5[25];
	int dx2by2[4], dx3by3[9], dx4by4[16], dx5by5[25];
	int dy2by2[4], dy3by3[9], dy4by4[16], dy5by5[25];
//	int dxy2by2[4], dxy3by3[9], dxy4by4[16], dxy5by5[25];

	int sum2by2_size = 4, sum3by3_size = 9, sum4by4_size = 16, sum5by5_size = 25;

	int offset = patch_size/2;
    cv::Mat win_integral_image(patch_size+1, patch_size+1, CV_32S);

	if(flag == true)
		rotatedIntegralImage(kpt.angle, kpt, img, patch_size, win_integral_image);

	for(int i = 0; i < sum2by2_size; i++){
		if(flag == false){
			int a = coordinates2by2_[i][1] * sum.cols + coordinates2by2_[i][0];
			int b = coordinates2by2_[i][3] * sum.cols + coordinates2by2_[i][2];
			int c = coordinates2by2_[i][5] * sum.cols + coordinates2by2_[i][4];
			int d = coordinates2by2_[i][7] * sum.cols + coordinates2by2_[i][6];

			sum2by2[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b);

			int e = coordinates2by2_[i][9] * sum.cols + coordinates2by2_[i][8];
			int f = coordinates2by2_[i][11] * sum.cols + coordinates2by2_[i][10];
			int g = coordinates2by2_[i][13] * sum.cols + coordinates2by2_[i][12];
			int h = coordinates2by2_[i][15] * sum.cols + coordinates2by2_[i][14];

			dx2by2[i] = sum2by2[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c));
			dy2by2[i] = sum2by2[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));
		}
		else{
			sum2by2[i] = win_integral_image.at<int>(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset)
				+ win_integral_image.at<int>(coordinates2by2_[i][7]+offset, coordinates2by2_[i][6]+offset)
				- win_integral_image.at<int>(coordinates2by2_[i][3]+offset, coordinates2by2_[i][2]+offset)
				- win_integral_image.at<int>(coordinates2by2_[i][5]+offset, coordinates2by2_[i][4]+offset);

			dx2by2[i] = sum2by2[i] 
			- 2*(win_integral_image.at<int>(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset)
				+ win_integral_image.at<int>(coordinates2by2_[i][13]+offset, coordinates2by2_[i][12]+offset)
				- win_integral_image.at<int>(coordinates2by2_[i][9]+offset, coordinates2by2_[i][8]+offset)
				- win_integral_image.at<int>(coordinates2by2_[i][5]+offset, coordinates2by2_[i][4]+offset));

			dy2by2[i] = sum2by2[i] 
			- 2*(win_integral_image.at<int>(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset)
				+ win_integral_image.at<int>(coordinates2by2_[i][11]+offset, coordinates2by2_[i][10]+offset)
				- win_integral_image.at<int>(coordinates2by2_[i][3]+offset, coordinates2by2_[i][2]+offset)
				- win_integral_image.at<int>(coordinates2by2_[i][15]+offset, coordinates2by2_[i][14]+offset));
		}
	}

	for(int i = 0; i < sum3by3_size; i++){
		if(flag == false){
			int a = coordinates3by3_[i][1] * sum.cols + coordinates3by3_[i][0];
			int b = coordinates3by3_[i][3] * sum.cols + coordinates3by3_[i][2];
			int c = coordinates3by3_[i][5] * sum.cols + coordinates3by3_[i][4];
			int d = coordinates3by3_[i][7] * sum.cols + coordinates3by3_[i][6];
			sum3by3[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b);

			int e = coordinates3by3_[i][9] * sum.cols + coordinates3by3_[i][8];
			int f = coordinates3by3_[i][11] * sum.cols + coordinates3by3_[i][10];
			int g = coordinates3by3_[i][13] * sum.cols + coordinates3by3_[i][12];
			int h = coordinates3by3_[i][15] * sum.cols + coordinates3by3_[i][14];

			dx3by3[i] = sum3by3[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c));
			dy3by3[i] = sum3by3[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));}
		else{
			sum3by3[i] = win_integral_image.at<int>(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset)
				+ win_integral_image.at<int>(coordinates3by3_[i][7]+offset, coordinates3by3_[i][6]+offset)
				- win_integral_image.at<int>(coordinates3by3_[i][3]+offset, coordinates3by3_[i][2]+offset)
				- win_integral_image.at<int>(coordinates3by3_[i][5]+offset, coordinates3by3_[i][4]+offset);

			dx3by3[i] = sum3by3[i] 
			- 2*(win_integral_image.at<int>(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset)
				+ win_integral_image.at<int>(coordinates3by3_[i][13]+offset, coordinates3by3_[i][12]+offset)
				- win_integral_image.at<int>(coordinates3by3_[i][9]+offset, coordinates3by3_[i][8]+offset)
				- win_integral_image.at<int>(coordinates3by3_[i][5]+offset, coordinates3by3_[i][4]+offset));

			dy3by3[i] = sum3by3[i] 
//.........这里部分代码省略.........

bool ConvertDepthCvMat16uToYellowCvMat(const cv::Mat & srcDepthMat, cv::Mat & destMat)
{
	if ( srcDepthMat.empty() || destMat.empty() ) {
		return false;
	}

	assert ( srcDepthMat.size() == destMat.size() );
	assert ( srcDepthMat.type() == CV_16UC1 );

	const int nXRes = srcDepthMat.size().width;
	const int nYRes = srcDepthMat.size().height;
	const int srcRowStep = srcDepthMat.step;

	const uchar * srcRowPtr = NULL;
	const unsigned short * srcDataPtr = NULL;

	float depthHist[OPENNI_MAX_DEPTH];
	memset(depthHist, 0, OPENNI_MAX_DEPTH * sizeof(float));

	unsigned int pointsNumber = 0;
	srcRowPtr = srcDepthMat.data;

	for (int y = 0; y < nYRes; ++y, srcRowPtr += srcRowStep) 
	{
		srcDataPtr = (const unsigned short *)(srcRowPtr);
	
		for (int x = 0; x < nXRes; ++x, ++srcDataPtr) 
		{
			unsigned short tmp = *srcDataPtr;
			if ( tmp ) {
				++depthHist[tmp];
				++pointsNumber;
			}
		}
	}
	for (int i = 1; i < OPENNI_MAX_DEPTH; ++i) {
		depthHist[i] += depthHist[i-1];
	}
	if (pointsNumber > 0)
	{
		for (int i = 1; i < OPENNI_MAX_DEPTH; ++i) {
			depthHist[i] = (unsigned int)(256 * (1.0f - depthHist[i] / pointsNumber));
		}
	}
	
	srcRowPtr = srcDepthMat.data;
	
	for (int y = 0; y < nYRes; ++y, srcRowPtr += srcRowStep) 
	{
		srcDataPtr = (const unsigned short *)(srcRowPtr);
		uchar * imageptr = destMat.ptr(y);
	
		for (int x = 0; x < nXRes; ++x, ++srcDataPtr, imageptr += destMat.channels()) 
		{
			unsigned short tmp = (*srcDataPtr);
			if ( tmp )
			{
				imageptr[0] = 0;
				imageptr[1] = depthHist[tmp];
				imageptr[2] = depthHist[tmp];
				// imageptr[3] = 0xff;
			}
			else 
			{
				// imageptr[3] = 0;
				imageptr[2] = imageptr[1] = imageptr[0] = 0;
			}
		}
	}

	return true;
}

	void setSource(cv::Mat frame)
	{
		bind();
		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, frame.cols, frame.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, frame.ptr());
	    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
	    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	    unbind();
	}

    cv::Mat Deformation::DeformByMovingLeastSquares(const cv::Mat& inputImg, 
            const std::vector<int>& originIndex, const std::vector<int>& targetIndex)
    {
        int imgW = inputImg.cols;
        int imgH = inputImg.rows;
        cv::Size imgSize(imgW, imgH);
        cv::Mat resImg(imgSize, CV_8UC3);
        int markNum = originIndex.size() / 2;
        std::vector<double> wList(markNum);
        std::vector<MagicMath::Vector2> pHatList(markNum);
        std::vector<MagicMath::Vector2> qHatList(markNum);
        MagicMath::Vector2 pStar, qStar;
        std::vector<MagicMath::Vector2> pList(markNum);
        for (int mid = 0; mid < markNum; mid++)
        {
            pList.at(mid) = MagicMath::Vector2(originIndex.at(mid * 2), originIndex.at(mid * 2 + 1));
        }
        std::vector<MagicMath::Vector2> qList(markNum);
        for (int mid = 0; mid < markNum; mid++)
        {
            qList.at(mid) = MagicMath::Vector2(targetIndex.at(mid * 2), targetIndex.at(mid * 2 + 1));
        }
        std::vector<std::vector<double> > aMatList(markNum);
        std::vector<bool> visitFlag(imgW * imgH, 0);

        for (int hid = 0; hid < imgH; hid++)
        {
            for (int wid = 0; wid < imgW; wid++)
            {
                MagicMath::Vector2 pos(wid, hid);
                //calculate w
                bool isMarkVertex = false;
                int markedIndex = -1;
                double wSum = 0;
                for (int mid = 0; mid < markNum; mid++)
                {
                    //double dTemp = (pos - pList.at(mid)).LengthSquared(); //variable
                    double dTemp = (pos - pList.at(mid)).Length();
                    //dTemp = pow(dTemp, 1.25);
                    if (dTemp < 1.0e-15)
                    {
                        isMarkVertex = true;
                        markedIndex = mid;
                        break;
                    }
                    dTemp = pow(dTemp, 1.25);
                    wList.at(mid) = 1.0 / dTemp;
                    wSum += wList.at(mid);
                }
                //
                if (isMarkVertex)
                {
                    const unsigned char* pPixel = inputImg.ptr(hid, wid);
                    int targetH = targetIndex.at(2 * markedIndex + 1);
                    int targetW = targetIndex.at(2 * markedIndex);
                    unsigned char* pResPixel = resImg.ptr(targetH, targetW);
                    pResPixel[0] = pPixel[0];
                    pResPixel[1] = pPixel[1];
                    pResPixel[2] = pPixel[2];
                    visitFlag.at(targetH * imgW + targetW) = 1;
                }
                else
                {
                    //Calculate pStar qStar
                    pStar = MagicMath::Vector2(0.0, 0.0);
                    qStar = MagicMath::Vector2(0.0, 0.0);
                    for (int mid = 0; mid < markNum; mid++)
                    {
                        pStar += (pList.at(mid) * wList.at(mid));
                        qStar += (qList.at(mid) * wList.at(mid));
                    }
                    pStar /= wSum;
                    qStar /= wSum;

                    //Calculate pHat qHat
                    for (int mid = 0; mid < markNum; mid++)
                    {
                        pHatList.at(mid) = pList.at(mid) - pStar;
                        qHatList.at(mid) = qList.at(mid) - qStar;
                    }
                    
                    //Calculate A
                    MagicMath::Vector2 col0 = pos - pStar;
                    MagicMath::Vector2 col1(col0[1], -col0[0]);
                    for (int mid = 0; mid < markNum; mid++)
                    {
                        std::vector<double> aMat(4);
                        MagicMath::Vector2 row1(pHatList.at(mid)[1], -pHatList.at(mid)[0]);
                        aMat.at(0) = pHatList.at(mid) * col0 * wList.at(mid);
                        aMat.at(1) = pHatList.at(mid) * col1 * wList.at(mid);
                        aMat.at(2) = row1 * col0 * wList.at(mid);
                        aMat.at(3) = row1 * col1 * wList.at(mid);
                        aMatList.at(mid) = aMat;
                    }

                    //Calculate fr(v)
                    MagicMath::Vector2 fVec(0, 0);
                    for (int mid = 0; mid < markNum; mid++)
                    {
                        fVec[0] += (qHatList.at(mid)[0] * aMatList.at(mid).at(0) + qHatList.at(mid)[1] * aMatList.at(mid).at(2));
//.........这里部分代码省略.........

unsigned long ipa_Utils::FilterTearOffEdges(cv::Mat& xyzImage, cv::Mat* mask, float piHalfFraction)
{
	/// Check if destination image has been initialized correctly
	CV_Assert(xyzImage.type() == CV_32FC3);

	float pi = 3.14159f;
	float t_lower =pi/piHalfFraction;
	float t_upper = pi - t_lower;

	if(mask)
	{
		mask->create(xyzImage.size() , CV_8UC3);
		mask->setTo(0);
	}

	for(int row=0; row < xyzImage.rows; row++)
	{
		int index_vLeft = -1;
		int index_vMiddle = -1;
		int index_vRight = -1;
		int index_vUp = -1;
		int index_vDown = -1;

		cv::Vec3f vLeft = cv::Vec3f::all(0);
		cv::Vec3f vMiddle = cv::Vec3f::all(0);
		cv::Vec3f vRight = cv::Vec3f::all(0);
		cv::Vec3f vUp = cv::Vec3f::all(0);
		cv::Vec3f vDown = cv::Vec3f::all(0);

		cv::Vec3f vDiff = cv::Vec3f::all(0);

		float* f_image_ptr_RowUp = 0;
		float* f_image_ptr_RowMiddle = 0;
		float* f_image_ptr_RowDown = 0;

		float dot = -1.f;
		float angle = -1.f;

		if (row-1 >= 0)
		{
			f_image_ptr_RowUp = xyzImage.ptr<float>(row-1);
		}

		f_image_ptr_RowMiddle = xyzImage.ptr<float>(row);

		if (row+1 < xyzImage.rows)
		{
			f_image_ptr_RowDown = xyzImage.ptr<float>(row+1);
		}

		/// Extract four surrounding neighbor vectors that have a non zero mask value
		///
		///    x
		///  x o x
		///    x
		///
		for(int col=0; col < xyzImage.cols; col++)
		{
			/// Counte the amount of times, we satisfy the thresholds
			int score = 0;

			/// Vector Middle (must exist)
			index_vMiddle = col;
			vMiddle[0] = f_image_ptr_RowMiddle[3*index_vMiddle];
			vMiddle[1] = f_image_ptr_RowMiddle[3*index_vMiddle + 1];
			vMiddle[2] = f_image_ptr_RowMiddle[3*index_vMiddle + 2];

			/// Vector Left
			if (col-1 >= 0)
			{
				index_vLeft = col-1;
				vLeft[0] = f_image_ptr_RowMiddle[3*index_vLeft];
				vLeft[1] = f_image_ptr_RowMiddle[3*index_vLeft + 1];
				vLeft[2] = f_image_ptr_RowMiddle[3*index_vLeft + 2];
				vDiff = vLeft - vMiddle;
				float vLeftNorm = std::sqrt((vLeft[0] * vLeft[0]) + 
					(vLeft[1] * vLeft[1]) + (vLeft[2] * vLeft[2]));
				vLeft[0] = vLeft[0]/vLeftNorm;
				vLeft[1] = vLeft[1]/vLeftNorm;
				vLeft[2] = vLeft[2]/vLeftNorm;
				//vLeft.Normalize();
				float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
					(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
				vDiff[0] = vDiff[0]/vDiffNorm;
				vDiff[1] = vDiff[1]/vDiffNorm;
				vDiff[2] = vDiff[2]/vDiffNorm;
				//vDiff.Normalize();
				dot = (float)vDiff.ddot(vLeft);
				//dot = vDiff.Dot(vLeft);
				angle = (float)std::acos(dot);
				//angle = Wm4::Math<float>::ACos( dot );
				if (angle > t_upper || angle < t_lower)
				{
					score++;
				}
				else
				{
					score--;
				}
			}
//.........这里部分代码省略.........

/****************************************************************************
**
** SaveAsPCX
**
** Write a PCX file version of the image
*/
int CPalette::SavePalettizedAsPCX( cv::Mat& image,	// matrix is Palettized pixels
			  char* pcxfile
			  )
{
	int stat = 0;
	double nrgbr = 63, nrgbg =63, nrgbb = 63;
	int resx = image.cols;
	int resy = image.rows;
	int npal = m_PaletteSize;
	ColorData *colortable = m_table;
//	COctreeNode *octree = m_pOctree;
    PCXhdr          hdr;
//    unsigned char   *buf;
	PCXCOLOR        *pal;
    int             i,k;
//	RGBType			color;
	unsigned char *pPix;

	FILE *pf;
	fopen_s(&pf,pcxfile, "wb");
	if (pf == NULL)
	{
        // fprintf(stderr, "Can't open file %s for writing\n", pcxfile);
		return 1;
    }

    /*
	** Set up the PCX Palettte.
    */
	pal = (PCXCOLOR *)malloc(256*sizeof(PCXCOLOR));
	if(!pal)
	{
		stat = 2;
		goto ESCAPE2;
	}
	for (k = 0; k < npal ; k++)
	{
		pal[k].r = (unsigned char)(colortable[k].color.r);
		pal[k].g = (unsigned char)(colortable[k].color.g);
		pal[k].b = (unsigned char)(colortable[k].color.b);
	}

    /*
	** Fill in the PCX header
    */
	hdr.manuf = (char)10;
    hdr.vers = (char)5;
	hdr.rle = (char)1;
    hdr.bitpx = 8;
	hdr.x1 = hdr.y1 = 0;
	hdr.x2 = resx - 1;
	hdr.y2 = resy - 1;
	hdr.hres = 640;
	hdr.vres = 480;
	hdr.hscreen = 0;
	hdr.vscreen = 0;
    hdr.bpline = hdr.x2;
	hdr.vmode = (char)0;
    hdr.nplanes = 1;
	hdr.palinfo = 1;
    memset(hdr.xtra,0x00,54);

    fwrite(&hdr, sizeof(PCXhdr), 1, pf);

    /*
	** Scan the image and pack pixels for the PCX image
    */

	for (i = 0; i < resy; i++)
	{
		pPix = image.ptr(i);
		/*
		** Encode this scan line and write it to the file.  If return is non-0
		** then the write failed
		*/
		if (encline(pPix, hdr.x2, pf))
		{
			stat = 3;
			goto ESCAPE1;
		}
	}
	/*
	** Store the palette
	*/
#if(0)
	for (i = 0; i < 256; i++)
	{
        pal[i].r *= 4;
        pal[i].g *= 4;
        pal[i].b *= 4;
	}
#endif
	putc(0x0c, pf);
	fwrite(pal, 768, 1, pf);
//.........这里部分代码省略.........

  void NormalizeResponse(
      const cv::Mat& src,
      cv::Mat& dst,
      int winsize,
      int ftzero,
      uchar* buf)
  {
    if (dst.empty())
    {
      dst.create(src.size(), CV_8U);
    }

    // Source code from OpenCV: modules/calib3d/src/stereobm.cpp
    int x, y, wsz2 = winsize / 2;
    int* vsum = reinterpret_cast<int*>(cv::alignPtr(buf + (wsz2 + 1) * sizeof(vsum[0]), 32));
    int scale_g = winsize * winsize / 8, scale_s = (1024 + scale_g) / (scale_g * 2);
    const int OFS = 256 * 5, TABSZ = OFS * 2 + 256;
    uchar tab[TABSZ];
    const uchar* sptr = src.ptr();
    int srcstep = src.step;
    cv::Size size = src.size();

    scale_g *= scale_s;

    for (x = 0; x < TABSZ; x++)
    {
      tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero * 2 : x - OFS + ftzero);
    }

    for (x = 0; x < size.width; x++)
    {
      vsum[x] = (ushort)(sptr[x] * (wsz2 + 2));
    }

    for (y = 1; y < wsz2; y++)
    {
      for (x = 0; x < size.width; x++)
      {
        vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);
      }
    }

    for (y = 0; y < size.height; y++)
    {
      const uchar* top = sptr + srcstep * MAX(y - wsz2 - 1, 0);
      const uchar* bottom = sptr + srcstep * MIN(y + wsz2, size.height - 1);
      const uchar* prev = sptr + srcstep * MAX(y - 1, 0);
      const uchar* curr = sptr + srcstep * y;
      const uchar* next = sptr + srcstep * MIN(y + 1, size.height - 1);
      uchar* dptr = dst.ptr<uchar>(y);

      for (x = 0; x < size.width; x++)
      {
        vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);
      }

      for (x = 0; x <= wsz2; x++)
      {
        vsum[-x-1] = vsum[0];
        vsum[size.width + x] = vsum[size.width - 1];
      }

      int sum = vsum[0] * (wsz2 + 1);
      for( x = 1; x <= wsz2; x++ )
      {
        sum += vsum[x];
      }

      int val = ((curr[0] * 5 + curr[1] + prev[0] + next[0]) * scale_g - sum * scale_s) >> 10;
      dptr[0] = tab[val + OFS];

      for( x = 1; x < size.width-1; x++ )
      {
        sum += vsum[x + wsz2] - vsum[x - wsz2 - 1];
        val = ((curr[x] * 4 + curr[x - 1] + curr[x + 1] + prev[x] + next[x]) * scale_g - sum * scale_s) >> 10;
        dptr[x] = tab[val + OFS];
      }

      sum += vsum[x+wsz2] - vsum[x - wsz2 - 1];
      val = ((curr[x] * 5 + curr[x - 1] + prev[x] + next[x]) * scale_g - sum * scale_s) >> 10;
      dptr[x] = tab[val + OFS];
    }
  }

void PPTestWindow::showResultMap(const cv::Mat& aImage, unsigned int aProgress, unsigned int aSolutionID, double aTotalDist) {
  result_tabs_views.push_back(new Ui::PPResultView);
  result_tabs_widgets.push_back(new QWidget(tabWidget));
  { std::stringstream ss;
  ss << "Result " << aSolutionID;
  tabWidget->addTab(result_tabs_widgets.back(), QString::fromStdString(ss.str()));
  result_tabs_views.back()->setupUi(result_tabs_widgets.back());
  ss << " with " << aProgress << " vertices, achieves a total traveled distance of " << aTotalDist;
  result_tabs_views.back()->label->setText(QString::fromStdString(ss.str())); };
  result_tabs_views.back()->canvas->setScene(new QGraphicsScene(result_tabs_views.back()->canvas));
  result_tabs_pixmaps.push_back(result_tabs_views.back()->canvas->scene()->addPixmap(QPixmap::fromImage(QImage(aImage.ptr(),
                                                                                                               aImage.size().width,
                                                                                                               aImage.size().height,
                                                                                                               QImage::Format_RGB888
                                                                                                              ).rgbSwapped()
                                                                                                       )
                                                                                    )
                               );
  { std::stringstream ss;
  ss << "rrt_result_v" << aProgress << "_s" << aSolutionID << "_d" << aTotalDist << ".png";
  result_names.push_back(ss.str()); };
};

void GaborFilter::filter(const cv::Mat& grayFrame, std::vector<cv::Mat>& projections,
		const cv::Mat& mask, int neighborhood, bool normalize) const {
	cv::Mat dilatedMask;
	if (!mask.empty() && neighborhood > 1) {
		cv::dilate(mask, dilatedMask, cv::Mat(neighborhood, neighborhood, CV_8U, cv::Scalar(255)));
	}
	else {
		dilatedMask = mask;
	}

	int dimensions = kernels.size();
	projections.resize(dimensions);
	for (int k = 0; k < dimensions; ++k) {
		projections[k].create(grayFrame.size(), CV_32F);
		projections[k].setTo(cv::Scalar(0));

		for (int y = 0; y < grayFrame.rows; ++y) {
			const uchar* maskPtr = (!dilatedMask.empty() ? dilatedMask.ptr(y) : NULL);
			float* projPtr = projections[k].ptr<float> (y);

			for (int x = 0; x < grayFrame.cols; ++x) {
				float val = 0;

				if (!maskPtr || maskPtr[x] > 0) {
					for (int dy = -kernelRadius; dy <= kernelRadius; ++dy) {
						int fy = y + dy;

						if (fy >= 0 && fy < grayFrame.rows) {
							const uchar* fPtr = grayFrame.ptr(fy);
							const float* kPtr = kernels[k].ptr<float> (dy + kernelRadius);

							for (int dx = -kernelRadius; dx <= kernelRadius; ++dx) {
								int fx = x + dx;

								if (fx >= 0 && fx < grayFrame.cols) {
									val += fPtr[fx] * kPtr[dx + kernelRadius];
								}
							}
						}
					}
				}

				projPtr[x] = val;
			}
		}

		if (neighborhood > 1) {
			cv::Mat integral;
			getIntegral(projections[k], integral);

			for (int y = 0; y < grayFrame.rows; ++y) {
				const uchar* maskPtr = (!mask.empty() ? mask.ptr(y) : NULL);
				float* projPtr = projections[k].ptr<float> (y);

				int y1 = std::max(y - (neighborhood - 1) / 2, 0);
				int y2 = std::min(y + neighborhood / 2, grayFrame.rows - 1);
				float* integralPtr1 = (y1 > 0 ? integral.ptr<float> (y1) : NULL);
				float* integralPtr2 = integral.ptr<float> (y2);

				for (int x = 0; x < grayFrame.cols; ++x) {
					float val = 0;

					if (!maskPtr || maskPtr[x] > 0) {
						int x1 = std::max(x - (neighborhood - 1) / 2, 0);
						int x2 = std::min(x + neighborhood / 2, grayFrame.cols - 1);

						val += integralPtr2[x2];
						if (integralPtr1) {
							val -= integralPtr1[x2];
						}
						if (x1 > 0) {
							val -= integralPtr2[x1 - 1];
						}
						if (integralPtr1 && x1 > 0) {
							val += integralPtr1[x1 - 1];
						}

						val /= ((x2 - x1) + 1) * ((y2 - y1) + 1);
					}

					projPtr[x] = val;
				}
			}
		}

		if (normalize) {
			cv::normalize(projections[k], projections[k], 0, 1, cv::NORM_MINMAX, -1, mask);
		}
	}
}

	//
	//Write an image to the stream
	void ImageSequenceIO::WriteImageToStream(const cv::Mat &image, const int frameId)
	{
		m_pState->m_ofs.write((char*)&m_pState->m_readFrameId,sizeof(int));
		m_pState->m_ofs.write((const char*)image.ptr(),m_pState->m_writeHeader.totalSize());
	}