Merge pull request #65 from FolkComputer/osnr/incremental-tag-detection

Shared memory peering + incremental tag detection + better incremental Collect + use non-sync Display

5 files changed, 125 insertions, 52 deletions

diff --git a/virtual-programs/apriltags.folk b/virtual-programs/apriltags.folk
index 3e95ee64..ac2c4f8b 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,52 +22,119 @@ 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
+    }
+}]
+
+# Incremental detector. Looks at regions where there were tags in the
+# old camera frame.
+On process {
+    source pi/AprilTags.tcl
+    AprilTags::init
+
+    # TODO: Clean this up. We retract these so that we don't bounce
+    # statements back to the main Folk process that it sends us.
+    Retract /anyone/ wishes $::thisProcess shares all wishes
+    Retract /anyone/ wishes $::thisProcess shares all claims
+    Wish $::thisProcess receives statements like \
+        [list /someone/ claims the camera frame is /grayFrame/ at /timestamp/]
+    Wish $::thisProcess receives statements like \
+        [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 \
+        [list /someone/ claims $::thisProcess has pid /pid/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ claims $::thisProcess detects tags /tags/ at /timestamp/ in time /aprilTime/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ wishes /something/ is labelled /text/]
+    Wish $::thisProcess shares statements like \
+        [list /someone/ wishes /something/ displays camera slice /slice/]
+
+    proc subimage {im x y subwidth subheight} {
+	dict with im {
+	    set x [expr {int($x)}]
+	    set y [expr {int($y)}]
+	    set subdata [expr {$data + ($y*$width + $x) * $components}]
+	    dict create \
+		width [int $subwidth] \
+		height [int $subheight] \
+		components $components \
+		bytesPerRow $bytesPerRow \
+		data [format 0x%x $subdata]
+	}
+    }
+
+    When the camera frame is /grayFrame/ at /timestamp/ & \
+         /process/ detects tags /prevTags/ at /something/ in time /something/ {
+
+        if {$process eq $::thisProcess} { return }
+
+        set tags [list]
+        set frameWidth [dict get $grayFrame width]
+        set frameHeight [dict get $grayFrame height]
+        set aprilTime 0
+        foreach prevTag $prevTags {
+            set size [dict get $prevTag size]
+            set corners [dict get $prevTag corners]
+            set x [min {*}[lmap corner $corners {lindex $corner 0}]]
+            set y [min {*}[lmap corner $corners {lindex $corner 1}]]
+            set x1 [max {*}[lmap corner $corners {lindex $corner 0}]]
+            set y1 [max {*}[lmap corner $corners {lindex $corner 1}]]
+            
+            set x [max [- $x $size] 0]
+            set y [max [- $y $size] 0]
+            set x1 [min [+ $x1 $size] $frameWidth]
+            set y1 [min [+ $y1 $size] $frameHeight]
+
+            set subimage [subimage $grayFrame $x $y [- $x1 $x] [- $y1 $y]]
+            set aprilTime [+ $aprilTime [baretime {
+                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)"
+
+        Claim $::thisProcess detects tags $tags at $timestamp in time $aprilTime
+        # Wish 6 is labelled "\n\nStep time $::stepTime\n[join [lmap s [dict values [Statements::all]] {statement short $s}] "\n"]"
     }
 }
 
-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]
+# This cache is used to remember the last seen position of each tag,
+# so that if the incremental detector blinks out, we still use the
+# tag's last-found position from it, instead of the older position
+# from the full detector, so as you move a tag its position doesn't
+# glitch backward.
+set ::tagsCache [dict create]
+# TODO: Garbage-collect this cache.
+
+When the collected matches for [list /someone/ detects tags /tags/ at /timestamp/ in time /aprilTime/] are /matches/ {
+    set tagsSeen [dict create]
+    foreach match $matches {
+        set timestamp [dict get $match timestamp]
+        foreach tag [dict get $match tags] {
+            set id [dict get $tag id]
+            dict set tag timestamp $timestamp
+
+            if {[dict exists $::tagsCache $id] &&
+                [dict get $::tagsCache $id timestamp] > $timestamp} {
+                set tag [dict get $::tagsCache $id]
+            } else {
+                dict set ::tagsCache $id $tag
+            }
+            dict set tagsSeen $id $tag
+        }
     }
-}
 
-# On process {
-#     source pi/AprilTags.tcl
-#     AprilTags::init
-
-#     # TODO: Clean this up. We retract these so that we don't bounce
-#     # statements back to the main Folk process that it sends us.
-#     Retract /anyone/ wishes $::thisProcess shares all wishes
-#     Retract /anyone/ wishes $::thisProcess shares all claims
-#     Wish $::thisProcess receives statements like \
-#         [list /someone/ claims the camera frame is /grayFrame/]
-#     Wish $::thisProcess shares statements like \
-#         [list /someone/ wishes /process/ receives statements like /pattern/]
-#     Wish $::thisProcess shares statements like \
-#         [list /someone/ claims tag /tag/ has center /center/ size /size/]
-#     Wish $::thisProcess shares statements like \
-#         [list /someone/ claims tag /tag/ has corners /corners/]
-#     Wish $::thisProcess shares statements like \
-#         [list /someone/ claims the AprilTag time is /aprilTime/]
-#     Wish $::thisProcess shares statements like \
-#         [list /someone/ claims $::thisProcess has pid /pid/]
-
-#     When the camera frame is /grayFrame/ {
-#         set aprilTime [time {
-#             set tags [AprilTags::detect $grayFrame]
-#         }]
-#         Commit {
-#             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]
-#             }
-#         }
-#     }
-# }
+    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}]
+}
diff --git a/virtual-programs/camera.folk b/virtual-programs/camera.folk
index 4f59a576..225a5880 100644
--- a/virtual-programs/camera.folk
+++ b/virtual-programs/camera.folk
@@ -20,7 +20,7 @@ On process {
 
     puts "Camera tid: [getTid] booting at [clock milliseconds]"
 
-    forever {
+    while true {
         set cameraTime [time {
             set grayFrame [Camera::grayFrame]
         }]
@@ -28,6 +28,7 @@ On process {
             Claim the camera time is $cameraTime
             Claim the camera frame is $grayFrame at [clock milliseconds]
         }
+        Step
     }
 }
 
diff --git a/virtual-programs/display.folk b/virtual-programs/display.folk
index d5623d10..b0b878ca 100644
--- a/virtual-programs/display.folk
+++ b/virtual-programs/display.folk
@@ -60,7 +60,7 @@ On process {
         [list /someone/ claims the display time is /displayTime/]
 
     set ::frames [list]
-    forever {
+    while true {
         set displayList [list]
         foreach match [Statements::findMatches {/someone/ wishes display runs /command/ on layer /layer/}] {
             lappend displayList [list [dict get $match layer] [dict get $match command]]
@@ -80,19 +80,22 @@ On process {
         }
 
         set displayCommands [lmap sublist [lsort -command lcomp $displayList] {lindex $sublist 1}]
+
         set renderTime [baretime [list foreach command $displayCommands { {*}$command }]]
         set commitTime [baretime commitThenClearStaging]
 
-        set inLastSecond 0
+        set framesInLastSecond 0
         set now [clock milliseconds]
         lappend frames $now
         foreach frame $frames {
             if {$frame > $now - 1000} {
-                incr inLastSecond
+                incr framesInLastSecond
             }
         }
-        set frames [lreplace $frames 0 end-$inLastSecond]
-        Commit { Claim the display time is "render $renderTime us + commit $commitTime us ($inLastSecond fps)" }
+        set frames [lreplace $frames 0 end-$framesInLastSecond]
+
+        Commit { Claim the display time is "render $renderTime us + commit $commitTime us ($::stepTime) ($framesInLastSecond fps)" }
+        Step
     }
 }
 # TODO: remove this compatibility hack
diff --git a/virtual-programs/tags-and-calibration.folk b/virtual-programs/tags-and-calibration.folk
index afb4aef4..b219b541 100644
--- a/virtual-programs/tags-and-calibration.folk
+++ b/virtual-programs/tags-and-calibration.folk
@@ -119,7 +119,8 @@ When (non-capturing) tag /tag/ has center /c/ size /size/ {
 }
 
 When (non-capturing) tag /tag/ is a tag {
-     puts "Added tag $tag"
+    puts "Added tag $tag"
+    On unmatch { puts "Removed tag $tag" }
 
     set tempPath "$::env(HOME)/folk-printed-programs/$tag.folk.temp"
 
diff --git a/virtual-programs/time.folk b/virtual-programs/time.folk
index 961b1863..1dff734d 100644
--- a/virtual-programs/time.folk
+++ b/virtual-programs/time.folk
@@ -1,3 +1,3 @@
 When $::thisProcess has step count /t/ {
-  Claim the clock time is [/ [clock milliseconds] 1000.0]
+    Claim the clock time is [/ [clock milliseconds] 1000.0]
 }