bezier曲线拟合，opencv，车道线拟合

#include <opencv2/opencv.hpp> #include <iostream> using namespace cv; using namespace std; const int WW_MAX_MARK_COUNT = 40; //最大40个控制点 int mark_count =4; int conner_pt_index =-1; Point3d Control_pts[WW_MAX_MARK_COUNT]; //IplImage *image =cvLoadImage("/home/ving/study_code/bezier/img6.jpg",-1);; //原始图像 Mat image; bool is_showControlLines = true; // 两个向量相加，p=p+q Point3d pointAdd(Point3d p, Point3d q){ p.x += q.x; p.y += q.y; p.z += q.z; return p; } // 向量和标量相乘p=c*p Point3d pointTimes(float c, Point3d p){ p.x *= c; p.y *= c; p.z *= c; return p; } // 计算贝塞尔方程的值 // 变量u的范围在0-1之间 //P1*t^3 + P2*3*t^2*(1-t) + P3*3*t*(1-t)^2 + P4*(1-t)^3 = Pnew Point3d Bernstein(float u, Point3d *p){ Point3d a, b, c, d, r; a = pointTimes(pow(u,3), p[0]); b = pointTimes(3*pow(u,2)*(1-u), p[1]); c = pointTimes(3*u*pow((1-u),2), p[2]); d = pointTimes(pow((1-u),3), p[3]); r = pointAdd(pointAdd(a, b), pointAdd(c, d)); return r; } //画控制线 void DrawControlLine(Point3d *p){ Point2i pc[4]; for(int i=0;i<4;i++) { pc[i].x = (int)p[i].x; pc[i].y = (int)p[i].y; } line(image,pc[0],pc[1],CV_RGB(0,0,255),1,CV_AA,0); line(image,pc[2],pc[3],CV_RGB(0,0,255),1,CV_AA,0); } //得到最近Control_pts的index int getNearPointIndex(Point2i mouse_pt) { Point2i pt; for(int i =0; i<mark_count;i++) { pt.x= mouse_pt.x - Control_pts[i].x; pt.y= mouse_pt.y - Control_pts[i].y; float distance = sqrt ((float)( pt.x*pt.x + pt.y*pt.y )); if(distance<10) return i; } return -1; } void on_mouse( int event, int x, int y, int flags, void *param ) { switch(event) { case CV_EVENT_LBUTTONDOWN: { Point2i pt = Point2i(x,y); //cout<<x<<","<<y<<endl; if(conner_pt_index >-1) conner_pt_index = -1; else { conner_pt_index = getNearPointIndex(pt); //添加新的控制点 if(conner_pt_index==-1) { if(mark_count<=(WW_MAX_MARK_COUNT-1)) { Control_pts[mark_count].x = pt.x; Control_pts[mark_count].y = pt.y; Control_pts[mark_count].z = 0; mark_count++; } } } } break; case CV_EVENT_MOUSEMOVE: { if(conner_pt_index >-1) { Control_pts[conner_pt_index].x = x; Control_pts[conner_pt_index].y = y; } } break; } }; int main(int argc, char* argv[]) { image.create(1000,1000,CV_8UC(3)); namedWindow("Win",0); setMouseCallback( "Win", on_mouse, 0 ); resizeWindow("Win",500,500); //初始化四个控制点 Control_pts[0].x = 100; Control_pts[0].y = 100; Control_pts[0].z = 0; Control_pts[1].x = 600; Control_pts[1].y = 800; Control_pts[1].z = 0; Control_pts[2].x = 700; Control_pts[2].y = 960; Control_pts[2].z = 0; Control_pts[3].x = 500; Control_pts[3].y = 100; Control_pts[3].z = 0; int divs = 50; //控制精细度 for(;;) { CvPoint pt_now,pt_pre; //绘制控制点 if(is_showControlLines) { for(int i =0;i<mark_count;i++) { CvPoint ptc; ptc.x = (int) Control_pts[i].x; ptc.y = (int) Control_pts[i].y; circle( image, ptc, 4, CV_RGB(255,0,0), 1,CV_AA, 0); } } //绘制Bezier曲线 Point3d *pControls = Control_pts; for(int j=0;j<mark_count-3;j+=3) { for (int i=0;i<=divs;i++) { float u = (float)i/divs; Point3d newPt = Bernstein(u,pControls); pt_now.x = (int)newPt.x; pt_now.y = (int)newPt.y; if(i>0) line(image,pt_now,pt_pre,CV_RGB(255,0,0),2,CV_AA, 0 ); pt_pre = pt_now; } //画控制线 if(is_showControlLines)DrawControlLine(pControls); pControls+=3; } imshow("Win",image); int keyCode = waitKey(20); if (keyCode==27) break; if(keyCode=='w'||keyCode=='W') divs+=2; if(keyCode=='s'||keyCode=='S') divs-=2; if(keyCode=='z'||keyCode=='Z') is_showControlLines = is_showControlLines^1; //cout<<"precision : "<<divs<<endl; } return 0; }