ThreadLocalize.h

Go to the documentation of this file.

/*
 * ThreadLocalize.h
 *
 *  Created on: 28.10.2014
 *      Author: phil
 */

#ifndef THREADLOCALIZE_H_
#define THREADLOCALIZE_H_

#include "ThreadSLAM.h"

#include <sensor_msgs/LaserScan.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>

#include "obvision/reconstruct/grid/SensorPolar2D.h"
#include "obvision/reconstruct/grid/TsdGrid.h"
#include "obvision/reconstruct/grid/RayCastPolar2D.h"
#include "obvision/registration/icp/icp_def.h"

#include <string>

namespace ohm_tsd_slam
{

class SlamNode;
class ThreadMapping;
class Localization;


namespace
{   //default values in case no launch parameters are set
const unsigned int ICP_ITERATIONS = 25;
const double TRNS_THRESH = 0.25;            //Thresholds for registration. If the gained transformation is out of these bounds,
const double ROT_THRESH = 0.17;             //the Transformation is not taken over
const double TRNS_MIN = 0.05;              //Minimal values for the pose change. Push is only needed when pose change
const double ROT_MIN = 0.03;               //greater than than one of these values
const double DIST_FILT_MIN = 0.1;
const double DIST_FILT_MAX = 1.0;
const int RANSAC_TRIALS =  50;
const double RANSAC_EPS_THRESH = 0.15;
const int RANSAC_CTRL_SET_SIZE = 180;
const double RANSAC_PHI_MAX = 30.0;
}

class ThreadLocalize: public ThreadSLAM
{
  enum EnumRegModes
  {
    ICP = 0,    
    EXP         
  };

public:
  ThreadLocalize(obvious::TsdGrid* grid, ThreadMapping* mapper, ros::NodeHandle* nh, std::string nameSpace,
      const double xOffset, const double yOffset);

  virtual ~ThreadLocalize();

  void laserCallBack(const sensor_msgs::LaserScan& scan);
protected:

  virtual void eventLoop(void);
private:

  void init(const sensor_msgs::LaserScan& scan);

  double calcAngle(obvious::Matrix* T);

  bool isPoseChangeSignificant(obvious::Matrix* lastPose, obvious::Matrix* curPose);

  obvious::Matrix doRegistration(obvious::SensorPolar2D* sensor,
      obvious::Matrix* M,
      obvious::Matrix* Mvalid,
      obvious::Matrix* N,
      obvious::Matrix* Nvalid,
      obvious::Matrix* S,
      obvious::Matrix* Svalid,
      const bool experimental
  );

  bool isRegistrationError(obvious::Matrix* T, const double trnsMax, const  double rotMax);

  void sendTransform(obvious::Matrix* T);

  void sendNanTransform();

  obvious::Matrix maskMatrix(obvious::Matrix* Mat, bool* mask, unsigned int maskSize, unsigned int validPoints);

  void reduceResolution(bool* const maskIn, const obvious::Matrix* matIn, bool* const maskOut, obvious::Matrix* matOut,
      unsigned int pointsIn, unsigned int pointsOut, unsigned int reductionFactor);


  ros::NodeHandle* _nh;

  ThreadMapping& _mapper;

  obvious::SensorPolar2D* _sensor;

  bool _initialized;

  const double _gridWidth;

  const double _gridHeight;

  boost::mutex _dataMutex;

  const double _gridOffSetX;

  const double _gridOffSetY;

  double _trnsMax;

  double _rotMax;

  const double _xOffset;

  const double _yOffset;

  EnumRegModes _regMode;

  /*
   * RANSAC Reduction: Use to scale down the number of points for ransac
   * Example: Points in Scan = 1080
   *          ransacReduceFactor = 4
   *          -> points for ransac = 1080 /ransacReduceFactor = 270;
   */
  //unsigned int _ransacReduceFactor;

  unsigned int _ranTrials;

  double _ranEpsThresh;

  unsigned int _ranSizeCtrlSet;

  double _ranPhiMax;

  std::string _nameSpace;

  std::deque<sensor_msgs::LaserScan*> _laserData;

  double* _modelCoords;

  double* _modelNormals;

  bool* _maskM;

  double* _scene;

  bool* _maskS;

  obvious::RayCastPolar2D* _rayCaster;

  obvious::PairAssignment* _assigner;

  obvious::OutOfBoundsFilter2D* _filterBounds;

  obvious::DistanceFilter* _filterDist;

  obvious::ReciprocalFilter* _filterReciprocal;

  obvious::IRigidEstimator* _estimator;

  obvious::Icp* _icp;

  obvious::Matrix* _lastPose;

  ros::Publisher _posePub;

  geometry_msgs::PoseStamped _poseStamped;

  tf::TransformBroadcaster _tfBroadcaster;

  tf::StampedTransform _tf;
};



} /* namespace ohm_tsd_slam */

#endif /* THREADLOCALIZE_H_ */