Project

General

Profile

Revision 534

Added by Jason knichel about 16 years ago

wrote some code to smooth robot identification across frames to help stop falsely losing robots
changed the vision thresholds

View differences:

trunk/code/projects/colonet/server/includes/PositionMonitor.h
15 15 

  		




  16
  16
  
    
#define MAX_POSITIONS 20


  		




  17
  17
  
    
#define SAME_ROBOT_DISTANCE_THRESHOLD 10


  		




  
  18
  
    
#define ROBOT_DELETE_BUFFER 5


  		




  18
  19
  
    

  		




  19
  20
  
    
using namespace std;


  		




  20
  21
  
    

  		




  ......




  35
  36
  
    

  		




  36
  37
  
    
 private:


  		




  37
  38
  
    
  map<int, VisionPosition> positionMap;


  		




  
  39
  
    
  map<int, int> deleteBufferMap;


  		




  38
  40
  
    
  bool isProbablySameRobot(VisionPosition p1, VisionPosition p2);


  		




  39
  41
  
    
  int newIdToAssign;


  		




  40
  42
  
    
  pthread_mutex_t position_map_lock;


  		












  

    
      trunk/code/projects/colonet/server/vision/vision.c
    
  





  14
  14
  
    
#include <stdio.h>


  		




  15
  15
  
    
#include <stdlib.h>


  		




  16
  16
  
    

  		




  17
  
  
    
#define MINH 150 //min threshold level


  		




  18
  
  
    
#define MAXH 200 //max threshold level


  		




  
  17
  
    
#define MINH 100 //min threshold level


  		




  
  18
  
    
#define MAXH 160 //max threshold level


  		




  
  19
  
    

  		




  19
  20
  
    
#define DEBUG 0 //Debug to find threshold level


  		




  20
  21
  
    

  		




  21
  22
  
    
struct CenterP {


  		












  

    
      trunk/code/projects/colonet/server/PositionMonitor.cpp
    
  





  62
  62
  
    
      if (isProbablySameRobot(newPos, oldPos)) {


  		




  63
  63
  
    
        //TODO: is this the right use of an iterator?


  		




  64
  64
  
    
        newPositionMap.insert(make_pair(iter->first, newPos));


  		




  
  65
  
    
	deleteBufferMap.insert(make_pair(iter->first, ROBOT_DELETE_BUFFER));


  		




  65
  66
  
    
        break;


  		




  66
  67
  
    
      }


  		




  67
  68
  
    
    }


  		




  ......




  73
  74
  
    
      


  		




  74
  75
  
    
      //a position was found that probably isn't a known robot so add it in case a new robot entered the field


  		




  75
  76
  
    
      newPositionMap.insert(make_pair(newIdToAssign, newPos));


  		




  
  77
  
    
      deleteBufferMap.insert(make_pair(newIdToAssign, ROBOT_DELETE_BUFFER));


  		




  76
  78
  
    
      newIdToAssign--;


  		




  77
  79
  
    
    }


  		




  78
  80
  
    
  }


  		




  79
  81
  
    

  		




  
  82
  
    
  map<int, VisionPosition>::iterator iter2;


  		




  
  83
  
    
  for (iter2 = positionMap.begin(); iter2 != positionMap.end(); iter2++) {


  		




  
  84
  
    
    int currId = iter2->first;


  		




  
  85
  
    
    map<int, VisionPosition>::iterator checkContains = newPositionMap.find(currId);


  		




  
  86
  
    
    if (checkContains == newPositionMap.end()) {


  		




  
  87
  
    
      int bufferValue = deleteBufferMap[iter2->first];


  		




  
  88
  
    
      bufferValue--;


  		




  
  89
  
    
      if (bufferValue > 0) {


  		




  
  90
  
    
	newPositionMap.insert(make_pair(currId, iter2->second));


  		




  
  91
  
    
	deleteBufferMap.erase(currId);


  		




  
  92
  
    
	deleteBufferMap.insert(make_pair(currId, bufferValue));


  		




  
  93
  
    
      } else {


  		




  
  94
  
    
	deleteBufferMap.erase(currId);


  		




  
  95
  
    
      }


  		




  
  96
  
    
    }


  		




  
  97
  
    
  }


  		




  
  98
  
    

  		




  80
  99
  
    
  positionMap = newPositionMap;


  		




  81
  100
  
    

  		




  
  101
  
    
  printf("\npositionMap size is %d and deleteBufferMap size is %d\n", positionMap.size(), deleteBufferMap.size());


  		




  
  102
  
    

  		




  82
  103
  
    
  //TODO: remove this debug information


  		




  83
  104
  
    
  map<int, VisionPosition>::iterator iter;


  		




  84
  105
  
    
  for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {


  		




  ......




  109
  130
  
    

  		




  110
  131
  
    
  positionMap.insert(make_pair(real_id, iter->second));


  		




  111
  132
  
    
  positionMap.erase(old_id);


  		




  
  133
  
    
  int oldDeleteBuffer = deleteBufferMap[old_id];


  		




  
  134
  
    
  deleteBufferMap.insert(make_pair(real_id, oldDeleteBuffer));


  		




  
  135
  
    
  deleteBufferMap.erase(old_id);


  		




  112
  136
  
    

  		




  113
  137
  
    
  pthread_mutex_unlock(&position_map_lock);


  		




  114
  138
  
    

  		












Also available in:  Unified diff