builtin-programs/tags-to-quads.folk
When libapriltag has been built with config /configCcWithLibapriltag/ {
fn configCcWithLibapriltag
set cc [C]
# Represents camera or projector intrinsics, including the classic
# intrinsic matrix but also dimensions at which the matrix was created
# + distortion coefficients.
#
# Intrinsic matrix:
# fx s cx
# 0 fy cy
# 0 0 1
$cc struct Intrinsics {
double width;
double height;
double fx;
double fy;
double cx;
double cy;
double s;
double k1;
double k2;
double p1;
double p2;
}
# Intrinsics helpers:
$cc proc distort {double fx double fy double cx double cy
double k1 double k2 double p1 double p2
double xy[2] double out[2]} void {
double x = (xy[0] - cx)/fx;
double y = (xy[1] - cy)/fy;
double r2 = x*x + y*y;
double radial = k1 * r2 + k2 * r2*r2;
double dx = 2*p1*x*y + p2*(r2 + 2*x*x);
double dy = p1*(r2 + 2*y*y) + 2*p2*x*y;
out[0] = (x * (1.0 + radial) + dx)*fx + cx;
out[1] = (y * (1.0 + radial) + dy)*fy + cy;
}
$cc proc project {Intrinsics intr double width double height
double[3] v} Jim_Obj* {
double out[3] = {
v[0]*intr.fx + v[1]*intr.s + v[2]*intr.cx,
0 + v[1]*intr.fy + v[2]*intr.cy,
0 + 0 + v[2]
};
out[0] /= out[2]; out[1] /= out[2];
distort(intr.fx, intr.fy, intr.cx, intr.cy,
intr.k1, intr.k2, intr.p1, intr.p2,
out, out);
out[0] *= width / intr.width;
out[1] *= height / intr.height;
Jim_Obj* retObjs[2] = {
Jim_NewDoubleObj(interp, out[0]),
Jim_NewDoubleObj(interp, out[1])
};
return Jim_NewListObj(interp, retObjs, 2);
}
$cc proc rescaleAndUndistort {Intrinsics intr
double cameraWidth double cameraHeight
double* in
double* out} void {
double x = in[0] * intr.width / cameraWidth;
double y = in[1] * intr.height / cameraHeight;
double x0 = (x - intr.cx) / intr.fx;
double y0 = (y - intr.cy) / intr.fy;
x = x0; y = y0;
for (int i = 0; i < 5; i++) {
double r2 = x*x + y*y;
double radial = 1.0 + intr.k1 * r2 + intr.k2 * r2*r2;
double dx = 2*intr.p1*x*y + intr.p2*(r2 + 2*x*x);
double dy = intr.p1*(r2 + 2*y*y) + 2*intr.p2*x*y;
x = (x0 - dx) / radial;
y = (y0 - dy) / radial;
}
out[0] = x*intr.fx + intr.cx;
out[1] = y*intr.fy + intr.cy;
}
configCcWithLibapriltag $cc
$cc include <apriltag_pose.h>
$cc include <math.h>
$cc include <common/matd.h>
$cc include <common/homography.h>
$cc code {
#define MATD_VAR(name, nr, nc) \
matd_t* name = alloca(sizeof(matd_t)); \
name->data = alloca((nr)*(nc)*sizeof(double)); \
name->nrows = nr; name->ncols = nc;
}
$cc proc pseudoInverse {matd_t* a} matd_t* {
matd_svd_t usv = matd_svd(a);
MATD_VAR(Sinv, usv.S->ncols, usv.S->nrows);
memset(Sinv->data, 0, sizeof(double)*Sinv->nrows*Sinv->ncols);
for (unsigned int i = 0; i < usv.S->nrows; i++) {
if (i >= usv.S->ncols) { break; }
double el = MATD_EL(usv.S, i, i);
if (el > MATD_EPS) {
MATD_EL(Sinv, i, i) = 1.0 / el;
}
}
matd_t* pinv = matd_op("M*M*(M')", usv.V, Sinv, usv.U);
matd_destroy(usv.V); matd_destroy(usv.S); matd_destroy(usv.U);
return pinv;
}
$cc code {
void rotationVectorToRotationMatrix(double r[3], double out[3][3]) {
double theta = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
if (fabs(theta) < 0.0001) {
double ret[3][3] = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 1}
};
memcpy(out, ret, sizeof(ret));
return;
}
double u[3] = {r[0]/theta, r[1]/theta, r[2]/theta};
double ret[3][3] = {
{cos(theta) + u[0]*u[0]*(1 - cos(theta)),
u[0]*u[1]*(1 - cos(theta)) - u[2]*sin(theta),
u[0]*u[2]*(1 - cos(theta)) + u[1]*sin(theta)},
{u[0]*u[1]*(1 - cos(theta)) + u[2]*sin(theta),
cos(theta) + u[1]*u[1]*(1 - cos(theta)),
u[1]*u[2]*(1 - cos(theta)) - u[0]*sin(theta)},
{u[0]*u[2]*(1 - cos(theta)) - u[1]*sin(theta),
u[1]*u[2]*(1 - cos(theta)) + u[0]*sin(theta),
cos(theta) + u[2]*u[2]*(1 - cos(theta))}
};
memcpy(out, ret, sizeof(ret));
} }
# Based on: https://visp-doc.inria.fr/doxygen/camera_localization/tutorial-pose-gauss-newton-opencv.html
# Outputs to dt and dR (which should have been allocated by the caller).
$cc proc exponentialMap {matd_t* v matd_t* dt matd_t* dR} void {
double vx = MATD_EL(v, 0, 0);
double vy = MATD_EL(v, 1, 0);
double vz = MATD_EL(v, 2, 0);
double vtux = MATD_EL(v, 3, 0);
double vtuy = MATD_EL(v, 4, 0);
double vtuz = MATD_EL(v, 5, 0);
MATD_VAR(tu, 3, 1); // theta u
MATD_EL(tu, 0, 0) = vtux;
MATD_EL(tu, 1, 0) = vtuy;
MATD_EL(tu, 2, 0) = vtuz;
// Rodrigues from tu to dR
rotationVectorToRotationMatrix(tu->data, (double (*)[3]) dR->data);
double theta = sqrt(matd_vec_dot_product(tu, tu));
double sinc = (fabs(theta) < 1.0e-8) ? 1.0 : sin(theta) / theta;
double mcosc = (fabs(theta) < 2.5e-4) ? 0.5 : (1.-cos(theta)) / theta / theta;
double msinc = (fabs(theta) < 2.5e-4) ? (1./6.) : (1.-sin(theta)/theta) / theta / theta;
MATD_EL(dt, 0, 0) = vx*(sinc + vtux*vtux*msinc)
+ vy*(vtux*vtuy*msinc - vtuz*mcosc)
+ vz*(vtux*vtuz*msinc + vtuy*mcosc);
MATD_EL(dt, 1, 0) = vx*(vtux*vtuy*msinc + vtuz*mcosc)
+ vy*(sinc + vtuy*vtuy*msinc)
+ vz*(vtuy*vtuz*msinc - vtux*mcosc);
MATD_EL(dt, 2, 0) = vx*(vtux*vtuz*msinc - vtuy*mcosc)
+ vy*(vtuy*vtuz*msinc + vtux*mcosc)
+ vz*(sinc + vtuz*vtuz*msinc);
}
# wX is the model 3D coordinates (for all 4 tag corners). x is the 4
# current detected tag corners in the image, in normalized
# coordinates. cRw and ctw are pose estimates that get refined (and
# replaced; they may be destroyed by us). The caller has the
# responsibility to free the returned cRw and ctw.
$cc proc poseGaussNewton {double[][3] wX double[][2] x int npoints
matd_t** cRw matd_t** ctw
int maxIters} void {
MATD_VAR(J, npoints*2, 6);
double lambda = 0.25;
MATD_VAR(xq, npoints*2, 1);
MATD_VAR(xn, npoints*2, 1);
double residual = 0; double residual_prev;
for (int i = 0; i < npoints; i++) {
MATD_EL(xn, i*2, 0) = x[i][0];
MATD_EL(xn, i*2+1, 0) = x[i][1];
}
MATD_VAR(wXi, 3, 1);
int iters = 0;
do {
for (int i = 0; i < npoints; i++) {
matd_set_data(wXi, wX[i]);
matd_t* cX = matd_op("(M*M)+M", *cRw, wXi, *ctw);
double Xi = MATD_EL(cX, 0, 0);
double Yi = MATD_EL(cX, 1, 0);
double Zi = MATD_EL(cX, 2, 0);
matd_destroy(cX);
double xi = Xi/Zi;
double yi = Yi/Zi;
// Update x(q)
MATD_EL(xq, i*2, 0) = xi; // x(q) = cX/cZ
MATD_EL(xq, i*2+1, 0) = yi; // y(q) = cY/cZ
// Update J using equation (11)
MATD_EL(J, i*2, 0) = -1 / Zi;
MATD_EL(J, i*2, 1) = 0;
MATD_EL(J, i*2, 2) = xi / Zi;
MATD_EL(J, i*2, 3) = xi * yi;
MATD_EL(J, i*2, 4) = -(1 + xi * xi);
MATD_EL(J, i*2, 5) = yi;
MATD_EL(J, i*2+1, 0) = 0;
MATD_EL(J, i*2+1, 1) = -1 / Zi;
MATD_EL(J, i*2+1, 2) = yi / Zi;
MATD_EL(J, i*2+1, 3) = 1 + yi * yi;
MATD_EL(J, i*2+1, 4) = -xi * yi;
MATD_EL(J, i*2+1, 5) = -xi;
}
matd_t* e_q = matd_op("M-M", xq, xn); // Equation (7)
matd_t* Jp = pseudoInverse(J);
matd_scale_inplace(Jp, -lambda);
matd_t* dq = matd_multiply(Jp, e_q);
MATD_VAR(dctw, 3, 1); MATD_VAR(dcRw, 3, 3);
exponentialMap(dq, dctw, dcRw);
matd_t* old_cRw = *cRw; matd_t* old_ctw = *ctw;
*cRw = matd_op("(M')*M", dcRw, *cRw);
*ctw = matd_op("(M')*(M-M)", dcRw, *ctw, dctw);
matd_destroy(old_cRw); matd_destroy(old_ctw);
residual_prev = residual;
residual = matd_vec_dot_product(e_q, e_q);
matd_destroy(e_q);
matd_destroy(Jp);
matd_destroy(dq);
if (iters++ > maxIters) {
FOLK_ERROR("tags-to-quads: TOO MANY ITERS");
break;
}
} while (fabs(residual - residual_prev) > MATD_EPS);
/* exit(1); */
}
# TagPose represents a rotation and translation from tag-space (where
# (0, 0, 0) is the center of the tag) to camera-space (where (0, 0, 0)
# is the center of the camera lens).
$cc struct TagPose {
double R[3][3];
double t[3][1];
}
$cc proc servoingEstimateTagPose {Intrinsics cameraIntrinsics
double cameraWidth double cameraHeight
double tagSize TagPose prevTagPose
double[4][2] p0} TagPose {
double r = tagSize / 2.0;
double wX[][3] = {
{-r, r, 0}, { r, r, 0},
{ r, -r, 0}, {-r, -r, 0}
};
double x[4][2];
for (int i = 0; i < 4; i++) {
// Change p0 so we can apply intrinsics:
rescaleAndUndistort(cameraIntrinsics, cameraWidth, cameraHeight,
p0[i], x[i]);
// Apply intrinsics to go from pixel coordinates to normalized
// image-plane coordinates:
x[i][0] = (x[i][0] - cameraIntrinsics.cx) / cameraIntrinsics.fx;
x[i][1] = (x[i][1] - cameraIntrinsics.cy) / cameraIntrinsics.fy;
}
matd_t* cRw = matd_create_data(3, 3, (double*) prevTagPose.R);
matd_t* ctw = matd_create_data(3, 1, (double*) prevTagPose.t);
poseGaussNewton(wX, x, 4, &cRw, &ctw, 50);
TagPose ret;
memcpy(ret.R, cRw->data, sizeof(ret.R));
memcpy(ret.t, ctw->data, sizeof(ret.t));
matd_destroy(cRw);
matd_destroy(ctw);
return ret;
}
$cc proc baseEstimateTagPose {Intrinsics cameraIntrinsics
double cameraWidth double cameraHeight
double tagSize
double[4][2] p0} TagPose {
// We'll fill this in with a new .p and .H with
// undistorted/rescaled coordinates.
apriltag_detection_t det;
// We'll fill in the right side of each correspondence in the
// loop.
float correspondences[4][4] = {
{-1.0f, 1.0f, 0, 0},
{1.0f, 1.0f, 0, 0},
{1.0f, -1.0f, 0, 0},
{-1.0f, -1.0f, 0, 0}
};
for (int i = 0; i < 4; i++) {
rescaleAndUndistort(cameraIntrinsics, cameraWidth, cameraHeight,
p0[i], det.p[i]);
correspondences[i][2] = det.p[i][0];
correspondences[i][3] = det.p[i][1];
}
zarray_t correspondencesArr = {
.el_sz = sizeof(float[4]), .size = 4, .alloc = 4,
.data = (char*) correspondences
};
det.H = homography_compute(&correspondencesArr,
HOMOGRAPHY_COMPUTE_FLAG_SVD);
apriltag_detection_info_t info = {
.det = &det,
.tagsize = tagSize,
.fx = cameraIntrinsics.fx, .fy = cameraIntrinsics.fy,
.cx = cameraIntrinsics.cx, .cy = cameraIntrinsics.cy
};
apriltag_pose_t pose;
estimate_pose_for_tag_homography(&info, &pose);
matd_destroy(det.H);
TagPose ret;
memcpy(ret.R, pose.R->data, sizeof(ret.R));
memcpy(ret.t, pose.t->data, sizeof(ret.t));
matd_destroy(pose.R);
matd_destroy(pose.t);
return ret;
}
$cc proc poseDistSq { TagPose p1 TagPose p2 } double {
double delta_x = p2.t[0][0] - p1.t[0][0];
double delta_y = p2.t[1][0] - p1.t[1][0];
double delta_z = p2.t[2][0] - p1.t[2][0];
return delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
}
set poseLib [$cc compile]
Claim the pose library is $poseLib
# All spaces are in meters. To transform a point from one space to another, it will execute
# a partially evaluated function located in `changers` (all implementations currently just
# translate and rotate)
When the quad library is /quadLib/ &\
the collected results for [list the changer from space /sourceSpace/ to space /targetSpace/ is /changer/] are /results/ {
set changers [dict create]
foreach result $results { dict with result {
dict set changers $sourceSpace $targetSpace $changer
} }
fn quadChange {q targetSpace} {
package require linalg
namespace import \
::math::linearalgebra::add \
::math::linearalgebra::sub \
::math::linearalgebra::matmul
set sourceSpace [$quadLib space $q]
if {$sourceSpace eq $targetSpace} { return $q }
set changer [dict get $changers $sourceSpace $targetSpace]
$quadLib create $targetSpace [lmap v [$quadLib vertices $q] {
{*}$changer $v
}]
}
Claim the quad changer is [fn quadChange]
}
When camera /camera/ to display /display/ has extrinsics /extrinsics/ {
package require linalg
set R [dict get $extrinsics R]
set t [dict get $extrinsics t]
Claim the changer from space $camera to space "display $display" is \
[list apply [list {R t v} {
add [matmul $R $v] $t
}] $R $t]
Claim the changer from space "display $display" to space $camera is \
[list apply [list {Rt t v} {
add [matmul $Rt [sub $v $t]] $v
}] [math::linearalgebra::transpose $R] $t]
}
set quadLib [library create quadLib {} {
package require linalg
namespace import \
::math::linearalgebra::add \
::math::linearalgebra::matmul \
::math::linearalgebra::sub \
::math::linearalgebra::norm \
::math::linearalgebra::unitLengthVector \
::math::linearalgebra::scale
rename scale scaleVector
proc create {space vertices} { list $space $vertices }
proc space {q} { lindex $q 0 }
proc vertices {q} { lindex $q 1 }
# Scales about the centroid of the quad, along the width and height
# directions of the quad. You can scale by a percentage or set a specific length
# in meters/centimeters/millimeters (which just sets the quad size
# along that axis).
proc scale {q args} {
if {[llength $args] == 1} {
set value [lindex $args 0]
set args [list width $value height $value]
}
lassign [vertices $q] topLeft topRight bottomRight bottomLeft
set width [::math::max [norm [sub $topRight $topLeft]] \
[norm [sub $bottomRight $bottomLeft]]]
set height [::math::max [norm [sub $bottomRight $topRight]] \
[norm [sub $bottomLeft $topLeft]]]
set sxp 1; set syp 1
foreach {dim value} $args {
if {![regexp {([\0-9\.]+)(cm|mm|m|%)?} $value -> value unit]} {
error "quad scale: Invalid scale value $value"
}
if {$dim eq "width"} {
if {$unit eq "px"} {
set sxp [* $sxp [/ $value $width]]
} elseif {$unit eq "%"} {
set sxp [* $sxp $value 0.01]
} elseif {$unit eq ""} {
set sxp [* $sxp $value]
}
} elseif {$dim eq "height"} {
if {$unit eq "px"} {
set syp [* $syp [/ $value $height]]
} elseif {$unit eq "%"} {
set syp [* $syp [* $value 0.01]]
} elseif {$unit eq ""} {
set syp [* $syp $value]
}
} else {
error "quad scale: Invalid dimension $dim"
}
}
# Calculate the centroid
set c [::math::linearalgebra::scale 0.25 \
[add [add $topLeft $bottomRight] \
[add $topRight $bottomLeft]]]
# Calculate width and height vectors from the quad's actual orientation
# Width goes from left to right (topLeft -> topRight, bottomLeft -> bottomRight)
# Height goes from top to bottom (topLeft -> bottomLeft, topRight -> bottomRight)
set widthVectorTop [sub $topRight $topLeft]
set widthVectorBottom [sub $bottomRight $bottomLeft]
set heightVectorLeft [sub $bottomLeft $topLeft]
set heightVectorRight [sub $bottomRight $topRight]
# Scale each vertex by moving from centroid along scaled width/height vectors
# Each vertex = centroid + (width_factor * width_vector) + (height_factor * height_vector)
set newTopLeft [add $c [scaleVector [expr {$sxp * -0.5}] $widthVectorTop]]
set newTopLeft [add $newTopLeft [scaleVector [expr {$syp * -0.5}] $heightVectorLeft]]
set newTopRight [add $c [scaleVector [expr {$sxp * 0.5}] $widthVectorTop]]
set newTopRight [add $newTopRight [scaleVector [expr {$syp * -0.5}] $heightVectorRight]]
set newBottomRight [add $c [scaleVector [expr {$sxp * 0.5}] $widthVectorBottom]]
set newBottomRight [add $newBottomRight [scaleVector [expr {$syp * 0.5}] $heightVectorRight]]
set newBottomLeft [add $c [scaleVector [expr {$sxp * -0.5}] $widthVectorBottom]]
set newBottomLeft [add $newBottomLeft [scaleVector [expr {$syp * 0.5}] $heightVectorLeft]]
create [space $q] [list $newTopLeft $newTopRight $newBottomRight $newBottomLeft]
}
proc buffer {q args} {
lassign [vertices $q] topLeft topRight bottomRight bottomLeft
foreach {direction distance} $args {
if {![regexp {([\-0-9\.]+)(cm|mm|m)?} $distance -> distance unit]} {
error "quad buffer: Invalid distance $distance"
}
if {$distance == 0} { continue }
if {$unit eq "mm"} {
set distance [* $distance 0.001]
set unit "m"
}
if {$unit eq "cm"} {
set distance [* $distance 0.01]
set unit "m"
}
if {$unit ne "" && $unit ne "m"} {
error "quad buffer: Invalid unit $unit"
}
if {$direction eq "top"} {
set leftVector [sub $topLeft $bottomLeft]
set rightVector [sub $topRight $bottomRight]
set topLeft [add $topLeft [scaleVector $distance [unitLengthVector $leftVector]]]
set topRight [add $topRight [scaleVector $distance [unitLengthVector $rightVector]]]
} elseif {$direction eq "bottom"} {
set leftVector [sub $bottomLeft $topLeft]
set rightVector [sub $bottomRight $topRight]
set bottomLeft [add $bottomLeft [scaleVector $distance [unitLengthVector $leftVector]]]
set bottomRight [add $bottomRight [scaleVector $distance [unitLengthVector $rightVector]]]
} elseif {$direction eq "left"} {
set topVector [sub $topLeft $topRight]
set bottomVector [sub $bottomLeft $bottomRight]
set bottomLeft [add $bottomLeft [scaleVector $distance [unitLengthVector $bottomVector]]]
set topLeft [add $topLeft [scaleVector $distance [unitLengthVector $topVector]]]
} elseif {$direction eq "right"} {
set topVector [sub $topRight $topLeft]
set bottomVector [sub $bottomRight $bottomLeft]
set bottomRight [add $bottomRight [scaleVector $distance [unitLengthVector $bottomVector]]]
set topRight [add $topRight [scaleVector $distance [unitLengthVector $bottomVector]]]
} else {
error "quad buffer: Invalid direction $direction"
}
}
return [create [space $q] [list $topLeft $topRight $bottomRight $bottomLeft]]
}
# Moves the quad left/right/up/down along the width and height
# directions of the quad (not the global x and y).
proc move {q args} {
lassign [vertices $q] topLeft topRight bottomRight bottomLeft
set width [::math::max [norm [sub $topRight $topLeft]] \
[norm [sub $bottomRight $bottomLeft]]]
set height [::math::max [norm [sub $bottomRight $topRight]] \
[norm [sub $bottomLeft $topLeft]]]
foreach {direction distance} $args {
if {![regexp {^([\-0-9\.]+)(cm|mm|m|%)?$} $distance -> distance unit]} {
error "quad move: Invalid distance $distance"
}
if {$distance == 0} { continue }
if {$unit eq "%"} {
if {$direction eq "up" || $direction eq "down"} {
set distance [* $height $distance 0.01]
} else {
set distance [* $width $distance 0.01]
}
set unit "m"
}
if {$unit eq "mm"} {
set distance [* $distance 0.001]
set unit "m"
}
if {$unit eq "cm"} {
set distance [* $distance 0.01]
set unit "m"
}
if {$unit ne "" && $unit ne "m"} {
error "quad move: Invalid unit $unit"
}
# Calculate displacement based on quad's actual orientation
if {$direction eq "up"} {
# Move in the direction opposite to height (from bottom to top)
set heightVectorLeft [sub $topLeft $bottomLeft]
set heightVectorRight [sub $topRight $bottomRight]
set avgHeightVector [scaleVector 0.5 [add $heightVectorLeft $heightVectorRight]]
set disp [scaleVector $distance [unitLengthVector $avgHeightVector]]
} elseif {$direction eq "down"} {
# Move in the height direction (from top to bottom)
set heightVectorLeft [sub $bottomLeft $topLeft]
set heightVectorRight [sub $bottomRight $topRight]
set avgHeightVector [scaleVector 0.5 [add $heightVectorLeft $heightVectorRight]]
set disp [scaleVector $distance [unitLengthVector $avgHeightVector]]
} elseif {$direction eq "left"} {
# Move in the direction opposite to width (from right to left)
set widthVectorTop [sub $topLeft $topRight]
set widthVectorBottom [sub $bottomLeft $bottomRight]
set avgWidthVector [scaleVector 0.5 [add $widthVectorTop $widthVectorBottom]]
set disp [scaleVector $distance [unitLengthVector $avgWidthVector]]
} elseif {$direction eq "right"} {
# Move in the width direction (from left to right)
set widthVectorTop [sub $topRight $topLeft]
set widthVectorBottom [sub $bottomRight $bottomLeft]
set avgWidthVector [scaleVector 0.5 [add $widthVectorTop $widthVectorBottom]]
set disp [scaleVector $distance [unitLengthVector $avgWidthVector]]
} else {
error "quad move: Invalid direction $direction"
}
set topLeft [add $topLeft $disp]
set topRight [add $topRight $disp]
set bottomRight [add $bottomRight $disp]
set bottomLeft [add $bottomLeft $disp]
}
return [create [space $q] [list $topLeft $topRight $bottomRight $bottomLeft]]
}
# Move an existing geometry geom (object with width and height
# keys) to align onto the plane & the top edge & left edge of the
# existing quad q.
proc alignGeometry {q geom} {
lassign [vertices $q] topLeft topRight bottomRight bottomLeft
set topDisp [scaleVector $geom(width) [unitLengthVector [sub $topRight $topLeft]]]
set leftDisp [scaleVector $geom(height) [unitLengthVector [sub $bottomLeft $topLeft]]]
set topRight [add $topLeft $topDisp]
set bottomRight [add $topRight $leftDisp]
set bottomLeft [add $topLeft $leftDisp]
return [create [space $q] [list $topLeft $topRight $bottomRight $bottomLeft]]
}
}]
Claim the quad library is $quadLib
When tag /tag/ has detection /any/ on camera /any/ at timestamp /timestamp/ {
# Setting aside this tag space (48600 to 48713) for calibration.
if {!($tag >= 0 && $tag < 48600)} { return }
Wish -keep 100ms $tag has resolved geometry
}
When camera /camera/ has width /cameraWidth/ height /cameraHeight/ &\
camera /camera/ has intrinsics /cameraIntrinsics/ {
# for position stabilization
set unfreezeThreshold 0.01
set unfreezeThresholdSq [expr {$unfreezeThreshold * $unfreezeThreshold}]
set unfreezeLength 0.3 ;# in seconds
When tag /tag/ has detection /det/ on camera $camera at timestamp /timestamp/ &\
/tag/ has resolved geometry /geom/ {
package require linalg
namespace import \
::math::linearalgebra::add \
::math::linearalgebra::matmul
set tagSize [dict get $geom tagSize]
set prevResults [Query! tag $tag has pose /tagPose/ at timestamp /timestamp/]
if {[llength $prevResults] > 0} {
set latestResult [lindex [lsort -command {apply {{a b} {
expr { [dict get $b timestamp] < [dict get $a timestamp] }
}}} $prevResults] 0]
set prevTimestamp [dict get $latestResult timestamp]
# Consider previous tag pose if it's less than 200ms old:
if {$timestamp - $prevTimestamp < 0.2} {
set prevTagPose [dict get $latestResult tagPose]
}
}
if {![info exists prevTagPose]} {
set prevTagPose [$poseLib baseEstimateTagPose $cameraIntrinsics \
$cameraWidth $cameraHeight \
$tagSize [dict get $det p]]
}
set tagPose [$poseLib servoingEstimateTagPose $cameraIntrinsics \
$cameraWidth $cameraHeight \
$tagSize $prevTagPose \
[dict get $det p]]
## tag stabilization ##
# (can't use When here as it can't exfiltrate info). Is there a better way to do this?
set stabilizedMatches [Query! /someone/ wishes tag $tag is stabilized]
set isTagStabilized [expr {[llength $stabilizedMatches] > 0}]
if {$isTagStabilized} {
set stateResults [Query! tag $tag is stabilized with state /state/]
if {[llength $stateResults] > 0} {
set state [dict get [lindex $stateResults 0] state]
dict with state {
set distSq [$poseLib poseDistSq $frozenPose $tagPose]
if {$distSq > $unfreezeThresholdSq} {
set frozenPose $tagPose
# reset timestamp even if it's already unfrozen to keep it unfrozen
set unfreezeTime $timestamp
set frozen 0
} elseif {!$frozen && $timestamp - $unfreezeTime > $unfreezeLength} {
set frozenPose $tagPose
set frozen 1
}
if {$frozen} {
set tagPose $frozenPose
}
}
} else {
# first time, so we'll init some stuff
# (starts unfrozen)
set state [dict create frozen 0 frozenPose $tagPose \
unfreezeTime $timestamp]
}
Hold! -key [list tag $tag stabilization] \
Claim tag $tag is stabilized with state $state
}
# Note that (for now) this pose statement is only imperatively
# queried (by future invocations of this same code), not
# reacted to. No need to give it a keep time.
Hold! -key [list tag pose $tag] \
Claim tag $tag has pose $tagPose at timestamp $timestamp
set r [expr {$tagSize / 2}]
# Vertex order needs to be clockwise to align with
# Vulkan. Top-left to bottom-left.
set vertices \
[list [list [- $r] [- $r] 0] \
[list $r [- $r] 0] \
[list $r $r 0] \
[list [- $r] $r 0]]
set R [dict get $tagPose R]; set t [dict get $tagPose t]
set vertices [lmap v $vertices {
add [matmul $R $v] $t
}]
set quad [$quadLib create $camera $vertices]
Claim tag $tag has quad $quad
}
}
When /tag/ has resolved geometry /geom/ &\
tag /tag/ has quad /q/ {
set pageQuad [$quadLib buffer $q \
top [dict getdef $geom top 0] \
right [dict getdef $geom right 0] \
left [dict getdef $geom left 0] \
bottom [dict getdef $geom bottom 0]]
Claim $tag has quad $pageQuad
}
When the quad changer is /quadChange/ &\
display /disp/ has width /displayWidth/ height /displayHeight/ &\
display /disp/ has intrinsics /displayIntrinsics/ &\
/tag/ has canvas /writableTextureId/ with /...wiOptions/ {
fn quadChange
# Composite each quad's canvas onto the screen based on
# its quad.
set displayToClip [list \
[list [expr {2.0/$displayWidth}] 0 -1] \
[list 0 [expr {2.0/$displayHeight}] -1] \
[list 0 0 1]]
When -atomically $tag has quad /quad/ {
lassign [lmap v [$quadLib vertices [quadChange $quad "display $disp"]] {
$poseLib project $displayIntrinsics \
$displayWidth $displayHeight $v
}] a b c d
Wish the GPU draws pipeline "image" with arguments \
[list [list $displayWidth $displayHeight] \
$displayToClip \
[dict get $wiOptions texture] $a $b $c $d] \
layer [dict getdef $wiOptions layer 0]
}
}
}