fixed road orientation (and limit for angleFB)

rr_drone.py

@@ -39,8 +39,12 @@ def approx4pts( poly ):
 def trapezoid2line( t ):
   if len(t) == 4:
     if (max( t[0][1], t[1][1] ) < min (t[2][1], t[3][1])) or (min( t[0][1], t[1][1] ) > max (t[2][1], t[3][1])) :
-      return [((t[0][0]+t[1][0])/2, (t[0][1]+t[1][1])/2), ((t[2][0]+t[3][0])/2, (t[2][1]+t[3][1])/2)]
-    return [((t[0][0]+t[3][0])/2, (t[0][1]+t[3][1])/2), ((t[2][0]+t[1][0])/2, (t[2][1]+t[1][1])/2)]
+      begin,end = [((t[0][0]+t[1][0])/2, (t[0][1]+t[1][1])/2), ((t[2][0]+t[3][0])/2, (t[2][1]+t[3][1])/2)]
+    else:
+      begin,end = [((t[0][0]+t[3][0])/2, (t[0][1]+t[3][1])/2), ((t[2][0]+t[1][0])/2, (t[2][1]+t[1][1])/2)]
+    if begin[1] < end[1]:
+      begin,end = end,begin
+    return [begin,end]
 
 def drawArrow( img, pt1, pt2, color, thickness=1 ):
   # inspiration from http://stackoverflow.com/questions/10161351/opencv-how-to-plot-velocity-vectors-as-arrows-in-using-single-static-image
@@ -137,7 +141,7 @@ def project2plane( imgCoord, coord, height, heading, angleFB, angleLR ):
   angleLR = -angleLR # we want to compensate the turn
   x,y = x*math.cos(angleLR)-y*math.sin(angleLR), y*math.cos(angleLR)+x*math.sin(angleLR)
   h = -x/1280*FOW + heading
-  tilt = y/1280*FOW - angleFB
+  tilt = y/1280*FOW + angleFB
   if tilt > -EPS:
     return None # close to 0.0 AND projection behind drone
 

rr_drone_test.py

@@ -24,7 +24,12 @@ def testProject2plane( self ):
     self.assertEqual( project2plane( imgCoord=(1280/2, 720/2), coord=(0.0, 0.0), height = 1.5,
         heading=math.radians(45), angleFB=math.radians(0), angleLR=0 ), None )
     (x,y) = project2plane( imgCoord=(200, 720/2), coord=(0.0, 0.0), height = 1.5,
-        heading=0, angleFB=0, angleLR=math.radians(10) )
+        heading=0, angleFB=0, angleLR=math.radians(-10) ) # angleLR is positive for turn right
+
+  def testTrapezoid2lineBug( self ):
+    "the line is oriented from drone to horizon"
+    begin,end = trapezoid2line( [(739, 497), (690, 579), (316, 570), (631, 460)] )
+    self.assertTrue( begin[1] > end[1] ) # in image coordinates
 
 if __name__ == "__main__":
   unittest.main()