1 | #include <math.h> |
---|
2 | #include <Detection/detection.hh> |
---|
3 | #include <param.hh> |
---|
4 | |
---|
5 | /** |
---|
6 | * areClose(Detection &, Detection &, Param &): |
---|
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 |
---|
10 | * at least one pixel of object2. |
---|
11 | */ |
---|
12 | |
---|
13 | bool areClose(Detection &obj1, Detection &obj2, Param &par) |
---|
14 | { |
---|
15 | |
---|
16 | bool close = false; // this will be the value returned |
---|
17 | |
---|
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 | */ |
---|
24 | |
---|
25 | bool flagAdj = par.getFlagAdjacent(); |
---|
26 | float threshS = par.getThreshS(); |
---|
27 | float threshV = par.getThreshV(); |
---|
28 | |
---|
29 | int gap; |
---|
30 | if(flagAdj) gap = 1; |
---|
31 | else gap = int( ceilf(threshS) ); |
---|
32 | long min1 = obj1.getXmin(); |
---|
33 | long min2 = obj2.getXmin(); |
---|
34 | long max1 = obj1.getXmax(); |
---|
35 | long max2 = obj2.getXmax(); |
---|
36 | // Test X ranges |
---|
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 ) ; |
---|
42 | |
---|
43 | // Test Y ranges |
---|
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 ) ); |
---|
53 | |
---|
54 | // Test Z ranges |
---|
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 ) ); |
---|
64 | |
---|
65 | if(areNear){ |
---|
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 | */ |
---|
71 | |
---|
72 | float *first = new float[3]; //just store x,y,z positions of objects. |
---|
73 | float *second = new float[3]; |
---|
74 | int counter = 0; |
---|
75 | int countermax = obj1.getSize()*obj2.getSize(); |
---|
76 | int size2 = obj2.getSize(); |
---|
77 | |
---|
78 | while(!close && counter<countermax ){ |
---|
79 | // run this until we run out of pixels or we find a close pair. |
---|
80 | |
---|
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); |
---|
87 | |
---|
88 | if(flagAdj){ |
---|
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. |
---|
91 | // For an overall match between the objects, we only require one |
---|
92 | // matching pair of pixels, hence the || in the "close" definition. |
---|
93 | close = close || |
---|
94 | ( (fabsf(first[0]-second[0]) <= 1.) //X vals adjacent? |
---|
95 | && (fabsf(first[1]-second[1]) <= 1.) //Y vals adjacent? |
---|
96 | && (fabsf(first[2]-second[2]) <= threshV) //Z vals close? |
---|
97 | ); |
---|
98 | } |
---|
99 | else{ |
---|
100 | close = close || |
---|
101 | ( (hypot(first[0]-second[0],first[1]-second[1])<=threshS) |
---|
102 | && (fabsf(first[2]-second[2]) <= threshV ) |
---|
103 | ); |
---|
104 | } |
---|
105 | |
---|
106 | counter++; |
---|
107 | } |
---|
108 | |
---|
109 | delete [] first; |
---|
110 | delete [] second; |
---|
111 | |
---|
112 | } |
---|
113 | |
---|
114 | return close; |
---|
115 | |
---|
116 | } |
---|