Alvaro Cassinelli
/
skinGames_forktest
just a test
Fork of scoreLight_Advanced by
classLaserSensingTrajectory.cpp@45:a3b984a79d5d, 2014-04-01 (annotated)
- Committer:
- mbedalvaro
- Date:
- Tue Apr 01 10:08:49 2014 +0000
- Revision:
- 45:a3b984a79d5d
- Parent:
- 42:c4e9c1116af4
working on tracking mode;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedalvaro | 32:52273c3291fe | 1 | #include "classLaserSensingTrajectory.h" |
mbedalvaro | 36:233b12d0b1f0 | 2 | using namespace std; |
mbedalvaro | 32:52273c3291fe | 3 | |
mbedalvaro | 45:a3b984a79d5d | 4 | LaserSensingTrajectory::LaserSensingTrajectory():lightTouched(false), lightState(ALL_DARK), |
mbedalvaro | 36:233b12d0b1f0 | 5 | min_contrast_ratio(MIN_CONTRAST_RATIO), threshold_factor(THRESHOLD_FACTOR), |
mbedalvaro | 42:c4e9c1116af4 | 6 | min_acceptable_intensity(MIN_ACCEPTABLE_INTENSITY), fixedThreshold(FIXED_THRESHOLD), delayMirrorSamples(0) |
mbedalvaro | 36:233b12d0b1f0 | 7 | { |
mbedalvaro | 36:233b12d0b1f0 | 8 | lsdTrajectory.clear(); // no need in principle! the constructor of the vector will give an empty vector! |
mbedalvaro | 36:233b12d0b1f0 | 9 | // attention: properly set the state of the threshold switch in the IO.init (which by the way, in THIS hardware |
mbedalvaro | 36:233b12d0b1f0 | 10 | // implementation indicates the state for ALL the objects..) |
mbedalvaro | 36:233b12d0b1f0 | 11 | modeThreshold=AUTO; |
mbedalvaro | 36:233b12d0b1f0 | 12 | // IO.setSwitchOneState(true); |
mbedalvaro | 32:52273c3291fe | 13 | } |
mbedalvaro | 32:52273c3291fe | 14 | |
mbedalvaro | 32:52273c3291fe | 15 | LaserSensingTrajectory::~LaserSensingTrajectory() { |
mbedalvaro | 32:52273c3291fe | 16 | // 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) |
mbedalvaro | 32:52273c3291fe | 17 | } |
mbedalvaro | 32:52273c3291fe | 18 | |
mbedalvaro | 32:52273c3291fe | 19 | |
mbedalvaro | 32:52273c3291fe | 20 | void LaserSensingTrajectory::setDelayMirrors(int delay) { |
mbedalvaro | 32:52273c3291fe | 21 | delayMirrorSamples=delay; |
mbedalvaro | 32:52273c3291fe | 22 | } |
mbedalvaro | 32:52273c3291fe | 23 | |
mbedalvaro | 32:52273c3291fe | 24 | void LaserSensingTrajectory::addDelayMirrors(int add_delay) { |
mbedalvaro | 32:52273c3291fe | 25 | delayMirrorSamples+=add_delay; |
mbedalvaro | 32:52273c3291fe | 26 | } |
mbedalvaro | 32:52273c3291fe | 27 | |
mbedalvaro | 36:233b12d0b1f0 | 28 | bool LaserSensingTrajectory::processSensedData() { |
mbedalvaro | 32:52273c3291fe | 29 | // Compute max and min intensity on the loop |
mbedalvaro | 32:52273c3291fe | 30 | maxI=0; |
mbedalvaro | 32:52273c3291fe | 31 | minI=255; // ratio has been normalized between 0 and 255 |
mbedalvaro | 36:233b12d0b1f0 | 32 | 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 |
mbedalvaro | 36:233b12d0b1f0 | 33 | // of the microprocessor can be larger. |
mbedalvaro | 32:52273c3291fe | 34 | |
mbedalvaro | 32:52273c3291fe | 35 | // Compute minimum and maximum intensities: |
mbedalvaro | 36:233b12d0b1f0 | 36 | for (unsigned short i = 0; i < auxSize; i++) { |
mbedalvaro | 32:52273c3291fe | 37 | unsigned char mesI=lsdTrajectory[i].intensity; |
mbedalvaro | 32:52273c3291fe | 38 | if (maxI<mesI) maxI=mesI; |
mbedalvaro | 32:52273c3291fe | 39 | if (minI>mesI) minI=mesI; |
mbedalvaro | 32:52273c3291fe | 40 | } |
mbedalvaro | 32:52273c3291fe | 41 | |
mbedalvaro | 32:52273c3291fe | 42 | // Compute autoThreshold: |
mbedalvaro | 33:43e8bc451ef0 | 43 | switch(modeThreshold) { |
mbedalvaro | 33:43e8bc451ef0 | 44 | case AUTO: |
mbedalvaro | 33:43e8bc451ef0 | 45 | if (minI==0) minI=1; |
mbedalvaro | 34:1244fa3f2559 | 46 | if (maxI<min_acceptable_intensity) autoThreshold=255;// (we consider that the saccade is FULL on something black - this is noise) |
mbedalvaro | 33:43e8bc451ef0 | 47 | else if (1.0*maxI/minI > min_contrast_ratio ) { |
mbedalvaro | 33:43e8bc451ef0 | 48 | autoThreshold = (unsigned char) (1.0 * (maxI-minI) * threshold_factor + minI); // threshold_factor = 2/3 or 1/2 is a good value. |
mbedalvaro | 45:a3b984a79d5d | 49 | } else {// ... otherwise, we consider that the saccade is FULL on something WHITE (convention...) |
mbedalvaro | 33:43e8bc451ef0 | 50 | autoThreshold=0; |
mbedalvaro | 33:43e8bc451ef0 | 51 | } |
mbedalvaro | 34:1244fa3f2559 | 52 | break; |
mbedalvaro | 33:43e8bc451ef0 | 53 | case FIXED: |
mbedalvaro | 33:43e8bc451ef0 | 54 | autoThreshold=fixedThreshold; |
mbedalvaro | 34:1244fa3f2559 | 55 | break; |
mbedalvaro | 32:52273c3291fe | 56 | } |
mbedalvaro | 34:1244fa3f2559 | 57 | |
mbedalvaro | 32:52273c3291fe | 58 | |
mbedalvaro | 32:52273c3291fe | 59 | // 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...): |
mbedalvaro | 32:52273c3291fe | 60 | // 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... |
mbedalvaro | 32:52273c3291fe | 61 | // 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 |
mbedalvaro | 32:52273c3291fe | 62 | // idea of the blob "constant" internal pressure... |
mbedalvaro | 36:233b12d0b1f0 | 63 | bool isLightZone=false, isDarkZone=false; |
mbedalvaro | 36:233b12d0b1f0 | 64 | for (unsigned short i = 0; i < auxSize; i++) { |
mbedalvaro | 36:233b12d0b1f0 | 65 | unsigned short delayedpoint=(i+auxSize+delayMirrorSamples)%auxSize; // this way we can have negative delayMirrorSamples if required (would be absurd though) |
mbedalvaro | 32:52273c3291fe | 66 | if (lsdTrajectory[delayedpoint].intensity>=autoThreshold) { // this means a WHITE zone: |
mbedalvaro | 37:fa6b1f15819f | 67 | lsdTrajectory[i].lightZone= -1; |
mbedalvaro | 36:233b12d0b1f0 | 68 | isLightZone=true; |
mbedalvaro | 36:233b12d0b1f0 | 69 | } else { // this means DARK ZONE |
mbedalvaro | 37:fa6b1f15819f | 70 | lsdTrajectory[i].lightZone= 2; |
mbedalvaro | 36:233b12d0b1f0 | 71 | isDarkZone=true; |
mbedalvaro | 32:52273c3291fe | 72 | } |
mbedalvaro | 32:52273c3291fe | 73 | } |
mbedalvaro | 36:233b12d0b1f0 | 74 | |
mbedalvaro | 36:233b12d0b1f0 | 75 | // 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; |
mbedalvaro | 36:233b12d0b1f0 | 76 | // 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 |
mbedalvaro | 36:233b12d0b1f0 | 77 | // 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 |
mbedalvaro | 45:a3b984a79d5d | 78 | // will be "light". |
mbedalvaro | 45:a3b984a79d5d | 79 | // 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 |
mbedalvaro | 45:a3b984a79d5d | 80 | // 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. |
mbedalvaro | 45:a3b984a79d5d | 81 | if (modeThreshold==FIXED) { |
mbedalvaro | 36:233b12d0b1f0 | 82 | lightTouched=isLightZone; |
mbedalvaro | 45:a3b984a79d5d | 83 | lightZone=isLightZone; |
mbedalvaro | 45:a3b984a79d5d | 84 | darkZone=isDarkZone; |
mbedalvaro | 45:a3b984a79d5d | 85 | if (lightTouched) lightState=TOUCHED; |
mbedalvaro | 45:a3b984a79d5d | 86 | else if (isLightZone) lightState=ALL_LIGHT; |
mbedalvaro | 45:a3b984a79d5d | 87 | else lightState=ALL_DARK; |
mbedalvaro | 45:a3b984a79d5d | 88 | } |
mbedalvaro | 45:a3b984a79d5d | 89 | else { |
mbedalvaro | 45:a3b984a79d5d | 90 | lightTouched=(isLightZone&&isDarkZone); // assuming only two modes for modeThreshold |
mbedalvaro | 45:a3b984a79d5d | 91 | lightZone=isLightZone; |
mbedalvaro | 45:a3b984a79d5d | 92 | darkZone=isDarkZone; |
mbedalvaro | 45:a3b984a79d5d | 93 | if (lightTouched) lightState=TOUCHED; |
mbedalvaro | 45:a3b984a79d5d | 94 | else if (isLightZone) lightState=ALL_LIGHT; |
mbedalvaro | 45:a3b984a79d5d | 95 | else lightState=ALL_DARK; |
mbedalvaro | 45:a3b984a79d5d | 96 | } |
mbedalvaro | 36:233b12d0b1f0 | 97 | // Return lightTouched for commodity: |
mbedalvaro | 45:a3b984a79d5d | 98 | //return(lightTouched); |
mbedalvaro | 45:a3b984a79d5d | 99 | return(lightState); |
mbedalvaro | 32:52273c3291fe | 100 | } |