trajectory.cpp

SLAM Gridsim use with sparer sensor

/*
  gridsim2 (c) 2006 Kris Beevers

  This file is part of gridsim2.

  gridsim2 is free software; you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  gridsim2 is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with gridsim2; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include "sim.hpp"

// maximum rate of steering angle change
inline double max_dG(const sensor_t &s)
{
  return (20*M_PI/180) / s.F; // FIXME: make this a parameter
}

// maximum steering angle
inline double max_G()
{
  return (30*M_PI/180); // FIXME: make this a parameter
}

bool load_waypoints(const char *file, std::list<point_t> &waypoints)
{
  waypoints.clear();
  FILE *in = fopen(file, "r");
  if(!in)
    return false;
  point_t p;
  while(!feof(in)) {
    if(fscanf(in, "%lg %lg\n", &p.x, &p.y) != 2)
      break;
    waypoints.push_back(p);
  }
  fclose(in);
  return true;
}

bool load_trajectory(const char *file, std::vector<pose_t> &trajectory)
{
  trajectory.clear(); 
  FILE *in = fopen(file, "r");
  if(!in)
    return false;
  pose_t p;
  while(!feof(in)) {
    if(fscanf(in, "%lg %lg %lg\n", &p.x, &p.y, &p.t) != 3)
      break;
    p.t = deg2rad(p.t);
    trajectory.push_back(p);
  }
  fclose(in);
  return true;
}

// generate the ground truth trajectory; basically borrowed from Tim
// Bailey's Matlab SLAM simulator
void generate_perfect_trajectory_from_waypoints
  (const std::list<point_t> &waypoints, const sensor_t &s,
   std::vector<pose_t> &trajectory)
{
  trajectory.clear();
  std::list<point_t>::const_iterator cwp = waypoints.begin(), nwp = cwp;
  ++nwp;
  // compute an initial heading (just point toward the 2nd waypoint)
  double theta = (nwp == waypoints.end()) ? 0 :
    clamp_angle(atan2(nwp->y - cwp->y, nwp->x - cwp->x));
  double G = 0; // steering angle
  pose_t p(cwp->x, cwp->y, theta);
  while(cwp != waypoints.end()) {
    // have we reached the current waypoint?
    if(squared_distance(*cwp, p) < 1.0) { // FIXME make this a parameter?
      // switch to the next waypoint
      ++cwp;
      if(cwp == waypoints.end())
	break;
    }

    // angle to current waypoint
    double dG = clamp_angle(atan2(cwp->y - p.y, cwp->x - p.x) - p.t - G);

    // limit rate of steering angle change
    if(fabs(dG) > max_dG(s))
      dG = sign(dG)*max_dG(s);

    // limit angle
    G = G+dG;
    if(fabs(G) > max_G())
      G = sign(G)*max_G();

    // compute perfect motion
    p.x += s.v * cos(G+p.t) / s.F;
    p.y += s.v * sin(G+p.t) / s.F;
    p.t += s.v * sin(G) / 2.0 / s.F; // FIXME: make wheelbase (2.0) a parameter
    trajectory.push_back(p);
  }
}

// "brownian motion": randomly select steering angles at each timestep
//  until time T.  assumes the grid is toroidal!
void generate_brownian_trajectory
  (double T, const sensor_t &s, std::vector<pose_t> &trajectory)
{
  // start at a random pose
  pose_t p(drand48()*(xmax-xmin)+xmin,
	   drand48()*(ymax-ymin)+ymin,
	   drand48()*2*M_PI-M_PI);
  trajectory.push_back(p);
  double G = 0;
  while(T > 0) {
    // pick a random steering angle change within +/- 20 degrees of
    // current heading
    // FIXME: make this a normal distribution!
    double dG = clamp_angle(drand48()*40*M_PI/180.0-20*M_PI/180.0);

    // limit rate of change
    if(fabs(dG) > max_dG(s))
      dG = sign(dG)*max_dG(s);

    // limit angle
    G = G+dG;
    if(fabs(G) > max_G())
      G = sign(G)*max_G();

    // compute perfect motion
    p.x += s.v * cos(G+p.t) / s.F;
    p.y += s.v * sin(G+p.t) / s.F;
    p.t += s.v * sin(G) / 2.0 / s.F; // FIXME: make wheelbase (2.0) a parameter

    // treat the world as a torus
    while(p.x < xmin) p.x = xmax - (xmin-p.x);
    while(p.x > xmax) p.x = xmin + (p.x-xmax);
    while(p.y < ymin) p.y = ymax - (ymin-p.y);
    while(p.y > ymax) p.y = ymin + (p.y-ymax);

    trajectory.push_back(p);
    T -= 1.0/s.F;
  }
}

// ignore velocity constraints and pick poses completely at random
void generate_uncorrelated_trajectory
  (double T, const sensor_t &s, std::vector<pose_t> &trajectory)
{
  trajectory.clear();
  uint32_t K = (uint32_t)ceil(T*s.F);
  for(; K > 0; --K)
    trajectory.push_back(pose_t(drand48()*(xmax-xmin)+xmin,
				drand48()*(ymax-ymin)+ymin,
				drand48()*2*M_PI-M_PI));
}