opencv中角点排序

// 角点检测.cpp : 定义控制台应用程序的入口点。
//


#include "stdafx.h"
#include <opencv2\opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;
#include <vector>


#define max_corners 100   //初始定义角点个数


typedef struct CornerPoint
{
CvPoint2D32f fp;     
bool flag;           //是否扫描过
int location;        //原数组中的下标
};


//快速排序，分 y方向 和 x 方向
void quickSort(vector<CornerPoint> &s, int l, int r ,bool nIsY=false)   
{  
if(nIsY)
{
if (l< r)  
{        
int i = l, j = r, x = s[l].fp.x,y=s[l].fp.y,locat=s[l].location;  
while (i < j)  
{  
while(i < j && s[j].fp.y>= y) // 从右向左找第一个小于y的数  
j--;   
if(i < j)  
{
s[i].fp=s[j].fp;
s[i++].location=s[j].location;
}
while(i < j && s[i].fp.y< y) // 从左向右找第一个大于等于y的数  
i++;   
if(i < j)
{
s[j].fp = s[i].fp; 
s[j--].location=s[i].location;
}
}  
s[i].fp.x = x;  
s[i].fp.y = y;
s[i].location=locat;
quickSort(s, l, i - 1,true); // 递归调用  
quickSort(s, i + 1, r,true);  
}  
}
else
{
if (l< r)  
{        
int i = l, j = r, x = s[l].fp.x,y=s[l].fp.y,locat=s[l].location;  
while (i < j)  
{  
while(i < j && s[j].fp.x>= x) // 从右向左找第一个小于x的数  
j--;   
if(i < j)  
{
s[i].fp=s[j].fp;
s[i++].location=s[j].location;
}
while(i < j && s[i].fp.x< x) // 从左向右找第一个大于等于x的数  
i++;   
if(i < j)
{
s[j].fp = s[i].fp; 
s[j--].location=s[i].location;
}
}  
s[i].fp.x = x;  
s[i].fp.y = y;
s[i].location=locat;
quickSort(s, l, i - 1); // 递归调用  
quickSort(s, i + 1, r);  
}  
}


}  


//判断是否为矩形，nCount 为判断的点数,thre为判断阈值
bool PredictRect(vector<CornerPoint> vecp,const int &nCount ,const int &thre)
{
//对按x值排好序的点，只需计算x的差值即可
int iCount=0;  //记数
for(int i=0;i<nCount;i++)
{
if(abs(vecp[0].fp.x-vecp[i].fp.x)<3)
{
iCount++;
}
}
if(iCount>thre)
{
return true;
}
return false;
}
//找出一条线上的点
void FindPointByLine(CvPoint2D32f p[],int len,vector<int> &vecIndex)  
{
vecIndex.clear();
CornerPoint ct;
vector<CornerPoint> vecp;
vecp.clear();
for(int i=0;i<len;i++)
{
ct.fp=p[i];
ct.flag=true;
ct.location=i;
vecp.push_back(ct);
}
quickSort(vecp,0,vecp.size()-1);


//判断正方形
if(PredictRect(vecp,4,3))
{
for(int k=0;k<vecp.size();)
{
quickSort(vecp,k,k+3,true);
for(int m=k;m<k+4;m++)
{
vecIndex.push_back(vecp[m].location);
}
k+=4;
}
return ;
}
bool bIsRun=false;
//找出4个点
for(int k=0;k<len;k++)
{


if(vecp[k].flag)
{
vecp[k].flag=false;
vecIndex.push_back(vecp[k].location);
double kt[3]={0,0,0};
int maxKIndex[3]={0,0,0}; //最大斜率值的索引
int nIsfirst=0;


for(int j=0;j<len;j++)
{
//cout<<"遍历的点: "<<j<<endl;
if(j==k)
{
continue;
}
if(vecp[j].flag)
{


if(vecp[k].fp.x-vecp[j].fp.x!=0)
{


double kTmp=(double)(vecp[k].fp.y-vecp[j].fp.y)/(vecp[k].fp.x-vecp[j].fp.x); 


if(nIsfirst<3)
{
kt[nIsfirst]=kTmp;
maxKIndex[nIsfirst]=j;
if(nIsfirst==2)
{
bIsRun=true;
if(kt[0]>kt[1])
{
swap(kt[0],kt[1]);
swap(maxKIndex[0],maxKIndex[1]);
}
if(kt[0]>kt[2])
{
swap(kt[0],kt[2]);
swap(maxKIndex[0],maxKIndex[2]);
}
if(kt[1]>kt[2])
{
swap(kt[1],kt[2]);
swap(maxKIndex[1],maxKIndex[2]);
}


}
nIsfirst++;
}
else
{
//cout<<"临时点<begin>: "<<maxKIndex[0]<<","<<maxKIndex[1]<<","<<maxKIndex[2]<<endl;
if(kTmp<kt[0])
{


swap(kt[0],kt[2]);
swap(maxKIndex[0],maxKIndex[2]);


swap(kt[1],kt[2]);
swap(maxKIndex[1],maxKIndex[2]);
kt[0]=kTmp;
maxKIndex[0]=j;


}
else if(kTmp<kt[1])
{
swap(kt[1],kt[2]);
swap(maxKIndex[1],maxKIndex[2]);
kt[1]=kTmp;
maxKIndex[1]=j;
}
else if(kTmp<kt[2])
{
kt[2]=kTmp;
maxKIndex[2]=j;
}


//cout<<"临时点<end>: "<<maxKIndex[0]<<","<<maxKIndex[1]<<","<<maxKIndex[2]<<endl;
}


}


} //end if




}//end for


if(bIsRun)
{
int tmp1,tmp2,tmp3;
tmp1=maxKIndex[0];
tmp2=maxKIndex[1];
tmp3=maxKIndex[2];
if(tmp1>tmp2)
{
swap(tmp1,tmp2);


}
if(tmp1>tmp3)
{
swap(tmp1,tmp3);
}
if(tmp2>tmp3)
{
swap(tmp2,tmp3);
}
vecIndex.push_back(vecp[tmp1].location);
vecIndex.push_back(vecp[tmp2].location);
vecIndex.push_back(vecp[tmp3].location);
vecp[maxKIndex[0]].flag=false;
vecp[maxKIndex[1]].flag=false;
vecp[maxKIndex[2]].flag=false;
//cout<<"保存的点: "<<maxKIndex[0]<<","<<maxKIndex[1]<<","<<maxKIndex[2]<<endl;
//vecIndex.push_back(maxKIndex[2]);
// vecIndex.push_back(maxKIndex[3]);
bIsRun=false;
}






}//end if




//break;


}
}


//得到按行排序好的角点，nTotal 为估计角点数，nCol为列数
void GetCornerAndSort(IplImage *srcImage,const int &nTotal,const int &nCol)
{
IplImage *grayImage=0;
IplImage *temp1Image=0;
IplImage *temp2Image=0;


int cornerCount=max_corners;
CvPoint2D32f corners[max_corners];


grayImage=cvCreateImage(cvGetSize(srcImage),IPL_DEPTH_8U,1);
cvCvtColor(srcImage,grayImage,CV_BGR2GRAY);


temp1Image=cvCreateImage(cvGetSize(srcImage),IPL_DEPTH_32F,1);
temp2Image=cvCreateImage(cvGetSize(srcImage),IPL_DEPTH_32F,1);


cvGoodFeaturesToTrack(grayImage,temp1Image,temp2Image,corners,&cornerCount,0.05,15,0,3,0,0.4);


if(cornerCount<nTotal)   //若小于一定角点个数，退出
{
return ;
}


vector<int> vecIndex;
FindPointByLine(corners,cornerCount,vecIndex);

//显示
int icount=0;      //点数记数
for(int m=0;m<vecIndex.size()-1;m++)
{
CvPoint tp,tp1;
tp.x=corners[vecIndex[m]].x; 
tp.y=corners[vecIndex[m]].y;
tp1.x=corners[vecIndex[m+1]].x;
tp1.y=corners[vecIndex[m+1]].y;
icount++;
cvCircle(srcImage,tp,3,cvScalar(255,0,0),3,8,0);
cvCircle(srcImage,tp1,3,cvScalar(255,0,0),3,8,0);
if(icount<nCol)
{
cvLine(srcImage,tp,tp1,cvScalar(0,0,255),1,8,0);
}
else
{
icount=0;
}


}

cvNamedWindow("res");
cvShowImage("res",srcImage);
cvWaitKey(0);
cvReleaseImage(&grayImage);
cvReleaseImage(&temp1Image);
cvReleaseImage(&temp2Image);
}


//测试
int _tmain(int argc, _TCHAR* argv[])
{
char*filename="example.bmp";

IplImage *srcImage=cvLoadImage(filename,1);

       if(srcImage == NULL)

return 0;

GetCornerAndSort(srcImage,16,4);
system("pause");
return 0;

}

效果图：