[3] | 1 | #include <math.h> |
---|
| 2 | #include <Detection/detection.hh> |
---|
| 3 | #include <param.hh> |
---|
| 4 | |
---|
| 5 | /** |
---|
| 6 | * areClose(Detection &, Detection &, Param &): |
---|
[86] | 7 | * A Function to test whether object1 and object2 are within the |
---|
| 8 | * spatial and velocity thresholds specified in the parameter set par. |
---|
| 9 | * Returns true if at least one pixel of object1 is close to |
---|
[3] | 10 | * at least one pixel of object2. |
---|
| 11 | */ |
---|
| 12 | |
---|
| 13 | bool areClose(Detection &obj1, Detection &obj2, Param &par) |
---|
| 14 | { |
---|
| 15 | |
---|
[86] | 16 | bool close = false; // this will be the value returned |
---|
[3] | 17 | |
---|
[86] | 18 | /* |
---|
| 19 | * First, check to see if the objects are nearby. |
---|
| 20 | * We will only do the pixel-by-pixel comparison if their pixel ranges overlap. |
---|
| 21 | * This saves a bit of time if the objects are big and are nowhere near |
---|
| 22 | * one another. |
---|
| 23 | */ |
---|
[3] | 24 | |
---|
[82] | 25 | bool flagAdj = par.getFlagAdjacent(); |
---|
| 26 | float threshS = par.getThreshS(); |
---|
| 27 | float threshV = par.getThreshV(); |
---|
[3] | 28 | |
---|
[82] | 29 | int gap; |
---|
| 30 | if(flagAdj) gap = 1; |
---|
[107] | 31 | else gap = int( ceil(threshS) ); |
---|
[82] | 32 | long min1 = obj1.getXmin(); |
---|
| 33 | long min2 = obj2.getXmin(); |
---|
| 34 | long max1 = obj1.getXmax(); |
---|
| 35 | long max2 = obj2.getXmax(); |
---|
[3] | 36 | // Test X ranges |
---|
[82] | 37 | bool areNear = |
---|
| 38 | ((min1-min2+gap)*(min1-max2-gap) <= 0 ) || |
---|
| 39 | ((max1-min2+gap)*(max1-max2-gap) <= 0 ) || |
---|
| 40 | ((min2-min1+gap)*(min2-max1-gap) <= 0 ) || |
---|
| 41 | ((max2-min1+gap)*(max2-max1-gap) <= 0 ) ; |
---|
[3] | 42 | |
---|
| 43 | // Test Y ranges |
---|
[82] | 44 | min1 = obj1.getYmin(); |
---|
| 45 | min2 = obj2.getYmin(); |
---|
| 46 | max1 = obj1.getYmax(); |
---|
| 47 | max2 = obj2.getYmax(); |
---|
| 48 | areNear = areNear && |
---|
| 49 | ( ((min1-min2+gap)*(min1-max2-gap) <= 0 ) || |
---|
| 50 | ((max1-min2+gap)*(max1-max2-gap) <= 0 ) || |
---|
| 51 | ((min2-min1+gap)*(min2-max1-gap) <= 0 ) || |
---|
| 52 | ((max2-min1+gap)*(max2-max1-gap) <= 0 ) ); |
---|
[3] | 53 | |
---|
| 54 | // Test Z ranges |
---|
[82] | 55 | min1 = obj1.getZmin(); |
---|
| 56 | min2 = obj2.getZmin(); |
---|
| 57 | max1 = obj1.getZmax(); |
---|
| 58 | max2 = obj2.getZmax(); |
---|
| 59 | areNear = areNear && |
---|
| 60 | ( ((min1-min2+threshV)*(min1-max2-threshV) <= 0 ) || |
---|
| 61 | ((max1-min2+threshV)*(max1-max2-threshV) <= 0 ) || |
---|
| 62 | ((min2-min1+threshV)*(min2-max1-threshV) <= 0 ) || |
---|
| 63 | ((max2-min1+threshV)*(max2-max1-threshV) <= 0 ) ); |
---|
[3] | 64 | |
---|
[82] | 65 | if(areNear){ |
---|
[86] | 66 | /* |
---|
| 67 | * If we get to here, the pixel ranges overlap -- so we do a pixel-by-pixel |
---|
| 68 | * comparison to make sure they are actually "close" according to the thresholds. |
---|
| 69 | * Otherwise, close=false, and so don't need to do anything else before returning. |
---|
| 70 | */ |
---|
[3] | 71 | |
---|
| 72 | float *first = new float[3]; //just store x,y,z positions of objects. |
---|
| 73 | float *second = new float[3]; |
---|
[82] | 74 | int counter = 0; |
---|
| 75 | int countermax = obj1.getSize()*obj2.getSize(); |
---|
| 76 | int size2 = obj2.getSize(); |
---|
[3] | 77 | |
---|
[82] | 78 | while(!close && counter<countermax ){ |
---|
[3] | 79 | // run this until we run out of pixels or we find a close pair. |
---|
| 80 | |
---|
[82] | 81 | first[0] = obj1.getX(counter/size2); |
---|
| 82 | first[1] = obj1.getY(counter/size2); |
---|
| 83 | first[2] = obj1.getZ(counter/size2); |
---|
| 84 | second[0] = obj2.getX(counter%size2); |
---|
| 85 | second[1] = obj2.getY(counter%size2); |
---|
| 86 | second[2] = obj2.getZ(counter%size2); |
---|
[3] | 87 | |
---|
[82] | 88 | if(flagAdj){ |
---|
[3] | 89 | //This step just tests to see if there is a pair of *adjacent* pixels spatially, |
---|
| 90 | // and if the velocity pixels are within the threshold. |
---|
[86] | 91 | // For an overall match between the objects, we only require one |
---|
| 92 | // matching pair of pixels, hence the || in the "close" definition. |
---|
[3] | 93 | close = close || |
---|
[125] | 94 | ( (fabs(first[0]-second[0]) <= 1.) //X vals adjacent? |
---|
| 95 | && (fabs(first[1]-second[1]) <= 1.) //Y vals adjacent? |
---|
| 96 | && (fabs(first[2]-second[2]) <= threshV) //Z vals close? |
---|
[3] | 97 | ); |
---|
| 98 | } |
---|
| 99 | else{ |
---|
| 100 | close = close || |
---|
[82] | 101 | ( (hypot(first[0]-second[0],first[1]-second[1])<=threshS) |
---|
[125] | 102 | && (fabs(first[2]-second[2]) <= threshV ) |
---|
[3] | 103 | ); |
---|
| 104 | } |
---|
| 105 | |
---|
[82] | 106 | counter++; |
---|
[3] | 107 | } |
---|
| 108 | |
---|
| 109 | delete [] first; |
---|
| 110 | delete [] second; |
---|
| 111 | |
---|
| 112 | } |
---|
| 113 | |
---|
| 114 | return close; |
---|
| 115 | |
---|
| 116 | } |
---|