controller update

configs/params.yaml

@@ -30,45 +30,56 @@ update rate: 30
 queue_size: 5
 
 imgResizeRatio: 100
+# for filtering contours
+minContourArea: 10
+
+
 
-stationaryDebug: True
+stationaryDebug: False
 # run time params
 # Mode 1: Driving forward with front camera (starting mode)
 # Mode 2: Driving forward with back camera
 # Mode 3: Driving backwards with back camera
 # Mode 4: Driving backwards with front camera
-navigationMode: 1
+navigationMode: 2
+
 # angular velocity scaler
 maxOmega: 0.05 
 minOmega: 0.01 
 omegaScaler: 0.1
 maxLinearVel: 0.5 
 minLinearVel: 0.01
+
 # Parameter for setting the number of rows to pass in one switch
-lines_to_pass: 1
+linesToPass: 1
 # Max Offset of the Window for extracting the turn features
-max_matching_dif_features: 100
+maxMatchingDifference: 100
 # Min Offset of the Window for extracting the turn features
-min_matching_dif_features: 0
+minMatchingDifference: 0
 # Threshold for keypoints
-matching_keypoints_th: 10
+matchingKeypointsNum: 10
+
 # scanner window params
-winSweepStart: 0
-winSweepEnd: 1280
-winMinWidth: 0
-# W/8 for small plants, W/5 for big plants
-winSize: 200
-# for filtering contours
-min_contour_area: 10
+scanSteps: 5
+scanStartPoint: 0
+scanEndPoint: 1280
+scanWindowWidth: 128
+
+# tracker params
+trackingBoxWidth: 200
+topOffset: 0
+bottomOffset: 0
+sacleRatio: 0.4
+
 # for dividing bushy rows
-max_coutour_height: 120
+maxCoutourHeight: 120
 # in case of using bigger size image size, we suggest to set ROI 
 enable_roi: True
 p1: 
  - 0
  - 0
 p2:
- - 300
+ - 350
  - 0
 p3:
  - 10
@@ -77,7 +88,7 @@ p4:
  - 0
  - 720
 p5: 
- - 880
+ - 830
  - 0
 p6:
  - 1280

scripts/featureMatching.py

@@ -1,21 +1,74 @@
 
 import cv2 as cv
+from matplotlib.image import BboxImage
 import numpy as np
 import itertools
+import copy
 from cv_bridge import CvBridge
 
+from shapely.geometry import Point
+from shapely.geometry.polygon import Polygon
+
 class featureMatching:
  def __init__(self, featureParams):
  self.featureParams = featureParams
+
+ def sampleCropRowFeatures(self, mode, rgbImg, greenIDx, mask, wLocs):
+ _greenIDx = greenIDx.copy()
+ _rgbImg = rgbImg.copy()
+ _rgbImgMasked = np.zeros(np.shape(_rgbImg), np.uint8)
+
+ # get the correct window depending on the current mode
+ rowId = None
+ if mode == 3:
+ rowId = 0
+ else:
+ rowId = -1
+ # int_coords = lambda x: np.array(x).round().astype(np.int32)
+ # exterior = np.array([int_coords(wLocs[rowId].exterior.coords)])
+ # bboxImg = self.cropBboxFromImage(_greenIDx, exterior)
+ idx = (mask!=0)
+ _rgbImgMasked[idx] = _rgbImg[idx]
+ self.refKeypoints, self.refDescriptors = self.detectTrackingFeatures(_rgbImgMasked)
+
+ self.drawKeyPoints(_rgbImg, self.refKeypoints)
+
+ def detectNewCropRow(self, mode, greenIDx, mask, wLocs):
+ # find new crop rows and compare to old ones!
+ # self.matchTrackingFeatures()
+ return False, None
+
+ def cropBboxFromImage(self, image, bbox):
+ imgWHeight, imgWidth = np.shape(image)
+ oneRowMask = np.zeros((imgWHeight, imgWidth), dtype=np.uint8)
+ cv.fillPoly(oneRowMask, bbox, (255))
+ res = cv.bitwise_and(image, image ,mask=oneRowMask)
+ bbox = cv.boundingRect(bbox) # returns (x,y,w,h) of the rect
+ bboxImg = res[bbox[1]: bbox[1] + bbox[3], bbox[0]: bbox[0] + bbox[2]]
+ # cv.imshow("test", bboxImg)
+ # cv.waitKey(0)
+ # cv.destroyAllWindows()
+ return bboxImg
+
+ def drawKeyPoints(self, rgbImg, keyPoints):
+ for kp in keyPoints:
+ ptX = kp.pt[0]
+ ptY = kp.pt[1]
+ rgbImg[int(ptY)-3:int(ptY)+3, int(ptX) -
+ 3:int(ptX)+3] = [0, 0, 255]
+ cv.imshow("test", rgbImg)
+ cv.waitKey(0)
+ cv.destroyAllWindows()
+ return rgbImg
 
- def detectTrackingFeatures(self, mode, rgbImg, greenIDX, mask, windowLocations, turnWindowWidth):
+ def detectTrackingFeatures(self, greenIDX):
  """ Function to compute the features of the line which has to be recognized
 
  Args:
  rgbImg (_type_): _description_
  greenIDX (_type_): _description_
  mode (_type_): _description_
- windowLocations (_type_): _description_
+ wLocs (_type_): _description_
  turnWindowWidth (_type_): _description_
  min_matching_dif_features (_type_): _description_
  max_matching_dif_features (_type_): _description_
@@ -24,37 +77,21 @@ def detectTrackingFeatures(self, mode, rgbImg, greenIDX, mask, windowLocations,
  _type_: _description_
  """
  print("#[INF] detect Tracking Features")
- self.imgWHeight, self.self.imgWidth, imgChannels = np.size(greenIDX)
  # Initiate SIFT detector
  sift = cv.xfeatures2d.SIFT_create()
  # get sift key points
  keyPointsRaw, descriptorsRaw = sift.detectAndCompute(greenIDX, None)
  matchingKeypoints = []
  featureDescriptors = []
- # get the correct window depending on the current mode
- if mode == 3:
- windowLoc = windowLocations[0]
- else:
- windowLoc = windowLocations[-1]
  # maintain the keypoints lying in the first window
  for kp, desc in itertools.izip(keyPointsRaw, descriptorsRaw):
- ptX = kp.pt[0]
- ptY = kp.pt[1]
- # if the computed keypoint is in the first window keep it
- # TODO check points in box with function!
- if ptX > (windowLoc - 2 * self.turnWindowWidth) and ptX < (windowLoc + 2 * self.turnWindowWidth):
- if ptY > self.featureParams["maxMatchingDifference"] and ptY < (self.imgWHeight - self.featureParams["minMatchingDifference"]):
- matchingKeypoints.append(kp)
- featureDescriptors.append(desc)
- # plot the first key points
- rgbImg[int(ptY)-3:int(ptY)+3, int(ptX) -
- 3:int(ptX)+3] = [0, 0, 255]
-
+ matchingKeypoints.append(kp)
+ featureDescriptors.append(desc)
+
  print(len(matchingKeypoints), " Key Points in the first window were detected")
- return rgbImg, matchingKeypoints, featureDescriptors
-
+ return matchingKeypoints, featureDescriptors
 
- def matchTrackingFeatures(self, mode, greenIDX, matchingKeypoints, featureDescriptors):
+ def matchTrackingFeatures(self, greenIDX, refDescriptors):
  """Function to compute the features in the second window
  """
  # Initiate SIFT detector
@@ -65,9 +102,9 @@ def matchTrackingFeatures(self, mode, greenIDX, matchingKeypoints, featureDescri
  descriptorsCurr = []
  # get the correct window depending on the current mode
  if mode == 3:
- windowLoc = self.windowLocations[1]
+ windowLoc = self.wLocs[1]
  else:
- windowLoc = self.windowLocations[-2]
+ windowLoc = self.wLocs[-2]
  # maintain the keypoints lying in the second window
  for kp, desc in itertools.izip(keyPointsRaw, descriptorsRaw):
  ptX = kp.pt[0]
@@ -90,7 +127,7 @@ def matchTrackingFeatures(self, mode, greenIDX, matchingKeypoints, featureDescri
  search_params = dict(checks=50)
  flann = cv.FlannBasedMatcher(index_params, search_params)
  matches = flann.knnMatch(np.asarray(
- featureDescriptors, np.float32), np.asarray(descriptorsCurr, np.float32), k=2)
+ refDescriptors, np.float32), np.asarray(descriptorsCurr, np.float32), k=2)
 
  # search for good matches (Lowes Ratio Test)
  for m, n in matches: