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

    void serialize(Archive & ar, ::cv::Mat_<T>& m, const unsigned int version)
    {
      if(Archive::is_loading::value == true)
      {
        int cols, rows;
        size_t elem_size, elem_type;

        ar & cols;
        ar & rows;
        ar & elem_size;
        ar & elem_type;

        m.create(rows, cols);

        size_t data_size = m.cols * m.rows * elem_size;
        ar & boost::serialization::make_array(m.ptr(), data_size);
      }
      else
      {
        size_t elem_size = m.elemSize();
        size_t elem_type = m.type();

        ar & m.cols;
        ar & m.rows;
        ar & elem_size;
        ar & elem_type;

        const size_t data_size = m.cols * m.rows * elem_size;
        ar & boost::serialization::make_array(m.ptr(), data_size);
      }
    }

int NewlyExtractAndComputeSubImageDaisyDescriptorsCost( daisy *desc, cv::Mat_<float> &costOut, const cv::Mat_<float> &descRef, float oy, float ox, int h, int w, float dy, float dx, float step, int ori)
{
	float ssinOri, scosOri;
	ssinOri = step*sin((float)ori/180.0*M_PI);
	scosOri = step*cos((float)ori/180.0*M_PI);

	//printf("%f %f %f y = %f x = %f fl = %f %f\n", step, ssinOri, scosOri, oy, ox, dy, dx);
	cv::Matx23f tranMat(scosOri, -ssinOri, dx, ssinOri, scosOri, dy);
	int cy, cx;
	int py, px;
	py = oy; 
	for (cy=0; cy<h; ++cy, ++py)
	{
		float *ptr = (float *)(descRef.ptr(cy*w));
		px = ox;
		for (cx=0; cx<w; ++cx, ++px)
		{
			cv::Matx31f p0(px, py, 1.0);
			cv::Matx21f p1 = tranMat*p0;
			//cv::Matx21f p1(px+dx, py+dy);
			// printf("%f %f dx = %f dy = %f px = %d py = %d oy = %f ox = %f\n", p1.val[0], p1.val[1], dx, dy, px, py, oy, ox);
			costOut[cy][cx] = desc->ExtractAndCalculateTheDistanceFromNormalizedHistogram(ptr, p1.val[1], p1.val[0], ori);
			ptr += DAISY_FEATURE_LENGTH;
		}
	}
	return 1;
}

 void find_zeros(const cv::Mat_<T>& binary, std::vector<cv::Point> &idx) {
     assert(binary.cols > 0 && binary.rows > 0 && binary.channels() == 1 && binary.channels() == 1);
     const int M = binary.rows;
     const int N = binary.cols;
     for (int m = 0; m < M; ++m) {
         const T* bin_ptr = (T*) binary.ptr(m);
         for (int n = 0; n < N; ++n) {
             if (bin_ptr[n] == 0) idx.push_back(cv::Point(n,m));
         }
     }
 }

int ExtractAndComputeSubImageDaisyDescriptorsCost( daisy *desc, cv::Mat_<float> &costOut, cv::Mat_<float> &descRef, float py, float px, float step, int ori, int h, int w, cv::Vec4f &tmpFl )
{
	//printf("here 0 %d %d py = %f px = %f\n", w, h, py, px);
	//descOut.create(w*h, DAISY_FEATURE_LENGTH);
	//printf("here 0-1\n");
	//descOut.setTo(cv::Scalar(0.0));
	//printf("here 0-2\n");
	int iy, ix;
	float cy, cx, ssinOri, scosOri;
	ssinOri = step*sin((float)ori/180.0*M_PI);
	scosOri = step*cos((float)ori/180.0*M_PI);
	float ry, rx; // the first pos of each row
	ry = py; rx = px;
	//printf("here 1\n");
	//float *thor = new float [DAISY_FEATURE_LENGTH];
	cv::Matx23f tranMat(scosOri, -ssinOri, tmpFl[0], 
		ssinOri, scosOri, tmpFl[1]);

	for (iy=0; iy<h; ++iy)
	{
		cy = ry; cx = rx;
		float *ptr = (float *)(descRef.ptr(iy*w));
		//printf("here 2 %f %f %f %f\n", cy, cx, ssinOri, scosOri);

		for (ix=0; ix<w; ++ix)
		{			
			//memset(thor, 0, sizeof(float)*desc->descriptor_size());
			// desc->get_descriptor(std::min<double>(limH-1.0, std::max<double>(cy, 0.0)), std::min<double>(limW-1.0, std::max<double>(cx, 0.0)), ori, ptr);
			costOut[iy][ix] = desc->ExtractAndCalculateTheDistanceFromNormalizedHistogram(ptr, cy, cx, ori);
			
			//cv::Matx31f p0(px-tmpFl[0]+ix, py-tmpFl[1]+iy, 1.0);
			//cv::Matx21f p1 = tranMat*p0;
			//printf("iy = %d ix = %d, cy = %f cx = %f, ori = %d, p1 = %f %f, ptr = %p\n", iy, ix, cy, cx, ori, p1.val[1], p1.val[0], ptr);
			//printf("py = %f px = %f, fl = %f %f, diff = %f %f\n", py, px, tmpFl[0], tmpFl[1], p1.val[0]-cx, p1.val[1]-cy);
			//costOut[iy][ix] = desc->ExtractAndCalculateTheDistanceFromNormalizedHistogram(ptr, p1.val[1], p1.val[0], ori);
			ptr += DAISY_FEATURE_LENGTH;
			//printf("here 2-1\n");
			//memcpy(ptr, thor, sizeof(float)*DAISY_FEATURE_LENGTH);
			//printf("here 2-2\n");
			//printf("here 2-3\n");
			cy = cy+ssinOri;
			cx = cx+scosOri;
		}
		ry = ry+scosOri;
		rx = rx-ssinOri;
		//printf("here 2\n");
	}
	//delete [] thor;
	//printf("here 3\n");
	return 1;
}

  inline void
  cvToCloudOrganized(const cv::Mat_<cv::Point3f>& points3d, pcl::PointCloud<PointT>& cloud, size_t width, size_t height,
                     const cv::Mat& mask = cv::Mat())
  {
    cloud.points.resize(width * height);
    cloud.width = width;
    cloud.height = height;

    for (size_t v = 0; v < height; ++v)
    {
      const float * begin = reinterpret_cast<const float*>(points3d.ptr(v));for (size_t u = 0; u < width; ++u)
      {
        PointT& p = cloud(u, v);
        p.x = *(begin++);
        p.y = *(begin++);
        p.z = *(begin++);
      }
    }
  }

int ExtractSubImageDaisyDescriptors(daisy *desc, cv::Mat_<float> &descOut, float py, float px, float step, int ori, int h, int w, float limH, float limW)
{
	//printf("here 0 %d %d py = %f px = %f\n", w, h, py, px);
	descOut.create(w*h, DAISY_FEATURE_LENGTH);
	//printf("here 0-1\n");
	descOut.setTo(cv::Scalar(0.0));
	//printf("here 0-2\n");
	int iy, ix;
	float cy, cx, ssinOri, scosOri;
	ssinOri = step*sin((float)ori/180.0*M_PI);
	scosOri = step*cos((float)ori/180.0*M_PI);
	float ry, rx; // the first pos of each row
	ry = py; rx = px;
	//printf("here 1\n");
	//float *thor = new float [DAISY_FEATURE_LENGTH];
	for (iy=0; iy<h; ++iy)
	{
		cy = ry; cx = rx;
		float *ptr = (float *)(descOut.ptr(iy*w));
		//printf("here 2 %f %f %f %f\n", cy, cx, ssinOri, scosOri);
		
		for (ix=0; ix<w; ++ix)
		{			
			//memset(thor, 0, sizeof(float)*desc->descriptor_size());
			//desc->get_descriptor(std::min<double>(limH-1.0, std::max<double>(cy, 0.0)), std::min<double>(limW-1.0, std::max<double>(cx, 0.0)), ori, ptr);
			desc->ExtractDescriptorFromNormalizedHistogram(ptr, std::min<double>(limH-1.0, std::max<double>(cy, 0.0)), std::min<double>(limW-1.0, std::max<double>(cx, 0.0)), ori);
			ptr += DAISY_FEATURE_LENGTH;
			//printf("here 2-1\n");
			//memcpy(ptr, thor, sizeof(float)*DAISY_FEATURE_LENGTH);
			//printf("here 2-2\n");
			//printf("here 2-3\n");
			cy = cy+ssinOri;
			cx = cx+scosOri;
		}
		ry = ry+scosOri;
		rx = rx-ssinOri;
		//printf("here 2\n");
	}
	//delete [] thor;
	//printf("here 3\n");
	return 1;
}

void CostBoxFilter::TheBoxFilter( const cv::Mat_<float> &bIn, cv::Mat_<float> &bOut, int radius )
{
	int height, width, iy, ix;
	height = bIn.rows;
	width = bIn.cols;

	bOut.create(height, width);
	cv::Mat_<float> cumHorSum(height, width);
	float *horSum = new float [width];
	for (iy=0; iy<height; ++iy)
	{
		float *bPtr = (float *)(bIn.ptr(iy));
		float *hPtr = horSum;
		float s = 0.0f;
		for (ix=0; ix<width; ++ix)
		{
			//s += bIn[iy][ix];
			//horSum[ix] = s;
			s += *bPtr++;
			*hPtr++ = s;
		}

		float *ptrR = horSum+radius;
		float *dPtr = (float *)(cumHorSum.ptr(iy));
		for (ix=0; ix<=radius; ++ix)
			*dPtr++ = *ptrR++;
		// cumHorSum[iy][ix] = horSum[ix+radius];


		float *ptrL = horSum;
		for (; ix<width-radius; ++ix)
			*dPtr++ = *ptrR++-*ptrL++;
		//cumHorSum[iy][ix] = horSum[ix+radius]-horSum[ix-radius-1];

		--ptrR;
		for (; ix<width; ++ix)
			*dPtr++ = *ptrR-*ptrL++;
		//cumHorSum[iy][ix] = horSum[width-1]-horSum[ix-radius-1];
	}

	const int W_FAC = width;
	float *colSum = new float [height];
	for (ix=0; ix<width; ++ix)
	{
		float s = 0.0f;
		float *cuPtr = (float *)(cumHorSum.ptr(0))+ix;
		float *coPtr = colSum;
		for (iy=0; iy<height; ++iy, cuPtr+=W_FAC)
		{
			//s += cumHorSum[iy][ix];
			//colSum[iy] = s;
			s += *cuPtr;
			*coPtr++ = s;
		}

		float *ptrD = colSum+radius;
		float *dPtr = (float *)(bOut.ptr(0))+ix;

		for (iy=0; iy<=radius; ++iy, dPtr+=W_FAC)
			//bOut[iy][ix] = colSum[iy+radius];
			*dPtr = *ptrD++;

		float *ptrU = colSum;
		for (; iy<height-radius; ++iy, dPtr+=W_FAC)
			//bOut[iy][ix] = colSum[iy+radius]-colSum[iy-radius-1];
			*dPtr = *ptrD++-*ptrU++;

		--ptrD;
		for (; iy<height; ++iy, dPtr+=W_FAC)
			// bOut[iy][ix] = colSum[height-1]-colSum[iy-radius-1];
			*dPtr = *ptrD-*ptrU++;
	}

	delete [] horSum;
	delete [] colSum;
}

void CFFilter::FastCLMF0FloatFilterPointer( const cv::Mat_<cv::Vec4b> &crMap, const cv::Mat_<float> &src, cv::Mat_<float> &dst )
{
//	qx_timer tt;
//	tt.start();

	int iy, ix, width, height;
	width = crMap.cols;
	height = crMap.rows;

	cv::Mat_<float> cost = src;

	// first iteration
	cv::Mat_<float> crossHorSum(height, width);
	cv::Mat_<int> sizeHorSum(height, width);
	cv::Mat_<float> crossHorSumTranspose(width, height);
	cv::Mat_<int> sizeHorSumTranspose(width, height);
	cv::Mat_<cv::Vec4b> crMapTranspose(width, height);
	cv::transpose(crMap, crMapTranspose);

	float *horSum = new float [width+1];
	float *rowSizeHorSum = new float [width+1];
	float *verSum = new float [height+1];
	int *colSizeVerSum = new int [height+1];

	float *costPtr = (float *)(cost.ptr(0));
	float *crossHorPtr = (float *)(crossHorSum.ptr(0));
	int *sizeHorPtr = (int *)(sizeHorSum.ptr(0));
	cv::Vec4b *crMapPtr = (cv::Vec4b *)(crMap.ptr(0));

	for (iy=0; iy<height; ++iy)
	{
		float s = 0.0;		
		float *horPtr = horSum;
		*horPtr++ = s;
		for (ix=0; ix<width; ++ix)
		{
			s += *costPtr++;
			*horPtr++ = s;
		}


		for (ix=0; ix<width; ++ix)
		{
			cv::Vec4b cross = *crMapPtr++;
			*crossHorPtr++ = horSum[ix+cross[2]+1]-horSum[ix-cross[0]];
			*sizeHorPtr++ = cross[2]+cross[0]+1;
		}
	}


	cv::transpose(crossHorSum, crossHorSumTranspose);
	cv::transpose(sizeHorSum, sizeHorSumTranspose);
	crossHorPtr = (float *)(crossHorSumTranspose.ptr(0));
	sizeHorPtr = (int *)(sizeHorSumTranspose.ptr(0));

	cv::Mat_<float> crossVerSum(height, width);
	cv::Mat_<int> sizeVerSum(height, width);
	cv::Mat_<float> crossVerSumTranpose(width, height);
	cv::Mat_<int> sizeVerSumTranpose(width, height);
	float *crossVerPtr = (float *)(crossVerSumTranpose.ptr(0));
	int *sizeVerPtr = (int *)(sizeVerSumTranpose.ptr(0));

	crMapPtr = (cv::Vec4b *)(crMapTranspose.ptr(0));
	const int W_FAC = width;
	for (ix=0; ix<width; ++ix)
	{
		float s = 0.0;
		int cs = 0;	
		

		float *verPtr = verSum;
		int *colSizeVerPtr = colSizeVerSum;
		*verPtr++ = s;
		*colSizeVerPtr++ = cs;
		for (iy=0; iy<height; ++iy)
		{
			s += *crossHorPtr++;
			*verPtr++ =s;
			cs += *sizeHorPtr++;
			*colSizeVerPtr++ = cs;
		}


		for (iy=0; iy<height; ++iy)
		{
			cv::Vec4b cross = *crMapPtr++;
			*crossVerPtr++ = verSum[iy+cross[3]+1]-verSum[iy-cross[1]];
			*sizeVerPtr++ = colSizeVerSum[iy+cross[3]+1]-colSizeVerSum[iy-cross[1]];
		}
	}

	cv::transpose(crossVerSumTranpose, crossVerSum);
	cv::transpose(sizeVerSumTranpose, sizeVerSum);

	// second iteration
	crossVerPtr = (float *)(crossVerSum.ptr(0));
	sizeVerPtr = (int *)(sizeVerSum.ptr(0));
	crossHorPtr = (float *)(crossHorSum.ptr(0));
	sizeHorPtr = (int *)(sizeHorSum.ptr(0));
	crMapPtr = (cv::Vec4b *)(crMap.ptr(0));
//.........这里部分代码省略.........