openCV识别定位五个圆的标识物进行定位和位姿确定

将以下的五个圆的标识物贴在物体上,对物体进行定位和位姿确定




#include <iostream>


// 载入OpenCV头文件
#include "opencv2/opencv.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp> 
 
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/legacy/legacy.hpp>
#include <opencv2/legacy/compat.hpp>
#include <vector>
using namespace std;
using namespace cv;


//vector<pair<int,int> >centers;
vector<Point2f> centers;
vector<double> longAxisCVec;


struct EllipsePara
{
CvPoint m_Pt;
CvSize m_size;
float m_angle;
};
vector<EllipsePara> Oval;


//Some defines we left out of the book


//void f( 
// IplImage* src, 
// IplImage* dst 
//
//{
// CvMemStorage* storage = cvCreateMemStorage(0);
// CvSeq* comp = NULL;
//
// cvPyrSegmentation( src, dst, storage, &comp, 4, 200, 50 );
// int n_comp = comp->total;
//
// for( int i=0; i<n_comp; i++ ) {
// CvConnectedComp* cc = (CvConnectedComp*) cvGetSeqElem( comp, i );
// // do_something_with( cc );
// }
// cvReleaseMemStorage( &storage );
//}


int main(int argc, char** argv)
{


cvNamedWindow("Example_pre", CV_WINDOW_AUTOSIZE);
cvNamedWindow("Example_post", CV_WINDOW_AUTOSIZE);  
cvNamedWindow("Example", CV_WINDOW_AUTOSIZE); 


const char* filename = "caise6.jpg";
IplImage* src= cvLoadImage(filename, 1);
IplImage* ddd = cvCreateImage( cvGetSize(src), src->depth, src->nChannels);
cvCopy(src,ddd);
/*cvZero(ddd);*/


if(!src) { printf("Couldn't seem to Open %s, sorry\n",argv[1]); return -1;}


cvShowImage( "Example_pre", src);


//IplImage* dst = cvCreateImage( cvGetSize(src), src->depth, src->nChannels);


//IplImage* dst = cvCreateImage( cvSize( src->width*2, src->height*2 ), src->depth, src->nChannels);//用于缩放图像


/*f( src, dst);*/


IplImage* dst = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);//创建目标图像  

cvCvtColor(src,dst,CV_BGR2GRAY);





cvSmooth(dst,dst,CV_GAUSSIAN,3,0,0,0);//模糊处理
cvThreshold( dst, dst, 100, 255, CV_THRESH_BINARY );//二值化

//IplConvKernel *element=cvCreateStructuringElementEx(3,1,0.5,0.5,CV_SHAPE_RECT,0);//自定义核
//cvMorphologyEx( dst, dst, NULL,element, CV_MOP_CLOSE, 8);//形态学开运算
//cvCanny( dst, dst, 10, 100, 3 );//canny边缘检测
cvCanny( dst, dst, 100, 150, 3 );//canny边缘检测

CvMemStorage* storage = cvCreateMemStorage();
/*CvSeq* contour = NULL;*/
CvSeq* contour=cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , storage);
   cvFindContours(dst,
storage, //所获得的轮廓点
&contour,
sizeof(CvContour),
CV_RETR_EXTERNAL, //获取轮廓的方法
CV_CHAIN_APPROX_NONE, cvPoint(0,0)); //轮廓近似的方法


   // Draw current contour.//绘制当前轮廓
   //cvDrawContours(dst,contour,CV_RGB(0,255,255),CV_RGB(0,255,255),0,2,8,cvPoint(0,0));




  for(;contour;contour = contour->h_next)
    {   
CvBox2D32f* box;
CvPoint* PointArray;
CvPoint2D32f* PointArray2D32f;


        int i; // Indicator of cycle.
        int count = contour->total; // This is number point in contour//轮廓个数
        CvPoint center;
        CvSize size;
        
        // Number point must be more than or equal to 6 (for cvFitEllipse_32f).        
        if( count < 6 )
            continue;
        
        // Alloc memory for contour point set.    
        PointArray = (CvPoint*)malloc( count*sizeof(CvPoint) );
        PointArray2D32f= (CvPoint2D32f*)malloc( count*sizeof(CvPoint2D32f) );
        
        // Alloc memory for ellipse data.//分配内存给椭圆数据
        box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
        
        // Get contour point set.
        cvCvtSeqToArray(contour, PointArray, CV_WHOLE_SEQ);
        
        // Convert CvPoint set to CvBox2D32f set.
        for(i=0; i<count; i++)
        {
            PointArray2D32f[i].x = (float)PointArray[i].x;
            PointArray2D32f[i].y = (float)PointArray[i].y;
        }
        
        // Fits ellipse to current contour.//拟合当前轮廓
        cvFitEllipse(PointArray2D32f, count, box);
        
     // Convert ellipse data from float to integer representation.
    
        center.x = cvRound(box->center.x);
        center.y = cvRound(box->center.y);


/*cout<< center.x<<" ";
cout<< center.y<<"\n";*/


        size.width = cvRound(box->size.width*0.5);
        size.height = cvRound(box->size.height*0.5);


/*cout<< size.width<<" ";
cout<< size.width<<"\n";*/


        box->angle = -box->angle;
        
        // Draw ellipse. //画椭圆
//if ((size.height <= 2 * size.width) && size.width < dst->width/32 && size.width > dst->width/128) 
//{
// cvEllipse(ddd, center, size,
// box->angle, 0, 360,
// CV_RGB(255,255,255), 1, CV_AA, 0);
// cout<< center.x<<" "<< center.y<<"\n";
//}


if ((size.width*size.height>30)&&(size.width*size.height<800))//像素面积
{
if ((size.height/size.width<1.01) && (size.height/size.width>0.99))//长短轴比例
{
if ((box->angle<0)&&(box->angle>-180))//角度
{
/* cvEllipse(ddd, center, size,
box->angle, 0, 360,
CV_RGB(255,255,255), 1, CV_AA, 0);*/
cout<< center.x<<" "<< center.y<<"\n";

EllipsePara m_oval;
m_oval.m_Pt=center;
m_oval.m_angle=box->angle;
m_oval.m_size=size;


Oval.push_back(m_oval);


Point2f cen;
cen.x = center.x;
cen.y = center.y;
centers.push_back(cen);//将元素添加到个矢量的末尾。






if(size.height>size.width)
{
longAxisCVec.push_back((double)size.height);
}
else
{
longAxisCVec.push_back((double)size.width);
}


}
}
}



        
        // Free memory.          
        free(PointArray);
        free(PointArray2D32f);
        free(box);
    }




  // 识别拓扑结构
  double distance  = 100.0;
  double radius = 40;
  double sigma = distance/radius;
  Mat distanceMatrix = Mat::zeros((int)Oval.size(),(int)Oval.size(),CV_64F);
  for(int i =0;i<distanceMatrix.rows;i++)
  {
 for(int j =0;j<distanceMatrix.cols;j++)
 {
 double juli1,juli2;
 juli1=Oval[j].m_Pt.x-Oval[i].m_Pt.x;
 juli2=Oval[j].m_Pt.y-Oval[i].m_Pt.y;
 distanceMatrix.at<double>(i,j) = sqrt(pow((juli1),2)+pow((juli2),2))/(longAxisCVec[i]*sigma);
 }
  
  }
  // vector<pair<int,Point2f>> Pointsvec;




   vector<pair<int,EllipsePara>>Ovalpara;//需要找出的5个点参数


  //寻找1点及周围四个点
  int adjacentNum = 0;
  int centerPointIndex = 0;
  bool findSuccess = false;
  vector<int> adjacentIndex;
  for(int i =0;i<distanceMatrix.rows;i++)
  {
 vector<int> IndexVec;
 adjacentNum = 0;
 for(int j =0;j<distanceMatrix.cols;j++)
 {
 double tempnum = distanceMatrix.at<double>(i,j);
 int colindex=0;
 if(distanceMatrix.at<double>(i,j)>0.8&&distanceMatrix.at<double>(i,j)<1.2)
 {
 colindex = j;
 adjacentNum++;
 IndexVec.push_back(colindex);
 }
 }
 if(adjacentNum==4)
 {
 centerPointIndex = i;
findSuccess = true;
adjacentIndex = IndexVec;
 break;
 }
 else
 {
 adjacentIndex.clear();
 }
  }
  if(!findSuccess)
  {
cout<<"can not find the center point!"<<endl;
  }
  //给1号点周围四个点进行区分编号
 // pair<int,Point2f> PointNum1;
pair<int,EllipsePara>PointNum1;


  PointNum1.first = 1;
  PointNum1.second = Oval[centerPointIndex];
  Ovalpara.push_back(PointNum1);


  vector<Point2f> tempdata2;
  vector<EllipsePara> tempdata;


  for(size_t k=0;k<adjacentIndex.size();k++)
  {
tempdata.push_back(Oval[adjacentIndex[k]]);
  }
 
  for(size_t k=0;k<adjacentIndex.size();k++)
  {
 tempdata2.push_back(centers[adjacentIndex[k]]);
  }




  Mat A1 = Mat(tempdata2).reshape(1);
  Mat PointsX = A1.col(0);
  Mat PointsY = A1.col(1);
  double MaxX,MinX,MaxY,MinY;
  //X代表行,Y 代表列
  minMaxIdx(PointsX,&MinX,&MaxX);
  minMaxIdx(PointsY,&MinY,&MaxY);






  for(size_t k=0;k<tempdata.size();k++)
  {
// pair<int,Point2f> tempPoint;
 pair<int,EllipsePara>tempPoint;
if(tempdata[k].m_Pt.x==MinX)
{
tempPoint.first = 2;
tempPoint.second = tempdata[k];
Ovalpara.push_back(tempPoint);
}
if(tempdata[k].m_Pt.x==MaxX)
{
tempPoint.first = 4;
tempPoint.second = tempdata[k];
Ovalpara.push_back(tempPoint);
}
if(tempdata[k].m_Pt.y==MaxY)
{
tempPoint.first = 3;
tempPoint.second = tempdata[k];
Ovalpara.push_back(tempPoint);
}
if(tempdata[k].m_Pt.y==MinY)
{
tempPoint.first = 5;
tempPoint.second = tempdata[k];
Ovalpara.push_back(tempPoint);
}
  }
  //针对找到的五个点进行序号排序,在点集中,排列顺序为1,2,3,4,5点


  for(size_t  i=0;i<Ovalpara.size()-1;i++)
  {
 for (size_t  j = i+1; j<Ovalpara.size(); j++)
 {
 if (Ovalpara[i].first > Ovalpara[j].first)
 {
 swap(Ovalpara[i], Ovalpara[j]);
 }
 }
  }


    //end




vector<pair<int,EllipsePara>>::iterator iter=Ovalpara.begin();
for(;iter!=Ovalpara.end();iter++)
{
cout<<(*iter).first<<" "<<(*iter).second.m_Pt.x<<" "<< (*iter).second.m_Pt.y <<" "<<(*iter).second.m_size.width<<" "<<(*iter).second.m_size.height<<" "<<(*iter).second.m_angle<<endl;

cvEllipse(ddd, (*iter).second.m_Pt, (*iter).second.m_size,
(*iter).second.m_angle, 0, 360,
CV_RGB(0,255,0), 1, CV_AA, 0);


 
}






  /*for (int i=0;i<centers.size();i++)
  {
 cout<<"(x,y):"<< centers[i].first<<" "<<centers[i].second<<endl;


  }*/


  //vector<pair<int,int>>::iterator iter=centers.begin();
  //for(;iter!=centers.end();iter++)
  //{
// cout<<(*iter).first<<" "<<(*iter).second<<endl;
  //}






 


  
//cvShowImage( "Example_pre", src );
cvShowImage( "Example_post", dst );
cvShowImage( "Example", ddd );


while( 1 ) { if( cvWaitKey( 100 ) == 27 ) break; }




//cvReleaseStructuringElement(&element);//释放自定义核




cvReleaseImage( &src );
cvReleaseImage( &dst );
cvReleaseImage( &ddd );
cvClearMemStorage(storage);//收回序列内存块
cvDestroyAllWindows();


return 0;




}
































展开阅读全文
博主设置当前文章不允许评论。

没有更多推荐了,返回首页