ORB-SLAM中 ORBextract.cpp代码解读

未经允许 禁止转载

转载请注明  CSDN Min220 原文网址

欢迎学习讨论交流！有误之处指出！

 

using namespace cv;
using namespace std;

namespace ORB_SLAM
{

	const float HARRIS_K = 0.04f;

	const int PATCH_SIZE = 31;
	const int HALF_PATCH_SIZE = 15;
	const int EDGE_THRESHOLD = 16;

	
	/*---------------------------------------get the HarrisResponse-------------------------------------------*/

	static void
		HarrisResponses(const Mat& img, vector<KeyPoint>& pts, int blockSize, float harris_k)
	{
		//CV_Assert() means if the value in() is false,return a mistake message
		CV_Assert( img.type() == CV_8UC1 && blockSize*blockSize <= 2048 );

		size_t ptidx, ptsize = pts.size();

		const uchar* ptr00 = img.ptr<uchar>();        // get the ptr00 row in image
		int step = (int)(img.step/img.elemSize1());   // the number of bytes of a element' one channel
		int r = blockSize/2;                           // circle radius R 

		float scale = (1 << 2) * blockSize * 255.0f;      // 255.0f f means float,we can ignore,1.0f is same
		scale = 1.0f / scale;
		float scale_sq_sq = scale * scale * scale * scale;

		AutoBuffer<int> ofsbuf(blockSize*blockSize);
		int* ofs = ofsbuf;
		for( int i = 0; i < blockSize; i++ )
			for( int j = 0; j < blockSize; j++ )
				ofs[i*blockSize + j] = (int)(i*step + j);

		for( ptidx = 0; ptidx < ptsize; ptidx++ )
		{
			int x0 = cvRound(pts[ptidx].pt.x - r);
			int y0 = cvRound(pts[ptidx].pt.y - r);

			const uchar* ptr0 = ptr00 + y0*step + x0;
			int a = 0, b = 0, c = 0;

			for( int k = 0; k < blockSize*blockSize; k++ )
			{
				const uchar* ptr = ptr0 + ofs[k];
				int Ix = (ptr[1] - ptr[-1])*2 + (ptr[-step+1] - ptr[-step-1]) + (ptr[step+1] - ptr[step-1]);
				int Iy = (ptr[step] - ptr[-step])*2 + (ptr[step-1] - ptr[-step-1]) + (ptr[step+1] - ptr[-step+1]);
				a += Ix*Ix;
				b += Iy*Iy;
				c += Ix*Iy;
			}

			// Harris response function
			pts[ptidx].response = ((float)a * b - (float)c * c -
				harris_k * ((float)a + b) * ((float)a + b))*scale_sq_sq;
		}
	}

	
	/*-----------------------------------get the angles of keypoints------------------------------------------*/
static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
	{
		int m_01 = 0, m_10 = 0;




		// center means a cvRound 2D coordinate in image 


		const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));


		// Treat the center line differently, v=0
		for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
			m_10 += u * center[u];


		// Go line by line in the circuI853lar patch
		int step = (int)image.step1();
		for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
		{
			// Proceed over the two lines 上下和左右两条线同时计算
			int v_sum = 0;
			int d = u_max[v];
			for (int u = -d; u <= d; ++u)
			{
				int val_plus = center[u + v*step], val_minus = center[u - v*step];
				v_sum += (val_plus - val_minus);//计算上下的时候是有符号的，所以这边是减
				m_10 += u * (val_plus + val_minus);//这边加是由于u已经确定好了符号
			}
			m_01 += v * v_sum;
		}


		return fastAtan2((float)m_01, (float)m_10);
	}


	
	/*-----------------------------------compute the descriptors------------------------------------------*/



	const float factorPI = (float)(CV_PI/180.f);
	static void computeOrbDescriptor(const KeyPoint& kpt,
		const Mat& img, const Point* pattern,uchar* desc)
	{
		float angle = (float)kpt.angle*factorPI;
		float a = (float)cos(angle), b = (float)sin(angle);


		const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
		const int step = (int)img.step;


#define GET_VALUE(idx) \
	center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
	cvRound(pattern[idx].x*a - pattern[idx].y*b)]




		for (int i = 0; i < 32; ++i, pattern += 16)
		{
			int t0, t1, val;
			t0 = GET_VALUE(0); t1 = GET_VALUE(1);
			val = t0 < t1;
			t0 = GET_VALUE(2); t1 = GET_VALUE(3);
			val |= (t0 < t1) << 1;
			t0 = GET_VALUE(4); t1 = GET_VALUE(5);
			val |= (t0 < t1) << 2;
			t0 = GET_VA