Experiments with opencv headtracking

protocols/Makefile

@@ -0,0 +1,8 @@
+all: house-schemas.bin
+
+clean:
+	rm -f house-schemas.bin
+
+house-schemas.bin: schemas/*.prs
+	preserves-schemac schemas > $@.tmp
+	mv $@.tmp $@

protocols/house-schemas.bin

@@ -0,0 +1,6 @@
+´³bundle·µ³scene„´³schema·³version‘³definitions·³Touch´³rec´³lit³touch„´³tupleµ´³named³subject´³atom³String„„´³named³object´³atom³String„„„„„³Portal´³rec´³lit³portal„´³tupleµ´³named³name´³atom³String„„´³named³destination´³refµ„³PortalDestination„„´³named³position´³refµ³shapes„³LiteralVector3„„„„„³Gravity´³rec´³lit³gravity„´³tupleµ´³named³	direction´³refµ³shapes„³LiteralVector3„„„„„³AmbientSound´³rec´³lit³
ambient-sound„´³tupleµ´³named³name´³atom³String„„´³named³spec´³refµ³shapes„³	SoundSpec„„„„„³PortalDestination´³orµµ±local´³embedded³any„„µ±remote´³refµ³
+gatekeeper„³Route„„„„„³embeddedType€„„µ³shapes„´³schema·³version‘³definitions·³Box´³rec´³lit³box„´³tupleµ„„„³CSG´³rec´³lit³csg„´³tupleµ´³named³expr´³refµ„³CSGExpr„„„„„³Mesh´³orµµ±Sphere´³refµ„³Sphere„„µ±Box´³refµ„³Box„„µ±Ground´³refµ„³Ground„„µ±Plane´³refµ„³Plane„„µ±External´³refµ„³External„„µ±turtle´³refµ³turtle„³Shape„„„„³Move´³rec´³lit³move„´³tupleµ´³named³
v´³refµ„³Vector3„„´³named³shape´³refµ„³Shape„„„„„³Name´³rec´³lit³name„´³tupleµ´³named³base´³atom³String„„´³named³shape´³refµ„³Shape„„„„„³Color´³orµµ±opaque´³rec´³lit³color„´³tupleµ´³named³
r´³refµ„³DoubleValue„„´³named³
g´³refµ„³DoubleValue„„´³named³
b´³refµ„³DoubleValue„„´³named³shape´³refµ„³Shape„„„„„„µ±transparent´³rec´³lit³color„´³tupleµ´³named³
r´³refµ„³DoubleValue„„´³named³
g´³refµ„³DoubleValue„„´³named³
b´³refµ„³DoubleValue„„´³named³alpha´³refµ„³DoubleValue„„´³named³shape´³refµ„³Shape„„„„„„„„³Floor´³rec´³lit³floor„´³tupleµ´³named³shape´³refµ„³Shape„„„„„³Light´³rec´³lit³hemispheric-light„´³tupleµ´³named³
v´³refµ„³Vector3„„„„„³Plane´³rec´³lit³plane„´³tupleµ„„„³Scale´³rec´³lit³scale„´³tupleµ´³named³
v´³refµ„³Vector3„„´³named³shape´³refµ„³Shape„„„„„³Shape´³orµµ±Mesh´³refµ„³Mesh„„µ±Light´³refµ„³Light„„µ±Scale´³refµ„³Scale„„µ±Move´³refµ„³Move„„µ±Rotate´³refµ„³Rotate„„µ±many´³seqof´³refµ„³Shape„„„µ±Texture´³refµ„³Texture„„µ±Color´³refµ„³Color„„µ±Sound´³refµ„³Sound„„µ±Name´³refµ„³Name„„µ±Floor´³refµ„³Floor„„µ±Nonphysical´³refµ„³Nonphysical„„µ±	Touchable´³refµ„³	Touchable„„µ±CSG´³refµ„³CSG„„µ±Skybox´³refµ„³Skybox„„„„³Sound´³rec´³lit³sound„´³tupleµ´³named³spec´³refµ„³	SoundSpec„„´³named³shape´³refµ„³Shape„„„„„³Ground´³rec´³lit³ground„´³tupleµ„„„³Rotate´³orµµ±euler´³rec´³lit³rotate„´³tupleµ´³named³
v´³refµ„³Vector3„„´³named³shape´³refµ„³Shape„„„„„„µ±
+quaternion´³rec´³lit³rotate„´³tupleµ´³named³
q´³refµ„³
+Quaternion„„´³named³shape´³refµ„³Shape„„„„„„„„³Skybox´³rec´³lit³skybox„´³tupleµ´³named³path´³atom³String„„„„„³Sphere´³rec´³lit³sphere„´³tupleµ„„„³Sprite´³rec´³lit³sprite„´³tupleµ´³named³name´³atom³String„„´³named³formals´³seqof´³atom³Symbol„„„´³named³shape´³refµ„³Shape„„„„„³CSGExpr´³orµµ±mesh´³rec´³lit³mesh„´³tupleµ´³named³shape´³refµ„³Mesh„„„„„„µ±scale´³rec´³lit³scale„´³tupleµ´³named³
v´³refµ„³LiteralVector3„„´³named³shape´³refµ„³CSGExpr„„„„„„µ±move´³rec´³lit³move„´³tupleµ´³named³
v´³refµ„³LiteralVector3„„´³named³shape´³refµ„³CSGExpr„„„„„„µ±rotate´³rec´³lit³rotate„´³tupleµ´³named³
v´³refµ„³LiteralVector3„„´³named³shape´³refµ„³CSGExpr„„„„„„µ±subtract´³rec´³lit³subtract„´³tupleµ´³tuplePrefixµ´³named³base´³refµ„³CSGExpr„„„´³named³more´³seqof´³refµ„³CSGExpr„„„„„„„„µ±union´³rec´³lit³union„´³tupleµ´³tuplePrefixµ´³named³base´³refµ„³CSGExpr„„„´³named³more´³seqof´³refµ„³CSGExpr„„„„„„„„µ±	intersect´³rec´³lit³	intersect„´³tupleµ´³tuplePrefixµ´³named³base´³refµ„³CSGExpr„„„´³named³more´³seqof´³refµ„³CSGExpr„„„„„„„„µ±invert´³rec´³lit³invert„´³tupleµ´³named³shape´³refµ„³CSGExpr„„„„„„„„³Texture´³rec´³lit³texture„´³tupleµ´³named³spec´³refµ„³TextureSpec„„´³named³shape´³refµ„³Shape„„„„„³Vector2´³orµµ±	immediate´³refµ„³ImmediateVector2„„µ±	reference´³atom³Symbol„„„„³Vector3´³orµµ±	immediate´³refµ„³ImmediateVector3„„µ±	reference´³atom³Symbol„„„„³External´³rec´³lit³external„´³tupleµ´³named³path´³atom³String„„„„„³Variable´³rec´³lit³variable„´³tupleµ´³named³
+spriteName´³atom³String„„´³named³variable´³atom³Symbol„„´³named³value³any„„„„³	SoundSpec´³orµµ±stream´³rec´³lit³stream„´³tupleµ´³named³url´³atom³String„„„„„„µ±loop´³rec´³lit³loop„´³tupleµ´³named³url´³atom³String„„„„„„„„³	Touchable´³rec´³lit³	touchable„´³tupleµ´³named³shape´³refµ„³Shape„„„„„³
+Quaternion´³orµµ±	immediate´³refµ„³ImmediateQuaternion„„µ±	reference´³atom³Symbol„„„„³DoubleValue´³orµµ±	immediate´³atom³Double„„µ±	reference´³atom³Symbol„„„„³Nonphysical´³rec´³lit³nonphysical„´³tupleµ´³named³shape´³refµ„³Shape„„„„„³TextureSpec´³orµµ±simple´³tupleµ´³named³path´³atom³String„„„„„µ±uv´³tupleµ´³named³path´³atom³String„„´³named³scale´³refµ„³Vector2„„´³named³offset´³refµ„³Vector2„„„„„µ±uvAlpha´³tupleµ´³named³path´³atom³String„„´³named³scale´³refµ„³Vector2„„´³named³offset´³refµ„³Vector2„„´³named³alpha´³refµ„³DoubleValue„„„„„„„³LiteralVector3´³rec´³lit³
v„´³tupleµ´³named³
x´³atom³Double„„´³named³
y´³atom³Double„„´³named³
z´³atom³Double„„„„„³ImmediateVector2´³rec´³lit³
v„´³tupleµ´³named³
x´³refµ„³DoubleValue„„´³named³
y´³refµ„³DoubleValue„„„„„³ImmediateVector3´³rec´³lit³
v„´³tupleµ´³named³
x´³refµ„³DoubleValue„„´³named³
y´³refµ„³DoubleValue„„´³named³
z´³refµ„³DoubleValue„„„„„³ImmediateQuaternion´³rec´³lit³
q„´³tupleµ´³named³
a´³refµ„³DoubleValue„„´³named³
b´³refµ„³DoubleValue„„´³named³
c´³refµ„³DoubleValue„„´³named³
d´³refµ„³DoubleValue„„„„„„³embeddedType€„„µ³turtle„´³schema·³version‘³definitions·³Block´³seqof´³refµ„³Token„„³Shape´³rec´³lit³turtle„´³tupleµ´³named³program´³refµ„³Program„„„„„³Token´³orµµ±
i´³atom³
SignedInteger„„µ±
d´³atom³Double„„µ±
b´³atom³Boolean„„µ±
s´³atom³String„„µ±
v´³atom³Symbol„„µ±block´³refµ„³Block„„„„³Program´³refµ„³Block„„³embeddedType€„„µ³tracking„´³schema·³version‘³definitions·³Marker´³rec´³lit³marker„´³tupleµ´³named³camera³any„´³named³id´³atom³
SignedInteger„„´³named³rotation´³refµ³shapes„³LiteralVector3„„´³named³translation´³refµ³shapes„³LiteralVector3„„´³named³time´³atom³Double„„„„„„³embeddedType€„„„„

protocols/schemas/tracking.prs

@@ -0,0 +1,9 @@
+version 1 .
+
+Marker = <marker
+          @camera any
+          @id int
+          @rotation shapes.LiteralVector3
+          @translation shapes.LiteralVector3
+          @time double
+         > .

scene/lobby.pr

@@ -190,4 +190,15 @@
     ]>
    >>>>
 
+? <marker ?cam ?id _ _ _> [
+  let ?name = stringify [track $cam $id]
+  <sprite $name [R T]
+   <move <v 0.0 1.6 0.0>
+    <move T <rotate R <scale <v 0.02 0.02 0.02> <box>>>>>>
+  ? <marker $cam $id ?r ?t ?time> [
+    <variable $name R $r>
+    <variable $name T $t>
+  ]
+]
+
 []

serve.sh

@@ -5,4 +5,4 @@ then
     openssl req -new -newkey rsa:2048 -days 365 -nodes -x509 -keyout dummykey.key -out dummykey.crt
     cat dummykey.key dummykey.crt > dummykey.pem
 fi
-exec syndicate-server -c ./config
+exec syndicate-server -c ./config -c ./tracking/tracking.pr

src/index.ts

@@ -5,6 +5,7 @@ import * as wsRelay from "@syndicate-lang/ws-relay";
 import * as wakeDetector from './wake-detector.js';
 import * as Shapes from './gen/shapes.js';
 import * as SceneProtocol from './gen/scene.js';
+import * as Tracking from './gen/tracking.js';
 import { md5 } from './md5.js';
 import { setupLog, log } from './log.js';
 import G = Schemas.gatekeeper;
@@ -267,6 +268,31 @@ function bootApp(ds: Ref, runningEngine: RunningEngine) {
                     at remoteDs {
                         stop on asserted SceneHandle($sceneDs_e: Embedded) => {
                             react {
+                                at remoteDs {
+                                    const ms: { [key: number]: true } = {};
+                                    on message $m0(Tracking.Marker({ "camera": "cam1", "id": _ })) => {
+                                        const m = Tracking.asMarker(m0);
+                                        if (!(m.id in ms)) {
+                                            console.log('Spawning marker', m.id);
+                                            ms[m.id] = true;
+                                            spawn linked named ['marker', m.id] {
+                                                field current: Tracking.Marker<Ref> = m;
+                                                on stop { delete ms[m.id]; }
+                                                on message $m1(Tracking.Marker({
+                                                    "camera": m.camera,
+                                                    "id": m.id,
+                                                    "rotation": _,
+                                                })) => {
+                                                    current.value = Tracking.asMarker(m1);
+                                                }
+                                                at sceneDs_e.embeddedValue {
+                                                    assert fromJS(current.value);
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+
                                 enterScene(route,
                                            id,
                                            runningEngine,

tracking/.envrc

@@ -0,0 +1,9 @@
+if ! [ -d .venv ]
+then
+    python -m venv .venv
+    . .venv/bin/activate
+    pip install -U setuptools setuptools_scm wheel
+    pip install -r requirements.txt
+else
+    . .venv/bin/activate
+fi

tracking/.gitignore

@@ -0,0 +1,2 @@
+/.venv/
+/__pycache__/

tracking/README.md

@@ -0,0 +1,3 @@
+# OpenCV for head tracking
+
+    sudo apt install libopencv-dev opencv-data python3-opencv

tracking/calibrate.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+import cv2
+
+def main():
+    imgSize = (640, 480)
+
+    video = cv2.VideoCapture(0)
+    video.set(cv2.CAP_PROP_FRAME_WIDTH, imgSize[0])
+    video.set(cv2.CAP_PROP_FRAME_HEIGHT, imgSize[1])
+
+    d = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+    board = cv2.aruco.CharucoBoard.create(3, 3, 0.02, 0.012, d)
+
+    allCorners = []
+    allIds = []
+
+    while True:
+        (ok, frame) = video.read()
+        if not ok:
+            raise Error('video.read() yielded False')
+
+        (corners, ids, rejectedImagePoints) = cv2.aruco.detectMarkers(frame, d)
+
+        if ids is not None:
+            cv2.aruco.drawDetectedMarkers(frame, corners, ids)
+            (response, boardCorners, boardIds) = cv2.aruco.interpolateCornersCharuco(corners, ids, frame, board)
+            if response >= 4:
+                allCorners.append(boardCorners)
+                allIds.append(boardIds)
+                print(len(allCorners), len(allIds))
+
+        cv2.imshow('calibrate', frame)
+        if cv2.waitKey(1) != -1:
+            break
+
+    (calibration, cameraMatrix, distCoeffs, _rvecs, _tvecs) = \
+        cv2.aruco.calibrateCameraCharuco(allCorners, allIds, board, imgSize, None, None)
+    with open('calibration.json', 'wt') as f:
+        import json
+        json.dump({
+            'cameraMatrix': cameraMatrix.tolist(),
+            'distCoeffs': distCoeffs.tolist(),
+        }, f, indent=4)
+
+if __name__ == '__main__':
+    main()

tracking/calibration.json

@@ -0,0 +1,28 @@
+{
+    "cameraMatrix": [
+        [
+            293.7923171378614,
+            0.0,
+            311.6628157931463
+        ],
+        [
+            0.0,
+            254.53720087509802,
+            246.53051367347976
+        ],
+        [
+            0.0,
+            0.0,
+            1.0
+        ]
+    ],
+    "distCoeffs": [
+        [
+            0.11935276438835996,
+            0.040571025979352617,
+            -0.010671890343277932,
+            0.00864586497856339,
+            -0.01991935448364431
+        ]
+    ]
+}

tracking/make_markers.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+import cv2
+import numpy as np
+
+def stamp(target, source, x, y):
+    target[y:y+source.shape[0], x:x+source.shape[1]] = source
+
+def main():
+    page = np.zeros((2970, 2100), np.uint8)
+    page[:,:] = 255
+
+    cv2.line(page, (20, 20), (1020, 20), 0, 2)
+    for offset in range(20, 1120, 100):
+        cv2.line(page, (offset, 0), (offset, 40), 0, 2)
+    cv2.putText(page, '10cm', (1040, 40), 0, 1, 0, 2)
+
+    cv2.line(page, (20, 20), (20, 1020), 0, 2)
+    for offset in range(20, 1120, 100):
+        cv2.line(page, (0, offset), (40, offset), 0, 2)
+
+    d = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+
+    stamp(page, cv2.aruco.drawCharucoDiamond(d, (0, 1, 2, 3), 200, 120), 220, 220)
+    cv2.putText(page, 'front', (220, 170), 0, 2, 0, 2)
+
+    stamp(page, cv2.aruco.drawCharucoDiamond(d, (4, 5, 6, 7), 160, 96), 220, 1220)
+    cv2.putText(page, 'back', (220, 1170), 0, 2, 0, 2)
+
+    stamp(page, cv2.aruco.drawMarker(d, 8, 200), 1220, 220)
+    cv2.putText(page, 'top', (1220, 170), 0, 2, 0, 2)
+
+    stamp(page, cv2.aruco.drawMarker(d, 9, 200), 1220, 620)
+    cv2.putText(page, 'left', (1220, 570), 0, 2, 0, 2)
+
+    stamp(page, cv2.aruco.drawMarker(d, 10, 200), 1220, 1020)
+    cv2.putText(page, 'right', (1220, 970), 0, 2, 0, 2)
+
+    cv2.imwrite('markers.png', page)
+
+if __name__ == '__main__':
+    main()

tracking/requirements.txt

@@ -0,0 +1,4 @@
+websockets
+preserves
+syndicate-py
+opencv-contrib-python

tracking/track.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+import sys
+import cv2
+import numpy as np
+import time
+import argparse
+
+from syndicate import relay, turn, Symbol
+from syndicate.during import During
+
+from preserves.schema import load_schema_file
+schemas = load_schema_file('../protocols/house-schemas.bin')
+LiteralVector3 = schemas.shapes.LiteralVector3
+tracking = schemas.tracking
+
+parser = argparse.ArgumentParser(description='Head tracker')
+parser.add_argument('camera_name', help='name for this camera', default=None)
+cli_args = parser.parse_args()
+
+def open_video():
+    while True:
+        try:
+            video = cv2.VideoCapture(0)
+            video.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+            video.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+            return video
+        except:
+            import traceback
+            traceback.print_exc()
+            time.sleep(2)
+
+import json
+with open('calibration.json', 'r') as f:
+    calibration = json.load(f)
+cameraMatrix = np.asarray(calibration['cameraMatrix'])
+distCoeffs = np.asarray(calibration['distCoeffs'])
+
+d = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+
+@relay.service(name = 'headtracker', debug = True)
+@During().add_handler
+def main(args):
+    turn.log.info(f'headtracker {cli_args} {args}')
+    turn.on_stop(lambda: turn.log.info('stopping'))
+
+    main_ds = args.get(Symbol('mainDataspace')).embeddedValue
+
+    @turn.linked_task(run_in_executor=True)
+    def capture_task(facet):
+        video = open_video()
+        while facet.alive:
+            (ok, frame) = video.read()
+            now = time.time()
+            if not ok:
+                facet.log.info(f'video.read() yielded false')
+                time.sleep(0.2)
+                video = open_video()
+                continue
+
+            (corners, ids, rejectedImagePoints) = \
+                cv2.aruco.detectMarkers(cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY), d)
+            cv2.aruco.drawDetectedMarkers(frame, corners, ids)
+
+            if ids is not None:
+                (rvecs, tvecs, _objPoints) = cv2.aruco.estimatePoseSingleMarkers(corners, 0.02, cameraMatrix, distCoeffs)
+                for i in range(len(rvecs)):
+                    rv = rvecs[i]
+                    tv = tvecs[i]
+                    mi = ids[i]
+                    if mi[0] <= 10: ## we only use markers [0-10]
+                        marker = tracking.Marker(cli_args.camera_name,
+                                                 mi[0],
+                                                 LiteralVector3(*rv[0].tolist()),
+                                                 LiteralVector3(*tv[0].tolist()),
+                                                 now)
+                        # facet.log.info(marker)
+                        turn.external(facet, lambda: turn.send(main_ds, marker))
+                        cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs, rv, tv, 0.02)
+
+            # if ids is not None:
+            #     (diamondCorners, diamondIds) = cv2.aruco.detectCharucoDiamond(frame, corners, ids, 200/120)
+            #     cv2.aruco.drawDetectedDiamonds(frame, diamondCorners, diamondIds)
+
+            cv2.imshow('track', frame)
+            cv2.waitKey(1) # for liveness??

tracking/tracking.pr

@@ -0,0 +1,17 @@
+<require-service <daemon tracking>>
+<daemon tracking {
+  argv: ". ./.envrc && exec ./track.py cam1"
+  dir: "./tracking"
+  protocol: application/syndicate
+}>
+? <MainDataspace ?ds> [
+  ? <service-object <daemon tracking> ?t> [
+    $t {
+      mainDataspace: $ds
+    }
+  ]
+  ; $ds ?? <marker "cam1" ?id ?r ?t ?ti> [
+  ;   $log ! <log $ti {line: [$id $r $t]}>
+  ; ]
+  []
+]