Permalink
Browse files

extra files deleted

  • Loading branch information...
1 parent 30e820e commit 8affaed1cf48f12ab9f72974835bf0accb4c2e08 @yglee committed Aug 23, 2012
Showing with 0 additions and 27,932 deletions.
  1. +0 −247 cpp/\
  2. +0 −27,685 cpp/t
View
247 cpp/\
@@ -1,247 +0,0 @@
-#include <iostream>
-#include <math.h>
-#include <vector>
-
-#include "fastslam2_sim.h"
-#include "add_control_noise.h"
-#include "predict.h"
-#include "observe_heading.h"
-#include "get_observations.h"
-#include "add_observation_noise.h"
-#include "TransformToGlobal.h"
-#include "line_plot_conversion.h"
-#include "data_associate_known.h"
-#include "sample_proposal.h"
-#include "feature_update.h"
-#include "resample_particles.h"
-#include "add_feature.h"
-
-
-using namespace config;
-using namespace std;
-
-vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
-{
- if (SWITCH_PREDICT_NOISE) {
- printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");
- }
- if (SWITCH_SAMPLE_PROPOSAL == 0) {
- printf("Sampling from optimal proposal is usually ON for FastSLAM 2.0\n");
- }
-
- Q << pow(sigmaV,2), 0,
- 0 , pow(sigmaG,2);
-
- R << sigmaR*sigmaR, 0,
- 0, sigmaB*sigmaB;
-
- float veh[2][3] = {{0,-WHEELBASE,-WHEELBASE},{0,-1,1}};
-
- //Particle *particles = new Particle[NPARTICLES];
- vector<Particle> particles(NPARTICLES);
-
- float uniformw = 1.0/(float)NPARTICLES;
- for (unsigned int p = 0; p < NPARTICLES; p++) {
- particles[p].setW(uniformw);
- }
- VectorXf xtrue(3);
- xtrue.setZero();
-
- float dt = DT_CONTROLS; //change in time btw predicts
- float dtsum = 0; //change in time since last observation
- vector<int> ftag;
-
- for (int i=0; i< lm.cols(); i++) {
- ftag.push_back(i); //ftag items are indexed from 1
- }
-
- VectorXf da_table(lm.cols());
- da_table.setZero();
-
- int iwp = 1; //index to first waypoint
- float G = 0; //initial steer angle
- MatrixXf plines; //will later change to list of points
-
- if (SWITCH_SEED_RANDOM !=0) {
- srand(SWITCH_SEED_RANDOM);
- }
-
- MatrixXf Qe = MatrixXf(Q);
- MatrixXf Re = MatrixXf(R);
-
- if (SWITCH_INFLATE_NOISE ==1) {
- Qe = 2*Q;
- Re = 2*R;
- }
-
- if (SWITCH_PROFILE) {
- //TODO:
- }
-
- vector<int> ftag_visible;
- MatrixXf z;
- while (iwp !=0) {
- //compute true data
- compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
- if (iwp ==0 && NUMBER_LOOPS > 1) {
- iwp = 1;
- NUMBER_LOOPS = NUMBER_LOOPS-1;
- }
- predict_true(xtrue,V,G,WHEELBASE,dt);
-
- //add process noise
- //TODO: need to truly randomize function in multivariate_gauss
- float* VnGn = new float[2];
- add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
- float Vn = VnGn[0];
- float Gn = VnGn[1];
-
- //predict step
- for (unsigned int i=0; i< NPARTICLES; i++) {
- predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
- observe_heading(particles[i], xtrue(2), SWITCH_HEADING_KNOWN); //if heading known, observe heading
- }
-
- #if 0
- cout<<"inside fastslam2"<<endl;
- cout<<"particle count: "<<particles.size()<<endl;
- cout<<"w: "<<particles[0].w()<<" " <<"should be 0.0100"<<endl;
- cout<<"xv: "<<particles[0].xv()<<" "<<"should be 0.0824, 0.0036, 0.0009"<<endl;
- cout<<"Pv: "<<particles[0].Pv()<<" "<<"should be 1.0e-0.4 *[0.5618, 0.0164, 0.0041; 0.0164, 0.1873, 0.0468; 0.0041, 0.0468, 0.0117]" <<endl;
- cout<<"xf: "<<particles[0].xf()<<" "<<"should be empty"<<endl;
- //cout<<"Pf: "<<particles[0].Pf()[0]<<" "<<"should be empty"<<endl;
- //cout<<"da: "<<particles[0].da()[0]<<" "<<"should be empty"<<endl;
- #endif
-
- //observe step
- dtsum = dtsum+dt;
- if (dtsum >= DT_OBSERVE) {
- dtsum=0;
-
- //compute true data, then add noise
- ftag_visible = vector<int>(ftag); //modify the copy, not the ftag
- z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
- #if 0
- cout<<"ftag"<<endl;
- vector<int>::iterator iter;
- for (iter =ftag.begin(); iter!=ftag.end(); iter++) {
- cout<<(*iter)<<" ";
- }
- cout<<endl;
-
- //z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
- cout<<"ftag_visible"<<endl;
- vector<int>::iterator iter2;
- for (iter2 =ftag_visible.begin(); iter2!=ftag_visible.end(); iter2++) {
- cout<<(*iter2)<<" ";
- }
- cout<<endl;
- cout<<"ftag_visible should be 0,21"<<endl;
-
- //TODO:commented out for testing
- //add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
- cout<<"get_observations returns z: "<<endl;
- cout<<z<<endl;
- cout<<"z should be [25.7745 25.2762; -1.4734,0.1384]"<<endl;
- #endif
-
- if(!z.isZero()){
- plines = make_laser_lines(z,xtrue);
- }
-
- #if 0
- cout<<"plines "<<endl;
- cout<<plines<<endl;
- cout<<"plines should be [0.6741, 2.9922, Nan, 0.6741, 25.7446; -0.0309,-25.7009,Nan, -0.0309, 3.2710]"<<endl;
- #endif
-
- //compute (known) data associations
- MatrixXf xfvar = particles[0].xf();
- int Nf = xfvar.cols();
- vector<int> idf;
-
- MatrixXf zf(z.rows(),z.cols());
- zf.setZero();
- MatrixXf zn(z.rows(),ftag_visible.size());
- zn.setZero();
-
- data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
-
- cout<<"zf "<<zf<<endl;
- cout<<"zf should be empty"<<endl;
-
- cout<<"idf";
- vector<int>::iterator iter;
- for (iter=idf.begin(); iter!=idf.end(); iter++) {
- cout<<(*iter)<<" ";
- }
- cout<<endl;
- cout<<"idf should be empty"<<endl;
-
- cout<<"zn"<<endl;
- cout<<zn<<endl;
- cout<<"zn should be [25.7745, 25.2762; -1.4734, 0.1384]"<<endl;
-
- cout<<"da_table"<<endl;
- cout<<da_table<<endl;
- //cout<<"da_table[0] = "<<da_table[0]<<" da_table[21]="<<da_table[21]<<endl;
- cout<<"da_table should be all zero except 1 at index 0, 2 at index 21"<<endl;
-
- //observe map features
- if (!zf.isZero()) {
- //isample from 'optimal' proposal distribution, then update map
- for (unsigned i=0; i<NPARTICLES; i++) {
- sample_proposal(particles[i], zf, idf, Re);
- feature_update(particles[i],zf,idf,Re);
- }
- //resample
- resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
- }
-
- //Observe new features, augment map
- 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
- VectorXf xv = multivariate_gauss(particles[i].xv(),
- particles[i].Pv(),1);
- particles[i].setXv(xv);
- MatrixXf pv(3,3);
- pv.setZero();
- particles[i].setPv(pv); //TODO: double check
- }
- add_feature(particles[i], zn, Re);
- }
- }
- }
- delete[] VnGn;
- }
- return particles;
-}
-
-MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv)
-{
- if (rb.isZero()) {
- return MatrixXf(0,0);
- }
-
- int len = rb.cols();
- MatrixXf lnes(4,len);
-
- MatrixXf globalMat(2,rb.cols());
- int j;
- for (j=0; j<globalMat.cols();j++) {
- globalMat(0,j) = rb(0,j)*cos(rb(1,j));
- globalMat(1,j) = rb(0,j)*sin(rb(1,j));
- }
-
- TransformToGlobal(globalMat,xv);
-
- for (int c=0; c<lnes.cols();c++) {
- lnes(0,c) = xv(0);
- lnes(1,c) = xv(1);
- lnes(2,c) = globalMat(0,c);
- lnes(3,c) = globalMat(1,c);
- }
-
- return line_plot_conversion(lnes);
-}
Oops, something went wrong.

0 comments on commit 8affaed

Please sign in to comment.