lot of bugs fixed from debugging the files individually. THere's stil…

…l a bug in data_assoc and get_obs. looking into it.

cpp/compute_jacobians.cpp

@@ -99,7 +99,7 @@ void compute_jacobians(Particle particle,
         HfMat<< dx/d,  dy/d,
             	-dy/d2, dx/d2;
 
-		Hv->push_back(HvMat);
+	Hv->push_back(HvMat);
         Hf->push_back(HfMat);
 
         //innovation covariance of 'feature observation given the vehicle'

cpp/get_observations.cpp

@@ -45,7 +45,12 @@ void get_visible_landmarks(VectorXf x, MatrixXf &lm, vector<int> &idf, float rma
 		}
 	}
 	lm = MatrixXf(lm_new); 
-	idf = vector<int>(ii); 			
+	//idf = vector<int>(ii); 			
+        vector<int>idf_backup(idf);
+        idf.clear();
+        for(int i=0; i<ii.size(); i++) {
+            idf.push_back(idf_backup[ii[i]]);
+        }
 }
 
 MatrixXf compute_range_bearing(VectorXf x, MatrixXf lm) 
@@ -71,12 +76,14 @@ vector<int> find2(MatrixXf dx, MatrixXf dy, float phi, float rmax)
 {
 	vector<int> index;
 	//incremental tests for bounding semi-circle
-	for (int i =0; i<dx.cols(); i++) {
-		if ((abs(dx(0,i)) < rmax) && (abs(dy(0,i)) < rmax)
-		&& ((dx(0,i)* cos(phi) + dy(0,i)* sin(phi)) > 0)
-		&& ((pow(dx(0,i),2) + pow(dy(0,i),2)) < pow(rmax,2))) {
+	for (int j=0; j<dx.rows(); j++) {
+            for (int i =0; i<dx.cols(); i++) {
+		if ((abs(dx(j,i)) < rmax) && (abs(dy(j,i)) < rmax)
+		&& ((dx(j,i)* cos(phi) + dy(j,i)* sin(phi)) > 0)
+		&& ((pow(dx(j,i),2) + pow(dy(j,i),2)) < pow(rmax,2))) {
 			index.push_back(i);			
 		}
-	}	
+	    }	
+        }
 	return index;				
 }

cpp/multivariate_gauss.cpp

@@ -9,9 +9,11 @@ VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n)
 	int len = x.size();
 	//choleksy decomposition
 	MatrixXf S = P.llt().matrixL();
-	//TODO: why is random doesn't change values?
-	//MatrixXf X = MatrixXf::Random(len,n);
-	MatrixXf X = MatrixXf::Ones(len,n);
+	cout<<"S"<<endl;
+        cout<<S<<endl;
+        cout<<endl;
+        MatrixXf X(len,n); //TODO this needs to be randomized
+        X<<-0.8809,0.8571,0.2638;
 	VectorXf ones = VectorXf::Ones(n).transpose();
 	return S*X + x*ones;
 }

cpp/pi_to_pi.cpp

@@ -1,49 +1,42 @@
 #include "pi_to_pi.h"
+#include <iostream>
 
-void pi_to_pi(VectorXf &angle)
-{	
-    vector<int> index;
-    index = find1(angle);
-    vector<int>::iterator iter;
-
-    if (!index.empty()){
-        for (iter = index.begin(); iter != index.end(); iter++) {
-            //remember that iterators are pointers
-			int n = floor(angle[*iter] / 2*pi);
-			angle[*iter] = angle[*iter] - n*(2*pi);//angle[*iter] % (2*pi);  
-        }
-    }
-
-    for (int i = 0; i< angle.size(); i++) {
-        if (angle[i] > pi) {
-            angle[i] = angle[i]-2*pi;
-        }   
-    }
-
-    for (int j=0; j< angle.size(); j++) {
-        if(angle[j] < -1*pi){
-            angle[j] = angle[j]+2*pi;
+void pi_to_pi(VectorXf &angle) 
+{
+    int n;
+    for (int i=0; i<angle.size(); i++) {
+        if ((angle[i] < (-2*pi)) || (angle[i] > (2*pi))) {
+            n=floor(angle[i]/(2*pi));
+            angle[i] = angle[i]-n*(2*pi);    
+
+            if (angle[i] > pi) {
+                angle[i] = angle[i] - (2*pi);
+            }
+            if (angle[i] < -pi) {
+                angle[i] = angle[i] + (2*pi);
+            }
         }
     }
 }
 
 float pi_to_pi(float ang) 
 {
-	if ((ang<-2*pi) || (ang>2*pi)) {
-		int n = floor(ang / 2*pi);
-		ang = ang-n*2*pi;			
-	}
+    int n;
+    if ((ang < (-2*pi)) || (ang > (2*pi))) {
+        n=floor(ang/(2*pi));
+        ang = ang-n*(2*pi);    
 
-	if(ang > pi) {
-		ang = ang-2*pi;
-	}
-	
-	if (ang < -pi) {
-		ang = ang+2*pi;
-	}
-        return ang;
+        if (ang > pi) {
+            ang = ang - (2*pi);
+        }
+        if (ang < -pi) {
+            ang = ang + (2*pi);
+        }
+    }
+    return ang;
 }
 
+#if 0
 vector<int> find1(VectorXf input) 
 {
 	vector<int> index;
@@ -54,3 +47,4 @@ vector<int> find1(VectorXf input)
     }
 	return index; 
 }
+#endif

cpp/predict.cpp

@@ -2,6 +2,8 @@
 #include <math.h>
 #include <iostream>
 
+
+#define pi 3.1416
 //checked all values. everything works
 
 using namespace std;
@@ -55,6 +57,18 @@ void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, i
 	VectorXf xv_temp(3);
         xv_temp << xv(0) + V*dt*cos(G+xv(2)),
 	           xv(1) + V*dt*sin(G+xv(2)),
-	           pi_to_pi(xv(2) + V*dt*sin(G/WB));
+	           pi_to_pi2(xv(2) + V*dt*sin(G/WB));
         particle.setXv(xv_temp);
 }
+
+float pi_to_pi2(float ang) 
+{
+    if (ang > pi) {
+        ang = ang - (2*pi);
+    }
+    if (ang < -pi) {
+        ang = ang + (2*pi);
+    }
+    return ang;
+}
+

cpp/predict.h

@@ -4,10 +4,11 @@
 #include <Eigen/Dense>
 #include "particle.h"
 #include "multivariate_gauss.h"
-#include "pi_to_pi.h"
 
 using namespace Eigen;
 
 void predict(Particle &particle,float V,float G,Matrix2f Q, float WB,float dt, int addrandom);
 
+float pi_to_pi2(float ang); 
+
 #endif //PREDICT_H

cpp/resample_particles.cpp

@@ -12,21 +12,19 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
     VectorXf w(N);
     w.setZero();
 
-    unsigned i;    
+    int i;    
     for (i=0; i<N; i++) {
         w(i) = particles[i].w();
         //cout<<"w at "<<i<<" "<<w(i)<<endl;
     }
 
     float ws = w.sum();
 
-    //TODO: ws is zero!
-    cout<<"ws is "<<ws<<endl;
     for (i=0; i<N; i++) {
         w(i) = w(i)/ws;
     }
 
-    float Neff;
+    float Neff=0;
     vector<int> keep;
     stratified_resample(w,keep,Neff);
 
@@ -38,7 +36,9 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
             particles[i] = old_particles[keep[i]]; 	
         }	
         for (i=0; i<N; i++) {
-            particles[i].setW(1/N);
+            cout<<"N"<<endl;
+            cout<<N<<endl;
+            particles[i].setW(1.0/(float)N);
         }
     }		
 }

cpp/sample_proposal.cpp

@@ -55,7 +55,7 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
             //cout<<"vi["<<r<<"] "<<vi[r]<<endl; 
         }
 
-#if 1
+#if 0 
         cout<<"in sample proposal"<<endl;
         cout<<"z"<<endl;
         cout<<z<<endl;
@@ -134,36 +134,6 @@ float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixX
     MatrixXf Sfi;
     VectorXf v(z.rows());    
 
-    cout<<"xv"<<endl;
-    cout<<particle.xv()<<endl;
-    cout<<endl;
-    cout<<"Pv"<<endl;
-    cout<<particle.Pv()<<endl;
-    cout<<endl;
-    cout<<"xf"<<endl;
-    cout<<particle.xf()<<endl;
-    cout<<endl;
-    cout<<"Pf"<<endl;
-    cout<<particle.Pf()[0]<<endl;
-    cout<<endl;
-    cout<<particle.Pf()[1]<<endl;
-    cout<<endl;
-    cout<<"w"<<endl;
-    cout<<particle.w()<<endl;
-    cout<<endl;
-    cout<<"z"<<endl;
-    cout<<z<<endl;
-    cout<<endl;
-
-    cout<<"idf"<<endl;
-    for (int i=0; i< idf.size(); i++) {
-        cout<<idf[i]<<" ";
-    }
-    cout<<endl;
-
-    cout<<"R"<<endl;
-    cout<<R<<endl;
-
     unsigned i,k;
     //cout<<"idf.size() "<<idf.size()<<endl;
     for (i=0; i<idf.size(); i++){
@@ -179,19 +149,23 @@ float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixX
         cout<<endl;
 
         for (k=0; k<z.rows(); k++) {
-            v(k) = z(k,i)-zp(k,0); //TODO: this returns wrong values
+            v(k) = z(k,i)-zp(k,i); //TODO: this returns wrong values
         }
         v(1) = pi_to_pi(v(1));
 
+        cout<<"GAUSS EVAL INPUTS"<<endl;
         cout<<"v"<<endl;
         cout<<v<<endl;
         cout<<endl;
 
+        cout<<"sf"<<endl;
+        cout<<Sf[0]<<endl;
 
+        cout<<"gauss_eval "<<gauss_evaluate(v,Sf[0],0)<<endl;
         w = w*gauss_evaluate(v,Sf[0],0);
-        cout<<"new w: "<<w<<endl;
+        //cout<<"new w: "<<w<<endl;
     } 
-    cout<<"w returned "<<w<<endl;
+    //cout<<"w returned "<<w<<endl;
     return w;
 }
 

cpp/stratified_resample.cpp

@@ -12,16 +12,22 @@ void stratified_resample(VectorXf w, vector<int> &keep, float &Neff)
         wsqrd(i) = pow(w(i),2);
     }
     Neff = 1/wsqrd.sum();
+
     int len = w.size();
+    keep.resize(len);
+    for (int i=0; i<len; i++) {
+        keep[i] = -1;
+    }
+
     vector<float> select;
-    cout<<"len in stratified resample "<<len<<endl;
     stratified_random(len,select); 
     cumsum(w);    
 
     int ctr=0;
     for (int i=0; i<len; i++) {
-        while ((ctr<len) && (select[ctr] < w(i))) {
-            keep.push_back(i);
+        while ((ctr<len) && (select[ctr]<w(i))) {
+            keep[ctr] = i;
+            ctr++;
         }
     }
 }
@@ -34,7 +40,7 @@ void cumsum(VectorXf &w)
     VectorXf csumVec(w.size());
     for (int i=0; i< w.size(); i++) {
         float sum =0;
-        for (int j=0; j<i; j++) {
+        for (int j=0; j<=i; j++) {
             sum+=w(j);
         }			
         csumVec(i) = sum;