Permalink
Browse files

getting zack's help on debugging

  • Loading branch information...
1 parent fcccc59 commit 38c517d547644b50a8e6982640098e9f3ceb77ca @yglee committed Aug 24, 2012
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
@@ -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
@@ -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;
@@ -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
}
Oops, something went wrong.

0 comments on commit 38c517d

Please sign in to comment.