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

傷城~ 2021-10-13 00:27 381阅读 0赞

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 }

转载于:https://www.cnblogs.com/lnxcj/p/11212044.html