Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

getting zack's help on debugging

  • Loading branch information...
commit 38c517d547644b50a8e6982640098e9f3ceb77ca 1 parent fcccc59
@yglee authored
Showing with 29,026 additions and 1,647 deletions.
  1. +67 −36 cpp/add_feature.cpp
  2. +3 −51 cpp/compute_jacobians.cpp
  3. +30 −42 cpp/data_associate_known.cpp
  4. +77 −52 cpp/fastslam2_sim.cpp
  5. +7 −2 cpp/feature_update.cpp
  6. +0 −1  cpp/get_observations.cpp
  7. +0 −1,130 cpp/log
  8. +1 −1  cpp/makefile
  9. +0 −298 cpp/output
  10. +0 −12 cpp/pi_to_pi.cpp
  11. +1 −1  cpp/pi_to_pi.h
  12. +6 −21 cpp/sample_proposal.cpp
  13. +5 −0 testcode/.gitignore
  14. +20 −0 testcode/KF_cholesky_update.cpp
  15. +10 −0 testcode/KF_cholesky_update.h
  16. +61 −0 testcode/KF_joseph_update.cpp
  17. +11 −0 testcode/KF_joseph_update.h
  18. +13 −0 testcode/README
  19. +30 −0 testcode/TransformToGlobal.cpp
  20. +11 −0 testcode/TransformToGlobal.h
  21. +17 −0 testcode/add_control_noise.cpp
  22. +11 −0 testcode/add_control_noise.h
  23. +91 −0 testcode/add_feature.cpp
  24. +13 −0 testcode/add_feature.h
  25. +20 −0 testcode/add_observation_noise.cpp
  26. +11 −0 testcode/add_observation_noise.h
  27. +105 −0 testcode/compute_jacobians.cpp
  28. +19 −0 testcode/compute_jacobians.h
  29. +60 −0 testcode/compute_steering.cpp
  30. +11 −0 testcode/compute_steering.h
  31. +50 −0 testcode/configfile.cpp
  32. +47 −0 testcode/configfile.h
  33. +81 −0 testcode/data_associate_known.cpp
  34. +13 −0 testcode/data_associate_known.h
  35. +56 −0 testcode/example_webmap.mat
  36. +277 −0 testcode/fastslam2_sim.cpp
  37. +22 −0 testcode/fastslam2_sim.h
  38. +76 −0 testcode/feature_update.cpp
  39. +17 −0 testcode/feature_update.h
  40. +79 −0 testcode/gauss_evaluate.cpp
  41. +26 −0 testcode/gauss_evaluate.h
  42. +88 −0 testcode/get_observations.cpp
  43. +14 −0 testcode/get_observations.h
  44. +36 −0 testcode/line_plot_conversion.cpp
  45. +10 −0 testcode/line_plot_conversion.h
  46. +136 −0 testcode/main.cpp
  47. +56 −0 testcode/makefile
  48. +20 −0 testcode/multivariate_gauss.cpp
  49. +11 −0 testcode/multivariate_gauss.h
  50. +39 −0 testcode/observe_heading.cpp
  51. +9 −0 testcode/observe_heading.h
  52. +26,003 −0 testcode/output_old
  53. +101 −0 testcode/particle.cpp
  54. +41 −0 testcode/particle.h
  55. +50 −0 testcode/pi_to_pi.cpp
  56. +16 −0 testcode/pi_to_pi.h
  57. +74 −0 testcode/predict.cpp
  58. +14 −0 testcode/predict.h
  59. +9 −0 testcode/predict_true.cpp
  60. +13 −0 testcode/predict_true.h
  61. +15 −0 testcode/printMat.h
  62. +44 −0 testcode/resample_particles.cpp
  63. +14 −0 testcode/resample_particles.h
  64. +164 −0 testcode/sample_proposal.cpp
  65. +27 −0 testcode/sample_proposal.h
  66. +33 −0 testcode/stratified_random.cpp
  67. +13 −0 testcode/stratified_random.h
  68. +49 −0 testcode/stratified_resample.cpp
  69. +13 −0 testcode/stratified_resample.h
  70. +234 −0 testcode/temp/main.cpp
  71. +225 −0 testcode/temp/old_main.cpp
View
103 cpp/add_feature.cpp
@@ -9,24 +9,30 @@ using namespace std;
// add new features
//
void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
-{
+{
+ cout<<"ADD_FEATURE"<<endl;
+ cout<<"z"<<endl;
+ cout<<z<<endl;
+ cout<<"R"<<endl;
+ cout<<R<<endl;
+ cout<<"particle.w"<<endl;
+ cout<<particle.w()<<endl;
+ cout<<"particle.xv"<<endl;
+ cout<<particle.xv()<<endl;
+ cout<<"particle.Pv"<<endl;
+ cout<<particle.Pv()<<endl;
+ cout<<"particle.xf"<<endl;
+ cout<<particle.xf()<<endl;
+ cout<<"particle.Pf"<<endl;
+ for (int i=0; i<(particle.Pf()).size();i++) {
+ cout<<particle.Pf()[i]<<endl;
+ }
+
int lenz = z.cols();
MatrixXf xf(2,lenz);
vector<MatrixXf> Pf;
VectorXf xv = particle.xv();
- //TODO: this doesn't match :(
- //TODO: fix this!!!
- #if 0
- cout<<"xv in add_feature"<<endl;
- cout<<xv<<endl;
- cout<<"should be"<<endl;
- cout<<"0.7648"<<endl;
- cout<<"0.0221"<<endl;
- cout<<"0.0050"<<endl;
- cout<<endl;
- #endif
-
float r,b,s,c;
MatrixXf Gz(2,2);
@@ -49,43 +55,68 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
ii.push_back(i);
}
- MatrixXf xfCopy = particle.xf();
- vector<MatrixXf> pfCopy(particle.Pf());
+ //MatrixXf xfCopy;// = particle.xf();
+ //vector<MatrixXf> pfCopy(particle.Pf());
+
+ cout<<"xf"<<endl;
+ cout<<xf<<endl;
+
+//TODO: there is a bug here. xfCopy (which is particle.xf) should grow in size
+//based on length of ii
+//new xfCopy columns = max(ii.size(),xfCopy.cols())
+//initialize to 0
+
+ //stupid dynamically sized matlab matrices :((((
+ int prows = particle.xf().rows();
+ int pcols = particle.xf().cols();
+ int iisize = ii.size();
+
+ //max value in ii
+ vector<int>::iterator maxelem;
+ maxelem = std::max_element(ii.begin(),ii.end());
+ int xfc = std::max((*maxelem)+1,pcols);
- for (unsigned c=0; c<ii.size(); c++) {
- #if 0
- cout<<"ii"<<endl;
- cout<<ii[c]<<endl;
- #endif
+ cout<<"particle.xf().cols()"<<endl;
+ cout<<pcols<<endl;
+ cout<<"max"<<endl;
+ cout<<xfc<<endl;
+ MatrixXf xfCopy(prows,xfc);
+ xfCopy.setZero();
+ for (int i =0; i<prows; i++) {
+ for (int j=0; j<pcols; j++) {
+ xfCopy(i,j) =particle.xf()(i,j);
+ }
+ }
+
+ //stupid matlab... I need to dynamically
+ //pfCopy
+ int pfcols = particle.Pf().size();
+ int Pfc = std::max((*maxelem)+1,pfcols);
+ vector<MatrixXf> pfCopy(Pfc);
+ for (int i=0; i<pfcols; i++) {
+ pfCopy[i] = particle.Pf()[i];
+ }
+
+ for (unsigned l=0; l<ii.size(); l++) {
+ if (ii.size() == 0) {
+ break;
+ }
if (xfCopy.isZero()) {
xfCopy = xf;
} else {
- for (unsigned r=0; r<xf.rows(); r++) {
- xfCopy(r,ii[c]) = xf(r,c);
+ for (unsigned k=0; k<xf.rows(); k++) {
+ xfCopy(k,ii[l]) = xf(k,l);
}
}
if (pfCopy.empty()) {
pfCopy = Pf;
} else {
- pfCopy[ii[c]] = Pf[c];
+ pfCopy[ii[l]] = Pf[l];
}
}
- #if 0
- cout<<"xf in add_feature"<<endl;
- cout<<xf<<endl;
- cout<<"should be"<<endl;
- cout<<"3.4011 25.7814"<<endl;
- cout<<"-25.6172 3.6347"<<endl;
- cout<<endl;
-
- cout<<"xfCopy"<<endl;
- cout<<xfCopy<<endl;
- cout<<endl;
- #endif
-
particle.setXf(xfCopy);
particle.setPf(pfCopy);
}
View
54 cpp/compute_jacobians.cpp
@@ -11,34 +11,15 @@ void compute_jacobians(Particle particle,
vector<MatrixXf> *Hf,
vector<MatrixXf> *Sf)
{
- zp.resize(2,idf.size());
- zp.setZero();
-
VectorXf xv = particle.xv();
- #if 0
- cout<<"compute_jacobians: particle.xf()"<<endl;
- cout<<particle.xf()<<endl;
- #endif
-
int rows = (particle.xf()).rows();
MatrixXf xf(rows,idf.size());
vector<MatrixXf> Pf;
- unsigned i,r;
-
- #if 0
- cout<<"compute jacobians:"<<endl;
- cout<<"particle.xf().rows() "<<(particle.xf()).rows()<<endl;
- cout<<"particle.xf().cols() "<<(particle.xf()).cols()<<endl;
- cout<<"idf.size() "<<idf.size()<<endl;
- cout<<"idf max "<<idf[idf.size()-1]<<endl;
- cout<<"xf.rows() "<<xf.rows()<<endl;
- cout<<"xf.cols() "<<xf.cols()<<endl;
- cout<<endl;
- #endif
-
- for (i=0; i<idf.size(); i++) {
+ unsigned i;
+ int r;
+ for (unsigned i=0; i<idf.size(); i++) {
for (r=0; r<(particle.xf()).rows(); r++) {
xf(r,i) = (particle.xf())(r,(idf[i]));
}
@@ -51,40 +32,11 @@ void compute_jacobians(Particle particle,
MatrixXf HvMat(2,3);
MatrixXf HfMat (2,2);
-#if 0
- cout<<"before for loop in compute_jacobians"<<endl;
- cout<<"idf.size()"<<idf.size()<<endl;
- cout<<"xv "<<endl;
- cout<<xv<<endl;
- cout<<"should be 1.5052"<<endl;
- cout<<" -0.0293"<<endl;
- cout<<" -0.0079"<<endl;
- cout<<endl;
- cout<<"xf"<<endl;
- cout<<xf<<endl;
- cout<<"should be 3.4011"<<endl;
- cout<<" -25.6172"<<endl;
- cout<<endl;
- cout<<"Pf"<<endl;
- cout<<Pf[0]<<endl;
- cout<<"should be 0.2005 0.0195"<<endl;
- cout<<" 0.0196 0.0120"<<endl;
- cout<<endl;
-#endif
-
for (i=0; i<idf.size(); i++) {
dx = xf(0,i) - xv(0);
dy = xf(1,i) - xv(1);
d2 = pow(dx,2) + pow(dy,2);
d = sqrt(d2);
-
-#if 0
- cout<<"dx"<<dx<<endl;
- cout<<"dy"<<dy<<endl;
- cout<<"d2"<<d2<<endl;
- cout<<"d"<<d<<endl;
- cout<<"about to print zp"<<endl;
-#endif
//predicted observation
zp(0,i) = d;
View
72 cpp/data_associate_known.cpp
@@ -4,65 +4,53 @@
void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
MatrixXf &zf, vector<int> &idf, MatrixXf &zn)
{
- zf.setZero();
- zn.setZero();
idf.clear();
-
- unsigned i,ii,r;
vector<int> idn;
+ unsigned i,ii,r;
+
+ //this extra loopis required to set the dimension of zn and zf
+ int zncols = 0;
+ int zfcols = 0;
+ for (i =0; i< idz.size(); i++){
+ ii = idz[i];
+ if (table(ii) ==-1) { //new feature
+ zncols++;
+ }
+ else {
+ zfcols++;
+ }
+ }
+
+ //resize
+ zn.resize(z.rows(),zncols);
+ zn.setZero();
+ zf.resize(z.rows(),zfcols);
+ zf.setZero();
+
+ int znc,zfc;
+ znc =0;
+ zfc =0;
for (i =0; i< idz.size(); i++){
ii = idz[i];
if (table(ii) ==-1) { //new feature
for (r=0; r<z.rows();r++) {
- zn(r,i) = z(r,i);
+ zn(r,znc) = z(r,i);
}
+ znc++;
idn.push_back(ii);
}
else {
for (r=0; r<z.rows(); r++) {
- zf(r,i) = z(r,i);
+ zf(r,zfc) = z(r,i);
}
+ zfc++;
idf.push_back(table(ii));
}
}
- #if 0
- //test code
- vector<int>::iterator idniter;
- cout<<"idn: "<<endl;
- for (idniter=idn.begin(); idniter!=idn.end(); idniter++) {
- cout<<(*idniter)<<" ";
- }
- cout<<endl;
- cout<<"idn should be 0 and 21"<<endl;
-
- vector<int>::iterator idfiter;
- cout<<"idf: "<<endl;
- for (idfiter=idf.begin(); idfiter!=idf.end(); idfiter++) {
- cout<<(*idfiter)<<" ";
+ for (int i=0; i<idn.size(); i++) {
+ table(idn[i]) = Nf+i;
}
- cout<<endl;
- cout<<"idf should be empty"<<endl;
-
- cout<<"Nf is "<<Nf<<endl;
- cout<<"Nf should be 0"<<endl;
- #endif
-
- //TODO: look into this later (table(ii) == 0) but counter starts from index 0...
- //add new feature IDs to lookup table
- vector<int> counter;
- for (unsigned int c=0; c<zn.cols(); c++) {
- counter.push_back(c+Nf);
- }
-
- for (unsigned int d=0; d<idn.size(); d++) {
- table(idn[d]) = counter[d];
- }
-
- #if 0
- cout<<"table"<<endl;
- cout<<table<<endl;
- #endif
}
View
129 cpp/fastslam2_sim.cpp
@@ -109,22 +109,8 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
//Predict step
for (unsigned int i=0; i< NPARTICLES; i++) {
- if(i==47) {
- cout<<"predict"<<endl<<flush;
- }
predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
- if(i==47) {
- cout<<particles[i].Pv()<<endl<<flush;
- cout<<particles[i].xv()<<endl<<flush;
- cout<<endl<<flush;
- cout<<"observe"<<endl<<flush;
- }
observe_heading(particles[i], xtrue(2), SWITCH_HEADING_KNOWN); //if heading known, observe heading
- if(i==47) {
- cout<<particles[i].Pv()<<endl<<flush;
- cout<<particles[i].xv()<<endl<<flush;
- cout<<endl<<flush;
- }
}
//Observe step
@@ -144,41 +130,96 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
//Compute (known) data associations
int Nf = (particles[0].xf()).cols();
vector<int> idf;
+ if (ftag_visible.size() ==2) {
+ cout<<"INPUT GET OBSERVE"<<endl;
+ cout<<"xtrue"<<endl;
+ cout<<xtrue<<endl;
+ cout<<"lm"<<endl;
+ cout<<lm<<endl;
+ cout<<"ftag_visible"<<endl;
+ for (int i=0; i<ftag.size(); i++) {
+ cout<<ftag[i]<<" ";
+ }
+ cout<<endl;
+ cout<<"max range: "<<MAX_RANGE<<endl;
+ cout<<"OUTPUT GET_OBSERVE"<<endl;
+ cout<<"ftag_visible"<<endl;
+ for (int i=0; i<ftag_visible.size(); i++) {
+ cout<<ftag_visible[i]<<" ";
+ }
+ cout<<"z"<<endl;
+ cout<<z<<endl;
+ cout<<endl;
+ }
+
MatrixXf zf(z.rows(),ftag_visible.size());
zf.setZero();
MatrixXf zn(z.rows(),ftag_visible.size());
zn.setZero();
- data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
-
- //observe map features
+
+ bool testflag= false;
+ if(ftag_visible.size() ==2) {
+ testflag = true;
+ cout<<"INPUT TO DATA_ASSOC"<<endl;
+ cout<<"z"<<endl;
+ cout<<z<<endl;
+ cout<<"ftag_visible"<<endl;
+ for (int i=0; i<ftag_visible.size(); i++) {
+ cout<<ftag_visible[i]<<" ";
+ }
+ cout<<endl;
+ cout<<"da_table"<<endl;
+ for (int i=0; i<da_table.size(); i++) {
+ cout<<da_table[i]<<" ";
+ }
+ cout<<endl;
+ cout<<"Nf"<<endl;
+ cout<<Nf<<endl;
+ cout<<"zf"<<endl;
+ cout<<zf<<endl;
+ for (int i=0; i<idf.size(); i++) {
+ cout<<idf[i]<<" ";
+ }
+ cout<<endl;
+ cout<<"zn"<<endl;
+ cout<<zn<<endl;
+ cout<<"idf"<<endl;
+ for (int i=0; i<idf.size(); i++) {
+ cout<<idf[i]<<" ";
+ }
+ }
+ data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
+ if(testflag==true) {
+ cout<<"OUTPUT DATA_ASSOC"<<endl;
+ cout<<"da_table"<<endl;
+ for (int i=0; i<da_table.size(); i++) {
+ cout<<da_table[i]<<" ";
+ }
+ cout<<endl;
+ cout<<"zf"<<endl;
+ cout<<zf<<endl;
+ for (int i=0; i<idf.size(); i++) {
+ cout<<idf[i]<<" ";
+ }
+ cout<<endl;
+ cout<<"zn"<<endl;
+ cout<<zn<<endl;
+ cout<<"idf"<<endl;
+ for (int i=0; i<idf.size(); i++) {
+ cout<<idf[i]<<" ";
+ }
+ cout<<endl;
+ }
+ //observe map features
if (!zf.isZero()) {
//isample from 'optimal' proposal distribution, then update map
for (unsigned i=0; i<NPARTICLES; i++) {
- //cout<<"sample Proposal i= "<<i<<endl<<flush;
- if (i==47) {
- cout<<"sample_proposal"<<endl<<flush;
- }
sample_proposal(particles[i], zf, idf, Re);
- if (i==47) {
- cout<<particles[i].Pv()<<endl<<flush;
- cout<<particles[i].xv()<<endl<<flush;
- cout<<endl<<flush;
- cout<<"feature_update"<<i<<endl<<flush;
- }
feature_update(particles[i],zf,idf,Re);
- if(i==47){
- cout<<particles[47].Pv()<<endl<<flush;
- cout<<particles[47].xv()<<endl<<flush;
- cout<<endl<<flush;
- }
}
//resample
- cout<<"resample_particles"<<endl<<flush;
resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
- cout<<particles[47].Pv()<<endl<<flush;
- cout<<particles[47].xv()<<endl<<flush;
- cout<<endl<<flush;
}
//Observe new features, augment map
@@ -186,31 +227,15 @@ vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
if (!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(i==47) {
- cout<<"multivar gauss"<<endl<<flush;
- }
VectorXf xv = multivariate_gauss(particles[i].xv(),
particles[i].Pv(),1);
- if(i==47) {
- cout<<particles[i].Pv()<<endl<<flush;
- cout<<particles[i].xv()<<endl<<flush;
- cout<<endl<<flush;
- }
//TODO: xv[0] seems to have an approximation error from chol.
particles[i].setXv(xv);
MatrixXf pv(3,3);
pv.setZero();
particles[i].setPv(pv);
}
- if (i==47) {
- cout<<"add_feature"<<endl<<flush;
- }
add_feature(particles[i], zn, Re);
- if(i==47) {
- cout<<particles[i].Pv()<<endl<<flush;
- cout<<particles[i].xv()<<endl<<flush;
- cout<<endl<<flush;
- }
}
}
count++;
View
9 cpp/feature_update.cpp
@@ -19,7 +19,8 @@ void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
Pf.push_back((particle.Pf())[idf[i]]); //particle.Pf is a array of matrices
}
- MatrixXf zp;
+ MatrixXf zp(z.rows(), idf.size());
+ zp.setZero();
vector<MatrixXf> Hv;
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
@@ -37,6 +38,10 @@ void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
cout<<endl;
#endif
+ cout<<"z"<<endl;
+ cout<<z.rows()<<" "<<z.cols()<<endl;
+ cout<<"zp"<<endl;
+ cout<<zp.rows()<<" "<<zp.cols()<<endl;
MatrixXf v = z-zp; //TODO: need to fix: idf.size() is zero. which makes this break.
unsigned c;
@@ -59,7 +64,7 @@ void feature_update(Particle &particle, MatrixXf z, vector<int>idf, MatrixXf R)
xfi(r) = xf(r,i); //xfi = xf.column(i);
}
KF_cholesky_update(xfi,Pfi,vi,R,Hfi);
- //TODO: insert xfi into xf, etc
+
for (r=0; r<xf.rows(); r++) {
xf(r,i) = xfi(r);
}
View
1  cpp/get_observations.cpp
@@ -22,7 +22,6 @@ void get_visible_landmarks(VectorXf x, MatrixXf &lm, vector<int> &idf, float rma
{
//select set of landmarks that are visible within vehicle's
//semi-circular field of view
-
MatrixXf dx(1,lm.cols());
MatrixXf dy(1,lm.cols());
for (int c=0; c<lm.cols(); c++) {
View
1,130 cpp/log
@@ -1,1130 +0,0 @@
-==10128== Memcheck, a memory error detector
-==10128== Copyright (C) 2002-2011, and GNU GPL'd, by Julian Seward et al.
-==10128== Using Valgrind-3.7.0 and LibVEX; rerun with -h for copyright info
-==10128== Command: ./fastslam
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083310: Particle::xv() const (DenseCoeffsBase.h:508)
-==10128== by 0x8085163: predict(Particle&, float, float, Eigen::Matrix<float, 2, 2, 0, 2, 2>, float, float, int) (predict.cpp:11)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808331B: Particle::xv() const (Assign.h:315)
-==10128== by 0x8085163: predict(Particle&, float, float, Eigen::Matrix<float, 2, 2, 0, 2, 2>, float, float, int) (predict.cpp:11)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c8 is 8 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80834E0: Particle::Pv() const (DenseCoeffsBase.h:508)
-==10128== by 0x8085180: predict(Particle&, float, float, Eigen::Matrix<float, 2, 2, 0, 2, 2>, float, float, int) (predict.cpp:12)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80834EB: Particle::Pv() const (Assign.h:315)
-==10128== by 0x8085180: predict(Particle&, float, float, Eigen::Matrix<float, 2, 2, 0, 2, 2>, float, float, int) (predict.cpp:12)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f948 is 8 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083BCE: Particle::setPv(Eigen::Matrix<float, -1, -1, 0, -1, -1>) (particle.cpp:82)
-==10128== by 0x8085C16: predict(Particle&, float, float, Eigen::Matrix<float, 2, 2, 0, 2, 2>, float, float, int) (predict.cpp:31)
-==10128== by 0x805750A: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:122)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083AFF: Particle::setXv(Eigen::Matrix<float, -1, 1, 0, -1, 1>) (particle.cpp:74)
-==10128== by 0x8085DE6: predict(Particle&, float, float, Eigen::Matrix<float, 2, 2, 0, 2, 2>, float, float, int) (predict.cpp:49)
-==10128== by 0x805750A: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:122)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836E0: Particle::xf() const (DenseCoeffsBase.h:508)
-==10128== by 0x8057A41: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:148)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836EB: Particle::xf() const (Assign.h:315)
-==10128== by 0x8057A41: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:148)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083310: Particle::xv() const (DenseCoeffsBase.h:508)
-==10128== by 0x808E9F3: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:10)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808331B: Particle::xv() const (Assign.h:315)
-==10128== by 0x808E9F3: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:10)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c8 is 8 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80834E0: Particle::Pv() const (DenseCoeffsBase.h:508)
-==10128== by 0x808EA10: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:11)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80834EB: Particle::Pv() const (Assign.h:315)
-==10128== by 0x808EA10: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:11)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f948 is 8 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A818: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A823: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c8 is 8 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A908: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A913: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f948 is 8 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A9F8: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA03: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA90: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA93: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AAF2: Particle::Particle(Particle const&) (PlainObjectBase.h:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AAF5: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABD6: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABDF: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABEC: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab708 is 0 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABF0: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab780 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABFC: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab788 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083AFF: Particle::setXv(Eigen::Matrix<float, -1, 1, 0, -1, 1>) (particle.cpp:74)
-==10128== by 0x80903F4: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:136)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083BCE: Particle::setPv(Eigen::Matrix<float, -1, -1, 0, -1, -1>) (particle.cpp:82)
-==10128== by 0x8090469: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:137)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A818: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A823: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c8 is 8 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A908: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A913: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f948 is 8 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A9F8: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA03: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA90: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA93: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AAF2: Particle::Particle(Particle const&) (PlainObjectBase.h:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AAF5: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABD6: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABDF: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABEC: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab708 is 0 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABF0: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab780 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABFC: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x808ED4A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:45)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab788 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083AFF: Particle::setXv(Eigen::Matrix<float, -1, 1, 0, -1, 1>) (particle.cpp:74)
-==10128== by 0x809058A: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:142)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083BCE: Particle::setPv(Eigen::Matrix<float, -1, -1, 0, -1, -1>) (particle.cpp:82)
-==10128== by 0x80905FD: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:143)
-==10128== by 0x80583CD: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:160)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A818: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x1: ???
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A823: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x1: ???
-==10128== Address 0x432f8c8 is 8 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A908: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A913: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x432f948 is 8 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808A9F8: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA03: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA90: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AA93: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AAF2: Particle::Particle(Particle const&) (PlainObjectBase.h:45)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808AAF5: Particle::Particle(Particle const&) (resample_particles.cpp:45)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABD6: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABDF: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABEC: Particle::Particle(Particle const&) (Assign.h:510)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab708 is 0 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABF0: Particle::Particle(Particle const&) (DenseCoeffsBase.h:508)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab780 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808ABFC: Particle::Particle(Particle const&) (Assign.h:315)
-==10128== by 0x809077C: sample_proposal(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (sample_proposal.cpp:146)
-==10128== by 0x1: ???
-==10128== Address 0x47ab788 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836E0: Particle::xf() const (DenseCoeffsBase.h:508)
-==10128== by 0x805B0E3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:10)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836EB: Particle::xf() const (Assign.h:315)
-==10128== by 0x805B0E3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:10)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836E0: Particle::xf() const (DenseCoeffsBase.h:508)
-==10128== by 0x805B270: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:16)
-==10128== by 0x1: ???
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836EB: Particle::xf() const (Assign.h:315)
-==10128== by 0x805B270: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:16)
-==10128== by 0x1: ???
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836E0: Particle::xf() const (DenseCoeffsBase.h:508)
-==10128== by 0x805B21C: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:17)
-==10128== by 0x1: ???
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80836EB: Particle::xf() const (Assign.h:315)
-==10128== by 0x805B21C: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:17)
-==10128== by 0x1: ???
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083870: Particle::Pf() const (particle.cpp:100)
-==10128== by 0x1: ???
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083873: Particle::Pf() const (particle.cpp:100)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80838D3: Particle::Pf() const (PlainObjectBase.h:45)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x80838D6: Particle::Pf() const (particle.cpp:100)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x808393E: Particle::Pf() const (Assign.h:510)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083947: Particle::Pf() const (Assign.h:510)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083954: Particle::Pf() const (Assign.h:510)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab708 is 0 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083958: Particle::Pf() const (DenseCoeffsBase.h:508)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab780 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083964: Particle::Pf() const (Assign.h:315)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab788 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8083870: Particle::Pf() const (particle.cpp:100)
-==10128== by 0x805B2B3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:19)
-==10128== by 0x1: ???
-==10128== Address 0x47ab71c is 20 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805B7A0: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (DenseCoeffsBase.h:508)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c0 is 0 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805B7AB: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:315)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f8c8 is 8 bytes inside a block of size 12 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805B8CC: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (DenseCoeffsBase.h:508)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f940 is 0 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805B8D7: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:315)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x432f948 is 8 bytes inside a block of size 36 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x808325D: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805B9F8: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (DenseCoeffsBase.h:508)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BA03: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:315)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab438 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BAB0: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:71)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BAB3: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:71)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BB17: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (PlainObjectBase.h:45)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BB1A: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:71)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BD5E: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:510)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BD67: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:510)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BD74: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:510)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab708 is 0 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BD78: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (DenseCoeffsBase.h:508)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab780 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x805BD83: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (Assign.h:315)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab788 is 8 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8083CA3: Particle::setXf(Eigen::Matrix<float, -1, -1, 0, -1, -1>) (DenseCoeffsBase.h:508)
-==10128== by 0x805CD17: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:69)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab430 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083252: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8082C16: _ZNSt6vectorIN5Eigen6MatrixIfLin1ELin1ELi0ELin1ELin1EEESaIS2_EEaSERKS4_.constprop.94 (DenseStorage.h:216)
-==10128== by 0x8083F21: Particle::setPf(std::vector<Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<float, -1, -1, 0, -1, -1> > >) (particle.cpp:94)
-==10128== by 0x805D503: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:70)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8082C1C: _ZNSt6vectorIN5Eigen6MatrixIfLin1ELin1ELi0ELin1ELin1EEESaIS2_EEaSERKS4_.constprop.94 (DenseStorage.h:216)
-==10128== by 0x8083F21: Particle::setPf(std::vector<Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<float, -1, -1, 0, -1, -1> > >) (particle.cpp:94)
-==10128== by 0x805D503: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:70)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8082C4C: _ZNSt6vectorIN5Eigen6MatrixIfLin1ELin1ELi0ELin1ELin1EEESaIS2_EEaSERKS4_.constprop.94 (DenseStorage.h:225)
-==10128== by 0x8083F21: Particle::setPf(std::vector<Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<float, -1, -1, 0, -1, -1> > >) (particle.cpp:94)
-==10128== by 0x805D503: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:70)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab70c is 4 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8082C4F: _ZNSt6vectorIN5Eigen6MatrixIfLin1ELin1ELi0ELin1ELin1EEESaIS2_EEaSERKS4_.constprop.94 (DenseStorage.h:226)
-==10128== by 0x8083F21: Particle::setPf(std::vector<Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<float, -1, -1, 0, -1, -1> > >) (particle.cpp:94)
-==10128== by 0x805D503: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:70)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab710 is 8 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid read of size 4
-==10128== at 0x8082C68: _ZNSt6vectorIN5Eigen6MatrixIfLin1ELin1ELi0ELin1ELin1EEESaIS2_EEaSERKS4_.constprop.94 (Assign.h:510)
-==10128== by 0x8083F21: Particle::setPf(std::vector<Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<float, -1, -1, 0, -1, -1> > >) (particle.cpp:94)
-==10128== by 0x805D503: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:70)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab708 is 0 bytes inside a block of size 24 free'd
-==10128== at 0x402ACFC: operator delete(void*) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083247: Particle::~Particle() (new_allocator.h:98)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== Invalid write of size 4
-==10128== at 0x8082C73: _ZNSt6vectorIN5Eigen6MatrixIfLin1ELin1ELi0ELin1ELin1EEESaIS2_EEaSERKS4_.constprop.94 (DenseCoeffsBase.h:508)
-==10128== by 0x8083F21: Particle::setPf(std::vector<Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<float, -1, -1, 0, -1, -1> > >) (particle.cpp:94)
-==10128== by 0x805D503: feature_update(Particle&, Eigen::Matrix<float, -1, -1, 0, -1, -1>, std::vector<int, std::allocator<int> >, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (feature_update.cpp:70)
-==10128== by 0x80585D4: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:161)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128== Address 0x47ab780 is 0 bytes inside a block of size 16 free'd
-==10128== at 0x402B06C: free (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x8083234: Particle::~Particle() (Memory.h:240)
-==10128== by 0x808A462: resample_particles(std::vector<Particle, std::allocator<Particle> >&, int, int) (stl_construct.h:94)
-==10128== by 0x805873D: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:165)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-fastslam: resample_particles.cpp:11: void resample_particles(std::vector<Particle>&, int, int): Assertion `particles.size() == config::NPARTICLES' failed.
-==10128==
-==10128== HEAP SUMMARY:
-==10128== in use at exit: 6,916 bytes in 40 blocks
-==10128== total heap usage: 167,384 allocs, 167,344 frees, 12,271,489 bytes allocated
-==10128==
-==10128== 168 bytes in 21 blocks are definitely lost in loss record 10 of 15
-==10128== at 0x402B454: operator new[](unsigned int) (in /usr/lib/valgrind/vgpreload_memcheck-x86-linux.so)
-==10128== by 0x805740C: fastslam2_sim(Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>) (fastslam2_sim.cpp:105)
-==10128== by 0x804B266: main (main.cpp:135)
-==10128==
-==10128== LEAK SUMMARY:
-==10128== definitely lost: 168 bytes in 21 blocks
-==10128== indirectly lost: 0 bytes in 0 blocks
-==10128== possibly lost: 0 bytes in 0 blocks
-==10128== still reachable: 6,748 bytes in 19 blocks
-==10128== suppressed: 0 bytes in 0 blocks
-==10128== Reachable blocks (those to which a pointer was found) are not shown.
-==10128== To see them, rerun with: --leak-check=full --show-reachable=yes
-==10128==
-==10128== For counts of detected and suppressed errors, rerun with: -v
-==10128== ERROR SUMMARY: 50205 errors from 100 contexts (suppressed: 0 from 0)
View
2  cpp/makefile
@@ -31,7 +31,7 @@ endif
CC := gcc
CXX := g++
-CXXFLAGS := -g -Wall -O3 -fmessage-length=0 -DGL_GLEXT_PROTOTYPES $(INCLUDE) $(MACROS)
+CXXFLAGS := -g -Wall -O0 -fmessage-length=0 -DGL_GLEXT_PROTOTYPES $(INCLUDE) $(MACROS)
LDFLAGS := $(FRAMEWORK) $(LIBRARY)
#-----------------------------------------
%.o : %.cpp
View
298 cpp/output
@@ -1,298 +0,0 @@
-predict
-5.62469e-05 -3.5628e-07 -8.907e-08
--3.5628e-07 1.54244e-05 3.85609e-06
- -8.907e-08 3.85609e-06 9.64023e-07
-0.0749971
--0.00065449
--0.000163624
-
-observe
-5.62469e-05 -3.5628e-07 -8.907e-08
--3.5628e-07 1.54244e-05 3.85609e-06
- -8.907e-08 3.85609e-06 9.64023e-07
-0.0749971
--0.00065449
--0.000163624
-
-predict
- 0.000112484 -1.0769e-06 -2.65278e-07
- -1.0769e-06 3.1442e-05 7.78684e-06
--2.65278e-07 7.78684e-06 1.92863e-06
-0.149986
--0.00197569
--0.000490873
-
-observe
- 0.000112484 -1.0769e-06 -2.65278e-07
- -1.0769e-06 3.1442e-05 7.78684e-06
--2.65278e-07 7.78684e-06 1.92863e-06
-0.149986
--0.00197569
--0.000490873
-
-predict
- 0.000168704 -2.16934e-06 -5.26626e-07
--2.16934e-06 4.80708e-05 1.17939e-05
--5.26626e-07 1.17939e-05 2.89421e-06
-0.224959
--0.00397576
--0.000981743
-
-observe
- 0.000168704 -2.16934e-06 -5.26626e-07
--2.16934e-06 4.80708e-05 1.17939e-05
--5.26626e-07 1.17939e-05 2.89421e-06
-0.224959
--0.00397576
--0.000981743
-
-predict
- 0.000224899 -3.64051e-06 -8.71049e-07
--3.64051e-06 6.53288e-05 1.58789e-05
--8.71049e-07 1.58789e-05 3.86114e-06
-0.299911
--0.00666681
--0.00163623
-
-observe
- 0.000224899 -3.64051e-06 -8.71049e-07
--3.64051e-06 6.53288e-05 1.58789e-05
--8.71049e-07 1.58789e-05 3.86114e-06
-0.299911
--0.00666681
--0.00163623
-
-predict
- 0.000281059 -5.49667e-06 -1.29641e-06
--5.49667e-06 8.32348e-05 2.00436e-05
--1.29641e-06 2.00436e-05 4.82983e-06
-0.374834
--0.0100609
--0.00245434
-
-observe
- 0.000281059 -5.49667e-06 -1.29641e-06
--5.49667e-06 8.32348e-05 2.00436e-05
--1.29641e-06 2.00436e-05 4.82983e-06
-0.374834
--0.0100609
--0.00245434
-
-predict
- 0.000337176 -7.74342e-06 -1.8005e-06
--7.74342e-06 0.000101808 2.42899e-05
- -1.8005e-06 2.42899e-05 5.80064e-06
-0.449721
--0.0141699
--0.00343606
-
-observe
- 0.000337176 -7.74342e-06 -1.8005e-06
--7.74342e-06 0.000101808 2.42899e-05
- -1.8005e-06 2.42899e-05 5.80064e-06
-0.449721
--0.0141699
--0.00343606
-
-predict
- 0.000393239 -1.03857e-05 -2.38104e-06
--1.03857e-05 0.000121067 2.86195e-05
--2.38104e-06 2.86195e-05 6.77398e-06
-0.524565
--0.0190057
--0.00458139
-
-observe
- 0.000393239 -1.03857e-05 -2.38104e-06
--1.03857e-05 0.000121067 2.86195e-05
--2.38104e-06 2.86195e-05 6.77398e-06
-0.524565
--0.0190057
--0.00458139
-
-predict
- 0.000449237 -1.34277e-05 -3.03567e-06
--1.34277e-05 0.000141033 3.30344e-05
--3.03567e-06 3.30344e-05 7.75023e-06
-0.599358
--0.0245802
--0.00589032
-
-observe
- 0.000449237 -1.34277e-05 -3.03567e-06
--1.34277e-05 0.000141033 3.30344e-05
--3.03567e-06 3.30344e-05 7.75023e-06
-0.599358
--0.0245802
--0.00589032
-
-multivar gauss
- 0.000449237 -1.34277e-05 -3.03567e-06
--1.34277e-05 0.000141033 3.30344e-05
--3.03567e-06 3.30344e-05 7.75023e-06
-0.599358
--0.0245802
--0.00589032
-
-add_feature
-0 0 0
-0 0 0
-0 0 0
-0.620553
--0.0133549
--0.0031437
-
-predict
- 5.59782e-05 -3.32022e-06 -7.86057e-07
--3.32022e-06 1.56931e-05 3.92064e-06
--7.86057e-07 3.92064e-06 9.79537e-07
-0.695303
--0.0194743
--0.00461623
-
-observe
- 5.59782e-05 -3.32022e-06 -7.86057e-07
--3.32022e-06 1.56931e-05 3.92064e-06
--7.86057e-07 3.92064e-06 9.79537e-07
-0.695303
--0.0194743
--0.00461623
-
-predict
- 0.000111874 -7.08182e-06 -1.64738e-06
--7.08182e-06 3.20491e-05 7.93069e-06
--1.64738e-06 7.93069e-06 1.96275e-06
-0.769986
--0.0263558
--0.00625235
-
-observe
- 0.000111874 -7.08182e-06 -1.64738e-06
--7.08182e-06 3.20491e-05 7.93069e-06
--1.64738e-06 7.93069e-06 1.96275e-06
-0.769986
--0.0263558
--0.00625235
-
-predict
- 0.000167673 -1.12884e-05 -2.58146e-06
--1.12884e-05 4.90901e-05 1.20322e-05
--2.58146e-06 1.20322e-05 2.95002e-06
-0.844595
--0.0340109
--0.00805205
-
-observe
- 0.000167673 -1.12884e-05 -2.58146e-06
--1.12884e-05 4.90901e-05 1.20322e-05
--2.58146e-06 1.20322e-05 2.95002e-06
-0.844595
--0.0340109
--0.00805205
-
-predict
- 0.000223363 -1.59428e-05 -3.58568e-06
--1.59428e-05 6.68381e-05 1.62273e-05
--3.58568e-06 1.62273e-05 3.94173e-06
-0.919118
--0.0424508
--0.0100153
-
-observe
- 0.000223363 -1.59428e-05 -3.58568e-06
--1.59428e-05 6.68381e-05 1.62273e-05
--3.58568e-06 1.62273e-05 3.94173e-06
-0.919118
--0.0424508
--0.0100153
-
-predict
- 0.000278928 -2.10468e-05 -4.65735e-06
--2.10468e-05 8.5316e-05 2.05181e-05
--4.65735e-06 2.05181e-05 4.93826e-06
-0.993547
--0.051687
--0.0121422
-
-observe
- 0.000278928 -2.10468e-05 -4.65735e-06
--2.10468e-05 8.5316e-05 2.05181e-05
--4.65735e-06 2.05181e-05 4.93826e-06
-0.993547
--0.051687
--0.0121422
-
-predict
- 0.000334353 -2.66014e-05 -5.79369e-06
--2.66014e-05 0.000104547 2.49068e-05
--5.79369e-06 2.49068e-05 5.93999e-06
-1.06787
--0.0617304
--0.0144325
-
-observe
- 0.000334353 -2.66014e-05 -5.79369e-06
--2.66014e-05 0.000104547 2.49068e-05
--5.79369e-06 2.49068e-05 5.93999e-06
-1.06787
--0.0617304
--0.0144325
-
-predict
- 0.000389621 -3.26066e-05 -6.99179e-06
--3.26066e-05 0.000124554 2.93954e-05
--6.99179e-06 2.93954e-05 6.94729e-06
-1.14208
--0.0725919
--0.0168865
-
-observe
- 0.000389621 -3.26066e-05 -6.99179e-06
--3.26066e-05 0.000124554 2.93954e-05
--6.99179e-06 2.93954e-05 6.94729e-06
-1.14208
--0.0725919
--0.0168865
-
-predict
- 0.000444717 -3.90612e-05 -8.24867e-06
--3.90612e-05 0.00014536 3.39863e-05
--8.24867e-06 3.39863e-05 7.96055e-06
-1.21616
--0.0842825
--0.0195039
-
-observe
- 0.000444717 -3.90612e-05 -8.24867e-06
--3.90612e-05 0.00014536 3.39863e-05
--8.24867e-06 3.39863e-05 7.96055e-06
-1.21616
--0.0842825
--0.0195039
-
-sample_proposal
--1.3237e-05 -1.3237e-05 3.36312e-44
--1.3237e-05 4.28797e-33 4.48416e-44
-4.30763e-33 -1.3237e-05 4.28289e-33
-1.23702
--0.0743896
--0.0170313
-
-feature_update47
--1.3237e-05 -1.3237e-05 3.36312e-44
--1.3237e-05 4.28797e-33 4.48416e-44
-4.30763e-33 -1.3237e-05 4.28289e-33
-1.23702
--0.0743896
--0.0170313
-
-resample_particles
-ws is 0
-len in stratified resample 100
-4.27414e-33 -1.3237e-05 3.36312e-44
--1.3237e-05 4.28797e-33 4.48416e-44
-4.30763e-33 -1.3237e-05 4.28289e-33
-4.27123e-33
--0.0743896
--0.0170313
-
View
12 cpp/pi_to_pi.cpp
@@ -36,15 +36,3 @@ float pi_to_pi(float ang)
return ang;
}
-#if 0
-vector<int> find1(VectorXf input)
-{
- vector<int> index;
- for (int i =0; i<input.size(); i++) {
- if ((input[i] > 2*pi) || (input[i] < -2*pi)){
- index.push_back(i);
- }
- }
- return index;
-}
-#endif
View
2  cpp/pi_to_pi.h
@@ -11,6 +11,6 @@ using namespace std;
void pi_to_pi(VectorXf &angle); //takes in array of floats, returna array
float pi_to_pi(float ang);
-vector<int> find1(VectorXf input);
+//vector<int> find1(VectorXf input);
#endif //PI_TO_PI_H
View
27 cpp/sample_proposal.cpp
@@ -17,7 +17,8 @@ void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
- MatrixXf zpi;
+ MatrixXf zpi(z.rows(),1);
+ zpi.setZero();
MatrixXf Hvi;
MatrixXf Hfi;
MatrixXf Sfi;
@@ -129,39 +130,23 @@ float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixX
vector<MatrixXf> Hv;
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
-
+
MatrixXf zp;
MatrixXf Sfi;
VectorXf v(z.rows());
unsigned i,k;
- //cout<<"idf.size() "<<idf.size()<<endl;
+
for (i=0; i<idf.size(); i++){
idfi.push_back(idf[i]);
+ zp.resize(z.rows(), idfi.size());
compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
- cout<<"at idf = "<<i<<endl;
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<endl;
- cout<<"zp"<<endl;
- cout<<zp<<endl;
- cout<<endl;
-
for (k=0; k<z.rows(); k++) {
- v(k) = z(k,i)-zp(k,i); //TODO: this returns wrong values
+ v(k) = z(k,i)-zp(k,i);
}
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;
}
View
5 testcode/.gitignore
@@ -0,0 +1,5 @@
+*.swp
+test/**/*
+*~
+*.o
+.DS_Store
View
20 testcode/KF_cholesky_update.cpp
@@ -0,0 +1,20 @@
+#include "KF_cholesky_update.h"
+
+void KF_cholesky_update(VectorXf &x, MatrixXf &P,VectorXf v,MatrixXf R,MatrixXf H)
+{
+ MatrixXf PHt = P*H.transpose();
+ MatrixXf S = H*PHt + R;
+
+ S = (S+S.transpose()) * 0.5; //make symmetric
+ MatrixXf SChol = S.llt().matrixL();
+ SChol.transpose();
+ SChol.conjugate();
+
+ MatrixXf SCholInv = SChol.inverse(); //tri matrix
+ MatrixXf W1 = PHt * SCholInv;
+ MatrixXf W = W1 * SCholInv.transpose();
+
+ //TODO: last three lines
+ x = x + W*v;
+ P = P - W1*W1.transpose();
+}
View
10 testcode/KF_cholesky_update.h
@@ -0,0 +1,10 @@
+#ifndef KF_CHOLESKY_UPDATE_H
+#define KF_CHOLESKY_UPDATE_H
+
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+void KF_cholesky_update(VectorXf &x,MatrixXf &P,VectorXf v,MatrixXf R,MatrixXf H);
+
+#endif
View
61 testcode/KF_joseph_update.cpp
@@ -0,0 +1,61 @@
+#include "KF_joseph_update.h"
+#include <iostream>
+#include <math.h>
+
+using namespace std;
+
+//TODO: check this!
+void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
+{
+ VectorXf PHt = P*H.transpose();
+ cout<<"KF_Joseph: PHt"<<endl;
+ cout<<PHt<<endl;
+ MatrixXf S = H*PHt;
+ S(0,0) += R;
+ cout<<"S"<<endl;
+ cout<<S<<endl;
+ MatrixXf Si = S.inverse();
+ cout<<"Si before symmetric"<<endl;
+ cout<<Si<<endl;
+ Si = make_symmetric(Si);
+ cout<<"Si after symmetric"<<endl;
+ cout<<Si<<endl;
+ MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
+ PSD_check.transpose();
+ PSD_check.conjugate();
+ cout<<"PSD_check"<<endl;
+ cout<<PSD_check<<endl;
+
+ VectorXf W = PHt*Si;
+ cout<<"W"<<endl;
+ cout<<W<<endl;
+ x = x+W*v;
+ cout<<"x"<<endl;
+ cout<<x<<endl;
+
+ //Joseph-form covariance update
+ MatrixXf eye(P.rows(), P.cols());
+ eye.setIdentity();
+ MatrixXf C = eye - W*H;
+ cout<<"C"<<endl;
+ cout<<C<<endl;
+ P = C*P*C.transpose() + W*R*W.transpose();
+ cout<<"P"<<endl;
+ cout<<P<<endl;
+
+ float eps = 2.2204*pow(10.0,-16); //numerical safety
+ cout<<"eps"<<endl;
+ cout<<eps<<endl;
+ P = P+eye*eps;
+ cout<<"P"<<endl;
+ cout<<P<<endl;
+
+ PSD_check = P.llt().matrixL();
+ PSD_check.transpose();
+ PSD_check.conjugate(); //for upper tri
+}
+
+MatrixXf make_symmetric(MatrixXf P)
+{
+ return (P + P.transpose())*0.5;
+}
View
11 testcode/KF_joseph_update.h
@@ -0,0 +1,11 @@
+#ifndef KF_JOSEPH_UPDATE_H
+#define KF_JOSEPH_UPDATE_H
+
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+void KF_joseph_update(VectorXf &x,MatrixXf &P,float v,float R, MatrixXf H);
+MatrixXf make_symmetric(MatrixXf P);
+
+#endif
View
13 testcode/README
@@ -0,0 +1,13 @@
+
+8/21/12
+even a little bit of numerical error results in exp(E) to go zero (when E<0) in gauss evaluate.
+The problem is the input to gauss evaluate, v. It's set in sample_proposal. line 172. v is wrong...
+
+Do a check:
+is likelhood_given_xv returning a 0 with the right inputs? Then, the function has a problem. If the inputs are wrong, then sample_particles might be wrong. Check this (add a break in likelihood_given_xv.
+
+
+w is zero in resample particles. w is set in sample_proposal. The likelihood given xv returns a 0 everytime, causing w to be 0. This results in nan's, which should throw a division by 0 error
+
+7/25/12
+fix the multivariate gauss's use of Random() (from eigen). It doesn't return diff. random numbers at each turn
View
30 testcode/TransformToGlobal.cpp
@@ -0,0 +1,30 @@
+#include "TransformToGlobal.h"
+#include "pi_to_pi.h"
+
+void TransformToGlobal(MatrixXf &p, VectorXf b)
+{
+ //rotate
+ MatrixXf rot(2,2);
+ rot<<cos(b(2)), -sin(b(2)), sin(b(2)), cos(b(2));
+
+ MatrixXf p_resized;
+ p_resized = MatrixXf(p);
+ p_resized.conservativeResize(2,p_resized.cols());
+ p_resized = rot*p_resized;
+
+ //translate
+ int c;
+ for (c=0;c<p_resized.cols();c++) {
+ p(0,c) = p_resized(0,c)+b(0);
+ p(1,c) = p_resized(1,c)+b(1);
+ }
+
+ float input;
+ //if p is a pose and not a point
+ if (p.rows() ==3){
+ for (int k=0; k<p_resized.cols();k++) {
+ input = p(2,k) +b(2);
+ p(2,k) = pi_to_pi(input);
+ }
+ }
+}
View
11 testcode/TransformToGlobal.h
@@ -0,0 +1,11 @@
+#ifndef TRANSFORMGLOBAL_H
+#define TRANSFORMGLOBAL_H
+
+#include <Eigen/Dense>
+#include <iostream>
+
+using namespace Eigen;
+
+void TransformToGlobal(MatrixXf &p, VectorXf b);
+
+#endif //TRANSFORMGLOBAL_H
View
17 testcode/add_control_noise.cpp
@@ -0,0 +1,17 @@
+#include "add_control_noise.h"
+#include <iostream>
+
+using namespace std;
+
+void add_control_noise(float V, float G, Matrix2f Q, int addnoise, float* VnGn)
+{
+ if (addnoise ==1) {
+ VectorXf A(2);
+ A(0) = V;
+ A(1) = G;
+ VectorXf C(2);
+ C = multivariate_gauss(A,Q,1);
+ VnGn[0] = C(0);
+ VnGn[1] = C(1);
+ }
+}
View
11 testcode/add_control_noise.h
@@ -0,0 +1,11 @@
+#ifndef ADD_CONTROL_NOISE
+#define ADD_CONTROL_NOISE
+
+#include <Eigen/Dense>
+#include "multivariate_gauss.h"
+
+using namespace Eigen;
+
+void add_control_noise(float V, float G, Matrix2f Q, int addnoise,float* VnGn);
+
+#endif //ADD_CONTROL_NOISE
View
91 testcode/add_feature.cpp
@@ -0,0 +1,91 @@
+#include "add_feature.h"
+#include <math.h>
+#include <vector>
+#include <iostream>
+
+using namespace std;
+
+//
+// add new features
+//
+void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
+{
+ int lenz = z.cols();
+ MatrixXf xf(2,lenz);
+ vector<MatrixXf> Pf;
+ VectorXf xv = particle.xv();
+
+ //TODO: this doesn't match :(
+ //TODO: fix this!!!
+ #if 0
+ cout<<"xv in add_feature"<<endl;
+ cout<<xv<<endl;
+ cout<<"should be"<<endl;
+ cout<<"0.7648"<<endl;
+ cout<<"0.0221"<<endl;
+ cout<<"0.0050"<<endl;
+ cout<<endl;
+ #endif
+
+ float r,b,s,c;
+ MatrixXf Gz(2,2);
+
+ for (int i=0; i<lenz; i++) {
+ r = z(0,i);
+ b = z(1,i);
+ s = sin(xv(2)+b);
+ c = cos(xv(2)+b);
+
+ xf(0,i) = xv(0) + r*c;
+ xf(1,i) = xv(1) + r*s;
+ Gz <<c,-r*s,s,r*c;
+
+ Pf.push_back(Gz*R*Gz.transpose());
+ }
+
+ int lenx = particle.xf().cols();
+ vector<int> ii;
+ for (int i=lenx; i<lenx+lenz; i++) {
+ ii.push_back(i);
+ }
+
+ MatrixXf xfCopy = particle.xf();
+ vector<MatrixXf> pfCopy(particle.Pf());
+
+
+ for (unsigned c=0; c<ii.size(); c++) {
+ #if 0
+ cout<<"ii"<<endl;
+ cout<<ii[c]<<endl;
+ #endif
+
+ if (xfCopy.isZero()) {
+ xfCopy = xf;
+ } else {
+ for (unsigned r=0; r<xf.rows(); r++) {
+ xfCopy(r,ii[c]) = xf(r,c);
+ }
+ }
+ if (pfCopy.empty()) {
+ pfCopy = Pf;
+ } else {
+ pfCopy[ii[c]] = Pf[c];
+ }
+ }
+
+ #if 0
+ cout<<"xf in add_feature"<<endl;
+ cout<<xf<<endl;
+ cout<<"should be"<<endl;
+ cout<<"3.4011 25.7814"<<endl;
+ cout<<"-25.6172 3.6347"<<endl;
+ cout<<endl;
+
+ cout<<"xfCopy"<<endl;
+ cout<<xfCopy<<endl;
+ cout<<endl;
+ #endif
+
+ particle.setXf(xfCopy);
+ particle.setPf(pfCopy);
+}
View
13 testcode/add_feature.h
@@ -0,0 +1,13 @@
+#ifndef ADD_FEATURE_H
+#define ADD_FEATURE_H
+
+#include <Eigen/Dense>
+
+#include "particle.h"
+
+using namespace Eigen;
+
+void add_feature(Particle &particle, MatrixXf z, MatrixXf R);
+
+#endif //ADD_FEATURE_H
+
View
20 testcode/add_observation_noise.cpp
@@ -0,0 +1,20 @@
+#include "add_observation_noise.h"
+
+//add random measurement noise. We assume R is diagnoal matrix
+void add_observation_noise(MatrixXf &z, MatrixXf R, int addnoise)
+{
+ if (addnoise == 1){
+ int len = z.cols();
+ if (len > 0) {
+ MatrixXf randM1(1,len);
+ MatrixXf randM2(1,len);
+ randM1.setRandom();
+ randM2.setRandom();
+
+ for(int c=0; c<len; c++) {
+ z(0,c) = z(0,c) + randM1(0,c)*sqrt(R(0,0));
+ z(1,c) = z(1,c) + randM2(0,c)*sqrt(R(1,1));
+ }
+ }
+ }
+}
View
11 testcode/add_observation_noise.h
@@ -0,0 +1,11 @@
+#ifndef ADD_OBSERVATION_NOISE_H
+#define ADD_OBSERVATION_NOISE_H
+
+#include <Eigen/Dense>
+#include <iostream>
+
+using namespace Eigen;
+
+void add_observation_noise(MatrixXf &z, MatrixXf R, int addnoise);
+
+#endif //ADD_OBSERVATION_NOISE_H
View
105 testcode/compute_jacobians.cpp
@@ -0,0 +1,105 @@
+#include <iostream>
+#include "compute_jacobians.h"
+#include "math.h"
+#include "pi_to_pi.h"
+
+void compute_jacobians(Particle particle,
+ vector<int> idf,
+ MatrixXf R,
+ MatrixXf &zp,
+ vector<MatrixXf> *Hv,
+ vector<MatrixXf> *Hf,
+ vector<MatrixXf> *Sf)
+{
+ VectorXf xv = particle.xv();
+
+ #if 0
+ cout<<"compute_jacobians: particle.xf()"<<endl;
+ cout<<particle.xf()<<endl;
+ #endif
+
+ int rows = (particle.xf()).rows();
+ MatrixXf xf(rows,idf.size());
+ vector<MatrixXf> Pf;
+
+ #if 0
+ cout<<"compute jacobians:"<<endl;
+ cout<<"particle.xf().rows() "<<(particle.xf()).rows()<<endl;
+ cout<<"particle.xf().cols() "<<(particle.xf()).cols()<<endl;
+ cout<<"idf.size() "<<idf.size()<<endl;
+ cout<<"idf max "<<idf[idf.size()-1]<<endl;
+ cout<<"xf.rows() "<<xf.rows()<<endl;
+ cout<<"xf.cols() "<<xf.cols()<<endl;
+ cout<<endl;
+ #endif
+ unsigned i;
+ int r;
+ for (unsigned i=0; i<idf.size(); i++) {
+ for (r=0; r<(particle.xf()).rows(); r++) {
+ xf(r,i) = (particle.xf())(r,(idf[i]));
+ }
+ if (particle.Pf().size() != 0) {
+ Pf.push_back((particle.Pf())[idf[i]]); //particle.Pf is a array of matrices
+ }
+ }
+
+ float dx,dy,d2,d;
+ MatrixXf HvMat(2,3);
+ MatrixXf HfMat (2,2);
+
+#if 0
+ cout<<"before for loop in compute_jacobians"<<endl;