Permalink
Browse files

testcode dir removed

  • Loading branch information...
yglee committed Aug 28, 2012
1 parent c8987a4 commit 43303ceb677957d5cefcf3abadfa407cbc1fb757
Showing with 0 additions and 28,382 deletions.
  1. +0 −5 testcode/.gitignore
  2. +0 −20 testcode/KF_cholesky_update.cpp
  3. +0 −10 testcode/KF_cholesky_update.h
  4. +0 −61 testcode/KF_joseph_update.cpp
  5. +0 −11 testcode/KF_joseph_update.h
  6. +0 −13 testcode/README
  7. +0 −30 testcode/TransformToGlobal.cpp
  8. +0 −11 testcode/TransformToGlobal.h
  9. +0 −17 testcode/add_control_noise.cpp
  10. +0 −11 testcode/add_control_noise.h
  11. +0 −122 testcode/add_feature.cpp
  12. +0 −13 testcode/add_feature.h
  13. +0 −20 testcode/add_observation_noise.cpp
  14. +0 −11 testcode/add_observation_noise.h
  15. +0 −61 testcode/compute_jacobians.cpp
  16. +0 −19 testcode/compute_jacobians.h
  17. +0 −60 testcode/compute_steering.cpp
  18. +0 −11 testcode/compute_steering.h
  19. +0 −50 testcode/configfile.cpp
  20. +0 −47 testcode/configfile.h
  21. +0 −56 testcode/data_associate_known.cpp
  22. +0 −13 testcode/data_associate_known.h
  23. +0 −56 testcode/example_webmap.mat
  24. +0 −277 testcode/fastslam2_sim.cpp
  25. +0 −22 testcode/fastslam2_sim.h
  26. +0 −76 testcode/feature_update.cpp
  27. +0 −17 testcode/feature_update.h
  28. +0 −79 testcode/gauss_evaluate.cpp
  29. +0 −26 testcode/gauss_evaluate.h
  30. +0 −88 testcode/get_observations.cpp
  31. +0 −14 testcode/get_observations.h
  32. +0 −36 testcode/line_plot_conversion.cpp
  33. +0 −10 testcode/line_plot_conversion.h
  34. +0 −136 testcode/main.cpp
  35. +0 −56 testcode/makefile
  36. +0 −20 testcode/multivariate_gauss.cpp
  37. +0 −11 testcode/multivariate_gauss.h
  38. +0 −39 testcode/observe_heading.cpp
  39. +0 −9 testcode/observe_heading.h
  40. +0 −26,003 testcode/output_old
  41. +0 −101 testcode/particle.cpp
  42. +0 −41 testcode/particle.h
  43. +0 −38 testcode/pi_to_pi.cpp
  44. +0 −16 testcode/pi_to_pi.h
  45. +0 −74 testcode/predict.cpp
  46. +0 −14 testcode/predict.h
  47. +0 −9 testcode/predict_true.cpp
  48. +0 −13 testcode/predict_true.h
  49. +0 −15 testcode/printMat.h
  50. +0 −44 testcode/resample_particles.cpp
  51. +0 −14 testcode/resample_particles.h
  52. +0 −164 testcode/sample_proposal.cpp
  53. +0 −27 testcode/sample_proposal.h
  54. +0 −33 testcode/stratified_random.cpp
  55. +0 −13 testcode/stratified_random.h
  56. +0 −49 testcode/stratified_resample.cpp
  57. +0 −13 testcode/stratified_resample.h
  58. +0 −57 testcode/test_main.cc
View
@@ -1,5 +0,0 @@
-*.swp
-test/**/*
-*~
-*.o
-.DS_Store
@@ -1,20 +0,0 @@
-#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();
-}
@@ -1,10 +0,0 @@
-#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
@@ -1,61 +0,0 @@
-#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;
-}
@@ -1,11 +0,0 @@
-#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
@@ -1,13 +0,0 @@
-
-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
@@ -1,30 +0,0 @@
-#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);
- }
- }
-}
@@ -1,11 +0,0 @@
-#ifndef TRANSFORMGLOBAL_H
-#define TRANSFORMGLOBAL_H
-
-#include <Eigen/Dense>
-#include <iostream>
-
-using namespace Eigen;
-
-void TransformToGlobal(MatrixXf &p, VectorXf b);
-
-#endif //TRANSFORMGLOBAL_H
@@ -1,17 +0,0 @@
-#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);
- }
-}
@@ -1,11 +0,0 @@
-#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
@@ -1,122 +0,0 @@
-#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)
-{
- 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();
-
- 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());
-
- 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);
-
- 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 k=0; k<xf.rows(); k++) {
- xfCopy(k,ii[l]) = xf(k,l);
- }
- }
- if (pfCopy.empty()) {
- pfCopy = Pf;
- } else {
- pfCopy[ii[l]] = Pf[l];
- }
- }
-
- particle.setXf(xfCopy);
- particle.setPf(pfCopy);
-}
View
@@ -1,13 +0,0 @@
-#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
-
@@ -1,20 +0,0 @@
-#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));
- }
- }
- }
-}
@@ -1,11 +0,0 @@
-#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
Oops, something went wrong.

0 comments on commit 43303ce

Please sign in to comment.