//* Thread safe robot map location and merging support class header file *

#ifndef _NAV_MAP_MATCH #define _NAV_MAP_MATCH #include "nav_map.h" class NAV_Map_Match : public NAV_Map { public: NAV_Map_Match (double resol=100, double robotradius=150, double sensorrange=2500) : NAV_Map(resol, robotradius, sensorrange) {} NAV_Map_Match (const NAV_Map &map) : NAV_Map(map) {} // create a new map based on parent map NAV_Map_Match (const NAV_Map_Match &map, double rotation) // create a new map based on rotated parent map : NAV_Map( map.getResolution(), map.getRobotRadius(), map.getSensorRange() ) { map_assign( map, ArPose(0,0,rotation) ); } NAV_Map_Match (const NAV_Map_Match &map, const ArPose &transform) // create a new map based on transformed parent map : NAV_Map( map.getResolution(), map.getRobotRadius(), map.getSensorRange() ) { map_assign( map, transform ); } void map_assign (const NAV_Map_Match &map, double rotation) { map_assign(map, ArPose(0,0,rotation)); } void map_assign (const NAV_Map_Match &map, const ArPose &transform); // create a new map based on transformed map protected: long int map_match_tr (const NAV_Map_Match &map, int tpx, int tpy) const; // evaluate match of global map with given map and translation (no rotation) inline short int point_compare (short int g, short int l) const; // must be inline to ensure speed of map matching public: long int map_match (const NAV_Map_Match &map, double tx=0, double ty=0) const { return(map_match_tr(map, static_cast<int>(tx/res), static_cast<int>(ty/res))); } void map_merge (const NAV_Map_Match &map, const ArPose &transl, unsigned char confidence=0x80); }; inline short int NAV_Map_Match::point_compare (short int g, short int l)const { if (!(maskUnknown(g)) && !(maskUnknown(l)) ) { int go = maskObstacle(g); int lo = maskObstacle(l); // fast fuzzy comparision without non-linearization return( ( 0x00FF - abs(go-lo) ) >> 6 ); /* Old formula: bool gb = ( maskObstacle(g) > 0x0060 ); bool lb = ( maskObstacle(l) > 0x0060 ); if ( (gb && lb) || (!gb && !lb) ) return(+1); else return(-1); */ } else return(0); } #endif