classLaserSensingTrajectory.cpp

00001 #include "classLaserSensingTrajectory.h"
00002 using namespace std;
00003 
00004 LaserSensingTrajectory::LaserSensingTrajectory():lightTouched(false), lightState(ALL_DARK), 
00005 min_contrast_ratio(MIN_CONTRAST_RATIO), threshold_factor(THRESHOLD_FACTOR), 
00006 min_acceptable_intensity(MIN_ACCEPTABLE_INTENSITY), fixedThreshold(FIXED_THRESHOLD), delayMirrorSamples(0) 
00007 {
00008     lsdTrajectory.clear(); // no need in principle! the constructor of the vector will give an empty vector!
00009     // attention: properly set the state of the threshold switch in the IO.init (which by the way, in THIS hardware
00010     // implementation indicates the state for ALL the objects..)
00011    modeThreshold=AUTO;
00012   // IO.setSwitchOneState(true);
00013 }
00014 
00015 LaserSensingTrajectory::~LaserSensingTrajectory() {
00016    // lsdTrajectory.clear(); // there is no need to clear the vector, the destructor of this vector is called by default (and it's NOT a vector of pointers)
00017 }
00018 
00019   
00020 void LaserSensingTrajectory::setDelayMirrors(int delay) {
00021     delayMirrorSamples=delay;
00022 }
00023 
00024 void LaserSensingTrajectory::addDelayMirrors(int add_delay) {
00025     delayMirrorSamples+=add_delay;
00026 }
00027 
00028 bool LaserSensingTrajectory::processSensedData() {
00029     // Compute max and min intensity on the loop
00030     maxI=0;
00031     minI=255; // ratio has been normalized between 0 and 255
00032     unsigned short auxSize=lsdTrajectory.size(); // could be an unsigned char in principle... no more than 255 points per object, but well, memory in future versions
00033     // of the microprocessor can be larger.
00034     
00035     // Compute minimum and maximum intensities:
00036     for (unsigned short i = 0; i < auxSize; i++) {
00037         unsigned char mesI=lsdTrajectory[i].intensity; 
00038         if (maxI<mesI)  maxI=mesI;
00039         if (minI>mesI)  minI=mesI;
00040     }
00041 
00042     // Compute autoThreshold:
00043     switch(modeThreshold) {
00044     case AUTO:
00045         if (minI==0) minI=1;
00046         if (maxI<min_acceptable_intensity) autoThreshold=255;// (we consider that the saccade is FULL on something black - this is noise)
00047         else if (1.0*maxI/minI > min_contrast_ratio ) {
00048             autoThreshold = (unsigned char) (1.0 * (maxI-minI) * threshold_factor + minI);    // threshold_factor = 2/3 or 1/2 is a good value.
00049         } else {// ... otherwise, we consider that the saccade is FULL on something WHITE (convention...)
00050             autoThreshold=0;
00051         }
00052     break;
00053     case FIXED:
00054         autoThreshold=fixedThreshold;
00055     break;
00056     }
00057   
00058 
00059     // Segment the trajectory (only two levels for the time being, but we can have more - meaning different forces, real or even complex values to have different angle forces...):
00060     // NOTE: if using 1 and -1 instead of 1 and 0, we can avoid having to add a "blob internal pressure"! -1 means a force towards the interior, and +1 outwards...
00061     // This means that the loop will naturally become inside-out depending on the color of the main surface! (but will mantain its normal size). Much better and elegant solution than the
00062     // idea of the blob "constant" internal pressure...
00063     bool isLightZone=false, isDarkZone=false;
00064     for (unsigned short i = 0; i <  auxSize; i++) {
00065         unsigned short delayedpoint=(i+auxSize+delayMirrorSamples)%auxSize; // this way we can have negative delayMirrorSamples if required (would be absurd though)
00066         if (lsdTrajectory[delayedpoint].intensity>=autoThreshold) { // this means a WHITE zone:
00067             lsdTrajectory[i].lightZone= -1;
00068             isLightZone=true;
00069         } else { // this means DARK ZONE
00070             lsdTrajectory[i].lightZone= 2;
00071             isDarkZone=true;
00072         }
00073     }
00074     
00075     // In the case of AUTO mode, we assume that something is touching the object when the trajectory has at least one light zone and one dark zone;
00076     // in the case of FIXED mode, we assume something is touching the object when there is at least one light zone (this is a convention, because it depends
00077     // on the background - but we can argue that "no background" corresponds to no reflection (then dark), and, say, a ping pong ball or finger is front 
00078     // will be "light".
00079     // Mmm... not convinced by the arguments above. Let's rather do this: lightTouched will mean ALWAYS that the saccade is touched by something, meaning it's intensity
00080     // goes from dark to light, i.e. at least one light AND one dark zone. In fact we should have THREE states: touched, all light, all dark. 
00081     if (modeThreshold==FIXED)  {
00082         lightTouched=isLightZone;
00083         lightZone=isLightZone;
00084         darkZone=isDarkZone;
00085         if (lightTouched) lightState=TOUCHED;
00086         else if (isLightZone) lightState=ALL_LIGHT;
00087         else lightState=ALL_DARK;
00088     } 
00089     else {
00090         lightTouched=(isLightZone&&isDarkZone); // assuming only two modes for modeThreshold
00091         lightZone=isLightZone;
00092         darkZone=isDarkZone;
00093         if (lightTouched) lightState=TOUCHED;
00094         else if (isLightZone) lightState=ALL_LIGHT;
00095         else lightState=ALL_DARK;
00096     }
00097     // Return lightTouched for commodity: 
00098     //return(lightTouched);
00099     return(lightState);
00100 }