1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44
|
double som1=0;
double som2=0;
int p5=0;
int p6=0;
int p3=0;
int p4=0;
for(int i = 0; i < number_of_features; i++)
{
if ( optical_flow_found_feature_1[i] == 0 || optical_flow_found_feature_2[i] == 0) continue;
CvPoint p1,q1;
p1.x = (int) frame_p_features[i].x;
p1.y = (int) frame_p_features[i].y;
q1.x = (int) frame_q_features_1[i].x;
q1.y = (int) frame_q_features_1[i].y;
CvPoint p2,q2;
p2.x = (int) frame_p_features[i].x;
p2.y = (int) frame_p_features[i].y;
q2.x = (int) frame_q_features_2[i].x;
q2.y = (int) frame_q_features_2[i].y;
double angle1; angle1 = 180*atan2( (double) p1.y - q1.y, (double) p1.x - q1.x )/pi;
double hypotenuse1; hypotenuse1 = sqrt( square(p1.y - q1.y) + square(p1.x - q1.x) );
double angle2; angle2 = 180*atan2( (double) p2.y - q2.y, (double) p2.x - q2.x )/pi;
double hypotenuse2; hypotenuse2 = sqrt( square(p2.y - q2.y) + square(p2.x - q2.x) );
if (hypotenuse1>0)
{
if (angle1>=0 && angle1<90)
p5++;
if (angle1>=90 && angle1<180)
p6++;
if (angle1>= -90 && angle1<0)
p4++;
if (angle1>= -180 && angle1<-90)
p3++;
}
som1=som1+hypotenuse1/number_of_features;
som2=som2+hypotenuse2/number_of_features;
} |