---

localizer.h

Go to the documentation of this file.

#ifndef LOCALIZER_H
#define LOCALIZER_H
/*
$Id: localizer.h,v 1.4 2007/02/07 03:49:10 derek Exp $ 
*/
#include <types.h>
#include <utility/fixedpoint.h>
#include <pose_chain.h>
#include <point.h>
#include <ir_base.h>
#include <range_sensor_ids.h>
// Maximum number of walls in environment.
#define MAXIMUM_LOCALIZER_WALLS 5



#define MINIMUM_HYPOTHESIS_THRESHOLD 1

#define BASE_FLOAT_TYPE FIXED_POINT

#define RADIAN_TYPE BASE_FLOAT_TYPE
#define DEGREE_TYPE BASE_FLOAT_TYPE
#define DISTANCE_TYPE BASE_FLOAT_TYPE

// Fixed Point Library
// SSS, cosC = ... know which side is C
#define fpSSS(_S1,_S2,_S3) (fpdiv((     fpmul(_S1,_S1) + fpmul(_S2,_S2) - fpmul(_S3,_S3)),(fpmul(FIXED_TWO,fpmul(_S1,_S2))) ))
#define fpSAS(_S1,_A,_S2)  (fpmul(_S1,_S1) + fpmul(_S2,_S2) - fpmul( fpmul(FIXED_TWO,fpmul(_S1,_S2)), fpcos(_A) ))

#define dpSSS(_S1,_S2,_S3) ( (_S1*_S1 + _S2*_S2 - _S3*_S3)/(2.0*_S1*_S2) )
// c^2 = a^2 + b^2 - 2abcos(A) 
#define dpSAS(_S1,_A,_S2)  (_S1*_S1 + _S2*_S2 - 2.0*_S1*_S2*cos(_A) )

FIXED_POINT FPangle(point, point);
FIXED_POINT FPdistance(point,point);
FIXED_POINT FPslope(point, point);
int FPintersection(point& I, const point& p0, FIXED_POINT M0, const point& p1,const point& p2);
FIXED_POINT FPrise(point, point);
FIXED_POINT FPrun(point, point);

#ifdef DOS
// Double Precision library
double DPslope(DPpoint, DPpoint);
double DPangle(DPpoint a, DPpoint b);
double DPdistance(DPpoint p1,DPpoint p2);
int DPintersection(DPpoint& I, const DPpoint& p0, double M0, const DPpoint& p1,const DPpoint& p2);
// Point equivalents.
double DPangle(point, point);
double DPdistance(point,point);
double DPslope(point, point);
double DPrise(point, point);
double DPrun(point, point);
#endif


// WARNING: DO NOT USE "d0" as a variable as it is a register in 68332 and will get
//          COPMPILE problems on MRM.




class Linear_Obstacle_T
{
public:
  Linear_Obstacle_T(void);
  point p1;
  point p2;
};
class Linear_Obstacle
{
public:
  Linear_Obstacle(void);
  Linear_Obstacle_T o[MAXIMUM_LOCALIZER_WALLS];

  int top;
};


struct Localizer 
{
public:
  Localizer(void);
  void wall_extract(void);
  void WSwrite(void);

  void localizer_event(RANGE_SENSOR rid, IR_base* const ir);
  #ifdef DOS
  void generate_statistics(void);
  #endif
protected:
  bool point_within_map(const Linear_Obstacle_T& lo, int x, int y);
  bool good_intersection(point p0, const Linear_Obstacle_T&);

  void find_bounds(point p1, point p2, point& Pmin, point& Pmax);
  #ifdef DOS
//  double DPSensorEvent(const DPpoint& p0, const Linear_Obstacle_T& lo, double Th, double& Siota, bool simple);
  bool DPSensorEvent(double& Newsigma, const DPpoint& PIsigma, const Linear_Obstacle_T& lo,double Th, double& Siota, bool simple);
  bool DPSetup(const DPpoint& p0, const Linear_Obstacle_T& lo, double& d0, double& RL, double M0, double M1);
  #else
  bool FPSensorEvent(FIXED_POINT&, const point& p0, const Linear_Obstacle_T& lo, FIXED_POINT Th, FIXED_POINT rs, bool simple);
  void FPSetup(const point& p0, const Linear_Obstacle_T& lo, FIXED_POINT& , FIXED_POINT& RL, FIXED_POINT M0, FIXED_POINT M1);
//  void FPSetup( FIXED_POINT& , FIXED_POINT& Ssigma, FIXED_POINT M0);
  #endif


  void generic_localizer_event(sensor_base* ir, short ir_value, bool simple);
  // wall extraction subroutines.
  bool valid_wall(DIRECTORY_OPCODE, Linear_Obstacle_T& LO);
  enum WALL_SLANT_TYPE { UNDETERMINED_SLANT, SLANT_E, SLANT_NE, SLANT_N, SLANT_NW, SLANT_W, MAX_SLANT };
  enum WALL_INSPECTION_TYPE { UNINSPECTED, INSPECTED, ENDPOINT, PART_OF_LINE};
  pose_chain _sensors;
  // A list of walls (obstacles) by endpoints.
  Linear_Obstacle walls; //[MAXIMUM_LOCALIZER_WALLS];

  short sign;
private:
  #ifdef DOS
  bool find_wall(const Linear_Obstacle_T& LO, DPpoint PIsigma, double Siota, double sigma);
  #else
  bool find_wall(const Linear_Obstacle_T& LO, point PIsigma, FIXED_POINT Siota, FIXED_POINT sigma);
  #endif
  bool _non_intersecting; // true if current obstacle is non solvable by current method.
  void it(uint8* wg, Linear_Obstacle_T& lo,short x,short y);
  bool obstacle_extraction1(uint8* wg, Linear_Obstacle_T& lo, uint8& slant, short cx,short cy,short& nx, short& ny);
  point PSO; // Note: can be integer as intersection must be on map cell node.
  #ifdef DOS
  // NOTE: items with sigma refer to simulant; items w/ iota refer to invariant.
  double Post_Processing(sensor_base* ir, const Linear_Obstacle_T& lo, double&, int Siota);
  DPpoint PIsigma; // position
  DPpoint Pi; // position of invariant.
  DPpoint Pr; // position of robot (mapx,mapy), but I don't want to keep grabbing it.
  DPpoint PIiota;
  double sigma;
  double Ssigma;
  #else
  FIXED_POINT FPPost_Processing(sensor_base* ir, const Linear_Obstacle_T& lo, FIXED_POINT&, int Siota);
  point PIsigma; // position
  point Pi; // position of invariant.
  point Pr; // position of robot (mapx,mapy), but I don't want to keep grabbing it.
  point PIiota;
  FIXED_POINT Ssigma;
  FIXED_POINT sigma;
  #endif
  point Pmin, Pmax; // extreme position of wall
  RANGE_SENSOR _current_ir;
  #ifdef DOS
  short disp_list[1000]; // history of displacements.
  float orient_list[1000]; // history of orientation errors.
  int _episode_number;
  #endif
};

#endif

---