Skip to content

Commit

Permalink
calibrate: WIP: Remove vectcl dependency, use apriltag matd
Browse files Browse the repository at this point in the history
  • Loading branch information
@osnr
osnr committed Jun 19, 2024
1 parent 253ef15 commit b1abe69
Show file tree
Hide file tree
Showing 2 changed files with 116 additions and 71 deletions.
29 changes: 2 additions & 27 deletions Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@ debug:
gdb --args tclsh8.6 main.tcl
remote-debug: sync
ssh -tt folk@$(FOLK_SHARE_NODE) -- 'sudo systemctl stop folk && make -C /home/folk/folk debug'
remote-valgrind: sync
ssh -tt folk@$(FOLK_SHARE_NODE) -- 'cd folk; sudo systemctl stop folk && valgrind --leak-check=yes tclsh8.6 main.tcl'

FOLK_SHARE_NODE := $(shell tclsh8.6 hosts.tcl shareNode)

Expand Down Expand Up @@ -70,31 +72,4 @@ enable-pubkey:
ssh-copy-id folk-live

install-deps:
sudo apt remove libtcl8.6 || true
sudo apt install console-data

cd ~ && \
curl -O https://netactuate.dl.sourceforge.net/project/tcl/Tcl/8.6.14/tcl8.6.14-src.tar.gz?viasf=1 && \
curl -O https://core.tcl-lang.org/tcllib/uv/tcllib-1.21.tar.gz && \
curl -o VecTcl-0.2.tar.gz https://codeload.github.com/auriocus/VecTcl/tar.gz/refs/tags/v0.2 && \
tar xfz tcl8.6.14-src.tar.gz && \
tar xfz tcllib-1.21.tar.gz && \
tar xfz VecTcl-0.2.tar.gz && \
rm tcl8.6.14-src.tar.gz tcllib-1.21.tar.gz VecTcl-0.2.tar.gz

cd ~/tcl8.6.14 && \
patch -p1 < ../VecTcl-0.2/tcl86_vectcl.patch && \
cd unix && \
./configure --enable-symbols && \
make -j2 && \
sudo make install

cd ~/tcl8.6.14 && \
ln -s ~/tcllib-1.21/modules/textutil library/textutil && \
ln -s ~/tcllib-1.21/modules/fileutil library/fileutil && \
ln -s ~/tcllib-1.21/modules/cmdline library/cmdline

cd ~/VecTcl-0.2 && \
./configure --enable-listpatch && \
make -j2 && \
sudo make install
158 changes: 114 additions & 44 deletions virtual-programs/calibrate/calibrate.folk
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,120 @@ foreach p {add norm sub scale matmul
namespace import ::math::linearalgebra::$p
}

package require math::geometry
namespace import ::math::geometry::pointInsidePolygon
# We try to use the AprilTag matrix library (instead of tcllib linalg)
# to do matrix/homography operations in the live calibration inner
# loop, to help with performance:
set cc [c create]
$cc cflags -I$::env(HOME)/apriltag $::env(HOME)/apriltag/libapriltag.a
$cc include <common/matd.h>
$cc include <common/homography.h>

$cc code {
extern Tcl_ObjType matd_ObjType;
void matd_freeIntRepProc(Tcl_Obj *objPtr) {
matd_destroy((matd_t*) objPtr->internalRep.otherValuePtr);
}
void matd_dupIntRepProc(Tcl_Obj *srcPtr, Tcl_Obj *dupPtr) {
dupPtr->internalRep.otherValuePtr = (void*) matd_copy((matd_t*) srcPtr->internalRep.otherValuePtr);
}
void matd_updateStringProc(Tcl_Obj *objPtr) {
matd_t* mat = (matd_t*) objPtr->internalRep.otherValuePtr;

Tcl_DString ds; Tcl_DStringInit(&ds);
for (unsigned int row = 0; row < mat->nrows; row++) {
Tcl_DStringStartSublist(&ds);
for (unsigned int col = 0; col < mat->ncols; col++) {
char el[16];
FOLK_ENSURE(snprintf(el, sizeof(el), "%f", MATD_EL(mat, row, col)) < 16);
Tcl_DStringAppendElement(&ds, el);
}
Tcl_DStringEndSublist(&ds);
}

objPtr->length = Tcl_DStringLength(&ds);
objPtr->bytes = ckalloc(objPtr->length + 1);
snprintf(objPtr->bytes, objPtr->length + 1, "%s", Tcl_DStringValue(&ds));
Tcl_DStringFree(&ds);
}
int matd_setFromAnyProc(Tcl_Interp *interp, Tcl_Obj *objPtr) {
// shimmer into a list, then iterate
int nrows; Tcl_Obj** rowObjs;
__ENSURE_OK(Tcl_ListObjGetElements(interp, objPtr, &nrows, &rowObjs));
__ENSURE(nrows > 0);
int ncols;
__ENSURE_OK(Tcl_ListObjLength(interp, rowObjs[0], &ncols));
__ENSURE(ncols > 0);

matd_t* mat = matd_create(nrows, ncols);
for (int row = 0; row < nrows; row++) {
int rowCols; Tcl_Obj** elObjs;
__ENSURE_OK(Tcl_ListObjGetElements(interp, rowObjs[row], &rowCols, &elObjs));
__ENSURE(rowCols == ncols);
for (int col = 0; col < ncols; col++) {
__ENSURE_OK(Tcl_GetDoubleFromObj(interp, elObjs[col], &MATD_EL(mat, row, col)));
}
}
objPtr->typePtr = &matd_ObjType;
objPtr->internalRep.otherValuePtr = mat;
return TCL_OK;
}
Tcl_ObjType matd_ObjType = (Tcl_ObjType) {
.name = "matd_t*",
.freeIntRepProc = matd_freeIntRepProc,
.dupIntRepProc = matd_dupIntRepProc,
.updateStringProc = matd_updateStringProc,
.setFromAnyProc = matd_setFromAnyProc,
};
}
$cc argtype matd_t* {
__ENSURE_OK(Tcl_ConvertToType(interp, $obj, &matd_ObjType));
matd_t* $argname;
$argname = (matd_t*) $obj->internalRep.otherValuePtr;
}
$cc rtype matd_t* {
$robj = Tcl_NewObj();
$robj->bytes = NULL;
$robj->typePtr = &matd_ObjType;
$robj->internalRep.otherValuePtr = $rvalue;
}

# Takes a list of at least 4 point pairs (model -> image) like
#
# [list \
# [list x0 y0 u0 v0]] \
# [list x1 y1 u1 v1] \
# [list x2 y2 u2 v2] \
# [list x3 y3 u3 v3]]
#
# Returns a 3x3 homography that maps model (x, y) to image (u, v)
# (using homogeneous coordinates).
$cc proc estimateHomographyImpl {int nPointPairs float[][4] pointPairs} matd_t* {
// HACK: (sort of) point to the existing data block.
zarray_t correspondencesArr = {
.el_sz = sizeof(float[4]),
.size = nPointPairs, .alloc = nPointPairs,
.data = (char*) pointPairs
};
return homography_compute(&correspondencesArr,
HOMOGRAPHY_COMPUTE_FLAG_SVD);
}
# HACK: wrapper because we have to give the array length as a separate
# arg (at least for now).
proc estimateHomography {pointPairs} {
estimateHomographyImpl [llength $pointPairs] $pointPairs
}
$cc proc applyHomography {matd_t* H double[2] xy} Tcl_Obj* {
double out[2];
homography_project(H, xy[0], xy[1], &out[0], &out[1]);

package require vectcl
namespace import ::vectcl::vexpr
Tcl_Obj* retObjs[2] = { Tcl_NewDoubleObj(out[0]), Tcl_NewDoubleObj(out[1]) };
return Tcl_NewListObj(2, retObjs);
}
$cc proc matdMul {matd_t* a matd_t* b} matd_t* {
return matd_multiply(a, b);
}
c loadlib $::env(HOME)/apriltag/libapriltag.so
$cc compile

set ROWS 4
set COLS 5
Expand Down Expand Up @@ -79,45 +188,6 @@ fn isProjectedTag {id} {
! [isPrintedTag $id]
}

# Takes a list of at least 4 point pairs (model -> image) like
#
# [list \
# [list x0 y0 u0 v0]] \
# [list x1 y1 u1 v1] \
# [list x2 y2 u2 v2] \
# [list x3 y3 u3 v3]]
#
# Returns a 3x3 homography that maps model (x, y) to image (u, v)
# (using homogeneous coordinates).
# HACK: to share with test
set ::estimateHomography {{pointPairs} {
set A [list]
set b [list]
foreach pair $pointPairs {
lassign $pair x y u v
lappend A [list $x $y 1 0 0 0 [expr {-$x*$u}] [expr {-$y*$u}]]
lappend A [list 0 0 0 $x $y 1 [expr {-$x*$v}] [expr {-$y*$v}]]
lappend b $u $v
}

vexpr {
x = A \ b
x = linsert(x, 8, 1.0)
H = reshape(x, 3, 3)
}
return $H
}}
fn estimateHomography {pointPairs} {
apply $::estimateHomography $pointPairs
}
proc ::applyHomography {H xy} {
lappend xy 1.0
return [vexpr {
uvw = H * reshape(xy, 3)
list(uvw[0]/uvw[2], uvw[1]/uvw[2])
}]
}

# Takes a dictionary of tag ID => {p, c} and rotates version tags
# according to version.
fn versionizeTags {tags version} {
Expand Down Expand Up @@ -575,7 +645,7 @@ When camera /camera/ has width /cameraWidth/ height /cameraHeight/ &\

# Update H_modelToDisplay to project the tags into the middle
# of the board next frame.
set H_modelToDisplay [vexpr { H_cameraToDisplay * H_modelToCameraViaPs }]
set H_modelToDisplay [matdMul $H_cameraToDisplay $H_modelToCameraViaPs]
Commit H_modelToDisplay {
set nextVersion [+ $version 1]
set nextModel [versionizeTags $baseModel $nextVersion]
Expand Down

0 comments on commit b1abe69

Please sign in to comment.