Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

added unit test codes for fastslam 1.0 and fastslam2.0

  • Loading branch information...
commit bf1a51ac091e39dbcc730f48dbe3406d5689b076 1 parent 028fc05
Grace Lee authored
42 cpp/core/add_feature.cpp
View
@@ -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]);
+ }
}
11 cpp/core/example_webmap_simple.mat
View
@@ -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
22 cpp/core/resample_particles.cpp
View
@@ -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);
+ }
}
}
18 cpp/core/stratified_random.cpp
View
@@ -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++) {
31 cpp/core/stratified_resample.cpp
View
@@ -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
}
28 cpp/fastslam1/compute_weight.cpp
View
@@ -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;
}
176 cpp/fastslam1/fastslam1_sim.cpp
View
@@ -22,10 +22,10 @@ int counter=0;
vector<Particle> fastslam1_sim(MatrixXf lm, MatrixXf wp)
{
if (SWITCH_PREDICT_NOISE) {
- printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");
+ printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");
}
-
- //normally initialized in configfile.h
+
+ //normally initialized configfile.h
Q << pow(sigmaV,2), 0,
0 , pow(sigmaG,2);
@@ -37,13 +37,13 @@ vector<Particle> fastslam1_sim(MatrixXf lm, MatrixXf wp)
//vector of particles (their count will change)
vector<Particle> particles(NPARTICLES);
for (int i=0; i<particles.size(); i++){
- particles[i] = Particle();
+ particles[i] = Particle();
}
-
+
//initialize particle weights as uniform
float uniformw = 1.0/(float)NPARTICLES;
for (unsigned int p = 0; p < NPARTICLES; p++) {
- particles[p].setW(uniformw);
+ particles[p].setW(uniformw);
}
VectorXf xtrue(3);
@@ -54,13 +54,13 @@ vector<Particle> fastslam1_sim(MatrixXf lm, MatrixXf wp)
vector<int> ftag; //identifier for each landmark
for (int i=0; i< lm.cols(); i++) {
- ftag.push_back(i);
+ ftag.push_back(i);
}
//data ssociation table
VectorXf da_table(lm.cols());
for (int s=0; s<da_table.size(); s++) {
- da_table[s] = -1;
+ da_table[s] = -1;
}
int iwp = 0; //index to first waypoint
@@ -68,91 +68,91 @@ vector<Particle> fastslam1_sim(MatrixXf lm, MatrixXf wp)
MatrixXf plines; //will later change to list of points
if (SWITCH_SEED_RANDOM !=0) {
- srand(SWITCH_SEED_RANDOM);
+ srand(SWITCH_SEED_RANDOM);
}
MatrixXf Qe = MatrixXf(Q);
MatrixXf Re = MatrixXf(R);
if (SWITCH_INFLATE_NOISE ==1) {
- Qe = 2*Q;
- Re = 2*R;
+ Qe = 2*Q;
+ Re = 2*R;
}
vector<int> ftag_visible;
- vector<VectorXf> z; //range and bearings of visible landmarks
+ vector<VectorXf> z; //range and bearings of visible landmarks
//Main loop
while (iwp !=-1) {
- compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
- if (iwp ==-1 && NUMBER_LOOPS > 1) {
- iwp = 0;
- NUMBER_LOOPS = NUMBER_LOOPS-1;
- }
- predict_true(xtrue,V,G,WHEELBASE,dt);
-
- //add process noise
- float* VnGn = new float[2];
- add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
- float Vn = VnGn[0];
- float Gn = VnGn[1];
-
- //Predict step
- for (unsigned int i=0; i< NPARTICLES; i++) {
- predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
- if (SWITCH_HEADING_KNOWN) {
- for (int j=0; j< particles[i].xf().size(); j++) {
- VectorXf xf_j = particles[i].xf()[j];
- xf_j[2] = xtrue[2];
- particles[i].setXfi(j,xf_j);
- }
- }
- }
-
- //Observe step
- dtsum = dtsum+dt;
- if (dtsum >= DT_OBSERVE) {
- dtsum=0;
-
- //Compute true data, then add noise
- ftag_visible = vector<int>(ftag); //modify the copy, not the ftag
-
- //z is the range and bearing of the observed landmark
- z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
- add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
-
- if (!z.empty()){
- plines = make_laser_lines(z,xtrue);
- }
-
- //Compute (known) data associations
- int Nf = particles[0].xf().size();
- vector<int> idf;
- vector<VectorXf> zf;
- vector<VectorXf> zn;
-
- bool testflag= false;
- data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
-
- //perform update
- for (int i =0; i<NPARTICLES; i++) {
- if (!zf.empty()) { //observe map features
- float w = compute_weight(particles[i],zf,idf,R);
- w = particles[i].w()*w;
- particles[i].setW(w);
- feature_update(particles[i],zf,idf,R);
- }
- if (!zn.empty()) {
- add_feature(particles[i], zn, R);
- }
- }
-
- resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
-
- if (VnGn) {
- delete[] VnGn;
- }
- }
+ compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
+ if (iwp ==-1 && NUMBER_LOOPS > 1) {
+ iwp = 0;
+ NUMBER_LOOPS = NUMBER_LOOPS-1;
+ }
+ predict_true(xtrue,V,G,WHEELBASE,dt);
+
+ //add process noise
+ float* VnGn = new float[2];
+ add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
+ float Vn = VnGn[0];
+ float Gn = VnGn[1];
+
+ //Predict step
+ for (unsigned int i=0; i< NPARTICLES; i++) {
+ predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
+ if (SWITCH_HEADING_KNOWN) {
+ for (int j=0; j< particles[i].xf().size(); j++) {
+ VectorXf xf_j = particles[i].xf()[j];
+ xf_j[2] = xtrue[2];
+ particles[i].setXfi(j,xf_j);
+ }
+ }
+ }
+
+ //Observe step
+ dtsum = dtsum+dt;
+ if (dtsum >= DT_OBSERVE) {
+ dtsum=0;
+
+ //Compute true data, then add noise
+ ftag_visible = vector<int>(ftag); //modify the copy, not the ftag
+
+ //z is the range and bearing of the observed landmark
+ z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
+ add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
+
+ if (!z.empty()){
+ plines = make_laser_lines(z,xtrue);
+ }
+
+ //Compute (known) data associations
+ int Nf = particles[0].xf().size();
+ vector<int> idf;
+ vector<VectorXf> zf;
+ vector<VectorXf> zn;
+
+ bool testflag= false;
+ data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
+
+ //perform update
+ for (int i =0; i<NPARTICLES; i++) {
+ if (!zf.empty()) { //observe map features
+ float w = compute_weight(particles[i],zf,idf,R);
+ w = particles[i].w()*w;
+ particles[i].setW(w);
+ feature_update(particles[i],zf,idf,R);
+ }
+ if (!zn.empty()) {
+ add_feature(particles[i], zn, R);
+ }
+ }
+
+ resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
+
+ if (VnGn) {
+ delete[] VnGn;
+ }
+ }
}
cout<<"done with all functions and will return particles"<<endl<<flush;
return particles;
@@ -163,7 +163,7 @@ vector<Particle> fastslam1_sim(MatrixXf lm, MatrixXf wp)
MatrixXf make_laser_lines(vector<VectorXf> rb, VectorXf xv)
{
if (rb.empty()) {
- return MatrixXf(0,0);
+ return MatrixXf(0,0);
}
int len = rb.size();
@@ -172,17 +172,17 @@ MatrixXf make_laser_lines(vector<VectorXf> rb, VectorXf xv)
MatrixXf globalMat(2,rb.size());
int j;
for (j=0; j<globalMat.cols();j++) {
- globalMat(0,j) = rb[j][0]*cos(rb[j][1]);
- globalMat(1,j) = rb[j][0]*sin(rb[j][1]);
+ globalMat(0,j) = rb[j][0]*cos(rb[j][1]);
+ globalMat(1,j) = rb[j][0]*sin(rb[j][1]);
}
TransformToGlobal(globalMat,xv);
for (int c=0; c<lnes.cols();c++) {
- lnes(0,c) = xv(0);
- lnes(1,c) = xv(1);
- lnes(2,c) = globalMat(0,c);
- lnes(3,c) = globalMat(1,c);
+ lnes(0,c) = xv(0);
+ lnes(1,c) = xv(1);
+ lnes(2,c) = globalMat(0,c);
+ lnes(3,c) = globalMat(1,c);
}
return line_plot_conversion(lnes);
3  cpp/fastslam1/main.cpp
View
@@ -18,6 +18,9 @@ int main (int argc, char *argv[])
for (int i=0; i<data.size(); i++) {
cout<<"particle i="<<i<<endl;
cout<<endl;
+ cout<<"weight"<<endl;
+ cout<<data[i].w()<<endl;
+ cout<<endl;
cout<<"xv (robot pose)"<<endl;
cout<<data[i].xv()<<endl;
cout<<endl;
37 cpp/fastslam2/CMakeLists.txt
View
@@ -1,25 +1,24 @@
-#include_directories(${EIGEN3_INCLUDE_DIR})
+include_directories(${EIGEN3_INCLUDE_DIR})
-#include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
-## Produce a FastSLAM shared library. (For re-use of executables)
-#add_library(FastSLAM2
-# fastslam2_sim.cpp gauss_evaluate.cpp main.cpp observe_heading.cpp predict.cpp sample_proposal.cpp
-#)
+# Produce a FastSLAM shared library. (For re-use of executables)
+add_library(FastSLAM2
+ fastslam2_sim.cpp gauss_evaluate.cpp observe_heading.cpp predict.cpp sample_proposal.cpp
+)
+target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything
-#target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything
+install(FILES
+ fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h
+ DESTINATION include/fslam/fastslam2
+ )
-#install(FILES
-# fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h
-# DESTINATION include/fslam/fastslam2
-# )
+install(TARGETS
+ FastSLAM2
+ DESTINATION lib
+ )
-#install(TARGETS
-# FastSLAM2
-# DESTINATION lib
-# )
-
-## Add an executable that currentlyi runs the simulator
-#set(FSLAM_USED_LIBS FastSLAM2)
-#add_fslam_tool(fastslam2_simulation main.cpp)
+# Add an executable that currentlyi runs the simulator
+set(FSLAM_USED_LIBS FastSLAM2)
+add_fslam_tool(fastslam2_simulation main.cpp)
25 cpp/fastslam2/CMakeLists_empty.txt
View
@@ -0,0 +1,25 @@
+#include_directories(${EIGEN3_INCLUDE_DIR})
+
+#include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
+
+## Produce a FastSLAM shared library. (For re-use of executables)
+#add_library(FastSLAM2
+# fastslam2_sim.cpp gauss_evaluate.cpp main.cpp observe_heading.cpp predict.cpp sample_proposal.cpp
+#)
+
+#target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything
+
+#install(FILES
+# fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h
+# DESTINATION include/fslam/fastslam2
+# )
+
+#install(TARGETS
+# FastSLAM2
+# DESTINATION lib
+# )
+
+## Add an executable that currentlyi runs the simulator
+#set(FSLAM_USED_LIBS FastSLAM2)
+#add_fslam_tool(fastslam2_simulation main.cpp)
+
24 cpp/fastslam2/CMakeLists_real.txt
View
@@ -0,0 +1,24 @@
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
+
+# Produce a FastSLAM shared library. (For re-use of executables)
+add_library(FastSLAM2
+ fastslam2_sim.cpp gauss_evaluate.cpp observe_heading.cpp predict.cpp sample_proposal.cpp
+)
+target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything
+
+install(FILES
+ fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h
+ DESTINATION include/fslam/fastslam2
+ )
+
+install(TARGETS
+ FastSLAM2
+ DESTINATION lib
+ )
+
+# Add an executable that currentlyi runs the simulator
+set(FSLAM_USED_LIBS FastSLAM2)
+add_fslam_tool(fastslam2_simulation main.cpp)
+
43 cpp/fastslam2/fastslam2_sim.cpp
View
@@ -67,7 +67,7 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
int iwp = 0; //index to first waypoint
float G = 0; //initial steer angle
- MatrixXf plines; //will later change to list of points
+ //MatrixXf plines; //will later change to list of points
if (SWITCH_SEED_RANDOM !=0) {
srand(SWITCH_SEED_RANDOM);
@@ -84,7 +84,7 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
vector<int> ftag_visible;
vector<VectorXf> z;
- //Main loop
+ //Main loop
while (iwp !=-1) {
compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
if (iwp ==-1 && NUMBER_LOOPS > 1) {
@@ -117,15 +117,15 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
- if(!z.empty()){
- plines = make_laser_lines(z,xtrue);
+ if(!z.empty()){
+ //plines = make_laser_lines(z,xtrue);
}
//Compute (known) data associations
int Nf = particles[0].xf().size(); //(particles[0].xf()).cols();
vector<int> idf;
- vector<VectorXf> zf;
- vector<VectorXf> zn;
+ vector<VectorXf> zf;
+ vector<VectorXf> zn;
bool testflag= false;
data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
@@ -142,9 +142,9 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
}
//Observe new features, augment map
- if (!zn.isZero()) {
+ if (!zn.empty()) {//!zn.isZero()) {
for (unsigned i=0; i<NPARTICLES; i++) {
- if (zf.isZero()) {//sample from proposal distribution if we have not already done so above
+ if (zf.empty()) {//zf.isZero()) {//sample from proposal distribution if we have not already done so above
VectorXf xv = multivariate_gauss(particles[i].xv(),
particles[i].Pv(),1);
particles[i].setXv(xv);
@@ -165,30 +165,3 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
return particles;
}
-MatrixXf make_laser_lines(vector<VectorXf> rb, VectorXf xv)
-{
- if (rb.isZero()) {
- return MatrixXf(0,0);
- }
-
- int len = rb.cols();
- MatrixXf lnes(4,len);
-
- MatrixXf globalMat(2,rb.cols());
- int j;
- for (j=0; j<globalMat.cols();j++) {
- globalMat(0,j) = rb(0,j)*cos(rb(1,j));
- globalMat(1,j) = rb(0,j)*sin(rb(1,j));
- }
-
- TransformToGlobal(globalMat,xv);
-
- for (int c=0; c<lnes.cols();c++) {
- lnes(0,c) = xv(0);
- lnes(1,c) = xv(1);
- lnes(2,c) = globalMat(0,c);
- lnes(3,c) = globalMat(1,c);
- }
-
- return line_plot_conversion(lnes);
-}
2  cpp/fastslam2/fastslam2_sim.h
View
@@ -17,6 +17,6 @@ using namespace std;
using namespace Eigen;
vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp);
-MatrixXf make_laser_lines(vector<VectorXf> rb, VectorXf xv);
+//MatrixXf make_laser_lines(vector<VectorXf> rb, VectorXf xv);
#endif //FASTSLAM2_SIM_H
156 cpp/fastslam2/main.cpp
View
@@ -1,136 +1,48 @@
-#include <stdio.h>
-#include <algorithm>
-#include <iterator>
-#include <unistd.h>
-#include <errno.h>
-#include <string>
-#include <vector>
-#include <Eigen/Dense>
-
#include "fastslam2_sim.h"
+#include "core/read_input_file.h"
#include "core/particle.h"
+#include "core/utilities.h"
using namespace Eigen;
using namespace std;
-void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp)
-{
- using std::ifstream;
- using std::istringstream;
-
- if(access(s.c_str(),R_OK) == -1) {
- std::cerr << "Unable to read input file" << s << std::endl;
- exit(EXIT_FAILURE);
- }
- ifstream in(s.c_str());
-
- int lineno = 0;
- int lm_rows =0;
- int lm_cols =0;
- int wp_rows =0;
- int wp_cols =0;
-
- while(in) {
- lineno++;
- string str;
- getline(in,str);
- istringstream line(str);
-
- vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<vector<string> > (tokens));
-
- if(tokens.size() ==0) {
- continue;
- }
- else if (tokens[0][0] =='#') {
- continue;
- }
- else if (tokens[0] == "lm") {
- if(tokens.size() != 3) {
- std::cerr<<"Wrong args for lm!"<<std::endl;
- std::cerr<<"Error occuredon line"<<lineno<<std::endl;
- std::cerr<<"line:"<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- lm_rows = strtof(tokens[1].c_str(),NULL);
- lm_cols = strtof(tokens[2].c_str(),NULL);
-
- lm->resize(lm_rows,lm_cols);
- for (int c =0; c<lm_cols; c++) {
- lineno++;
- if (!in) {
- std::cerr<<"EOF after reading" << std::endl;
- exit(EXIT_FAILURE);
- }
- getline(in,str);
- istringstream line(str);
- vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<vector<string> > (tokens));
- if(tokens.size() < lm_rows) {
- std::cerr<<"invalid line for lm coordinate!"<<std::endl;
- std::cerr<<"Error occured on line "<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
-
- for (unsigned r=0; r< lm_rows; r++) {
- (*lm)(r,c) = strtof(tokens[r].c_str(),NULL);
- }
- }
- }
- else if (tokens[0] == "wp") {
- if(tokens.size() != 3) {
- std::cerr<<"Wrong args for wp!"<<std::endl;
- std::cerr<<"Error occured on line"<<lineno<<std::endl;
- std::cerr<<"line:"<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- wp_rows = strtof(tokens[1].c_str(),NULL);
- wp_cols = strtof(tokens[2].c_str(),NULL);
- wp->resize(wp_rows, wp_cols);
- for (int c =0; c<wp_cols; c++) {
- lineno++;
- if (!in) {
- std::cerr<<"EOF after reading" << std::endl;
- exit(EXIT_FAILURE);
- }
- getline(in,str);
- istringstream line(str);
- std::vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<std::vector<string> > (tokens));
- if(tokens.size() < wp_rows) {
- std::cerr<<"invalid line for wp coordinate!"<<std::endl;
- std::cerr<<"Error occured on line "<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
-
- for (int r=0; r< lm_rows; r++) {
- (*wp)(r,c) = strtof(tokens[r].c_str(),NULL);
- }
- }
- }
- else {
- std::cerr<<"Unkwown command"<<tokens[0] <<std::endl;
- std::cerr<<"Error occured on line"<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- }
-}
-
-
int main (int argc, char *argv[])
{
+ MyTimer Timer = MyTimer();
+ Timer.Start();
+
MatrixXf lm; //landmark positions
MatrixXf wp; //way points
read_input_file("example_webmap.mat", &lm, &wp);
vector<Particle> data = fastslam2_sim(lm,wp);
+ for (int i=0; i<data.size(); i++) {
+ cout<<"particle i="<<i<<endl;
+ cout<<endl;
+ cout<<"weight"<<endl;
+ cout<<data[i].w()<<endl;
+ cout<<endl;
+ cout<<"xv (robot pose)"<<endl;
+ cout<<data[i].xv()<<endl;
+ cout<<endl;
+ cout<<"Pv (controls)"<<endl;
+ cout<<data[i].Pv()<<endl;
+ cout<<endl;
+ cout<<"xf (EFK means)"<<endl;
+ for(int j=0; j<data[i].xf().size(); j++) {
+ cout<<data[i].xf()[j]<<endl;
+ cout<<endl;
+ }
+ cout<<endl;
+ cout<<"Pf (covariance mat)"<<endl;
+ for(int k=0; k<data[i].Pf().size(); k++) {
+ cout<<data[i].Pf()[k]<<endl;
+ cout<<endl;
+ }
+ cout<<endl;
+ cout<<endl;
+ }
+
+ Timer.Stop();
+ Timer.Print("fastslam 1.0 ");
}
60 cpp/fastslam2/sample_proposal.cpp
View
@@ -17,14 +17,15 @@ void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, Ma
vector<MatrixXf> Hv;
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
-
- MatrixXf zpi(z.rows(),1);
- zpi.setZero();
+ vector<VectorXf> zp;
+
+ VectorXf zpi;
MatrixXf Hvi;
MatrixXf Hfi;
MatrixXf Sfi;
- VectorXf vi(z.rows());
+ //VectorXf vi(z.rows());
+ vector<VectorXf> vi;
//process each feature, incrementally refine proposal distribution
unsigned i,r;
@@ -32,26 +33,31 @@ void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, Ma
vector<int> j;
j.push_back(idf[i]);
- compute_jacobians(particle,j,R,zpi,&Hv,&Hf,&Sf);
- assert(zpi.cols() == 1);
+ compute_jacobians(particle,j,R,zp,&Hv,&Hf,&Sf);
+ //assert(zpi.cols() == 1);
+ zpi = zp[i];
Hvi = Hv[i];
Hfi = Hf[i];
Sfi = Sf[i].inverse();
- for (r=0; r<z.rows(); r++) {
- vi[r] = z(r,i) - zpi(r,0);
- }
-
- vi[1] = pi_to_pi(vi[1]);
+ VectorXf vi = z[i] - zpi;
+ vi[1] = pi_to_pi(vi[1]);
+ /*
+ //vi[1] = pi_to_pi(vi[1]);
+ for (int j=0; j<z.size(); j++) {
+ VectorXf v_j = z[j]-zpi;
+ v_j[1] = pi_to_pi(v_j[1]);
+ vi.push_back(v_j);
+ }*/
//proposal covariance
- MatrixXf Pv_inv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));
- Pv = Hvi.transpose() * Sfi * Hvi + Pv_inv;//Pv.inverse();
- Pv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));//Pv.inverse();
+ //MatrixXf Pv_inv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));
+ Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();//Pv_inv;//Pv.inverse();
+ Pv = Pv.inverse();//Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));//Pv.inverse();
//proposal mean
- xv = xv + Pv * Hvi.transpose() * Sfi *vi;
+ xv = xv + Pv * Hvi.transpose() * Sfi * vi;
particle.setXv(xv);
particle.setPv(Pv);
}
@@ -68,10 +74,13 @@ void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, Ma
float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
- particle.setW(particle.w() * like * prior / prop);
+ float newW = particle.w() * like * prior / prop;
+ particle.setW(newW);
+ //particle.setW(particle.w() * like * prior / prop);
}
-float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R)
+//float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R)
+float likelihood_given_xv(Particle particle, vector<VectorXf> z, vector<int>idf, MatrixXf R)
{
float w = 1;
vector<int> idfi;
@@ -80,22 +89,23 @@ float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixX
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
- MatrixXf zp;
+ //MatrixXf zp;
+ vector<VectorXf> zp;
MatrixXf Sfi;
- VectorXf v(z.rows());
+ VectorXf v(z.size());//z.rows());
unsigned i,k;
-
for (i=0; i<idf.size(); i++){
idfi.push_back(idf[i]);
- zp.resize(z.rows(), idfi.size());
+ //zp.resize(z.rows(), idfi.size());
compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
- for (k=0; k<z.rows(); k++) {
- v(k) = z(k,i)-zp(k,i);
- }
+ //for (k=0; k<z.rows(); k++) {
+ // v(k) = z(k,i)-zp(k,i);
+ //}
+ v = z[i] - zp[0];
v(1) = pi_to_pi(v(1));
-
+
w = w*gauss_evaluate(v,Sf[0],0);
}
return w;
6 cpp/fastslam2/sample_proposal.h
View
@@ -13,9 +13,9 @@
using namespace Eigen;
using namespace std;
-void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R);
-
-float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R);
+void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, MatrixXf R);
+//float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R);
+float likelihood_given_xv(Particle particle, vector<VectorXf> z, vector<int>idf, MatrixXf R);
VectorXf delta_xv(VectorXf xv1, VectorXf xv2);
#endif
2  cpp/gtest/CMakeLists.txt
View
@@ -4,7 +4,7 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
# Define here any libraries that you'll use later on in this
# file. Things like OpenCL or Boost libraries would go here. (Or your
# own FSLAM libs).
-set(FSLAM_USED_LIBS FastSLAM1 FastSLAM_core)
+set(FSLAM_USED_LIBS FastSLAM1 FastSLAM2 FastSLAM_core)
# Tests
fslam_add_test(TestFS1AgainstMatlab.cpp)
813 cpp/gtest/TestFS1AgainstMatlab.cpp
View
@@ -12,17 +12,73 @@
#include "fastslam1/predict.h"
#include <gtest/gtest.h>
#include "core/read_input_file.h"
+#include "core/stratified_resample.h"
+#include "core/stratified_random.h"
+#include "fastslam2/sample_proposal.h"
+
+#include <iostream>
+
+#define ZERO 1e-10
+#define SMALL 1e-4
+#define DELTA 1e-10
+#define isZero(A) ( (A < ZERO) && (A > -ZERO) )
+#define isSmall(A) ( (A < SMALL) && (A > -SMALL) )
+#define isSame(A, B) ( ((A-B) < ZERO) && ((A-B) > -ZERO) )
+#define isSimilar(A, B) ( ((A-B) < SMALL) && ((A-B) > -SMALL) )
+#define isBetween(A, B, C) ( ((A-B) > -ZERO) && ((A-C) < ZERO) )
+
+
+///////////////////////////////////////
+//Helpers
+
+template<class T>
+bool EqualVectors(vector<T> a, vector<T> b);
+
+template<class T>
+void printVector(vector<T> vec);
+
+ template<class T>
+void printVector(vector<T> vec)
+{
+ for(int i=0; i<vec.size(); i++) {
+ cout<<vec[i]<<" ";
+ }
+ cout<<endl;
+}
+
+template<class T>
+bool EqualVectors(vector<T> a, vector<T> b) {
+ if (a.size() != b.size()) {
+ return false;
+ }
+
+ bool c=true;
+ for (int i=0; i < a.size(); i++) {
+ c &= (a[i]==b[i]);
+ }
+
+ if(c == false) {
+ printVector(a);
+ cout<<endl;
+ printVector(b);
+ }
+
+ return c;
+}
+
+/////////////////////////////////////////
+//Tests
TEST(FASTSLAM_TEST,compute_steering_test)
{
MatrixXf lm; //landmark positions
- MatrixXf wp; //way points
-
- read_input_file("example_webmap.mat", &lm, &wp);
-
+ MatrixXf wp; //way points
+
+ read_input_file("example_webmap.mat", &lm, &wp);
+
VectorXf xtrue(3);
xtrue.setZero();
-
+
int iwp =0;
float minD = 1;
float G = 0;
@@ -31,7 +87,7 @@ TEST(FASTSLAM_TEST,compute_steering_test)
float dt = 0.0250;
compute_steering(xtrue,wp,iwp,minD,G,rateG,maxG,dt);
-
+
EXPECT_NE(G,-0.0087);
EXPECT_EQ(iwp,0);
}
@@ -40,17 +96,17 @@ TEST(FASTSLAM_TEST,predict_true_test)
{
VectorXf xtrue(3);
xtrue.setZero();
-
+
float V = 3;
float G = -0.0087;
int WB = 4;
float dt = 0.0250;
-
+
predict_true(xtrue,V,G,WB,dt);
-
+
VectorXf xtrue_out(3);
xtrue_out<<0.0750,-0.0007,-0.0002;
-
+
EXPECT_NE(xtrue,xtrue_out);
}
@@ -59,13 +115,746 @@ TEST(FASTSLAM_TEST,add_control_noise_test) {
float G = -0.0087;
Matrix2f Q;
Q <<0.0900,0,
- 0,0.0027;
+ 0,0.0027;
int SWITCH_CONTROL_NOISE = 1;
float* VnGn = new float[2];
add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
-
+
float Vn = 2.6056;
float Gn = -0.0305;
EXPECT_NE(VnGn[0],Vn);
EXPECT_NE(VnGn[1],Gn);
}
+
+TEST(FASTSLAM_TEST,predict_test){
+ float w = 0.01;
+ VectorXf xv(3);
+ xv.setZero();
+
+ vector<VectorXf> xf;
+ vector<MatrixXf> Pf;
+
+ float Vn = 3.0194;
+ float Gn = 0.0227;
+ MatrixXf Qe(2,2);
+ Qe<< 0.0900, 0,
+ 0,0.0027;
+
+ float WB = 4;
+ float dt = 0.0250;
+
+ int SWITCH_PREDICT_NOISE = 0;
+
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+ //predict
+ predict(particle,Vn,Gn,Qe,WB,dt,SWITCH_PREDICT_NOISE);
+
+ float w_after = 0.01;
+ VectorXf xv_after(3);
+ xv_after<<0.0755,0.0017,0.0004;
+ vector<VectorXf> xf_out;
+ vector<MatrixXf> Pf_out;
+
+ //EXPECT_NE(w_after, particle.w());
+ EXPECT_NE(xv_after[0], particle.xv()[0]);
+ EXPECT_NE(xv_after[1], particle.xv()[1]);
+ EXPECT_NE(xv_after[2], particle.xv()[2]);
+ EXPECT_TRUE(particle.xf().empty());
+ EXPECT_TRUE(particle.Pf().empty ());
+}
+
+TEST(FASTSLAM_TEST,get_observations_test)
+{
+
+ VectorXf xtrue(3);
+ xtrue<<0.6741,-0.0309,-0.0074;
+ MatrixXf lm; //landmark positions
+ MatrixXf wp; //way points
+
+ read_input_file("example_webmap.mat", &lm, &wp);
+
+ vector<int> ftag;
+ for (int i=0; i<lm.cols(); i++) {
+ ftag.push_back(i);
+ }
+
+ vector<int> ftag_visible = vector<int>(ftag);
+ EXPECT_EQ(ftag_visible.size(), ftag.size());
+ float rmax = 30;
+
+ vector<VectorXf> z_out = get_observations(xtrue,lm,ftag_visible,rmax);
+ vector<VectorXf> z_gt;
+
+ VectorXf a(2);
+ a<<25.7745,-1.4734;
+ VectorXf b(2);
+ b<<25.2762, 0.1384;
+ z_gt.push_back(a);
+ z_gt.push_back(b);
+
+ vector<int> ftag_visible_gt;
+ ftag_visible_gt.push_back(0);
+ ftag_visible_gt.push_back(21);
+
+ EXPECT_TRUE(EqualVectors(ftag_visible_gt,ftag_visible));
+ //EXPECT_TRUE(EqualVectors(z_gt,z_out));
+}
+
+TEST(FASTSLAM_TEST,add_observation_noise_test)
+{
+ vector<VectorXf> z;
+ VectorXf a(2);
+ a<<25.7745,-1.4734;
+ VectorXf b(2);
+ b<<25.2762,0.1384;
+ z.push_back(a);
+ z.push_back(b);
+
+ MatrixXf R(2,2);
+ R<<0.0100, 0,
+ 0, 0.0003;
+
+ int SWITCH_SENSOR_NOISE = 1;
+
+ add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
+
+ vector<VectorXf> z_gt;
+ VectorXf a_gt(2);
+ a_gt<<25.8522,-1.4621;
+ VectorXf b_gt(2);
+ b_gt<<25.3384,0.1309;
+ z_gt.push_back(a_gt);
+ z_gt.push_back(b_gt);
+
+ //EXPECT_TRUE(EqualVectors(z_gt,z));
+}
+
+TEST(FASTSLAM_TEST,data_associate_known_test)
+{
+ vector<VectorXf> z;
+ VectorXf a(2);
+ a<<25.7301,-1.4884;
+ VectorXf b(2);
+ b<<25.1439,0.1137;
+ z.push_back(a);
+ z.push_back(b);
+
+ vector<int> ftag_visible;
+ ftag_visible.push_back(0);
+ ftag_visible.push_back(21);
+
+ VectorXf da_table(35);
+ for (int i=0; i<35; i++) {
+ da_table[i]=-1;
+ }
+
+ int Nf = 0;
+
+ vector<VectorXf> zf;
+ vector<int> idf;
+ vector<VectorXf> zn;
+ data_associate_known(z,ftag_visible,da_table, Nf,zf,idf,zn);
+
+ vector<VectorXf> zn_gt;
+ VectorXf c(2);
+ c<<25.7301,-1.4884;
+ VectorXf d(2);
+ d<<25.1439,0.1137;
+ zn_gt.push_back(c);
+ zn_gt.push_back(d);
+
+ //check [zf,idf,zn,da_table]
+ EXPECT_TRUE(zf.empty());
+ EXPECT_TRUE(idf.empty());
+
+ VectorXf da_table_gt(35);
+ for (int i=0; i<35; i++) {
+ da_table_gt[i]=-1;
+ }
+ da_table_gt[0] = 0;
+ da_table_gt[21] =1;
+
+ EXPECT_TRUE(da_table_gt==da_table);
+ EXPECT_TRUE(EqualVectors(zn,zn_gt));
+}
+
+
+TEST(FASTSLAM_TEST,compute_jacobian_test)
+{
+ float w = 0.01;
+
+ VectorXf xv(3);
+ xv << 1.3393, -0.1241, -0.0278;
+
+ vector<VectorXf> xf;
+ VectorXf a(2);
+ a<<3.4924 ,-25.7649;
+ VectorXf b(2);
+ b<<25.6182,3.9462;
+ xf.push_back(a);
+ xf.push_back(b);
+
+ vector<MatrixXf> Pf;
+ MatrixXf c(2,2);
+ c<< 0.2016,0.0210,
+ 0.0210, 0.0123;
+ MatrixXf d(2,2);
+ d<< 0.0146, -0.0288,
+ -0.0288, 0.1898;
+ Pf.push_back(c);
+ Pf.push_back(d);
+
+ vector<VectorXf> zf;
+ VectorXf e(2);
+ e<<25.5817,-1.4737;
+ VectorXf f(2);
+ f<<24.6040,0.1458;
+ zf.push_back(e);
+ zf.push_back(f);
+
+ vector<int> idf;
+ idf.push_back(0);
+ idf.push_back(1);
+
+ MatrixXf R(2,2);
+ R<< 0.0100, 0,
+ 0, 0.0003;
+
+ //particle
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+ vector<VectorXf> zp;
+ vector<MatrixXf> Hv;
+ vector<MatrixXf> Hf;
+ vector<MatrixXf> Sf;
+ compute_jacobians(particle,idf,R,zp,&Hv,&Hf,&Sf);
+
+ //compute jacobians
+ vector<VectorXf> zp_gt;
+ VectorXf g(2);
+ g<<25.7310,-1.4593;
+ VectorXf h(2);
+ f<<24.6177,0.1939;
+ zp_gt.push_back(g);
+ zp_gt.push_back(f);
+
+ vector<MatrixXf> Hv_gt;
+ MatrixXf i(2,3);
+ i<<-0.0837,0.9965,0,
+ -0.0387,-0.0033,-1.0;
+ MatrixXf j(2,3);
+ j<< -0.9862,-0.1653,0,
+ 0.0067,-0.0401,-1.0;
+ Hv_gt.push_back(i);
+ Hv_gt.push_back(j);
+
+ vector<MatrixXf> Hf_gt;
+ MatrixXf m(2,2);
+ m<<0.0837,-0.9965,
+ 0.0387, 0.0033;
+ MatrixXf n(2,2);
+ n<<0.9862, 0.1653,
+ -0.0067, 0.0401;
+ Hf_gt.push_back(m);
+ Hf_gt.push_back(n);
+
+
+ vector<MatrixXf> Sf_gt;
+ MatrixXf k(2,2);
+ k<< 0.0201, -0.0002,
+ -0.0002, 0.0006;
+ MatrixXf l(2,2);
+ l<< 0.0200, 0.0001,
+ 0.0001, 0.0006;
+ Sf_gt.push_back(k);
+ Sf_gt.push_back(l);
+
+ //tests (zp,Hv,Hf,Sf)
+ //EXPECT_TRUE(EqualVectors(zp,zp_gt));
+ //EXPECT_TRUE(EqualVectors(Hv,Hv_gt));
+ //EXPECT_TRUE(EqualVectors(Hf,Hf_gt));
+ //EXPECT_TRUE(EqualVectors(Sf,Sf_gt));
+}
+
+TEST(FASTSLAM_TEST,compute_weight_test)
+{
+ //w
+ float w = 0.01;
+
+ //xv
+ VectorXf xv(3);
+ xv << 1.2746 ,-0.1712 ,-0.0380;
+
+ //xf
+ vector<VectorXf> xf;
+ VectorXf a(2);
+ a<<3.7387,-25.7130;
+ VectorXf b(2);
+ b<<25.9565,2.8205;
+ xf.push_back(a);
+ xf.push_back(b);
+
+ //Pf
+ vector<MatrixXf> Pf;
+ MatrixXf c(2,2);
+ c<<0.2005, 0.0230,
+ 0.0230,0.0128;
+ MatrixXf d(2,2);
+ d<<0.0124, -0.0211,
+ -0.0211,0.1953;
+ Pf.push_back(c);
+ Pf.push_back(d);
+
+ //particle
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+ //zf
+ vector<VectorXf> zf;
+ VectorXf e(2);
+ e<<25.6975,-1.4795;
+ VectorXf f(2);
+ f<<24.5414,0.1713;
+ zf.push_back(e);
+ zf.push_back(f);
+
+ //idf
+ vector<int> idf;
+ idf.push_back(0);
+ idf.push_back(1);
+
+ //R
+ MatrixXf R(2,2);
+ R<< 0.0100, 0,
+ 0, 0.0003;
+
+ float w_out = compute_weight(particle, zf,idf,R);
+ float w_gt = 29.6451;
+ EXPECT_NE(w_out,w_gt);
+}
+
+TEST(FASTSLAM_TEST,feature_update_test)
+{
+ //w
+ float w = 2.6993;
+
+ //xv
+ VectorXf xv(3);
+ xv << 1.3010, -0.1420, -0.0315;
+
+ //xf
+ vector<VectorXf> xf;
+ VectorXf a(2);
+ a<<2.7368,-25.5656;
+ VectorXf b(2);
+ b<<25.9803,2.4939;
+ xf.push_back(a);
+ xf.push_back(b);
+
+ //Pf
+ vector<MatrixXf> Pf;
+ MatrixXf c(2,2);
+ c<<0.1983, 0.0155,
+ 0.0155,0.0113;
+ MatrixXf d(2,2);
+ d<<0.0119, -0.0187,
+ -0.0187,0.1958;
+ Pf.push_back(c);
+ Pf.push_back(d);
+
+ //particle
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+ //zf
+ vector<VectorXf> zf;
+ VectorXf e(2);
+ e<<25.4653,-1.4981;
+ VectorXf f(2);
+ f<<24.6960,0.1803;
+ zf.push_back(e);
+ zf.push_back(f);
+
+
+ //idf
+ vector<int> idf;
+ idf.push_back(0);
+ idf.push_back(1);
+
+ //R
+ MatrixXf R(2,2);
+ R<< 0.0100, 0,
+ 0, 0.0003;
+
+ //feature update
+ feature_update(particle,zf,idf,R);
+
+ //w_gt
+ float w_gt = 2.6993;
+
+ //xv_gt
+ VectorXf xv_gt(3);
+ xv_gt<< 1.3010, -0.1420, -0.0315;
+
+ //xf_gt
+ vector<VectorXf> xf_gt;
+ VectorXf g(2);
+ g<<2.5430,-25.5796;
+ VectorXf h(2);
+ h<<25.8635,3.0197;
+ xf_gt.push_back(g);
+ xf_gt.push_back(h);
+
+ //Pf_gt
+ vector<MatrixXf> Pf_gt;
+ MatrixXf i(2,2);
+ i<< 0.0985, 0.0065,
+ 0.0065, 0.0055;
+ MatrixXf j(2,2);
+ j<< 0.0060, -0.0094,
+ -0.0094,0.0953;
+ Pf_gt.push_back(i);
+ Pf_gt.push_back(j);
+
+ EXPECT_EQ(particle.w(),w_gt);
+ EXPECT_EQ(particle.xv(),xv_gt);
+ //EXPECT_TRUE(EqualVectors(particle.xf(),xf_gt));
+ //EXPECT_TRUE(EqualVectors(particle.Pf(),Pf_gt));
+}
+
+
+TEST(FASTSLAM_TEST,add_feature_test)
+{
+ float w=0.0100;
+ //xv
+ VectorXf xv(3);
+ xv<<0.6981, -0.0280,-0.0067;
+
+ //xf
+ vector<VectorXf> xf;
+
+ //Pf
+ vector<MatrixXf> Pf;
+
+ //particle
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+ //zf
+ vector<VectorXf> zn;
+ VectorXf a(2);
+ a<<25.8047,-1.4638;
+ VectorXf b(2);
+ b<<25.2023,0.1198;
+ zn.push_back(a);
+ zn.push_back(b);
+
+ //R
+ MatrixXf R(2,2);
+ R<< 0.0100, 0,
+ 0, 0.0003;
+
+
+ //add_feature
+ add_feature(particle,zn,R);
+
+ float w_out = 0.01;
+
+ //xv_out
+ VectorXf xv_out(3);
+ xv_out<<0.6981,-0.0280,-0.0067;
+
+ //Xf out
+ vector<VectorXf> xf_out;
+ VectorXf c(2);
+ c<<3.2830, -25.7030;
+ VectorXf d(2);
+ d<< 25.7394, 2.8163;
+ xf_out.push_back(c);
+ xf_out.push_back(d);
+
+ //Pf out
+ vector<MatrixXf> Pf_out;
+ MatrixXf e(2,2);
+ e<<0.2009, 0.0192,
+ 0.0192,0.0119;
+ MatrixXf f(2,2);
+ f<<0.0123, -0.0206,
+ -0.0206, 0.1911;
+ Pf_out.push_back(e);
+ Pf_out.push_back(f);
+
+ EXPECT_EQ(w_out,particle.w());
+ EXPECT_EQ(xv_out,particle.xv());
+ //EXPECT_TRUE(EqualVectors(xf_out, particle.xf()));
+ //EXPECT_TRUE(EqualVectors(Pf_out, particle.Pf()));
+}
+
+TEST(FASTSLAM_TEST, stratified_random_test)
+{
+ int N = 10;
+ vector<float> s;
+ stratified_random(10, s);
+
+ vector<float> s_gt;
+ s_gt.push_back(0.0948);
+ s_gt.push_back(0.1334);
+ s_gt.push_back(0.2390);
+ s_gt.push_back(0.3150);
+ s_gt.push_back(0.4334);
+ s_gt.push_back(0.5554);
+ s_gt.push_back(0.6550);
+ s_gt.push_back(0.7160);
+ s_gt.push_back(0.8117);
+ s_gt.push_back(0.9399);
+
+ int i;
+ cout<<"s is"<<endl;
+ for (i=0; i<s.size(); i++) {
+ cout<<s[i]<<" ";
+ }
+ cout<<endl;
+
+ cout<<"s_gt is"<<endl;
+ for (i=0; i<s_gt.size(); i++) {
+ cout<<s_gt[i]<<" ";
+ }
+ cout<<endl;
+
+ //EXPECT_TRUE(EqualVectors(s,s_gt));
+}
+
+TEST(FASTSLAM_TEST, stratified_resample_test)
+{
+ vector<int> keep;
+ float Neff;
+
+ VectorXf w(10);
+ w<< 0.8074, 0.2651, 0.8959, 0.6080, 0.0144, 0.3440, 0.5337, 0.6278, 0.4467, 0.8111;
+ stratified_resample(w, keep,Neff);
+
+ vector<int> keep_gt;
+ keep_gt.push_back(0);
+ keep_gt.push_back(0);
+ keep_gt.push_back(2);
+ keep_gt.push_back(2);
+ keep_gt.push_back(3);
+ keep_gt.push_back(6);
+ keep_gt.push_back(7);
+ keep_gt.push_back(7);
+ keep_gt.push_back(8);
+ keep_gt.push_back(9);
+
+ int i;
+ cout<<"keep"<<endl;
+ for (i=0; i<keep.size(); i++) {
+ cout<<keep[i]<<" ";
+ }
+ cout<<endl;
+
+ cout<<"keep_gt"<<endl;
+ for (i=0; i<keep_gt.size(); i++) {
+ cout<<keep_gt[i]<<" ";
+ }
+ cout<<endl;
+
+ EXPECT_TRUE(EqualVectors(keep_gt, keep));
+ EXPECT_NE(Neff, 8.0764);
+}
+
+/*
+TEST(FASTSLAM_TEST, fastslam1_sim_test)
+{
+ MatrixXf lm; //landmark positions
+ MatrixXf wp; //way points
+
+ read_input_file("example_webmap.mat", &lm, &wp);
+
+ vector<Particle> p = fastslam1_sim(lm,wp);
+ VectorXf xv_gt;
+ xv_gt<<-12.0425,-63.0278,1.1090;
+ //EXPECT_NE(p[50].xv(),xv_gt);
+ //EXPECT_NE(p[50].Pf[0],);
+}
+*/
+
+TEST(FASTSLAM_TEST, sample_proposal_test)
+{
+ //w
+ float w = 0.01;
+
+ //xv
+ VectorXf xv(3);
+ xv << 1.2767, -0.1756, -0.0394;
+
+ //Pv
+ MatrixXf Pv(3,3);
+ Pv<< 0.0004927, -0.0000653, -0.0000126,
+ -0.0000653, 0.0001592, 0.0000369,
+ -0.0000126, 0.0000369, 0.0000086;
+
+ //xf
+ vector<VectorXf> xf;
+ VectorXf a(2);
+ a<<2.4261,-25.8041;
+ VectorXf b(2);
+ b<<25.7095,3.2392;
+ xf.push_back(a);
+ xf.push_back(b);
+
+ //Pf
+ vector<MatrixXf> Pf;
+ MatrixXf c(2,2);
+ c<< 0.2019, 0.0133,
+ 0.0133, 0.0109;
+ MatrixXf d(2,2);
+ d <<0.0131, -0.0239,
+ -0.0239, 0.1915;
+ Pf.push_back(c);
+ Pf.push_back(d);
+
+ //particle
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setPv(Pv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+
+ vector<VectorXf> zf;
+ VectorXf i(2);
+ i<<25.74325,-1.4901;
+ VectorXf j(2);
+ j<<24.4729, 0.1353;
+ zf.push_back(i);
+ zf.push_back(j);
+
+ //idf
+ vector<int> idf;
+ idf.push_back(0);
+ idf.push_back(1);
+
+ //R
+ MatrixXf R(2,2);
+ R<< 0.0100, 0,
+ 0, 0.0003;
+
+ sample_proposal(particle,zf, idf, R);
+
+ // sample_proposal
+ float w_gt = 1.6005;
+
+ VectorXf xv_gt(3);
+ xv_gt<< 1.2935, -0.1718, -0.0387;
+
+ MatrixXf Pv_gt(3,3);
+ Pv_gt.setZero();
+
+ vector<VectorXf> xf_gt;
+ VectorXf e(2);
+ a<<2.4261,-25.8041;
+ VectorXf f(2);
+ b<<25.7095,3.2392;
+ xf_gt.push_back(a);
+ xf_gt.push_back(b);
+
+ vector<MatrixXf> Pf_gt;
+ MatrixXf g(2,2);
+ c<< 0.2019, 0.0133,
+ 0.0133, 0.0109;
+ MatrixXf h(2,2);
+ d <<0.0131, -0.0239,
+ -0.0239, 0.1915;
+ Pf_gt.push_back(c);
+ Pf_gt.push_back(d);
+
+ EXPECT_EQ(w_gt,particle.w());
+ EXPECT_EQ(xv_gt,particle.xv());
+ EXPECT_EQ(Pv_gt,particle.Pv());
+ EXPECT_TRUE(EqualVectors(xf_gt,particle.xf()));
+ EXPECT_TRUE(EqualVectors(Pf_gt,particle.Pf()));
+}
+
+TEST(FASTSLAM_TEST, likelihood_given_xv_test)
+{
+ //w
+ float w = 0.01;
+
+ //xv
+ VectorXf xv(3);
+ xv << 1.3502, -0.1073, -0.0237;
+
+ //Pv
+ MatrixXf Pv(3,3);
+ Pv.setZero();
+
+ //xf
+ vector<VectorXf> xf;
+ VectorXf a(2);
+ a<<2.2237,-25.8443;
+ VectorXf b(2);
+ b<<25.6484,3.6701;
+ xf.push_back(a);
+ xf.push_back(b);
+
+ //Pf
+ vector<MatrixXf> Pf;
+ MatrixXf c(2,2);
+ c<< 0.2029, 0.0117,
+ 0.0117, 0.0107;
+ MatrixXf d(2,2);
+ d<< 0.0140, -0.0268,
+ -0.0268, 0.1904;
+ Pf.push_back(c);
+ Pf.push_back(d);
+
+ //particle
+ Particle particle = Particle();
+ particle.setW(w);
+ particle.setXv(xv);
+ particle.setPv(Pv);
+ particle.setXf(xf);
+ particle.setPf(Pf);
+
+ vector<VectorXf> zf;
+ VectorXf i(2);
+ i<<25.6029,-1.4864;
+ VectorXf j(2);
+ j<<24.7119, 0.1680;
+ zf.push_back(i);
+ zf.push_back(j);
+
+ //idf
+ vector<int> idf;
+ idf.push_back(0);
+ idf.push_back(1);
+
+ //R
+ MatrixXf R(2,2);
+ R<< 0.0100, 0,
+ 0, 0.0003;
+
+
+ likelihood_given_xv(particle, zf,idf, R);
+
+ EXPECT_NE(particle.w(), 0.01);
+}
Please sign in to comment.
Something went wrong with that request. Please try again.