Permalink
Browse files

fastslam1 generates particles without seg fault. victory is nearby

  • Loading branch information...
1 parent 43303ce commit b86119812aba4b919422a8e90b27872fbb3a888f @yglee committed Aug 28, 2012
View
@@ -8,79 +8,56 @@ using namespace std;
//
// add new features
//
-void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
+void add_feature(Particle &particle, vector<VectorXf> 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;
+ int lenz = z.size();//z.cols();
+ //MatrixXf xf(2,lenz); //measurements (range, bearing)
+ vector<VectorXf> xf;
+ 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);
+ r = z[i][0];//z(0,i);
+ b = z[i][1];//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;
+
+ VectorXf measurement(2);
+ measurement(0) = xv(0) + r*c;
+ measurement(1) = xv(1) + r*s;
+ xf.push_back(measurement);
+ //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();
+ //int lenx = particle.xf().cols();
+ int lenx = particle.xf().size();
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;
+ //int pcols = particle.xf().cols();
+ int pcols = particle.xf().size();
maxelem = std::max_element(ii.begin(),ii.end());
- int xfc = std::max((*maxelem)+1,pcols);
+ int xf_size = std::max((*maxelem)+1,pcols);
+ particle.xf().resize(xf_size);
+ */
- cout<<"particle.xf().cols()"<<endl;
- cout<<pcols<<endl;
- cout<<"max"<<endl;
- cout<<xfc<<endl;
+ for(int j=0; j<ii.size(); j++) {
+ particle.setXfi(ii[j],xf[j]);
+ }
+
+/*
MatrixXf xfCopy(prows,xfc);
xfCopy.setZero();
for (int i =0; i<prows; i++) {
@@ -89,11 +66,19 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
}
}
+*/
//stupid matlab... I need to dynamically
//pfCopy
+/*
int pfcols = particle.Pf().size();
int Pfc = std::max((*maxelem)+1,pfcols);
+ particle.Pf().resize(Pfc);
+*/
+ for(int i=0; i<ii.size(); i++) {
+ particle.setPfi(ii[i],Pf[i]);
+ }
+#if 0
vector<MatrixXf> pfCopy(Pfc);
for (int i=0; i<pfcols; i++) {
pfCopy[i] = particle.Pf()[i];
@@ -119,4 +104,5 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
particle.setXf(xfCopy);
particle.setPf(pfCopy);
+#endif
}
View
@@ -7,7 +7,7 @@
using namespace Eigen;
-void add_feature(Particle &particle, MatrixXf z, MatrixXf R);
+void add_feature(Particle &particle, vector<VectorXf> z, MatrixXf R);
#endif //ADD_FEATURE_H
@@ -4,58 +4,73 @@
#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)
+ vector<int> idf,
+ MatrixXf R, //
+ //MatrixXf &zp,
+ vector<VectorXf> &zp, //measurement (range, bearing)
+ vector<MatrixXf> *Hv, // measurement function fcn of (mean, pose)
+ vector<MatrixXf> *Hf, // measurement function' fcn of (mean, pose)
+ vector<MatrixXf> *Sf) //measurement covariance
{
- VectorXf xv = particle.xv();
-
- int rows = (particle.xf()).rows();
- MatrixXf xf(rows,idf.size());
- vector<MatrixXf> Pf;
-
- 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);
-
- 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);
-
- //predicted observation
- zp(0,i) = d;
- zp(1,i) = atan2(dy,dx) - xv(2);
- zp(1,i) = pi_to_pi(zp(1,i));
-
- //Jacobian wrt vehicle states
- HvMat<< -dx/d, -dy/d, 0,
- dy/d2, -dx/d2,-1;
-
- //Jacobian wrt feature states
- HfMat<< dx/d, dy/d,
- -dy/d2, dx/d2;
-
- Hv->push_back(HvMat);
- Hf->push_back(HfMat);
-
- //innovation covariance of 'feature observation given the vehicle'
- MatrixXf SfMat = HfMat*Pf[i]*HfMat.transpose() + R;
- Sf->push_back(SfMat);
- }
+ VectorXf xv = particle.xv();
+
+ //int rows = (particle.xf()).rows();
+ int rows = particle.xf().size();
+ //MatrixXf xf(rows,idf.size());
+ vector<VectorXf> xf;
+ vector<MatrixXf> Pf;
+
+ unsigned i;
+ int r;
+ for (unsigned i=0; i<idf.size(); i++) {
+ //for (r=0; r<rows; r++) {
+ // xf(r,i) = (particle.xf())(r,(idf[i]));
+ xf.push_back(particle.xf()[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);
+
+ for (i=0; i<idf.size(); i++) {
+ dx = xf[i](0) - xv(0);//xf(0,i) - xv(0);
+ dy = xf[i](1) - xv(1);//xf(1,i) - xv(1);
+ d2 = pow(dx,2) + pow(dy,2);
+ d = sqrt(d2);
+
+ VectorXf zp_vec(2);
+
+ //predicted observation
+ //zp(0,i) = d;
+ // zp[i][0] = d;
+ zp_vec[0] = d;
+
+ //zp(1,i) = atan2(dy,dx) - xv(2);
+ // zp[i][1] = atan2(dy,dx) - xv(2);
+ zp_vec[1] = atan2(dy,dx) - xv(2);
+
+ //zp(1,i) = pi_to_pi(zp(1,i));
+ // zp[i][1] = pi_to_pi(zp[i][1]);
+ zp_vec[1] = pi_to_pi(zp_vec[1]);
+ zp.push_back(zp_vec);
+
+ //Jacobian wrt vehicle states
+ HvMat<< -dx/d, -dy/d, 0,
+ dy/d2, -dx/d2,-1;
+
+ //Jacobian wrt feature states
+ HfMat<< dx/d, dy/d,
+ -dy/d2, dx/d2;
+
+ Hv->push_back(HvMat);
+ Hf->push_back(HfMat);
+
+ //innovation covariance of feature observation given the vehicle'
+ MatrixXf SfMat = HfMat*Pf[i]*HfMat.transpose() + R;
+ Sf->push_back(SfMat);
+ }
}
@@ -11,8 +11,9 @@ using namespace Eigen;
void compute_jacobians(Particle particle,
vector<int> idf,
MatrixXf R,
- MatrixXf &zp,
- vector<MatrixXf> *Hv,
+ //MatrixXf &zp,
+ vector<VectorXf> &zp,
+ vector<MatrixXf> *Hv,
vector<MatrixXf> *Hf,
vector<MatrixXf> *Sf);
@@ -1,15 +1,20 @@
#include "data_associate_known.h"
#include <iostream>
-void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
- MatrixXf &zf, vector<int> &idf, MatrixXf &zn)
+//void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
+ vector<VectorXf> &zf, vector<int> &idf, vector<VectorXf> &zn)
+
+//z is range and bearing of visible landmarks
+void data_associate_known(vector<VectorXf> z, vector<int> idz, VectorXf &table, int Nf, \
+ vector<VectorXf> &zf, vector<int> &idf, vector<VectorXf> &zn)
{
idf.clear();
vector<int> idn;
unsigned i,ii,r;
-
- //this extra loopis required to set the dimension of zn and zf
+
+ #if 0
+ //this extra loop is required to set the dimension of zn and zf
int zncols = 0;
int zfcols = 0;
for (i =0; i< idz.size(); i++){
@@ -21,34 +26,42 @@ void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf,
zfcols++;
}
}
-
- //resize
+
+ //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++){
+ #endif
+
+ for (i =0; i< idz.size(); i++){
ii = idz[i];
+ VectorXf z_i;
if (table(ii) ==-1) { //new feature
- for (r=0; r<z.rows();r++) {
- zn(r,znc) = z(r,i);
- }
- znc++;
+ //for (r=0; r<z.size(); r++) {
+ z_i = z[i];
+ //z_i(r) = z(r,i);
+ //}
+ zn.push_back(z_i);
+ //znc++;
idn.push_back(ii);
}
else {
- for (r=0; r<z.rows(); r++) {
- zf(r,zfc) = z(r,i);
- }
- zfc++;
+ //for (r=0; r<z.rows(); r++) {
+ z_i = z[i];
+ // zf(r,zfc) = z(r,i);
+ //}
+ //zfc++;
+ zf.push_back(z_i);
idf.push_back(table(ii));
}
}
+ assert(idn.size() == zn.size());
for (int i=0; i<idn.size(); i++) {
table(idn[i]) = Nf+i;
}
@@ -7,7 +7,10 @@
using namespace std;
using namespace Eigen;
-void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
- MatrixXf &zf, vector<int> &idf, MatrixXf &zn);
+void data_associate_known(vector<VectorXf> z, vector<int> idz, VectorXf &table, int Nf, \
+ vector<VectorXf> &zf, vector<int> &idf, vector<VectorXf> &zn);
+
+//void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
+ vector<VectorXf> &zf, vector<int> &idf, vector<VectorXf> &zn);
#endif //DATA_ASSOCIATE_KNOWN_H
Oops, something went wrong.

0 comments on commit b861198

Please sign in to comment.