apriltag_tracking.py

# هذا العمل مرخص بموجب ترخيص MIT.
# حقوق النشر (c) 2013-2023 OpenMV LLC. جميع الحقوق محفوظة.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# يوضح هذا المثال كيفية استخدام التحكم في نافذة القراءة لقراءة جزء صغير من
# مصفوفة بكسلات حساس الكاميرا بسرعة عالية جدًا وتحريك نافذة القراءة هذه.

# تم تصميم واختبار هذا المثال على كاميرا OpenMV Cam H7 Plus باستخدام حساس OV5640.

import csi
import time

# يفرض سكربت هذا المثال قيمة ثابتة للتعريض الضوئي طوال الوقت. مع ذلك، قد
# ترغب في ضبط التعريض الضوئي ديناميكيًا عندما تتقلص نافذة القراءة إلى حجم صغير.
EXPOSURE_MICROSECONDS = 20000

SEARCHING_RESOLUTION = csi.QVGA
TRACKING_RESOLUTION = csi.QQVGA  # أو csi.QQQVGA

TRACKING_LOW_RATIO_THRESHOLD = (
    0.2  # الانتقال إلى نافذة قراءة أصغر عندما يكون حجم العلامة أصغر مقارنة بالدقة.
)
TRACKING_HIGH_RATIO_THRESHOLD = (
    0.8  # الانتقال إلى نافذة قراءة أكبر عندما يكون حجم العلامة أكبر مقارنة بالدقة.
)

csi0 = csi.CSI()
csi0.reset()  # إعادة ضبط المستشعر وتهيئته.
csi0.pixformat(csi.GRAYSCALE)  # تعيين صيغة البكسل إلى GRAYSCALE
csi0.framesize(SEARCHING_RESOLUTION)
csi0.snapshot(time=1000)  # انتظر حتى تدخل الإعدادات حيز التنفيذ.
clock = time.clock()  # أنشئ كائن ساعة (clock) لتتبع معدل الإطارات (FPS).

csi0.auto_gain(False)  # أوقفها لأنها ستتذبذب.
csi0.auto_exposure(False, exposure_us=EXPOSURE_MICROSECONDS)
csi0.snapshot(time=1000)

# sensor_w وsensor_h هما عرض/ارتفاع البكسلات الخام لحساس الصورة (x/y تساوي 0 في البداية).
x, y, sensor_w, sensor_h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)

while True:
    clock.tick()
    img = csi0.snapshot()

    # يتتبع TAG36H11 افتراضيًا.
    tags = img.find_apriltags()

    if len(tags):
        best_tag = max(tags, key=lambda x: x.decision_margin)
        img.draw_rectangle(best_tag.rect)

        # يجب أن يكون هذا أقل من مخرج الحساس الافتراضي حتى نتمكن من تحريكه.
        readout_window_w = ((sensor_w // csi0.width()) * csi0.width()) / 2
        readout_window_h = ((sensor_h // csi0.height()) * csi0.height()) / 2

        def get_mapped_centroid(t):
            # بشكل افتراضي، يتم ضبط نافذة القراءة على كامل مصفوفة بكسلات الحساس مع x/y==0.
            # الدقة التي تراها تُنتج بأخذ البكسلات من نافذة القراءة على
            # الكاميرا. الموقع x/y نسبي إلى مركز الحساس.
            x, y, w, h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)

            # سيحاول مشغّل الكاميرا التحجيم ليلائم أي دقة تمررها إلى أقصى
            # عرض/ارتفاع يلائم الحساس مع الحفاظ على نسبة الأبعاد.
            ratio = min(w / float(csi0.width()), h / float(csi0.height()))

            # إسناد cx() إلى مركز نافذة العرض ثم التحجيم إلى نافذة القراءة.
            mapped_cx = (t.cx - (csi0.width() / 2.0)) * ratio
            # بما أننا نحافظ على نسبة الأبعاد، فقد يكون هناك إزاحة في x.
            mapped_cx += (w - (csi0.width() * ratio)) / 2.0
            # إضافة إزاحتنا من مركز الحساس
            mapped_cx += x + (sensor_w / 2.0)

            # إسناد cy() إلى مركز نافذة العرض ثم التحجيم إلى نافذة القراءة.
            mapped_cy = (t.cy - (csi0.height() / 2.0)) * ratio
            # بما أننا نحافظ على نسبة الأبعاد، فقد يكون هناك إزاحة في y.
            mapped_cy += (h - (csi0.height() * ratio)) / 2.0
            # إضافة إزاحتنا من مركز الحساس
            mapped_cy += y + (sensor_h / 2.0)

            return (mapped_cx, mapped_cy)  # الموقع X/Y على مصفوفة الحساس.

        def center_on_tag(t, res):
            global readout_window_w
            global readout_window_h
            mapped_cx, mapped_cy = get_mapped_centroid(t)

            # التبديل إلى الدقة (إذا لم تتغير الدقة فلن يفعل هذا شيئًا).
            csi0.framesize(res)

            # بناء نافذة القراءة. x/y هما إزاحتان من المركز.
            x = int(mapped_cx - (sensor_w / 2.0))
            y = int(mapped_cy - (sensor_h / 2.0))
            w = int(readout_window_w)
            h = int(readout_window_h)

            # التركيز على مركز الثقل (centroid).
            csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (x, y, w, h))

            # التحقق مما إذا كنا نصل إلى الحافة.
            new_x, new_y, w, h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)

            # يمكنك استخدام قيم الخطأ هذه لتشغيل محركات servo لتحريك الكاميرا إذا أردت.
            x_error = x - new_x
            y_error = y - new_y

            if x_error < 0:
                print("-X Limit Reached ", end="")
            if x_error > 0:
                print("+X Limit Reached ", end="")
            if y_error < 0:
                print("-Y Limit Reached ", end="")
            if y_error > 0:
                print("+Y Limit Reached ", end="")

        center_on_tag(best_tag, TRACKING_RESOLUTION)

        loss_count = 0

        # ستقوم هذه الحلقة بتتبع العلامة بسرعة قراءة أعلى بكثير ودقة أقل.
        while True:
            clock.tick()
            img = csi0.snapshot()

            # يتتبع TAG36H11 افتراضيًا.
            tags = img.find_apriltags()

            # إذا فقدنا العلامة فيجب علينا إيجاد علامة جديدة.
            if not len(tags):
                # التعامل مع بضعة إطارات سيئة بسبب وميض العلامة.
                if loss_count < 2:
                    loss_count += 1
                    continue
                # إعادة ضبط الدقة.
                csi0.framesize(SEARCHING_RESOLUTION)
                csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (sensor_w, sensor_h))
                break

            loss_count = 0

            # تضييق قائمة الكتل وتظليل الكتلة.
            best_tag = max(tags, key=lambda x: x.decision_margin)
            img.draw_rectangle(best_tag.rect)

            print(clock.fps(), "TAG cx:%d, cy:%d" % get_mapped_centroid(best_tag))

            w_ratio = best_tag.w / csi0.width()
            h_ratio = best_tag.h / csi0.height()

            # تصغير نافذة التتبع حتى تلائم العلامة.
            while (w_ratio < TRACKING_LOW_RATIO_THRESHOLD) or (
                h_ratio < TRACKING_LOW_RATIO_THRESHOLD
            ):
                readout_window_w /= 2
                readout_window_h /= 2
                w_ratio *= 2
                h_ratio *= 2

            # تكبير نافذة التتبع حتى تلائم العلامة.
            while (TRACKING_HIGH_RATIO_THRESHOLD < w_ratio) or (
                TRACKING_HIGH_RATIO_THRESHOLD < h_ratio
            ):
                readout_window_w *= 2
                readout_window_h *= 2
                w_ratio /= 2
                h_ratio /= 2

            center_on_tag(best_tag, TRACKING_RESOLUTION)

    print(clock.fps())