turingbirds · March 18, 2025 22:51 · Mar 18, 2025 · Mar 18, 2025 · Mar 18, 2025 · Mar 18, 2025
diff --git a/screenshot.png b/screenshot.png
diff --git a/cam_streamer_with_opencv.py b/cam_streamer_with_opencv.py
@@ -1,11 +1,12 @@
-# Get the orientation of a QR code and plot a little XYZ axis gizmo in red/green/blue on top of the image.
+# Get the distance and orientation of a QR code and plot a little XYZ axis gizmo in red/green/blue on top of the image.
 # This gets really nice low latency across a local wifi network, but the QR code detection can be a bit jumpy sometimes.
 # We try both the zbar and opencv libraries for QR code detection.
 #
 # This software is distributed under the "CC0 1.0 Universal (CC0 1.0)" license.
 # You should have received a copy of the CC0 Public Domain Dedication along with this software. If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
 
 import datetime
+import math
 import numpy as np
 import os
 import threading
@@ -18,7 +19,6 @@
 import cv2
 
 
-
 def get_qr_code_corner_points(array: np.ndarray):
     """
     Detect the corner points of QR codes in a uint8 NumPy array using zbar.
@@ -42,117 +42,148 @@ def get_qr_code_corner_points(array: np.ndarray):
     # Initialize a zbar image scanner
     scanner = zbar.ImageScanner()
     scanner.parse_config('enable')
- 
+
     # Create a zbar Image
     zbar_image = zbar.Image(width, height, 'Y800', raw_data)
- 
+
     # Scan for symbols (QR codes, etc.)
     scanner.scan(zbar_image)
- 
+
     # List to store the corner points of all QR codes found
     qr_corner_points = []
+    qr_code_data = None
     for symbol in zbar_image:
         # The location attribute contains the corner points of the detected code
         corner_points = symbol.location
         qr_corner_points.append(corner_points)
-
-    del zbar_image
-
-    return qr_corner_points
-
-
-
-def reconstruct_pose_from_frame(imgInput, points):
-        """
-        camera matrix:
-        [ fx 0 cx ]
-        [ 0 fy cy ]
-        [ 0 0 1 ]
-
-        • The intrinsic camera parameters (fx, fy) – focal lengths in terms of pixel dimensions along the x and y axes.
-        • The principal point (cx, cy) – usually near the center of the image.
-        """
+        qr_code_data = symbol.data
 
-        height, width = imgInput.shape[:2]
-        cx = width / 2.0
-        cy = height / 2.0
-        focal_length = imgInput.shape[1]
-
-        camera_matrix = np.array([[focal_length,   0,cx],
-                                    [  0, focal_length, cy],
-                                    [  0,   0,   1]], dtype=np.float32)
-        dist_coeffs = np.zeros((4, 1), dtype=np.float32)
-
-        # 3D model points for the QR code corners (centered at (0,0))
-
-        object_points = np.array([
-            [-0.5, -0.5, 0],
-            [ 0.5, -0.5, 0],
-            [ 0.5,  0.5, 0],
-            [-0.5,  0.5, 0]
-        ], dtype=np.float32)
-
-        # Solve for the pose (rvec, tvec)
-        # points[0] has shape (4,2): the four detected corners
-        retval, rvec, tvec = cv2.solvePnP(object_points, points[0], camera_matrix, dist_coeffs)
+    del zbar_image
 
-        # Define a small axis in 3D to project (XYZ)
-        axis_3d_points = np.float32([
-            [0, 0, 0],
-            [0.5, 0, 0],
-            [0, 0.5, 0],
-            [0, 0, -0.5]
-        ])
+    return qr_corner_points, qr_code_data
 
-        # Project 3D axis onto the 2D image
-        imgpts, _ = cv2.projectPoints(axis_3d_points, rvec, tvec, camera_matrix, dist_coeffs)
 
-        center = tuple(imgpts[0].ravel().astype(int))
-        x_axis = tuple(imgpts[1].ravel().astype(int))
-        y_axis = tuple(imgpts[2].ravel().astype(int))
-        z_axis = tuple(imgpts[3].ravel().astype(int))
+def reconstruct_pose_from_frame(imgInput, points):
+    """
+    camera matrix:
+    [ fx 0 cx ]
+    [ 0 fy cy ]
+    [ 0 0 1 ]
 
-        # Draw the axes: red (X), green (Y), blue (Z)
-        cv2.line(imgInput, center, x_axis, (0, 0, 255), 5)
-        cv2.line(imgInput, center, y_axis, (0, 255, 0), 5)
-        cv2.line(imgInput, center, z_axis, (255, 0, 0), 5)
+    • The intrinsic camera parameters (fx, fy) – focal lengths in terms of pixel dimensions along the x and y axes.
+    • The principal point (cx, cy) – usually near the center of the image.
+    """
 
-        return imgInput
+    height, width = imgInput.shape[:2]
+    cx = width / 2.0
+    cy = height / 2.0
+    focal_length = imgInput.shape[1]
+
+    camera_matrix = np.array([[focal_length,   0,cx],
+                    [  0, focal_length, cy],
+                    [  0,   0,   1]], dtype=np.float32)
+    dist_coeffs = np.zeros((4, 1), dtype=np.float32)
+
+    # 3D model points for the QR code corners (centered at (0,0))
+
+    object_points = np.array([
+        [-0.5, -0.5, 0],
+        [ 0.5, -0.5, 0],
+        [ 0.5,  0.5, 0],
+        [-0.5,  0.5, 0]
+    ], dtype=np.float32)
+
+    # Solve for the pose (rvec, tvec)
+    # points[0] has shape (4,2): the four detected corners
+    retval, rvec, tvec = cv2.solvePnP(object_points, points[0], camera_matrix, dist_coeffs)
+
+    # Define a small axis in 3D to project (XYZ)
+    axis_3d_points = np.float32([
+        [0, 0, 0],
+        [0.5, 0, 0],
+        [0, 0.5, 0],
+        [0, 0, -0.5]
+    ])
+
+    # Project 3D axis onto the 2D image
+    imgpts, _ = cv2.projectPoints(axis_3d_points, rvec, tvec, camera_matrix, dist_coeffs)
+
+    center = tuple(imgpts[0].ravel().astype(int))
+    x_axis = tuple(imgpts[1].ravel().astype(int))
+    y_axis = tuple(imgpts[2].ravel().astype(int))
+    z_axis = tuple(imgpts[3].ravel().astype(int))
+
+    # Draw the axes: red (X), green (Y), blue (Z)
+    cv2.line(imgInput, center, x_axis, (0, 0, 255), 5)
+    cv2.line(imgInput, center, y_axis, (0, 255, 0), 5)
+    cv2.line(imgInput, center, z_axis, (255, 0, 0), 5)
 
+    return imgInput
 
 
 def find_qr_code(imgInput):
-    print("Detecting QR code...")
     global opencv_qr_detector
 
-    #cv2.polylines(imgInput, [(0,0),(255,255)], isClosed=False, color=(64, 128, 255), thickness=5)
-
-    points = get_qr_code_corner_points(imgInput)
+    points, qr_code_data = get_qr_code_corner_points(imgInput)
     ret_qr = len(points) > 0
     if ret_qr:
         print("\tQR code detected (zbar)!")
-        print(points[0])
         points = np.array(points)
         cv2.polylines(imgInput, [points.astype(int)], isClosed=True, color=(64, 128, 255), thickness=5)
         imgInput = reconstruct_pose_from_frame(imgInput, points.astype(np.float32))
-        #import pdb;pdb.set_trace()
+        imgInput = qr_code_distance_estimation(imgInput, points.astype(np.float32))
 
     ret_qr, points = opencv_qr_detector.detect(imgInput)
     if ret_qr:
-        #points = np.array(points[0])
         print("\tQR code detected (opencv)!")
-        print(points[0])
         cv2.polylines(imgInput, [points[0].astype(int)], isClosed=True, color=(128, 255, 0), thickness=5)
-        #imgInput = reconstruct_pose_from_frame(imgInput, points)
-        #import pdb;pdb.set_trace()
+        qr_code_data, _ = opencv_qr_detector.decode(imgInput, points)
+        imgInput = reconstruct_pose_from_frame(imgInput, points.astype(np.float32))
+        imgInput = qr_code_distance_estimation(imgInput, points.astype(np.float32))
+
+
+    if qr_code_data:
+        height, width = imgInput.shape[:2]
+        y = height - 10
+        s = "Data: " + qr_code_data
+        cv2.putText(imgInput, s, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 6)
+        cv2.putText(imgInput, s, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
 
     return imgInput
 
 
+def qr_code_distance_estimation(imgInput, corners, qr_size_mm=50.):
+    distance_mm = None
+
+    if len(corners) == 1:
+        corners = corners[0]
 
+    assert len(corners) == 4
 
+    # Calculate the perimeter of the QR code in pixels
+    perimeter_px = 0
+    for i in range(4):
+        p1 = corners[i]
+        p2 = corners[(i + 1) % 4]
+        perimeter_px += np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)
 
+    # Average side length in pixels
+    side_length_px = perimeter_px / 4
 
+    # Calculate focal length (you would typically calibrate this)
+    # For simplicity, we'll use a formula based on the camera's field of view
+    # Assuming a 60-degree horizontal field of view for a typical webcam
+    image_width = imgInput.shape[1]
+    focal_length_px = image_width / (2 * math.tan(math.radians(60) / 2))
+
+    # Calculate distance using similar triangles
+    distance_mm = (qr_size_mm * focal_length_px) / side_length_px
+
+    distance_text = f"Distance: {distance_mm:.1f} mm ({distance_mm/1000:.2f} m)"
+    cv2.putText(imgInput, distance_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 6)
+    cv2.putText(imgInput, distance_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+
+    return imgInput
 
 
 
@@ -189,7 +220,6 @@ def gen_frames():
         frame = picam2.capture_array()
         print(frame.dtype)
         print(frame.shape)
-        print("Frame!")
         if not success:
             break
         else:
@@ -202,15 +232,18 @@ def gen_frames():
             yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n')
 
+
 @app.route('/')
 def index():
     # Render an HTML template that displays the video stream
     return render_template('index.html')
 
+
 @app.route('/video_feed')
 def video_feed():
     # Return the response generated along with the specific media type (mime type)
     return Response(gen_frames(), mimetype='multipart/x-mixed-replace; boundary=frame')
 
+
 if __name__ == '__main__':
     app.run(host='0.0.0.0', port=5001, debug=True)
diff --git a/cam_streamer_with_opencv.py b/cam_streamer_with_opencv.py
@@ -1,6 +1,10 @@
+# Get the orientation of a QR code and plot a little XYZ axis gizmo in red/green/blue on top of the image.
+# This gets really nice low latency across a local wifi network, but the QR code detection can be a bit jumpy sometimes.
+# We try both the zbar and opencv libraries for QR code detection.
+#
 # This software is distributed under the "CC0 1.0 Universal (CC0 1.0)" license.
 # You should have received a copy of the CC0 Public Domain Dedication along with this software. If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
-  
+
 import datetime
 import numpy as np
 import os

diff --git a/screenshot.png b/screenshot.png
diff --git a/cam_streamer_with_opencv.py b/cam_streamer_with_opencv.py
@@ -0,0 +1,212 @@
+# This software is distributed under the "CC0 1.0 Universal (CC0 1.0)" license.
+# You should have received a copy of the CC0 Public Domain Dedication along with this software. If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
+
+import datetime
+import numpy as np
+import os
+import threading
+import time
+
+import zbar
+from picamera2 import Picamera2
+import libcamera
+from flask import Flask, render_template, Response
+import cv2
+
+
+
+def get_qr_code_corner_points(array: np.ndarray):
+    """
+    Detect the corner points of QR codes in a uint8 NumPy array using zbar.
+    The array can be grayscale (H x W) or RGB (H x W x 3).
+    Returns a list of lists, where each inner list contains tuples of (x, y)
+    coordinates for the corners of one detected QR code.
+    """
+
+    # Ensure we have a 2D grayscale image
+    if len(array.shape) == 3 and array.shape[2] == 3:
+        # Convert from RGB to grayscale
+        gray = (0.299 * array[:, :, 0] +
+                0.587 * array[:, :, 1] +
+                0.114 * array[:, :, 2]).astype(np.uint8)
+    else:
+        gray = array
+
+    height, width = gray.shape
+    raw_data = gray.tobytes()
+
+    # Initialize a zbar image scanner
+    scanner = zbar.ImageScanner()
+    scanner.parse_config('enable')
+
+    # Create a zbar Image
+    zbar_image = zbar.Image(width, height, 'Y800', raw_data)
+
+    # Scan for symbols (QR codes, etc.)
+    scanner.scan(zbar_image)
+
+    # List to store the corner points of all QR codes found
+    qr_corner_points = []
+    for symbol in zbar_image:
+        # The location attribute contains the corner points of the detected code
+        corner_points = symbol.location
+        qr_corner_points.append(corner_points)
+
+    del zbar_image
+
+    return qr_corner_points
+
+
+
+def reconstruct_pose_from_frame(imgInput, points):
+        """
+        camera matrix:
+        [ fx 0 cx ]
+        [ 0 fy cy ]
+        [ 0 0 1 ]
+
+        • The intrinsic camera parameters (fx, fy) – focal lengths in terms of pixel dimensions along the x and y axes.
+        • The principal point (cx, cy) – usually near the center of the image.
+        """
+
+        height, width = imgInput.shape[:2]
+        cx = width / 2.0
+        cy = height / 2.0
+        focal_length = imgInput.shape[1]
+
+        camera_matrix = np.array([[focal_length,   0,cx],
+                                    [  0, focal_length, cy],
+                                    [  0,   0,   1]], dtype=np.float32)
+        dist_coeffs = np.zeros((4, 1), dtype=np.float32)
+
+        # 3D model points for the QR code corners (centered at (0,0))
+
+        object_points = np.array([
+            [-0.5, -0.5, 0],
+            [ 0.5, -0.5, 0],
+            [ 0.5,  0.5, 0],
+            [-0.5,  0.5, 0]
+        ], dtype=np.float32)
+
+        # Solve for the pose (rvec, tvec)
+        # points[0] has shape (4,2): the four detected corners
+        retval, rvec, tvec = cv2.solvePnP(object_points, points[0], camera_matrix, dist_coeffs)
+
+        # Define a small axis in 3D to project (XYZ)
+        axis_3d_points = np.float32([
+            [0, 0, 0],
+            [0.5, 0, 0],
+            [0, 0.5, 0],
+            [0, 0, -0.5]
+        ])
+
+        # Project 3D axis onto the 2D image
+        imgpts, _ = cv2.projectPoints(axis_3d_points, rvec, tvec, camera_matrix, dist_coeffs)
+
+        center = tuple(imgpts[0].ravel().astype(int))
+        x_axis = tuple(imgpts[1].ravel().astype(int))
+        y_axis = tuple(imgpts[2].ravel().astype(int))
+        z_axis = tuple(imgpts[3].ravel().astype(int))
+
+        # Draw the axes: red (X), green (Y), blue (Z)
+        cv2.line(imgInput, center, x_axis, (0, 0, 255), 5)
+        cv2.line(imgInput, center, y_axis, (0, 255, 0), 5)
+        cv2.line(imgInput, center, z_axis, (255, 0, 0), 5)
+
+        return imgInput
+
+
+
+def find_qr_code(imgInput):
+    print("Detecting QR code...")
+    global opencv_qr_detector
+
+    #cv2.polylines(imgInput, [(0,0),(255,255)], isClosed=False, color=(64, 128, 255), thickness=5)
+
+    points = get_qr_code_corner_points(imgInput)
+    ret_qr = len(points) > 0
+    if ret_qr:
+        print("\tQR code detected (zbar)!")
+        print(points[0])
+        points = np.array(points)
+        cv2.polylines(imgInput, [points.astype(int)], isClosed=True, color=(64, 128, 255), thickness=5)
+        imgInput = reconstruct_pose_from_frame(imgInput, points.astype(np.float32))
+        #import pdb;pdb.set_trace()
+
+    ret_qr, points = opencv_qr_detector.detect(imgInput)
+    if ret_qr:
+        #points = np.array(points[0])
+        print("\tQR code detected (opencv)!")
+        print(points[0])
+        cv2.polylines(imgInput, [points[0].astype(int)], isClosed=True, color=(128, 255, 0), thickness=5)
+        #imgInput = reconstruct_pose_from_frame(imgInput, points)
+        #import pdb;pdb.set_trace()
+
+    return imgInput
+
+
+
+
+
+
+
+
+
+app = Flask(__name__, template_folder='/home/pi/adeept_darkpaw/server')
+
+picam2 = None
+
+opencv_qr_detector = cv2.QRCodeDetector()
+
+@app.before_first_request
+def initialize():
+    global picam2
+    # Run the app on all interfaces so it is accessible from your network
+    picam2 = Picamera2()
+
+    print("Available sensor modes (resolutions):")
+    # Each mode is a dict with a "size" key among others.
+    for mode in picam2.sensor_modes:
+        print("\t" + str(mode.get("size")))
+
+
+    picam2.configure(picam2.create_still_configuration(main={"size": (640, 360)},
+                                                       transform = libcamera.Transform(vflip=True)))
+    time.sleep(1)
+    picam2.start()
+    time.sleep(1)
+
+
+def gen_frames():
+    global picam2
+    while True:
+        success = True
+        print("Capturing frame...")
+        frame = picam2.capture_array()
+        print(frame.dtype)
+        print(frame.shape)
+        print("Frame!")
+        if not success:
+            break
+        else:
+            #frame = cv2.flip(frame, 0)  # flip up-down
+            frame = find_qr_code(frame)
+            frame = frame[..., [2, 1, 0]]   # swap B and R channels
+            ret, buffer = cv2.imencode('.jpg', frame)
+            frame = buffer.tobytes()
+            # Concatenate frame with HTTP multipart headers
+            yield (b'--frame\r\n'
+                   b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n')
+
+@app.route('/')
+def index():
+    # Render an HTML template that displays the video stream
+    return render_template('index.html')
+
+@app.route('/video_feed')
+def video_feed():
+    # Return the response generated along with the specific media type (mime type)
+    return Response(gen_frames(), mimetype='multipart/x-mixed-replace; boundary=frame')
+
+if __name__ == '__main__':
+    app.run(host='0.0.0.0', port=5001, debug=True)
diff --git a/index.html b/index.html
@@ -0,0 +1,11 @@
+<!DOCTYPE html>
+<html>
+  <head>
+    <meta charset="utf-8">
+    <title>Picamera2 Stream</title>
+  </head>
+  <body>
+    <h1>Live Camera Feed</h1>
+    <img src="{{ url_for('video_feed') }}" alt="Camera Feed">
+  </body>
+</html>
No results found