中山大学夏令营13组激光雷达定位算法代码 

 1 #include<cmath>
 2 #include<cstdio>
 3 #include<algorithm>
 4 #include<iostream>
 5 using namespace std;
 6 #define SIZE 1005
 7 #define INF 0x3f3f3f3f
 8 int n,m,cnt,tot,mrk,tp,num,cnt_M,First_P;
 9 int cnn[SIZE],usd[SIZE],M[SIZE];
10 int stk[SIZE],ans[SIZE];
11 double PIT[SIZE][SIZE];
12 double CNT[SIZE];
13 double dis[SIZE],agl[SIZE];
14 double xp[SIZE],yp[SIZE];
15 double K[SIZE],B[SIZE];
16 double Max_x=-INF,Min_x=INF,Max_y=-INF,Min_y=INF,Max_x_i,Max_y_i,Min_x_i,Min_y_i;
17 struct node{
18     double x;
19     double y;
20     node(){}
21     node(double x,double y):x(x),y(y){}
22 }pos[SIZE],pit[SIZE];
23 const double pi=acos(-1.0);
24 bool compare(double a,double b)    {    return fabs(a-b)<150;    }
25 double changetopi(double s)    {    return (s*pi)/180;    }
26 void change(){
27     double ale1=acos((pit[First_P].y-pit[First_P==m?1:First_P+1].y)/(sqrt((pit[First_P].y-pit[First_P==m?1:First_P+1].y)*(pit[First_P].y-pit[First_P==m?1:First_P+1].y)+(pit[First_P].x-pit[First_P==m?1:First_P+1].x)*(pit[First_P].x-pit[First_P==m?1:First_P+1].x))));
28     double ale2=acos((yp[M[2]]-yp[M[1]])/(sqrt((xp[M[2]]-xp[M[1]])*(xp[M[2]]-xp[M[1]])+(yp[M[2]]-yp[M[1]])*(yp[M[2]]-yp[M[1]]))));
29     double drt=ale2-ale1;
30     double xnw=dis[M[1]]*cos(agl[M[1]]+drt);
31     double ynw=dis[M[1]]*sin(agl[M[1]]+drt);
32     double x0=pit[First_P].x-xnw;
33     double y0=pit[First_P].y-ynw;
34     printf("x=%.4lf    y=%.4lf    theta=%.4lf\n",x0,y0,drt);
35 }
36 void F_ind(){
37     bool flag = false;
38     for(int i=1;i<=m;i++){
39         int jj=1;
40         for(int j=1;j<=m;j++) if(compare(PIT[i][j],CNT[jj+1])) jj++;
41          if(jj>=cnt_M-1){
42             flag=true;
43             First_P=i;
44             break;
45         }
46     }
47     return;
48 }
49 void PREPARATION()
50 {
51     freopen("data.txt","r",stdin);
52     //freopen("13_ans.out","w",stdout);
53     scanf("%d",&m);
54     for(int i=1;i<=m;i++) scanf("%lf%lf",&pit[i].x,&pit[i].y);
55     //按照逆时针读入数据; 
56     for(int i=1;i<=m;i++){
57         int jj=0;
58         for(int j=i+1;j<=m;j++)
59             PIT[i][++jj] = sqrt((pit[i].x-pit[j].x)*(pit[i].x-pit[j].x)+(pit[i].y-pit[j].y)*(pit[i].y-pit[j].y));
60         for(int j=1;j<=i;j++)
61             PIT[i][++jj] = sqrt((pit[i].x-pit[j].x)*(pit[i].x-pit[j].x)+(pit[i].y-pit[j].y)*(pit[i].y-pit[j].y));
62     }
63     scanf("%d",&cnt);
64     for(int i=1;i<=cnt;i++){
65         scanf("%lf%lf%d",&agl[i],&dis[i],&mrk);
66         agl[i]=changetopi(agl[i]);
67         xp[i]=dis[i]*cos(agl[i]);
68         yp[i]=dis[i]*sin(agl[i]);
69         if(yp[i]<=Min_y) Min_y=yp[i],M[1]=i;//Min_y_i=i;
70         if(xp[i]>=Max_x) Max_x=xp[i],M[2]=i;//Max_x_i=i;
71         if(yp[i]>=Max_y) Max_y=yp[i],M[3]=i;//Max_y_i=i;
72         if(xp[i]<=Min_x) Min_x=xp[i],M[4]=i;//Min_x_i=i;
73     }
74     for(int i=1;i<4;i++)
75         for(int j=i+1;j<=4;j++)
76             if(M[i]==M[j])
77                 for(int w=j+1;w<=4;w++)
78                     M[w-1]=M[w];
79     int t;
80     for(int i=4;i>=1;i++)
81         if(M[i]){
82             t=i;
83             break;
84         }
85     cnt_M=t;
86     for(int i=2;i<=cnt_M;i++)
87         CNT[i]=sqrt((xp[M[1]]-xp[M[i]])*(xp[M[1]]-xp[M[i]])+(yp[M[1]]-yp[M[i]])*(yp[M[1]]-yp[M[i]]));
88 }
89 int main(){
90     PREPARATION();
91     F_ind();change();
92     return 0;
93 }

 

posted @ 2019-07-19 11:06  Mr_Handsome  阅读(441)  评论(0)    收藏  举报