pyramidgl.cpp

SiftGPU is an implementation of SIFT [1] for GPU. SiftGPU processes pixels parallely to build Gaussi

				glFinish();
				t3 = CLOCK();
				ot1 += (t2 - t1); ot2 += ( t3 - t2);
				std::cout<<int(fcount)<<"\t";
			}
			GLTexImage::UnbindMultiTex(2);


		}
		if(GlobalUtil::_timingO)
		{	
			ts1 += ot1; ts2 += ot2; 
			std::cout << "| \t" << int(ocount) << " :\t(" << ot1 <<",\t" << ot2 << ")\n";
		}
	}
	if(GlobalUtil::_timingS)glFinish();
	if(GlobalUtil::_verbose)
	{
		std::cout<<"#Features:\t"<<_featureNum<<"\n";
	}
}


void PyramidGL::GenerateFeatureDisplayVBO()
{
	//use a big VBO to save all the SIFT box vertices
	int w, h, esize; GLint bsize;
	int nvbo = _octave_num * param._dog_level_num;
	if(_featureDisplayVBO==NULL)
	{
		//initialize the vbos
		_featureDisplayVBO = new GLuint[nvbo];
		_featurePointVBO = new GLuint[nvbo];
		glGenBuffers( nvbo, _featureDisplayVBO );	
		glGenBuffers(nvbo, _featurePointVBO);
	}

	FrameBufferObject fbo;
	glReadBuffer(GL_COLOR_ATTACHMENT0_EXT);
	glDrawBuffer(GL_COLOR_ATTACHMENT0_EXT);
	glActiveTexture(GL_TEXTURE0);
	
	//
	GLTexImage & tempTex = *_descriptorTex;
	//
	for(int i = 0, idx = 0; i < _octave_num; i++)
	{
		for(int j = 0; j < param._dog_level_num; j ++, idx++)
		{
			GLTexImage * ftex  = _featureTex + idx;

			if(_levelFeatureNum[idx]<=0)continue;
			//box display vbo
			int count = _levelFeatureNum[idx]* 10;
			GetAlignedStorageSize(count, _alignment, w, h);
			w = (int)ceil(double(count)/ h);

			//input
			fbo.BindFBO();
			ftex->BindTex();

			//output
			tempTex.AttachToFBO(0);
			GlobalUtil::FitViewPort(w, h);
			//shader
			ShaderMan::UseShaderGenVBO(	(float)ftex->GetImgWidth(),  (float) w, 
				param.GetLevelSigma(j + param._level_min + 1));
			GLTexImage::DrawQuad(0,  (float)w, 0, (float)h);
		
			//
			glBindBuffer(GL_PIXEL_PACK_BUFFER_ARB, _featureDisplayVBO[ idx]);
			glGetBufferParameteriv(GL_PIXEL_PACK_BUFFER_ARB, GL_BUFFER_SIZE, &bsize);
			esize = w*h * sizeof(float)*4;
			//increase size when necessary
			if(bsize < esize) glBufferData(GL_PIXEL_PACK_BUFFER_ARB, esize*3/2,	NULL, GL_STATIC_DRAW_ARB);
			
			//read back if we have enough buffer	
			glGetBufferParameteriv(GL_PIXEL_PACK_BUFFER_ARB, GL_BUFFER_SIZE, &bsize);
			if(bsize >= esize)	glReadPixels(0, 0, w, h, GL_RGBA, GL_FLOAT, 0);
			else glBufferData(GL_PIXEL_PACK_BUFFER_ARB, 0,	NULL, GL_STATIC_DRAW_ARB);
			glBindBuffer(GL_PIXEL_PACK_BUFFER_ARB, 0);


			//copy the texture into vbo
			fbo.BindFBO();
			tempTex.AttachToFBO(0);

			ftex->BindTex();
			ftex->FitTexViewPort();
			ShaderMan::UseShaderCopyKeypoint();
			ftex->DrawQuad();

			glBindBuffer(GL_PIXEL_PACK_BUFFER_ARB,  _featurePointVBO[ idx]);
			glGetBufferParameteriv(GL_PIXEL_PACK_BUFFER_ARB, GL_BUFFER_SIZE, &bsize);
			esize = ftex->GetImgHeight() * ftex->GetImgWidth()*sizeof(float) *4;

			//increase size when necessary
			if(bsize < esize)	glBufferData(GL_PIXEL_PACK_BUFFER_ARB, esize*3/2 ,	NULL, GL_STATIC_DRAW_ARB);

			//read back if we have enough buffer
			glGetBufferParameteriv(GL_PIXEL_PACK_BUFFER_ARB, GL_BUFFER_SIZE, &bsize);
			if(bsize >= esize) glReadPixels(0, 0, ftex->GetImgWidth(), ftex->GetImgHeight(), GL_RGBA, GL_FLOAT, 0);
			else  glBufferData(GL_PIXEL_PACK_BUFFER_ARB, 0,	NULL, GL_STATIC_DRAW_ARB);

			glBindBuffer(GL_PIXEL_PACK_BUFFER_ARB, 0);
			
		}
	}
	glReadBuffer(GL_NONE);
	glFinish();

}





void PyramidNaive::GetFeatureOrientations()
{
	GLTexImage * gtex;
	GLTexImage * stex = NULL;
	GLTexImage * ftex = _featureTex;
	GLTexImage * otex = _orientationTex;
	int sid = 0; 
	int * count	 = _levelFeatureNum;
	float sigma, sigma_step = powf(2.0f, 1.0f/param._dog_level_num);
	FrameBufferObject fbo;
	if(_orientationTex)
	{
		GLenum buffers[] = { GL_COLOR_ATTACHMENT0_EXT, GL_COLOR_ATTACHMENT1_EXT };
		glDrawBuffers(2, buffers);
	}else
	{
		glDrawBuffer(GL_COLOR_ATTACHMENT0_EXT);
	}
	for(int i = 0; i < _octave_num; i++)
	{
		gtex = GetLevelTexture(i+_octave_min, param._level_min + GlobalUtil::_GradientLevelOffset);
		if(GlobalUtil::_SubpixelLocalization || GlobalUtil::_KeepExtremumSign)
			stex = GetBaseLevel(i+_octave_min, DATA_KEYPOINT) + 2;

		for(int j = 0; j < param._dog_level_num; j++, ftex++, otex++, count++, gtex++, stex++)
		{
			if(*count<=0)continue;

			sigma = param.GetLevelSigma(j+param._level_min+1);

			//
			ftex->FitTexViewPort();

			glActiveTexture(GL_TEXTURE0);
			ftex->BindTex();
			glActiveTexture(GL_TEXTURE1);
			gtex->BindTex();
			//
			ftex->AttachToFBO(0);
			if(_orientationTex)		otex->AttachToFBO(1);
			if(!_existing_keypoints && (GlobalUtil::_SubpixelLocalization|| GlobalUtil::_KeepExtremumSign))
			{
				glActiveTexture(GL_TEXTURE2);
				stex->BindTex();
				sid = * stex;
			}
			ShaderMan::UseShaderOrientation(gtex->GetTexID(),
				gtex->GetImgWidth(), gtex->GetImgHeight(),
				sigma, sid, sigma_step, _existing_keypoints);
			ftex->DrawQuad();
	//		glFinish();
			
		}
	}

	GLTexImage::UnbindMultiTex(3);
	if(GlobalUtil::_timingS)glFinish();

	if(_orientationTex)	fbo.UnattachTex(GL_COLOR_ATTACHMENT1_EXT);

}



//to compare with GPU feature list generation
void PyramidNaive::GenerateFeatureListCPU()
{

	FrameBufferObject fbo;
	_featureNum = 0;
	GLTexImage * tex = GetBaseLevel(_octave_min);
	float * mem = new float [tex->GetTexWidth()*tex->GetTexHeight()];
	vector<float> list;
	int idx = 0;
	for(int i = 0; i < _octave_num; i++)
	{
		for(int j = 0; j < param._dog_level_num; j++, idx++)
		{
			tex = GetBaseLevel(_octave_min + i, DATA_KEYPOINT) + j + 2;
			tex->BindTex();
			glGetTexImage(GlobalUtil::_texTarget, 0, GL_RED, GL_FLOAT, mem);
			//tex->AttachToFBO(0);
			//tex->FitTexViewPort();
			//glReadPixels(0, 0, tex->GetTexWidth(), tex->GetTexHeight(), GL_RED, GL_FLOAT, mem);
			//
			//make a list of 
			list.resize(0);
			float * p = mem;
			int fcount = 0 ;
			for(int k = 0; k < tex->GetTexHeight(); k++)
			{
				for( int m = 0; m < tex->GetTexWidth(); m ++, p++)
				{
					if(*p==0)continue;
					if(m ==0 || k ==0 || k >= tex->GetImgHeight() -1 || m >= tex->GetImgWidth() -1 ) continue;
					list.push_back(m+0.5f);
					list.push_back(k+0.5f);
					list.push_back(0);
					list.push_back(1);
					fcount ++;


				}
			}
			if(fcount==0)continue;


			
			GLTexImage * ftex = _featureTex+idx;
			_levelFeatureNum[idx] = (fcount);
			SetLevelFeatureNum(idx, fcount);

			_featureNum += (fcount);


			int fw = ftex->GetImgWidth();
			int fh = ftex->GetImgHeight();

			list.resize(4*fh*fw);

			ftex->BindTex();
			ftex->AttachToFBO(0);
	//		glTexImage2D(GlobalUtil::_texTarget, 0, GlobalUtil::_iTexFormat, fw, fh, 0, GL_BGRA, GL_FLOAT, &list[0]);
			glTexSubImage2D(GlobalUtil::_texTarget, 0, 0, 0, fw, fh, GL_RGBA, GL_FLOAT, &list[0]);
			//
		}
	}
	GLTexImage::UnbindTex();
	delete mem;
	if(GlobalUtil::_verbose)
	{
		std::cout<<"#Features:\t"<<_featureNum<<"\n";
	}
}


void PyramidGL::ReshapeFeatureListCPU()
{
	//make a compact feature list
	//each wit only one orientation

	///download orientations list, and the featue list
	//reshape it and upload it

	FrameBufferObject fbo;
	int i, szmax =0, sz;
	int n = param._dog_level_num*_octave_num;
	for( i = 0; i < n; i++)
	{
		sz = _featureTex[i].GetImgWidth() * _featureTex[i].GetImgHeight();
		if(sz > szmax ) szmax = sz;
	}
	float * buffer = new float[szmax*24];
	float * buffer1 = buffer;
	float * buffer2 = buffer + szmax*4;
	float * buffer3 = buffer + szmax*8;

	glReadBuffer(GL_COLOR_ATTACHMENT0_EXT);
	glDrawBuffer(GL_COLOR_ATTACHMENT0_EXT);


	_featureNum = 0;

#ifdef NO_DUPLICATE_DOWNLOAD
	const double twopi = 2.0*3.14159265358979323846;
	_keypoint_buffer.resize(0);
	float os = _octave_min>=0? float(1<<_octave_min): 1.0f/(1<<(-_octave_min));
	if(_down_sample_factor>0) os *= float(1<<_down_sample_factor); 
	float offset = GlobalUtil::_LoweOrigin? 0 : 0.5f;
#endif

	for(i = 0; i < n; i++)
	{
		if(_levelFeatureNum[i]==0)continue;

		_featureTex[i].AttachToFBO(0);
		_featureTex[i].FitTexViewPort();
		glReadPixels(0, 0, _featureTex[i].GetImgWidth(), _featureTex[i].GetImgHeight(),GL_RGBA, GL_FLOAT, buffer1);
		
		int fcount =0, ocount;
		float * src = buffer1;
		float * orientation  = buffer2;
		float * des = buffer3;
		if(GlobalUtil::_OrientationPack2 == 0)
		{	
			//read back orientations from another texture
			_orientationTex[i].AttachToFBO(0);
			glReadPixels(0, 0, _orientationTex[i].GetImgWidth(), _orientationTex[i].GetImgHeight(),GL_RGBA, GL_FLOAT, buffer2);
			//make the feature list
			for(int j = 0; j < _levelFeatureNum[i]; j++, src+=4, orientation+=4)
			{
				if(_existing_keypoints) 
				{
					des[0] = src[0];
					des[1] = src[1];
					des[2] = orientation[0];
					des[3] = src[3];			
					fcount++;
					des += 4;
				}else
				{
					ocount = (int)src[2];
					for(int k = 0 ; k < ocount; k++, des+=4)
					{
						des[0] = src[0];
						des[1] = src[1];
						des[2] = orientation[k];
						des[3] = src[3];			
						fcount++;
					}
				}
			}
		}else
		{
			_featureTex[i].DetachFBO(0);
			const static double factor  = 2.0*3.14159265358979323846/65535.0;
			for(int j = 0; j < _levelFeatureNum[i]; j++, src+=4)
			{
				unsigned short * orientations = (unsigned short*) (&src[2]);
				if(_existing_keypoints) 
				{
					des[0] = src[0];
					des[1] = src[1];
					des[2] = float( factor* orientations[0]);
					des[3] = src[3];			
					fcount++;
					des += 4;
				}else
				{
					if(orientations[0] != 65535)
					{
						des[0] = src[0];
						des[1] = src[1];
						des[2] = float( factor* orientations[0]);
						des[3] = src[3];			
						fcount++;
						des += 4;

						if(orientations[1] != 65535)
						{
							des[0] = src[0];
							des[1] = src[1];
							des[2] = float(factor* orientations[1]);
							des[3] = src[3];			
							fcount++;
							des += 4;
						}
					}
				}
			}
		}
		//texture size
		SetLevelFeatureNum(i, fcount);
		//
		int nfw = _featureTex[i].GetImgWidth();
		int nfh = _featureTex[i].GetImgHeight();
		int sz = nfh * nfw;
		if(sz > fcount) memset(des, 0, sizeof(float) * (sz - fcount) * 4);
		//std::fill(des, des + 4*(sz - fcount), 0);
		//for(int u = fcount; u < sz; u++, des+=4)
		//{
		//	des[0] = des[1] = des[2] = des[3] = 0;
		//}

		//glDrawPixels or gltexSubImage2D
		//_featureTex[i].AttachToFBO(0);
		//_featureTex[i].FitTexViewPort();
		//glRasterPos2i(0,0);
		//glDrawPixels(nfw, nfh, GL_RGBA, GL_FLOAT, buffer3);
		//glFinish();

		_featureTex[i].BindTex();
		glTexSubImage2D(GlobalUtil::_texTarget, 0, 0, 0, nfw, nfh, GL_RGBA, GL_FLOAT, buffer3);
		_featureTex[i].UnbindTex();

		
#ifdef NO_DUPLICATE_DOWNLOAD
		float oss = os * (1 << (i / param._dog_level_num));
		_keypoint_buffer.resize((_featureNum + fcount) * 4);
		float* ds = &_keypoint_buffer[_featureNum * 4];
		float* fs = buffer3;
		for(int k = 0;  k < fcount; k++, ds+=4, fs+=4)
		{
			ds[0] = oss*(fs[0]-0.5f) + offset;	//x
			ds[1] = oss*(fs[1]-0.5f) + offset;	//y
			ds[3] = (float)fmod(twopi-fs[2], twopi);	//orientation, mirrored
			ds[2] = oss*fs[3];  //scale
		}
#endif
		_levelFeatureNum[i] = fcount;
		_featureNum += fcount;
	}

	delete[] buffer;
	if(GlobalUtil::_verbose)
	{
		std::cout<<"#Features MO:\t"<<_featureNum<<endl;
	}

}


inline void PyramidGL::SetLevelFeatureNum(int idx, int fcount)
{
	int fw, fh;
	GLTexImage * ftex = _featureTex + idx;
	//set feature texture size. normally fh will be one
	GetTextureStorageSize(fcount, fw, fh);
	if(fcount >  ftex->GetTexWidth()*ftex->GetTexHeight())
	{
		if(GlobalUtil::_verbose)
			std::cout<<"Too many features, reallocate texture\n";

		ftex->InitTexture(fw, fh, 0);
		if(_orientationTex)			_orientationTex[idx].InitTexture(fw, fh, 0);

	}
	if(GlobalUtil::_NarrowFeatureTex)
		fh = fcount ==0? 0:(int)ceil(double(fcount)/fw);
	else
		fw = fcount ==0? 0:(int)ceil(double(fcount)/fh);
	ftex->SetImageSize(fw, fh);
	if(_orientationTex)		_orientationTex[idx].SetImageSize(fw, fh);
}

void PyramidNaive::GetSimplifiedOrientation()
{
	//
	int idx = 0;
//	int n = _octave_num  * param._dog_level_num;
	float sigma, sigma_step = powf(2.0f, 1.0f/param._dog_level_num); 
	GLTexImage * ftex = _featureTex;

	FrameBufferObject fbo;
	glDrawBuffer(GL_COLOR_ATTACHMENT0_EXT);
	for(int i = 0; i < _octave_num; i++)
	{
		GLTexImage *gtex = GetLevelTexture(i+_octave_min, 2+param._level_min);
		for(int j = 0; j < param._dog_level_num; j++, ftex++,  gtex++, idx ++)
		{
			if(_levelFeatureNum[idx]<=0)continue;
			sigma = param.GetLevelSigma(j+param._level_min+1);

			//
			ftex->AttachToFBO(0);
			ftex->FitTexViewPort();

			glActiveTexture(GL_TEXTURE0);
			ftex->BindTex();
			glActiveTexture(GL_TEXTURE1);
			gtex->BindTex();

			ShaderMan::UseShaderSimpleOrientation(gtex->GetTexID(), sigma, sigma_step);
			ftex->DrawQuad();
		}
	}

	GLTexImage::UnbindMultiTex(2);

}


#ifdef USE_SSE_FOR_SIFTGPU
	static inline float dotproduct_128d(float * p)
	{
		float z = 0.0f;
		__m128 sse =_mm_load_ss(&z);
		float* pf = (float*) (&sse);
		for( int i = 0; i < 32; i++, p+=4)
		{
			__m128 ps = _mm_loadu_ps(p);
			sse = _mm_add_ps(sse,  _mm_mul_ps(ps, ps));
		}
		return pf[0] + pf[1] + pf[2] + pf[3];

	}
	static inline void multiply_and_truncate_128d(float* p, float m)
	{
		float z = 0.2f;
		__m128 t = _mm_load_ps1(&z);
		__m128 r = _mm_load_ps1(&m);
		for(int i = 0; i < 32; i++, p+=4)
		{
			__m128 ps = _mm_loadu_ps(p);
			_mm_storeu_ps(p, _mm_min_ps(_mm_mul_ps(ps, r), t));
		}
	}
	static inline void multiply_128d(float* p, float m)
	{
		__m128 r = _mm_load_ps1(&m);
		for(int i = 0; i < 32; i++, p+=4)
		{