将以下的五个圆的标识物贴在物体上，对物体进行定位和位姿确定
#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; }
