updates to algorithm

Count.py

@@ -9,20 +9,23 @@ import time
 ############################################################################################################
 # Initialize Variables
 
-cy1=550
-offset=30 
-ids=set()
+cy1 = 550
+offset = 90 
+ids = set()
 pecanCount = 0 
 avgPecanWeight = 0.0270453125 # lbs
 refThroughput = 0 # lbs / 15 seconds
-avgSampleTime = 0.5 # seconds
+samplePeriod = 0.25 # seconds
+width = 1280
+height = 720
+totalPecanCount = 0
 
 # Load the YOLO11 model and set device
 model = YOLO("yolo11m-pecan.pt")
 device = 'cuda'
 
 # Open the video file (use video file or webcam, here using webcam)
-cap = cv2.VideoCapture(0)
+cap = cv2.VideoCapture('rtsp://192.168.1.10:8554/stream')
 
 ############################################################################################################
 # Unscented Kalman Filter
@@ -39,8 +42,8 @@ def hx(x):
 
 points = MerweScaledSigmaPoints(n=1, alpha=0.1, beta=2, kappa=0)# Define sigma points
 
-ukf = UKF(dim_x=1, dim_z=1, fx=fx, hx=hx, points=points) # Initialize UKF
-ukf.x = np.array([refThroughput / 15 / avgPecanWeight * avgSampleTime ]) # Initial state estimate
+ukf = UKF(dim_x=1, dim_z=1, fx=fx, hx=hx, points=points, dt=samplePeriod) # Initial State Estimate
+ukf.x = np.array([refThroughput / 15 / avgPecanWeight * samplePeriod]) # Initial state estimate
 ukf.Q = np.array([[0.02]]) # Process noise covariance (Q) - controls how much the state changes naturally
 ukf.R = np.array([[1]]) # Measurement noise covariance (R) - how noisy the measurements are
 ukf.P = np.eye(1) * 0.1 # Initial state covariance (P) - initial uncertainty
@@ -50,7 +53,8 @@ ukf.P = np.eye(1) * 0.1 # Initial state covariance (P) - initial uncertainty
 
 MQTT_BROKER = "192.168.1.110"
 MQTT_PORT = 1883
-MQTT_TOPIC = "/jc/feedrate/count"
+MQTT_TOPIC = "/jc/feedrate/"
+MQTT_COUNT_TOPIC = "/jc/feedrate/count/"
 
 def on_connect(client, userdata, flags, rc, properties=None):
     print("Connected with result code " + str(rc))
@@ -90,14 +94,27 @@ client.loop_start()  # Starts MQTT loop in the background
 # ema_filter = FastEMA(alpha=0.1)  # Adjust alpha (lower = smoother)
 ############################################################################################################
 
+sampleStart = time.time()
+frameTime = time.time()
+frameCount = 0
+
 while True:
     ret,frame = cap.read()
     if not ret:
         break
-    
-    sampleStart = time.time() # Sample Start Time
 
-    pecanCount = 0 # Reset count for new sample period
+    # Define the black box position (adjust as needed)
+    top_left = (0, 0) 
+    bottom_right = (1280, 220)
+    
+    # Draw a black rectangle (filled)
+    cv2.rectangle(frame, top_left, bottom_right, (0, 0, 0), thickness=-1)
+
+    frameCount += 1 
+
+    # sampleStart = time.time() # Sample Start Time
+
+    # pecanCount = 0 # Reset count for new sample period
 
     # Run YOLO11 tracking on the frame, persisting tracks between frames
     results = model.track(frame, persist=True, classes=0, device = device)
@@ -115,23 +132,45 @@ while True:
             cy = int(y1+y2)//2
             if cy<(cy1+offset) and cy>(cy1-offset) and track_id not in ids:
                 pecanCount += 1
+                totalPecanCount += 1
+                print(f'New Count: {pecanCount}')
                 ids.add(track_id)
 
     # filtered_count = ema_filter.update(pecanCount, refThroughput) # Applies exponential moving average filter
     
-    ukf.predict()
-    ukf.update(np.array([pecanCount]))
-    filtered_count = ukf.x[0]
+    sampleEnd = time.time()
+    print(f'Pecan Count: {pecanCount}')
+    print(f'Total Count: {totalPecanCount}')
+    if (sampleEnd - sampleStart) > samplePeriod:
+        ukf.predict()
+        print(f'Predicted State: {ukf.x[0]}')
+        ukf.update(np.array([pecanCount]))
+        print(f'Updated State: {ukf.x[0]}')
+        filtered_count = ukf.x[0]
+        print(filtered_count)
+        measuredThroughput = (filtered_count * avgPecanWeight) / (samplePeriod) * 15 # lbs / 15 seconds
+        print(f'Published Throughput: {measuredThroughput}')
+        client.publish(MQTT_COUNT_TOPIC, str(measuredThroughput))
+        pecanCount = 0
+        sampleStart = time.time()
+        print(f'Publish Time: {sampleStart-sampleEnd}')
 
-    sampleEnd = time.time() # End Sample Timer
+    # sampleEnd = time.time() # End Sample Timer
 
-    samplePeriod = sampleEnd - sampleStart
-    print(samplePeriod) 
+    # samplePeriod = sampleEnd - sampleStart
 
-    measuredThroughput = (filtered_count * avgPecanWeight) / (samplePeriod) * 15 # lbs / 15 seconds
-
-    client.publish(MQTT_TOPIC, str(measuredThroughput))
+    # measuredThroughput = (filtered_count * avgPecanWeight) / (samplePeriod) * 15 # lbs / 15 seconds
+    
+    #if (new_time := time.time()) > last_time + 0.25:
+        #client.publish(MQTT_COUNT_TOPIC, str(measuredThroughput))
+        #print(samplePeriod) 
+        #print(str(last_time) + " " + str(new_time))
+        #last_time = new_time
 
+    if (time.time()-frameTime) > 10:
+        fps = frameCount / (time.time() - frameTime)
+        print(fps)
+        break
 
 # Release the video capture object and close the display window
 cap.release()