Further tuning to improve performance.

Tuned alphas.
Increased particles to 5000.
Reduced free space threshold to 0.999 to fix issue where sometimes ray casting is cut short.

include/config.hh

@@ -2,29 +2,30 @@
 #define _CONFIG_H
 
 // General parameters
-#define NUM_PARTICLES 1
+#define NUM_PARTICLES 5000
 #define MAP_FILE_PATH "../data/map/wean.dat"
 #define LOG_FILE_PATH "../data/log/robotdata1.log"
 #define SKIP_ODO_READINGS
 #define MAP_VISUALIZE
+//#define FLIP_Y_AXIS
 
 // Motion model parameters
-#define ALPHA_1 0.01
-#define ALPHA_2 0.01
-#define ALPHA_3 0.1
-#define ALPHA_4 0.1
+#define ALPHA_1 0.0005
+#define ALPHA_2 0.0005
+#define ALPHA_3 0.05
+#define ALPHA_4 0.05
 
 // Sensor model parameters
 #define Z_HIT 0.7
 #define Z_SHORT 0.24
 #define Z_MAX 0.0055
 #define Z_RAND 0.0545
 #define LASER_MAX_RANGE 8191.0 // cm
-#define LASER_THETA_STEP 1 // degrees
+#define LASER_THETA_STEP 5 // degrees
 #define LASER_DIST_STEP 1 // cm
 #define P_HIT_STD 20 // cm
 #define LAMBDA_SHORT 0.01
 #define LASER_OFFSET 25.0 // cm
-#define FREE_SPACE_THRESH 0.9999
+#define FREE_SPACE_THRESH 0.999
 
 #endif /* _CONFIG_H */

include/motionModel.hh

@@ -14,9 +14,6 @@ class MotionModel
 
  public:
  MotionModel(double a1, double a2, double a3, double a4);
-
- ~MotionModel();
-
  state_t update(vector<double>, vector<double>, state_t);
 };
 

src/main.cc

@@ -20,9 +20,14 @@ vector<state_t> init_particles(int num_particles, map_type map, bool freeSpace=f
  double res = map.resolution;
  double start_x = map.min_x * res;
  double end_x = map.max_x * res;
+#ifdef FLIP_Y_AXIS
  // Account for flipped y axis.
  double start_y = (map.size_y - map.max_y) * res;
  double end_y = (map.size_y - map.min_y) * res;
+#else
+ double start_y = map.max_y * res;
+ double end_y = map.min_y * res;
+#endif
  uniform_real_distribution<double> dist_x(start_x, end_x);
  uniform_real_distribution<double> dist_y(start_y, end_y);
  uniform_real_distribution<double> dist_theta(-3.14, 3.14);
@@ -36,8 +41,13 @@ vector<state_t> init_particles(int num_particles, map_type map, bool freeSpace=f
  x = dist_x(generator);
  y = dist_y(generator);
  theta = dist_theta(generator);
+#ifdef FLIP_Y_AXIS
  } while (freeSpace and (map.prob[(int)(x/res)][map.size_y - (int)(y/res)] == -1 ||
- map.prob[(int)(x/res)][map.size_y - (int)(y/res)] <= 0.9));
+ map.prob[(int)(x/res)][map.size_y - (int)(y/res)] <= FREE_SPACE_THRESH));
+#else
+ } while (freeSpace and (map.prob[(int)(x/res)][(int)(y/res)] == -1 ||
+ map.prob[(int)(x/res)][(int)(y/res)] <= FREE_SPACE_THRESH));
+#endif
 
  state_t meas = {x, y, theta, w};
  x_bar_init[m] = meas;

src/mapReader.cc

@@ -75,7 +75,6 @@ int MapReader::read_map()
 
  printf("Map size: %dx%d\n", map.size_x, map.size_y);
 
- // allocate memory for map prob
  map.min_x = map.size_x;
  map.max_x = 0;
  map.min_y = map.size_y;
@@ -133,23 +132,28 @@ int MapReader::visualize_map(vector<state_t> x_bar, bool storeForVideo, bool vis
  {
  for (unsigned int j = 0; j < image.cols; j++)
  {
+#ifdef FLIP_Y_AXIS
+ // Mat is accessed as (row,col), which means (y,x)
+ // Assuming that map.dat was stored as (x,y)
+ // ^
+ // |
+ // Y
+ // |
+ // 0/0 --X-->
+ // So we should access as map.prob[j][i]
+ // But image axis is:
+ // 0/0 --X-->
+ // |
+ // Y
+ // |
+ // v
+ // So we should access as map.prob[j][image.rows-i-1]
  if (map.prob[j][image.rows-i-1] > 0.0)
- // Mat is accessed as (row,col), which means (y,x)
- // Assuming that map.dat was stored as (x,y)
- // ^
- // |
- // Y
- // |
- // 0/0 --X-->
- // So we should access as map.prob[j][i]
- // But image axis is:
- // 0/0 --X-->
- // |
- // Y
- // |
- // v
- // So we should access as map.prob[j][image.rows-i-1]
  image.at<float>(i, j) = map.prob[j][image.rows-i-1];
+#else
+ if (map.prob[i][j] > 0.0)
+ image.at<float>(j, i) = map.prob[i][j];
+#endif
  }
  }
 
@@ -168,7 +172,6 @@ int MapReader::visualize_map(vector<state_t> x_bar, bool storeForVideo, bool vis
  double x = x_t1.x + 25 * cos(x_t1.theta);
  double y = x_t1.y + 25 * sin(x_t1.theta);
  Point ray_start = Point_<int>((int)(x/res), (int)(y/res));
-
  for (int j = 0; j < 180; j+=5)
  {
  // Angle wrt x axis

src/motionModel.cc

@@ -19,11 +19,6 @@ MotionModel::MotionModel(double a1, double a2, double a3, double a4)
 }
 
 
-MotionModel::~MotionModel()
-{
-}
-
-
 state_t MotionModel::update(vector<double> u_t0, vector<double> u_t1, state_t x_t0)
 {
  double del_rot1, del_trans, del_rot2;
@@ -35,17 +30,17 @@ state_t MotionModel::update(vector<double> u_t0, vector<double> u_t1, state_t x_
  del_trans = sqrt(pow((u_t0[0] - u_t1[0]), 2) + pow((u_t0[1] - u_t1[1]), 2));
  del_rot2 = u_t1[2] - u_t0[2] - del_rot1;
 
- del_rot1 = del_rot1 < 0 ? 2 * M_PI + del_rot1 : del_rot1;
- del_rot2 = del_rot2 < 0 ? 2 * M_PI + del_rot2 : del_rot2;
+// del_rot1 = del_rot1 < 0 ? 2 * M_PI + del_rot1 : del_rot1;
+// del_rot2 = del_rot2 < 0 ? 2 * M_PI + del_rot2 : del_rot2;
 
  // Standard deviation for each parmeter used in the next segment
  term1 = alpha1 * del_rot1 + alpha2 * del_trans;
  term2 = alpha3 * del_trans + alpha4 * (del_rot1 + del_rot2);
  term3 = alpha1 * del_rot2 + alpha2 * del_trans;
 
- normal_distribution<double> d1{0, sqrt(term1)};
- normal_distribution<double> d2{0, sqrt(term2)};
- normal_distribution<double> d3{0, sqrt(term3)};
+ normal_distribution<double> d1{0, sqrt(abs(term1))};
+ normal_distribution<double> d2{0, sqrt(abs(term2))};
+ normal_distribution<double> d3{0, sqrt(abs(term3))};
 
  // Gaussian noise distribution with zero mean and sd for each paramter,
  // thereby modelling the noise of each parameter
@@ -65,150 +60,3 @@ state_t MotionModel::update(vector<double> u_t0, vector<double> u_t1, state_t x_
 
  return st;
 }
-
-
-#ifdef MOTION_MODEL_TEST
-
-vector<vector<double>> init_particles_random(int num_particles)
-{
- vector<vector<double>> x_bar_init(num_particles, vector<double>(4));
- default_random_engine generator;
- uniform_real_distribution<double> dist_x(3000, 7000);
- uniform_real_distribution<double> dist_y(0, 7000);
- uniform_real_distribution<double> dist_theta(-3.14, 3.14);
- for (int m = 0; m < num_particles; m++)
- {
- double x = dist_x(generator);
- double y = dist_y(generator);
- double theta = dist_theta(generator);
- double w = 1 / num_particles;
- vector<double> meas = {x, y, theta, w};
- x_bar_init[m] = meas;
- }
-
- return x_bar_init;
-}
-
-int main(int argc, const char * argv[])
-{
- /*
- * Description of variables used:
- * u_t0: particle state odometry reading [x, y, theta] at time (t-1)
- * [odometry_frame] 
- * u_t1: particle state odometry reading [x, y, theta] at time t
- * [odometry_frame]
- * x_t0: particle state belief [x, y, theta] at time (t-1)
- * [world_frame]
- * x_t1: particle state belief [x, y, theta] at time t
- * [world_frame]
- * x_bar: [num_particles x 4] sized array containing [x, y, theta, wt]
- * values for all particles
- * z_t: array of 180 range measurements for each laser scan
- */
-
- /*
- * Initialize Parameters
- */
- // loading map and log file containing odometry data
- string src_path_map = "../data/map/wean.dat";
- string src_path_log = "../data/log/robotdata1.log";
-
- // Calling motion model only
- MotionModel motion_model(0.1,0.1,0.1,0.1);
-
- // Checking with only one particle 
- int num_particles = 1;
- vector<vector<double>> x_bar;
- x_bar = init_particles_random(num_particles);
-
- ifstream log_file (src_path_log); // Read the log file
- if (log_file.is_open())
- {
- vector<double> u_t0;
- vector<double> u_t1;
- vector<double> x_t0;
- vector<double> x_t1;
- vector<double> z_t;
-
- string line;
- int time_idx = 0;
- while (getline(log_file, line))
- {
- time_idx++;
- vector<double> path_u_t0x;
- vector<double> path_u_t0y;
- vector<double> path_x_t0x;
- vector<double> path_x_t0y;
- vector<double> odometry_robot; // Odometry reading: [x, y, theta]
- vector<double> odometry_laser; // Laser coordinates in O frame
- vector<double> meas_vals; // numerical values
- vector<double> ranges; // 180 range measurements
- char meas_type = line[0]; // L: laser; O: odometry
- string meas = line.substr(2); // slice first two chars
- stringstream stream(meas);
- while (1)
- {
- double n;
- stream >> n;
- if (!stream)
- break;
- meas_vals.push_back(n);
- }
-
- for (int i = 0; i < 3; i++)
- odometry_robot.push_back(meas_vals[i]);
- double time_stamp = meas_vals[meas_vals.size() - 1];
-
- // if ((time_stamp <= 0.0) | (meas_type == "O")): # ignore pure
- // odometry measurements for now (faster debugging) continue
-
- if (meas_type == 'L')
- {
- for (int i = 3; i < 6; i++)
- odometry_laser.push_back(meas_vals[i]);
- for (int i = 6; i < meas_vals.size() - 1; i++)
- ranges.push_back(meas_vals[i]);
- }
-
- cout << "Processing time step " << time_idx + 1;
- cout << " at time " << time_stamp;
- cout << endl;
-
- if (time_idx == 1)
- {
- u_t0 = odometry_robot;
- continue;
- }
-
- vector<vector<double>> x_bar_new(num_particles, vector<double>(4));
- u_t1 = odometry_robot;
-
- for (int m = 0; m < num_particles; m++)
- {
- // MOTION MODEL (Sanjana)
- x_t0 = vector<double>(x_bar[m].begin(), x_bar[m].begin() + 3);
- x_t1= motion_model.update(u_t0, u_t1, x_t0);
- x_bar_new[m] = x_t1;
- }
-
- x_bar = x_bar_new;
- u_t0 = u_t1;
-
- path_u_t0x.push_back(u_t0[0]);
- path_u_t0y.push_back(u_t0[1]);
- path_x_t0x.push_back(x_bar[0][0]);
- path_x_t0y.push_back(x_bar[1][1]);
- }
-
- log_file.close();
- }
-
- else
- {
- cout << "Log file " << src_path_log << " could not be opened.";
- cout << endl;
- }
-
- return 0;
-}
-#endif /* MOTION_MODEL_TEST */

src/sensorModel.cc

@@ -132,8 +132,10 @@ double SensorModel::ray_casting(state_t x_t1, double angle)
  {
  double x_end = (x + dist * cos(angle)) / map_res;
  double y_end = (y + dist * sin(angle)) / map_res;
+#ifdef FLIP_Y_AXIS
  // Account for flipped y axis.
  y_end = sm_params.occupancy_map.size_y - y_end;
+#endif
  if (x_end > sm_params.occupancy_map.max_x or
  x_end < sm_params.occupancy_map.min_x or
  y_end > sm_params.occupancy_map.max_y or