TransformToGlobal* added and fast_slam2_sim changed.

cpp/TransformToGlobal.cpp

@@ -0,0 +1,31 @@
+#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);
+   			pi_to_pi(input);
+			p(2,k) = input;
+		}		
+	}	
+}

cpp/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

cpp/\

@@ -1,68 +1,130 @@
-#include "get_observations.h"
 #include <iostream>
 #include <math.h>
 
-MatrixXf get_observations(VectorXf x, MatrixXf lm, vector<int> &idf, float rmax)
-{
-	get_visible_landmarks(x,lm,idf,rmax);
-	return compute_range_bearing(x,lm);	
-}
+#include "fastslam2_sim.h"
+#include "particle.h"
+#include "add_control_noise.h"
+#include "predict.h"
+#include "observe_heading.h"
+#include "get_observations.h"
+#include "add_observation_noise.h"
+
+using namespace config;
 
-void get_visible_landmarks(VectorXf x, MatrixXf &lm, vector<int> &idf, float rmax)
+void fastslam2_sim(MatrixXf lm, MatrixXf wp) 
 {
-	//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++) {
-		dx(0,c) = lm(0,c) - x(0);
-		dy(1,c) = lm(1,c) - x(1);
-	}	
-	float phi = x(2);
-	vector<int> ii = find2(dx,dy,phi,rmax); 
+	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");
+	}
 
-	//lm(:[2 3]) return matrix of column 2 and colum 3 of lm
-	MatrixXf lm_new (lm.rows(), ii.size()); 	
-	vector<int>::iterator iter;
-	for (int j=0; j<lm.rows(); j++){
-		for(iter = ii.begin(); iter != ii.end(); iter++){
-			lm_new(j,*iter) = lm(j,*iter);	
-		}
+	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];
+	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();	
 
-	lm = lm_new; //TODO: does this properly copy it over? I need a copy constructor
-	idf = ii; //check this too			
-}
+	int iwp = 1; //index to first waypoint
+	float G = 0; //initial steer angle
+	MatrixXf plines; //will later change to list of points
 
-MatrixXf compute_range_bearing(VectorXf x, MatrixXf lm) 
-{
-	MatrixXf dx(1,lm.cols());
-	MatrixXf dy(1,lm.cols());
-	for (int c=0; c<lm.cols(); c++) {
-		dx(0,c) = lm(0,c) - x(0);
-		dy(1,c) = lm(1,c) - x(1);
-	}	
-	float phi = x(2);
+	if (SWITCH_SEED_RANDOM !=0) {
+		srand(SWITCH_SEED_RANDOM);
+	} 		
 
-	MatrixXf z(2,dx.size());
-	for (int i =0; i<dx.size(); i++) {
-		z(1,i) = pow(dx[i],2) + pow(dy[i],2);		
-		z(2,i) = atan2(dy[i],dx[i]) - phi;	
+	Matrix2f Qe = Matrix2f(Q);
+	Matrix2f Re = Matrix2f(R);
+
+	if (SWITCH_INFLATE_NOISE ==1) {
+		Qe = 2*Q;
+		Re = 2*R;
 	}
 
-	return z; 
+	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 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
+		}
+
+		//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);
+			add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
+			if(!z.isZero()){
+				plines = make_laser_lines(z,xtrue);
+			}		
+		}
+	}		
+
 }
 
-vector<int> find2(VectorXf dx, VectorXf dy, float phi, float rmax)
+MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv) 
 {
-	vector<int> index;
-	//incremental tests for bounding semi-circle
-	for (int i =0; i<input.size(); i++) {
-		if ((abs(dx[i]) < rmax) && (abs(dy[i]) < rmax)
-		&& ((dx[i]* cos(phi) + dy[i]* sin(phi)) > 0)
-		&& ((pow(dx[i],2) + pow(dy[i],2)) < pow(rmax,2))) {
-			index.push_back(i);			
-		}
-	}	
-	return index;				
+	if (rb.isZero()) {
+		return MatrixXf(0,0);
+	}
+
+	int len = rb.cols();
+	MatrixXf lnes(4,2);
+
+	MatrixXf globalMat(2,rb.cols());
+	int i,j;
+	for (j=0; j<globalMat.cols();j++) {
+		globalMat(0,j) = rb(0,j)*cos(rb(1,j));   	
+	}
+
+
+	for (int c=0; c<lnes.cols();c++) {
+		lnes(0,c) = xv(0);	
+		lnes(1,c) = xv(1);
+	}
 }

cpp/compute_steering.cpp

@@ -8,55 +8,53 @@
 using namespace std;
 
 void compute_steering(VectorXf x, MatrixXf wp, int& iwp, float minD, 
-						float& G, float rateG, float maxG, float dt)
+				float& G, float rateG, float maxG, float dt)
 {
-        /*
-        % INPUTS:
-        %   x - true position
-        %   wp - waypoints
-        %   iwp - index to current waypoint
-        %   minD - minimum distance to current waypoint before switching to next
-        %   G - current steering angle
-        %   rateG - max steering rate (rad/s)
-        %   maxG - max steering angle (rad)
-        %   dt - timestep
-        */
-
-	//determine if current waypoint reached
-	Vector2d cwp;
-        cwp[0] = wp(0,iwp-1); //-1 since indexed from 0     
-	cwp[1] = wp(1,iwp-1);
-
-	float d2 = pow((cwp[0] - x[0]),2) + pow((cwp[1]-x[1]),2);     
-
-	if (d2 < minD*minD) {
-		iwp = iwp+1; //switch to next
-		if (iwp > wp.cols()) {
-			iwp =0;
-			return;	
-		}
+		/*
+		   % INPUTS:
+		   %   x - true position
+		   %   wp - waypoints
+		   %   iwp - index to current waypoint
+		   %   minD - minimum distance to current waypoint before switching to next
+		   %   G - current steering angle
+		   %   rateG - max steering rate (rad/s)
+		   %   maxG - max steering angle (rad)
+		   %   dt - timestep
+		 */
 
-    		cwp[0] = wp(0,iwp-1); //-1 since indexed from 0
+		//determine if current waypoint reached
+		Vector2d cwp;
+		cwp[0] = wp(0,iwp-1); //-1 since indexed from 0     
 		cwp[1] = wp(1,iwp-1);
-	}
-
-	//compute change in G to point towards current waypoint
-        std::vector<float> anglesArray;
-        anglesArray.push_back(atan2(cwp[1]-x[1], cwp[0]-x[0]) - x[2] - G);
-        pi_to_pi(anglesArray);
-        float deltaG = anglesArray[0];
-
-        //limit rate
-	float maxDelta = rateG*dt;
-	if (abs(deltaG) > maxDelta) {
-		int sign = (deltaG > 0) ? 1 : ((deltaG < 0) ? -1 : 0);
-		deltaG = sign*maxDelta;	
-	}	
-
-	//limit angle
-	G = G+deltaG;
-	if (abs(G) > maxG) {
-		int sign2 = (G > 0) ? 1: ((G < 0) ? -1 : 0);
-		G = sign2*maxG;
-	}
+
+		float d2 = pow((cwp[0] - x[0]),2) + pow((cwp[1]-x[1]),2);     
+
+		if (d2 < minD*minD) {
+				iwp = iwp+1; //switch to next
+				if (iwp > wp.cols()) {
+						iwp =0;
+						return;	
+				}
+
+				cwp[0] = wp(0,iwp-1); //-1 since indexed from 0
+				cwp[1] = wp(1,iwp-1);
+		}
+
+		//compute change in G to point towards current waypoint
+		float deltaG = atan2(cwp[1]-x[1], cwp[0]-x[0]) - x[2] - G;
+		pi_to_pi(deltaG);
+
+		//limit rate
+		float maxDelta = rateG*dt;
+		if (abs(deltaG) > maxDelta) {
+				int sign = (deltaG > 0) ? 1 : ((deltaG < 0) ? -1 : 0);
+				deltaG = sign*maxDelta;	
+		}	
+
+		//limit angle
+		G = G+deltaG;
+		if (abs(G) > maxG) {
+				int sign2 = (G > 0) ? 1: ((G < 0) ? -1 : 0);
+				G = sign2*maxG;
+		}
 }

cpp/fastslam2_sim.cpp

@@ -8,6 +8,7 @@
 #include "observe_heading.h"
 #include "get_observations.h"
 #include "add_observation_noise.h"
+#include "TransformToGlobal.h"
 
 using namespace config;
 
@@ -49,7 +50,7 @@ void fastslam2_sim(MatrixXf lm, MatrixXf wp)
 
 	int iwp = 1; //index to first waypoint
 	float G = 0; //initial steer angle
-	MatrixXf plines = NULL; //will later change to list of points
+	MatrixXf plines; //will later change to list of points
 
 	if (SWITCH_SEED_RANDOM !=0) {
 		srand(SWITCH_SEED_RANDOM);
@@ -99,7 +100,7 @@ void fastslam2_sim(MatrixXf lm, MatrixXf wp)
 			ftag_visible = vector<int>(ftag); //modify the copy, not the ftag	
 			z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
 			add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
-			if(!z.empty()){
+			if(!z.isZero()){
 				plines = make_laser_lines(z,xtrue);
 			}		
 		}
@@ -109,7 +110,28 @@ void fastslam2_sim(MatrixXf lm, MatrixXf wp)
 
 MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv) 
 {
-	if (rb.empty()) {
-		return NULL;
+	if (rb.isZero()) {
+		return MatrixXf(0,0);
 	}
+
+	int len = rb.cols();
+	MatrixXf lnes(4,2);
+
+	MatrixXf globalMat(2,rb.cols());
+	int i,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(1,c);
+		lnes(3,c) = globalMat(2,c);
+	}
+	return MatrixXf(0,0);
+	//return line_plot_conversion(lnes);
 }

cpp/observe_heading.cpp

@@ -18,10 +18,8 @@ void observe_heading(Particle &particle, float phi, int useheading)
     MatrixXf H(1,3);
     H<<0,0,1;            
 
-    std::vector<float> vecV;
-    vecV.push_back(phi-xv(2));
-    pi_to_pi(vecV);	
-    float v = vecV[0]; 
+    float v = phi-xv(2);
+    pi_to_pi(v);	
 
     KF_joseph_update(xv,Pv,v,pow(sigmaPhi,2),H);
     particle.setXv(xv);