apriltags: WIP: First working incremental detector

1 files changed, 36 insertions, 24 deletions

diff --git a/virtual-programs/apriltags.folk b/virtual-programs/apriltags.folk
index f8b94da2..d9557644 100644
--- a/virtual-programs/apriltags.folk
+++ b/virtual-programs/apriltags.folk
@@ -1,6 +1,7 @@
 if {$::isLaptop} return
 
-On process {
+# Plain detector. Runs on entire camera frame.
+set mainDetectorProcess [On process {
     source pi/AprilTags.tcl
     AprilTags::init
 
@@ -21,20 +22,12 @@ On process {
         set aprilTime [time {
             set tags [AprilTags::detect $grayFrame]
         }]
-        Commit {
-            Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime
-        }
+        Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime
     }
-}
-
-When /someone/ detects tags /tags/ at /timestamp/ in time /aprilTime/ {
-    Claim the AprilTag time is $aprilTime
-    foreach tag $tags {
-        Claim tag [dict get $tag id] has center [dict get $tag center] size [dict get $tag size]
-        Claim tag [dict get $tag id] has corners [dict get $tag corners]
-    }
-}
+}]
 
+# Incremental detector. Looks at regions where there were tags in the
+# old camera frame.
 On process {
     source pi/AprilTags.tcl
     AprilTags::init
@@ -46,7 +39,7 @@ On process {
     Wish $::thisProcess receives statements like \
         [list /someone/ claims the camera frame is /grayFrame/ at /timestamp/]
     Wish $::thisProcess receives statements like \
-        [list /someone/ claims /process/ detects tags /tags/ at /timestamp/ in time /aprilTime/]
+        [list /someone/ claims $mainDetectorProcess detects tags /tags/ at /timestamp/ in time /aprilTime/]
     Wish $::thisProcess shares statements like \
         [list /someone/ wishes /process/ receives statements like /pattern/]
     Wish $::thisProcess shares statements like \
@@ -95,21 +88,40 @@ On process {
             set y1 [min [+ $y1 $size] $frameHeight]
 
             set subimage [subimage $grayFrame $x $y [- $x1 $x] [- $y1 $y]]
-            Wish 6 displays camera slice $subimage
             set aprilTime [+ $aprilTime [baretime {
-                lappend tags {*}[AprilTags::detect $subimage]
+                foreach tag [AprilTags::detect $subimage] {
+                    dict with tag {
+                        set center [vec2 add $center [list $x $y]]
+                        set corners [lmap corner $corners {vec2 add $corner [list $x $y]}]
+                    }
+                    lappend tags $tag
+                }
             }]]
         }
 
-        Wish 6 is labelled "[llength $prevTags] -> [llength $tags] ($aprilTime us)"
+        # Wish 6 is labelled "[llength $prevTags] -> [llength $tags] ($aprilTime us)"
 
-        # Commit {
-        #     Claim the AprilTag time is $aprilTime
+        Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime
+    }
+}
+
+When the collected matches for [list /someone/ detects tags /tags/ at /timestamp/ in time /aprilTime/] are /matches/ {
+    # Find all tag IDs, then find the latest for each ID.
+    # Sort by timestamp, earliest first.
+    proc mcomp {a b} {> [dict get $a timestamp] [dict get $b timestamp]}
+    set matches [lsort -command mcomp $matches]
+
+    set tagsSeen [dict create]
+    foreach match $matches {
+        set tags [dict get $match tags]
+        foreach tag $tags {
+            dict set tagsSeen [dict get $tag id] $tag
+        }
+    }
 
-        #     foreach tag $tags {
-        #         Claim tag [dict get $tag id] has center [dict get $tag center] size [dict get $tag size]
-        #         Claim tag [dict get $tag id] has corners [dict get $tag corners]
-        #     }
-        # }
+    dict for {id tag} $tagsSeen {
+        Claim tag $id has center [dict get $tag center] size [dict get $tag size]
+        Claim tag $id has corners [dict get $tag corners]
     }
+    Claim the AprilTag time is [lmap m $matches {dict get $m aprilTime}]
 }