added unit test codes for fastslam 1.0 and fastslam2.0

cpp/core/add_feature.cpp

@@ -12,39 +12,39 @@ void add_feature(Particle &particle, vector<VectorXf> z, MatrixXf R)
 {
     int lenz = z.size();
     vector<VectorXf> xf;
-	vector<MatrixXf> Pf;
+    vector<MatrixXf> Pf;
     VectorXf xv = particle.xv();
 
     float r,b,s,c;
     MatrixXf Gz(2,2);
 
     for (int i=0; i<lenz; i++) {
-        r = z[i][0];
-        b = z[i][1];
-        s = sin(xv(2)+b);
-        c = cos(xv(2)+b);
-		
-		VectorXf measurement(2);
-		measurement(0) = xv(0) + r*c;
-		measurement(1) = xv(1) + r*s;
- 		xf.push_back(measurement);
-        Gz <<c,-r*s,s,r*c;
-
-        Pf.push_back(Gz*R*Gz.transpose()); 	
+	r = z[i][0];
+	b = z[i][1];
+	s = sin(xv(2)+b);
+	c = cos(xv(2)+b);
+
+	VectorXf measurement(2);
+	measurement(0) = xv(0) + r*c;
+	measurement(1) = xv(1) + r*s;
+	xf.push_back(measurement);
+	Gz <<c,-r*s,s,r*c;
+
+	Pf.push_back(Gz*R*Gz.transpose()); 	
     }
 
     int lenx = particle.xf().size();
     vector<int> ii;
     for (int i=lenx; i<lenx+lenz; i++) {
-        ii.push_back(i);
+	ii.push_back(i);
     }	
 
-	for(int j=0; j<ii.size(); j++) {
-		particle.setXfi(ii[j],xf[j]);
-	}
-	
+    for(int j=0; j<ii.size(); j++) {
+	particle.setXfi(ii[j],xf[j]);
+    }
+
 
-	for(int i=0; i<ii.size(); i++) {
-		particle.setPfi(ii[i],Pf[i]);
-	}
+    for(int i=0; i<ii.size(); i++) {
+	particle.setPfi(ii[i],Pf[i]);
+    }
 }

cpp/core/example_webmap_simple.mat

@@ -0,0 +1,11 @@
+#type columns rows
+lm 2 3
+-16.9355 1.0234
+-3.1106 32.3099
+19.0092 17.3977
+
+wp 2 4
+0 0 
+-19.9309 8.3333
+11.1751 29.0936
+15.3226 10.9649

cpp/core/resample_particles.cpp

@@ -13,13 +13,13 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
 
     int i;    
     for (i=0; i<N; i++) {
-        w(i) = particles[i].w();
+	w(i) = particles[i].w();
     }
 
     float ws = w.sum();
 
     for (i=0; i<N; i++) {
-        w(i) = w(i)/ws;
+	w(i) = w(i)/ws;
     }
 
     float Neff=0;
@@ -30,14 +30,14 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
     particles.resize(keep.size());	
 
     if ((Neff < Nmin) && (doresample == 1)) {
-        for(i=0; i< keep.size(); i++) {
-            particles[i] = old_particles[keep[i]]; 	
-        }	
-        for (i=0; i<N; i++) {
-            cout<<"N"<<endl;
-            cout<<N<<endl;
-			float new_w = 1.0f/(float)N;
-            particles[i].setW(new_w);
-        }
+	for(i=0; i< keep.size(); i++) {
+	    particles[i] = old_particles[keep[i]]; 	
+	}	
+	for (i=0; i<N; i++) {
+	    cout<<"N"<<endl;
+	    cout<<N<<endl;
+	    float new_w = 1.0f/(float)N;
+	    particles[i].setW(new_w);
+	}
     }		
 }

cpp/core/stratified_random.cpp

@@ -10,13 +10,21 @@ void stratified_random(int N, vector<float> &di)
 
     //deterministic intervals
     float temp = k/2;
-    while (temp < (1-k/2)) {
-        di.push_back(temp);
+    di.push_back(temp);
+    while (temp < 1-k/2) {
         temp = temp+k;
+        di.push_back(temp);
     }
-
-    assert(di.size() == N); 
-
+    /* 
+    cout<<"di"<<endl;
+    for (int i=0; i< di.size(); i++) {
+	cout<<di[i]<<" "<<endl;
+    }
+ 
+    cout<<"N "<<N<<endl;
+    cout<<"di size "<<di.size()<<endl; 
+    //assert(di.size() == N); 
+    */
     //dither within interval
     vector<float>::iterator diter; 
     for (diter = di.begin(); diter != di.end(); diter++) {

cpp/core/stratified_resample.cpp

@@ -7,11 +7,14 @@ using namespace std;
 void stratified_resample(VectorXf w, vector<int> &keep, float &Neff)
 {
     VectorXf wsqrd(w.size());
+    float wsum = w.sum();    
+
     for (int i=0; i<w.size(); i++) {
-        w(i) = w(i)/w.sum();
-        wsqrd(i) = pow(w(i),2);
+        w(i) = w(i)/ wsum;
+        wsqrd(i) = (float)pow(w(i),2);
     }
-    Neff = 1/wsqrd.sum();
+
+    Neff = 1.0f/(float)wsqrd.sum();
 
     int len = w.size();
     keep.resize(len);
@@ -21,6 +24,18 @@ void stratified_resample(VectorXf w, vector<int> &keep, float &Neff)
 
     vector<float> select;
     stratified_random(len,select); 
+    /*
+    select.push_back(0.0948);
+    select.push_back(0.1334);
+    select.push_back(0.239);
+    select.push_back(0.315);
+    select.push_back(0.4334);
+    select.push_back(0.5554);
+    select.push_back(0.655);
+    select.push_back(0.716);
+    select.push_back(0.8117); 
+    select.push_back(0.9399); 
+    */
     cumsum(w);    
 
     int ctr=0;
@@ -37,13 +52,13 @@ void stratified_resample(VectorXf w, vector<int> &keep, float &Neff)
 //
 void cumsum(VectorXf &w) 
 {
-    VectorXf csumVec(w.size());
+    VectorXf csumVec = VectorXf(w);
+
     for (int i=0; i< w.size(); i++) {
         float sum =0;
         for (int j=0; j<=i; j++) {
-            sum+=w(j);
-        }			
-        csumVec(i) = sum;
+	    sum+=csumVec(j);
+	}			
+	w(i) = sum;
     }
-    w = VectorXf(csumVec); //copy constructor. Double check
 }

cpp/fastslam1/compute_weight.cpp

@@ -11,33 +11,35 @@ using namespace std;
 //
 //compute particle weight for sampling
 //
-float compute_weight(Particle &particle, vector<VectorXf> z, vector<int> idf, MatrixXf R) 
+float compute_weight(Particle &particle, vector<VectorXf> z, vector<int> idf,
+	MatrixXf R) 
 {
     vector<MatrixXf> Hv;
     vector<MatrixXf> Hf;
     vector<MatrixXf> Sf;
-	vector<VectorXf> zp;     
+    vector<VectorXf> zp;     
 
     //process each feature, incrementally refine proposal distribution
     compute_jacobians(particle,idf,R,zp,&Hv,&Hf,&Sf);
 
     vector<VectorXf> v;
+    for (int j =0; j<z.size(); j++) {
+	VectorXf v_j = z[j] - zp[j];
+	v_j[1] = pi_to_pi(v_j[1]);
+	v.push_back(v_j);
+    }
+
 
-	for (int j =0; j<z.size(); j++) {
-		VectorXf v_j = z[j] - zp[j];
-		v_j[1] = pi_to_pi(v_j[1]);
-		v.push_back(v_j);
-	}
+    float w = 1.0f;
 
-    float w = 1.0;
-
     MatrixXf S;
     float den, num;
     for (int i=0; i<z.size(); i++) {
-        S = Sf[i];
-        den = 2*pi*sqrt(S.determinant());
-        num = std::exp(-0.5 * v[i].transpose() * S.inverse() * v[i]);
-        w = w* num/den; 
+	S = Sf[i];
+	den = 2*pi*sqrt(S.determinant());
+	num = std::exp(-0.5 * v[i].transpose() * S.inverse() * v[i]);
+	w *= (float)num/(float) den; 
     }
+
     return w;
 }