粒子群算法即PSO是典型的非线性优化算法，之前对这类智能优化算法（粒子群、遗传、退火、鸟群、鱼群、蚁群、各种群。。。）研究过一段时间，这类算法在我看来有个共同的特点——依靠随机产生“可能解”，在迭代的过程中，通过适用度函数fitness function（或称代价函数cost function）在“优良”的“可能解”附近增加随机量，剔除或者减少“劣质”的“可能解”的影响，如此迭代下去，逼近全局最优解，有点达尔文的“优胜劣汰，适者生存”的意思。这类算法的缺点不言而喻，在一定工况下可能陷入局部最优解无法自拔，当然也有各种改进算法，但是我迄今还没遇到，可能是工况都是比较巧的“单峰”的情况吧。同样，对于PSO算法，基础知识不再赘述，只把自己认为重要的点mark下来。（贴一份开源代码：把原本在lidar坐标系下不水平的地面点云，通过PSO求取一个pitch和roll(代码里是一个alpha和beta)将地面点云3D变换后，变成lidar坐标系下水平的，详见paper）
1.PSO_algorithm函数读了几遍后，应该想起来了，整个算法的核心部分就是求取v,，然后用v更新粒子。其中v是所谓的粒子飞行速度，其实就是当前解x与目前的局部（单粒子）最优解personal_best_x和目前的全局（全部粒子）最优解globalbest_x之间的差值error，利用这个error更新当前解x,恨明显就使得x趋向一个“良好”的方向，在趋向的同时（也就是计算v）增加一点随机，保证粒子的多样性，来增加获取最优解的概率。
2.博主在面试的时候，被问到过，在标定lidar的时候，怎么解决pitch和roll的解耦问题的，一时语塞，没说明白。确实，用欧拉角做3D变换的时候，一方面，旋转的顺序不同会导致不同的结果；另一方面，旋转pitch会影响roll，旋转roll会影响pitch。现在总结一下：
a.首先3D变换时旋转顺序肯定是固定的因为3D变换的代码只有一个，所以本标定lidar问题只有唯一解。
b.其次，PSO在任何一次迭代中pitch和roll都是同时计算，同时更新的，也就是说两个参数同时求解，不存在先求pitch后求roll一说，那么问题就类似求最高峰（z最大）的解（x,y）的问题了。
3.还被问到过，你怎么验证你的标定精度的，现总结下：人为产生一个已知倾斜角度pitch和roll的地面点云，因为产生的时候，是先水平，后倾斜，倾斜时也是固定的3D变换顺序，所以也是唯一解，将此地面点云输入到算法中，求解pitch和roll。
1 for(uint32_t i=0;i<ParticleSize;i++) 2 { 3 for (uint32_t j=0;j<2;j++) 4 { 5 v[i][j]=w*v[i][j]+c1*my_rand()*(personalbest_x[i][j]-x[i][j])+
6 c2*my_rand()*(globalbest_x[j]-x[i][j]); // update the velocity 7 if(v[i][j]>vmax) v[i][j]=vmax; // limit the velocity 8 else if(v[i][j]<-vmax) v[i][j]=-vmax; 9 } 10 x[i][0]=x[i][0]+v[i][0]; // update the position 11 x[i][1]=x[i][1]+v[i][1]; 12 }
下面就是整个应用的代码了
1 #include "calculate_alpha_beta.h" 2 3 /*============================================================================== 4 FileName : my_min 5 CreatDate : 2017/4/6 6 Describe : calculate the min data in an array 7 Parameter : 8 Author : cgb 9 ==============================================================================*/ 10 void my_min(double personalbest_faval[],uint32_t num,double *globalbest_faval,uint32_t *index) 11 { 12 for(uint32_t i=0;i<num;i++) 13 { 14 if(personalbest_faval[i]<*globalbest_faval) 15 { 16 *globalbest_faval=personalbest_faval[i]; 17 *index=i; 18 } 19 } 20 } 21 /*============================================================================== 22 FileName : my_rand 23 CreatDate : 2017/4/6 24 Describe : produce a random num 0~1 25 Parameter : 26 Author : cgb 27 ==============================================================================*/ 28 double my_rand(void) 29 { 30 static int flag_once=0; 31 if(0==flag_once) 32 { 33 srand((uint32_t)time(NULL)*65535); 34 flag_once=1; 35 } 36 return ((rand()%100)/99.0); 37 } 38 /*============================================================================== 39 FileName : fit_plane 40 CreatDate : 2017/4/6 41 Describe : least squares fitting plane equation 42 plane equation: Z=a*X+b*Y+c 43 Parameter : 44 Author : cgb 45 ==============================================================================*/ 46 double* fit_plane(pointVector points) 47 { 48 double X1=0,Y1=0,Z1=0,X2=0,Y2=0,X1Y1=0,X1Z1=0,Y1Z1=0,C,D,E,G,H; 49 uint64_t N ,point_num=points.size(); 50 static double para[3]; 51 if (point_num<3) 52 { 53 para[0]=-8888; 54 para[1]=-8888; 55 para[2]=-8888; 56 cout<<"**** less than 3 points can't fit the plane ****"<<endl; 57 } 58 else 59 { 60 for (uint64_t i=0 ;i<point_num;i++) 61 { 62 X1=X1+points[i].x; 63 Y1=Y1+points[i].y; 64 Z1=Z1+points[i].z; 65 X2=X2+points[i].x*points[i].x; 66 Y2=Y2+points[i].y*points[i].y; 67 X1Y1=X1Y1+points[i].x*points[i].y; 68 X1Z1=X1Z1+points[i].x*points[i].z; 69 Y1Z1=Y1Z1+points[i].y*points[i].z; 70 } 71 72 N=point_num; 73 C=N*X2-X1*X1; // X2!=X1*X1 !!! 74 D=N*X1Y1-X1*Y1; 75 E=-(N*X1Z1-X1*Z1); 76 G=N*Y2-Y1*Y1; 77 H=-(N*Y1Z1-Y1*Z1); 78 79 para[0]=(H*D-E*G)/(C*G-D*D); 80 para[1]=(H*C-E*D)/(D*D-G*C); 81 para[2]=(Z1-para[0]*X1-para[1]*Y1)/N; 82 } 83 return para; 84 } 85 void trans_cloud(CloudPtr_xyzrgb cloud_hdl, CloudPtr_xyzrgb cloud_veh_hdl, trans_t trans) 86 { 87 /* calibrate horizontal plane to vehicle angle */ 88 Eigen::Matrix4f trans_matrix = Eigen::Matrix4f::Identity(); 89 float alpha = trans.alpha * M_PI / 180.f; 90 float beta = trans.beta * M_PI / 180.f; 91 float gamma = trans.gamma * M_PI / 180.f; 92 float x_offset = trans.x_offset; 93 float y_offset = trans.y_offset; 94 float z_offset = trans.z_offset; 95 96 trans_matrix(0,0) = (float)(cos(beta) * cos(gamma)); 97 trans_matrix(0,1) = (float)(sin(alpha) * sin(beta) * cos(gamma) - 98 cos(alpha) * sin(gamma)); 99 trans_matrix(0,2) = (float)(cos(alpha) * sin(beta) * cos(gamma) + 100 sin(alpha) * sin(gamma)); 101 trans_matrix(0,3) = (float)x_offset; 102 trans_matrix(1,0) = (float)(cos(beta) * sin(gamma)); 103 trans_matrix(1,1) = (float)(sin(alpha) * sin(beta) * sin(gamma) + 104 cos(alpha) * cos(gamma)); 105 trans_matrix(1,2) = (float)(cos(alpha) * sin(beta) * sin(gamma) - 106 sin(alpha) * cos(gamma)); 107 trans_matrix(1,3) = (float)y_offset; 108 trans_matrix(2,0) = -(float)sin(beta); 109 trans_matrix(2,1) = (float)(sin(alpha) * cos(beta)); 110 trans_matrix(2,2) = (float)(cos(alpha) * cos(beta)); 111 trans_matrix(2,3) = (float)z_offset; 112 trans_matrix(3,0) = 0; 113 trans_matrix(3,1) = 0; 114 trans_matrix(3,2) = 0; 115 trans_matrix(3,3) = 1.0; 116 pcl::transformPointCloud(*cloud_hdl, *cloud_veh_hdl, trans_matrix); 117 } 118 /*============================================================================== 119 FileName : fitness_function 120 CreatDate : 2017/4/6 121 Describe : 122 Parameter : 123 Author : cgb 124 ==============================================================================*/ 125 double fitness_function(double alpha, double beta, CloudPtr_xyzrgb cloud_in) 126 { 127 CloudPtr_xyzrgb cloud_out( new pcl::PointCloud<pcl::PointXYZRGB>() ); 128 double *para,levelness; 129 trans_t trans; 130 trans.alpha=alpha; 131 trans.beta=beta; 132 133 trans_cloud(cloud_in, cloud_out, trans); 134 para = fit_plane(cloud_out->points); 135 levelness=fabs(para[0])+fabs(para[1]); 136 137 return levelness; 138 } 139 /*============================================================================== 140 FileName : PSO_algorithm 141 CreatDate : 2017/4/6 142 Describe : calibrate lidar pitch & roll angle with PSO algorithm 143 Parameter : Name ----------- Content --------------- Recommend Value 144 E0 ------------- allowed error --------- 0.001 145 MaxNum --------- max num of iteration -- 200 146 narvs ---------- num of x -------------- 147 ParticleSize --- size of particle ------ 30 148 c1 ------------- personal study para --- 2 149 c2 ------------- social study para ----- 2 150 w ----------- weight ---------------- 0.6 151 vmax ----------- max flying vel -------- 0.8 152 Author : cgb 153 ==============================================================================*/ 154 pso_out_t PSO_algorithm(CloudPtr_xyzrgb cloud) 155 { 156 mtime_test(0); 157 uint32_t index,iteration_cnt=0,ParticleSize=50; 158 double MaxNum,c1,c2,vmax,w,E0,x1_range,x2_range,globalbest_faval; 159 double v[ParticleSize][2],x[ParticleSize][2],personalbest_x[ParticleSize][2],globalbest_x[2]; 160 double f[ParticleSize],personalbest_faval[ParticleSize]; 161 pso_out_t pso_out; 162 163 //PSO parameter 164 MaxNum = 200; 165 c1 = 2; 166 c2 = 2; 167 w = 0.6; 168 vmax = 0.8; 169 E0 = 0.00000001; 170 171 x1_range =45; 172 x2_range =45; 173 // init 174 for (uint32_t i=0;i<ParticleSize;i++) 175 { 176 x[i][0]=my_rand()*x1_range*2-x1_range; // init the position by random 177 x[i][1]=my_rand()*x2_range*2-x2_range; 178 v[i][0]=my_rand()*2; // init the velocity by random 179 v[i][1]=my_rand()*2; 180 } 181 // first calculation 182 for (uint32_t i=0;i<ParticleSize;i++) 183 { 184 f[i]=fitness_function(x[i][0],x[i][1],cloud); 185 personalbest_x[i][0]=x[i][0]; 186 personalbest_x[i][1]=x[i][1]; 187 personalbest_faval[i]=f[i]; 188 } 189 190 globalbest_faval=personalbest_faval[0]; //init the globalbest_faval 191 my_min(personalbest_faval,ParticleSize,&globalbest_faval,&index); 192 globalbest_x[0]=personalbest_x[index][0]; // get the global best val 193 globalbest_x[1]=personalbest_x[index][1]; 194 195 // loop calculation 196 while(iteration_cnt<MaxNum) 197 { 198 for(uint32_t i=0;i<ParticleSize;i++) // calculate the best val in local 199 { 200 f[i]=fitness_function(x[i][0],x[i][1],cloud); 201 if (f[i]<personalbest_faval[i]) 202 { 203 personalbest_faval[i]=f[i]; 204 personalbest_x[i][0]=x[i][0]; 205 personalbest_x[i][1]=x[i][1]; 206 } 207 } 208 my_min(personalbest_faval,ParticleSize, 209 &globalbest_faval,&index); // calculate the best val in global 210 globalbest_x[0]=personalbest_x[index][0]; // get the global best val 211 globalbest_x[1]=personalbest_x[index][1]; 212 213 for(uint32_t i=0;i<ParticleSize;i++) 214 { 215 for (uint32_t j=0;j<2;j++) 216 { 217 v[i][j]=w*v[i][j]+c1*my_rand()*(personalbest_x[i][j]-x[i][j])+c2*my_rand()* 218 (globalbest_x[j]-x[i][j]); // update the velocity 219 if(v[i][j]>vmax) v[i][j]=vmax; // limit the velocity 220 else if(v[i][j]<-vmax) v[i][j]=-vmax; 221 } 222 x[i][0]=x[i][0]+v[i][0]; // update the position 223 x[i][1]=x[i][1]+v[i][1]; 224 } 225 if (fabs(globalbest_faval)<E0) 226 { 227 break; 228 } 229 iteration_cnt++; 230 } 231 232 double *para; 233 CloudPtr_xyzrgb cloud_out( new pcl::PointCloud<pcl::PointXYZRGB>() ); 234 trans_t trans; 235 trans.alpha=globalbest_x[0]; 236 trans.beta=globalbest_x[1]; 237 trans_cloud(cloud, cloud_out, trans); 238 para = fit_plane(cloud_out->points); 239 240 pso_out.z_offset = -para[2]; 241 pso_out.alpha = globalbest_x[0]; 242 pso_out.beta = globalbest_x[1]; 243 pso_out.levelness = globalbest_faval; 244 pso_out.iteration_cnt = iteration_cnt; 245 246 cout<<" ----------------------- END ----------------------- "<<endl; 247 cout.precision(5);cout<<setiosflags(ios::showpoint); 248 cout<<"alpha is : "<<pso_out.alpha<<" deg"<<endl; 249 cout<<"beta is : "<<pso_out.beta<<" deg"<<endl; 250 cout<<"z_offset is : "<<pso_out.z_offset<<" m"<<endl; 251 cout<<"levelness : "<<pso_out.levelness<<endl; 252 cout<<"iteration cnt : "<<pso_out.iteration_cnt<<endl; 253 mtime_test(1); 254 cout<<" ---------------------------------------------------"<<endl; 255 return pso_out; 256 } 257 /*============================================================================== 258 FileName : calculate_alpha_beta 259 CreatDate : 2017/4/6 260 Describe : 261 Parameter : frameID--the PCD file to calculate alpha & beta 262 Author : cgb 263 ==============================================================================*/ 264 void calculate_alpha_beta(uint32_t frameID, scale_t scale) 265 { 266 CloudPtr_xyzrgb cloud_final(new pcl::PointCloud<pcl::PointXYZRGB>()); 267 CloudPtr_xyzrgb cloud_raw (new pcl::PointCloud<pcl::PointXYZRGB>()); 268 trans_t trans;//must use the init trans_t value 269 Viewer viewer; 270 viewer_init(viewer); 271 272 load_PCD(frameID, cloud_final, cloud_raw, scale ,trans); 273 274 pso_out_t pso_put=PSO_algorithm(cloud_final); 275 276 viewer_update(viewer,cloud_final); 277 viewer_waitstop(viewer); 278 279 uint64_t num_add= plot_plane(cloud_final,255,0,0,30); 280 viewer_update(viewer,cloud_final); 281 viewer_waitstop(viewer); 282 283 erase_addpoints(cloud_final,num_add); 284 trans.alpha = pso_put.alpha; 285 trans.beta = pso_put.beta; 286 trans.z_offset = pso_put.z_offset; 287 trans_cloud(cloud_final,cloud_final,trans); 288 plot_plane(cloud_final,0,255,0,30); 289 viewer_update(viewer,cloud_final); 290 viewer_waitstop(viewer); 291 }