there was a bug in resample_particles. there's a bug on weight sum be…

…coming zero, which makes normalized weight go to inf

CMakeCache.txt

@@ -0,0 +1,40 @@
+# This is the CMakeCache file.
+# For build in directory: /Users/ylee8/FastSLAM
+# It was generated by CMake: /opt/local/bin/cmake
+# You can edit this file to change values found and used by cmake.
+# If you do not want to change any of the values, simply exit the editor.
+# If you do want to change a value, simply edit, save, and exit the editor.
+# The syntax for the file is as follows:
+# KEY:TYPE=VALUE
+# KEY is the name of a variable in the cache.
+# TYPE is a hint to GUI's for the type of VALUE, DO NOT EDIT TYPE!.
+# VALUE is the current value for the KEY.
+
+########################
+# EXTERNAL cache entries
+########################
+
+
+########################
+# INTERNAL cache entries
+########################
+
+//This is the directory where this CMakeCache.txt was created
+CMAKE_CACHEFILE_DIR:INTERNAL=/Users/ylee8/FastSLAM
+//Major version of cmake used to create the current loaded cache
+CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2
+//Minor version of cmake used to create the current loaded cache
+CMAKE_CACHE_MINOR_VERSION:INTERNAL=8
+//Patch version of cmake used to create the current loaded cache
+CMAKE_CACHE_PATCH_VERSION:INTERNAL=9
+//Path to CMake executable.
+CMAKE_COMMAND:INTERNAL=/opt/local/bin/cmake
+//Path to cpack program executable.
+CMAKE_CPACK_COMMAND:INTERNAL=/opt/local/bin/cpack
+//Path to ctest program executable.
+CMAKE_CTEST_COMMAND:INTERNAL=/opt/local/bin/ctest
+//Path to cache edit program executable.
+CMAKE_EDIT_COMMAND:INTERNAL=/opt/local/bin/ccmake
+//Path to CMake installation.
+CMAKE_ROOT:INTERNAL=/opt/local/share/cmake-2.8
+

CMakeFiles/cmake.check_cache

@@ -0,0 +1 @@
+# This file is generated by cmake for dependency checking of the CMakeCache.txt file

CMakeLists.txt

@@ -47,6 +47,28 @@ find_package(Eigen3 REQUIRED)
 find_package(GooglePerfTools)
 find_package(OpenCL REQUIRED)
 
+#Find Boost
+#FIND_PACKAGE(Boost)
+#IF (Boost_FOUND)
+#    INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
+#    ADD_DEFINITIONS( "-DHAS_BOOST" )
+#ENDIF()
+find_package(Boost 1.44.0)
+
+# uncomment the following line to let the env variable BOOST_DIR set the Boost_DIR path
+# set(Boost_DIR $ENV{BOOST_ROOT})
+
+# just print out the variables to see their values
+message("Boost_DIR : " ${Boost_DIR})
+message("BOOST_ROOT: " $ENV{BOOST_ROOT})
+
+# now see if boost was actually found
+if (Boost_FOUND)
+  message("Boost WAS found!")
+endif (Boost_FOUND)
+
+
+
 # Finish up local custom options based on what was found above
 fslam_enable_testing()
 if (ENABLE_PROFILING AND GOOGLE_PERFTOOLS_FOUND)

TODO

@@ -0,0 +1 @@
+multivariate_gauss and other files that use instance of "nRandMat" are wrong. Replace with boost library's normally distributed random function?

cpp/core/add_control_noise.h

@@ -3,9 +3,13 @@
 
 #include <Eigen/Dense>
 #include "multivariate_gauss.h"
+ # include <cstdlib>
+# include <cmath>
 
 using namespace Eigen;
 
+//MatrixXf randn(int m, int n); 
+//MatrixXf rand(int m, int n);
 void add_control_noise(float V, float G, Matrix2f Q, int addnoise,float* VnGn);
 
 #endif //ADD_CONTROL_NOISE

cpp/core/add_observation_noise.cpp

@@ -1,8 +1,7 @@
 #include "add_observation_noise.h"
-
+#if 0
 //http://moby.ihme.washington.edu/bradbell/mat2cpp/randn.cpp.xml
-
-MatrixXf nRandMat::randn(int m, int n) 
+MatrixXf randn(int m, int n) 
 {	
 	// use formula 30.3 of Statistical Distributions (3rd ed)
 	// Merran Evans, Nicholas Hastings, and Brian Peacock
@@ -40,7 +39,7 @@ MatrixXf nRandMat::randn(int m, int n)
 	return x;
 }
 
-MatrixXf nRandMat::rand(int m, int n) 
+MatrixXf rand(int m, int n) 
 {	
 	MatrixXf x(m,n);	
 	int i, j;
@@ -52,15 +51,30 @@ MatrixXf nRandMat::rand(int m, int n)
 	} 
 	return x;
 }
-
+#endif
 //add random measurement noise. We assume R is diagnoal matrix
 void add_observation_noise(vector<VectorXf> &z, MatrixXf R, int addnoise)
 {
+    float LO = -1.0f;
+    float HI = 1.0f;
+
 	if (addnoise == 1){
 		int len = z.size();	
 		if (len > 0) {
-			MatrixXf randM1 = nRandMat::randn(1,len);
-			MatrixXf randM2 = nRandMat::randn(1,len);
+			//MatrixXf randM1 = nRandMat::randn(1,len);
+            MatrixXf randM1(1,len);
+            for (int i=0; i< len; i++) {
+                float r3 = LO + (float)rand()/((float)RAND_MAX/(HI-LO));
+                randM1(0,i) = r3; 
+            }
+			//MatrixXf randM2 = nRandMat::randn(1,len);
+			MatrixXf randM2(1,len);
+            for (int j=0; j< len; j++) {
+                float r4 = LO + (float)rand()/((float)RAND_MAX/(HI-LO));
+                randM2(0,j) = r4; 
+            }
+            //cout<<"randM1"<<endl;
+            //cout<<randM1<<endl;
 
 			for (int c=0; c<len; c++) {
 				z[c][0] = z[c][0] + randM1(0,c)*sqrt(R(0,0));

cpp/core/multivariate_gauss.cpp

@@ -15,10 +15,22 @@ VectorXf multivariate_gauss(VectorXf x, MatrixXf P, int n)
 	int len = x.size();
 	//choleksy decomposition
 	MatrixXf S = P.llt().matrixL();
-    MatrixXf X;
-	X = nRandMat::randn(len,n);	
-
-	//VectorXf ones = //VectorXf::Ones(n).transpose();
+	MatrixXf X(len,n);
+
+
+    float LO = -1.0f;
+    float HI = 1.0f;
+
+    for (int i = 0; i < len; i++) {
+        for (int j=0; j< n; j++) {
+            float r3 = LO + (float)rand()/((float)RAND_MAX/(HI-LO));
+            X(i,j) = r3;
+        }
+    }
+
+    //TODO: this does not work. Also fixed other instances of nRandMat	
+	//X = nRandMat::randn(len,n);	
+
 	MatrixXf ones = MatrixXf::Ones(1,n);	
 	return S*X + x*ones;
 }

cpp/core/particle.cpp

@@ -16,7 +16,7 @@ Particle::Particle()
 	_da = NULL;
 }
 
-Particle::Particle(float &w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da)
+Particle::Particle(float w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da)
 {
 	_w = w;
 	_xv = xv;
@@ -62,7 +62,7 @@ float* Particle::da() const
 }
 
 //setters
-void Particle::setW(float &w)
+void Particle::setW(float w)
 {
 	_w = w;
 }

cpp/core/particle.h

@@ -10,7 +10,7 @@ using namespace std;
 class Particle{
 public:
 	Particle();
-	Particle(float &w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da);
+	Particle(float w, VectorXf &xv, MatrixXf &Pv, vector<VectorXf> &xf, vector<MatrixXf> &Pf, float* da);
 	~Particle();
 
 	//getters	
@@ -22,7 +22,7 @@ class Particle{
 	float* da() const; //
 
 	//setters
-	void setW(float &w);
+	void setW(float w);
 	void setXv(VectorXf &xv);
 	void setPv(MatrixXf &Pv);
 	void setXf(vector<VectorXf> &xf);

cpp/core/resample_particles.cpp

@@ -1,25 +1,31 @@
+#include <math.h>
 #include "resample_particles.h"
 #include "configfile.h"
 #include "stratified_resample.h"
 #include <iostream>
+#include <assert.h>
 
 using namespace std;
 
 void resample_particles(vector<Particle> &particles, int Nmin, int doresample) 
 {
-    unsigned int N = particles.size();
+    int N = particles.size();
     VectorXf w(N);
     w.setZero();
 
-    int i;    
-    for (i=0; i<N; i++) {
-	w(i) = particles[i].w();
+    for (int i=0; i<N; i++) {
+        w(i) = particles[i].w();
     }
 
     float ws = w.sum();
+    assert(ws != 0);
 
-    for (i=0; i<N; i++) {
-	w(i) = w(i)/ws;
+    for (int i=0; i<N; i++) {
+        w(i) = w(i)/ws;
+    }
+
+    for (int i=0; i<N; i++) {
+        particles[i].setW(particles[i].w()/ws);
     }
 
     float Neff=0;
@@ -28,16 +34,18 @@ void resample_particles(vector<Particle> &particles, int Nmin, int doresample)
 
     vector<Particle> old_particles = vector<Particle>(particles);
     particles.resize(keep.size());	
+    //do I need to clear this? 
+
 
     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(int i=0; i< keep.size(); i++) {
+            particles[i] = old_particles[keep[i]]; 	
+        }	
+        for (int i=0; i<N; i++) {
+            float new_w = 1.0f/(float)N;
+            assert(isfinite(new_w));
+            assert(N == 100);
+            particles[i].setW(new_w);
+        }
     }		
 }