00001
00002
00003
00004
00008 void ParticleFilter3D::initializeNormalRandom(unsigned int NumParticles, double mx, double my, double mz, double mroll, double mpitch, double myaw,
00009 double vx, double vy, double vz, double vroll, double vpitch, double vyaw)
00010 {
00011 for(unsigned int i=0;i<NumParticles;i++){
00012
00013 double x = mx + myrand.normalRandom() * vx;
00014 double y = my + myrand.normalRandom() * vy;
00015 double z = mz + myrand.normalRandom() * vz;
00016
00017 double roll = mroll + myrand.normalRandom() * vroll;
00018 double pitch = mpitch + myrand.normalRandom() * vpitch;
00019 double yaw = myaw + myrand.normalRandom() * vyaw;
00020
00021 PoseParticle P(x,y,z,roll,pitch,yaw);
00022 P.lik = 1.0;
00023 P.p = 1.0 / (double) NumParticles;
00024 pcloud.push_back(P);
00025 }
00026 }
00027
00037 void ParticleFilter3D::SIRUpdate(){
00038 std::vector<PoseParticle> tmp;
00039 tmp.resize(pcloud.size());
00040 double U=0,Q=0;
00041 int i=0,j=0,k=0;
00042
00043 int NumOfParticles = pcloud.size();
00044 U = myrand.uniformRandom() / (double) NumOfParticles;
00045
00046
00047
00048 while(U < 1.0){
00049
00050 if(Q>U){
00051 U += 1.0/(double)NumOfParticles;
00052
00054 if(k>=NumOfParticles || i>=NumOfParticles){
00055 fprintf(stderr,"SIR error i=%d k=%d N=%d",i,k,NumOfParticles);
00056 break;
00057 }
00058 tmp[i]=pcloud[k];
00059 tmp[i].p = 1.0 / (double)NumOfParticles;
00060 i++;
00061 }
00062 else{
00063 j++;
00064 k=j;
00065
00066 if(j>=NumOfParticles){
00067
00068 break;
00069 }
00070 Q += pcloud[j].p;
00071 }
00072 }
00073
00074 if(i<(NumOfParticles-1)) fprintf(stderr,"SIR error(3) i=%d k=%d N=%d\n",i,k,NumOfParticles);
00075 while(i<NumOfParticles){
00076 if(k>=NumOfParticles) k=NumOfParticles-1;
00077 tmp[i]=pcloud[k];
00078 tmp[i].p = 1.0 / NumOfParticles;
00079 i++;
00080 }
00081
00082 pcloud = tmp;
00083 }
00084
00091 void ParticleFilter3D::normalize(){
00092 int i;
00093 double summ=0;
00094
00095 for(unsigned i=0;i<pcloud.size();i++){
00096 pcloud[i].p *= pcloud[i].lik;
00097 summ+=pcloud[i].p;
00098 }
00099 if(summ > 0){
00100 for(i=0;i<pcloud.size();i++){
00101 pcloud[i].p = pcloud[i].p/summ;
00102 }
00103 }else{
00104 for(i=0;i<pcloud.size();i++){
00105 pcloud[i].p = 1.0/(double)pcloud.size();
00106 }
00107 }
00108 }
00109
00110 void ParticleFilter3D::predict(Eigen::Affine3d Tmotion, double vx, double vy, double vz, double vroll, double vpitch, double vyaw){
00111 Eigen::Vector3d tr = Tmotion.translation();
00112 Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
00113
00114 for(unsigned int i=0;i<pcloud.size();i++){
00115 double x = tr[0] + myrand.normalRandom() * vx;
00116 double y = tr[1] + myrand.normalRandom() * vy;
00117 double z = tr[2] + myrand.normalRandom() * vz;
00118
00119 double roll = rot[0] + myrand.normalRandom() * vroll;
00120 double pitch = rot[1] + myrand.normalRandom() * vpitch;
00121 double yaw = rot[2] + myrand.normalRandom() * vyaw;
00122
00123 pcloud[i].T = pcloud[i].T *(xyzrpy2affine(x,y,z,roll,pitch,yaw));
00124 }
00125 }
00126
00127 Eigen::Affine3d ParticleFilter3D::getMean(){
00128 double mx=0, my=0,mz=0;
00129
00130 double roll_x = 0, roll_y=0;
00131 double pitch_x = 0, pitch_y=0;
00132 double yaw_x = 0, yaw_y=0;
00133
00134
00135
00136 for(unsigned int i=0;i<pcloud.size();i++){
00137
00138
00139 Eigen::Vector3d tr = pcloud[i].T.translation();
00140 mx += pcloud[i].p * tr[0];
00141 my += pcloud[i].p * tr[1];
00142 mz += pcloud[i].p * tr[2];
00143
00144
00145 Eigen::Vector3d rot = pcloud[i].T.rotation().eulerAngles(0,1,2);
00146 roll_x+=pcloud[i].p*cos(rot[0]);
00147 roll_y+=pcloud[i].p*sin(rot[0]);
00148
00149 pitch_x+=pcloud[i].p*cos(rot[1]);
00150 pitch_y+=pcloud[i].p*sin(rot[1]);
00151
00152 yaw_x+=pcloud[i].p*cos(rot[2]);
00153 yaw_y+=pcloud[i].p*sin(rot[2]);
00154 }
00155 return xyzrpy2affine(mx,my,mz, atan2(roll_y,roll_x), atan2(pitch_y,pitch_x), atan2(yaw_y,yaw_x));
00156
00157
00158
00159
00160
00161
00162 }
00163
00164
00165