revert changes to Camera.tcl and pi.tcl

2 files changed, 2 insertions, 314 deletions

diff --git a/pi/Camera.tcl b/pi/Camera.tcl
index 70d2f4e1..9c77570e 100644
--- a/pi/Camera.tcl
+++ b/pi/Camera.tcl
@@ -224,26 +224,6 @@ camc proc rgbToGray {image_t rgb} image_t {
         .data = gray
     };
 }
-camc proc thresholdGray {image_t gray} image_t {
-    uint8_t* newGray = calloc(gray.width * gray.height, sizeof (uint8_t));
-    for (int y = 0; y < gray.height; y++) {
-        for (int x = 0; x < gray.width; x++) {
-            int i = (y * gray.width + x) * 1;
-            uint8_t g = gray.data[i];
-            if (g > 128) {
-                g = 255;
-            } else {
-                g = 0;
-            }
-            newGray[y * gray.width + x] = g;
-        }
-    }
-    return (image_t) {
-        .width = gray.width, .height = gray.height,
-        .bytesPerRow = gray.width,
-        .data = newGray
-    };
-}
 camc proc freeUint8Buffer {uint8_t* buf} void {
     free(buf);
 }
@@ -255,7 +235,6 @@ camc proc freeImage {image_t image} void {
     free(image.data);
 }
 
-# On one of my RPi, path needs to be changed to /sbin/ldconfig
 c loadlib [expr {$tcl_platform(os) eq "Darwin" ? "/opt/homebrew/lib/libjpeg.dylib" : [lindex [exec /usr/sbin/ldconfig -p | grep libjpeg] end]}]
 camc compile
 
@@ -317,10 +296,8 @@ if {([info exists ::argv0] && $::argv0 eq [info script]) || \
     while true {
         set rgb [Camera::frame]
         set gray [rgbToGray $rgb]
-        set gray2 [thresholdGray $gray]
         # FIXME: hacky
-        Display::grayImage $Display::fb $Display::WIDTH $Display::HEIGHT "(uint8_t*) [dict get $gray2 data]" $Camera::WIDTH $Camera::HEIGHT
-        freeImage $gray2
+        Display::grayImage $Display::fb $Display::WIDTH $Display::HEIGHT "(uint8_t*) [dict get $gray data]" $Camera::WIDTH $Camera::HEIGHT
         freeImage $gray
         freeImage $rgb
     }
@@ -388,291 +365,3 @@ namespace eval AprilTags {
         detectInit
     }
 }
-
-
-namespace eval LaserBlobTracker {
-    rename [c create] apc
-    apc cflags -I$::env(HOME)/apriltag
-    apc include <apriltag.h>
-    apc include <math.h>
-    apc include <assert.h>
-    apc code {
-#include <stdio.h>
-#include <stdlib.h>
-#include <assert.h>
-
-// Note from Haip: code copied from https://www.ocf.berkeley.edu/~fricke/projects/hoshenkopelman/hk.c
-// because I haven't figure out where I could import this file from
-// https://www.ocf.berkeley.edu/~fricke/projects/hoshenkopelman/hoshenkopelman.html
-
-/* Implementation of Union-Find Algorithm */
-
-
-/* The 'labels' array has the meaning that labels[x] is an alias for the label x; by
-   following this chain until x == labels[x], you can find the canonical name of an
-   equivalence class.  The labels start at one; labels[0] is a special value indicating
-   the highest label already used. */
-
-int *labels;
-int  n_labels = 0;     /* length of the labels array */
-
-/*  uf_find returns the canonical label for the equivalence class containing x */
-
-int uf_find(int x) {
-  int y = x;
-  while (labels[y] != y)
-    y = labels[y];
-  
-  while (labels[x] != x) {
-    int z = labels[x];
-    labels[x] = y;
-    x = z;
-  }
-  return y;
-}
-
-/*  uf_union joins two equivalence classes and returns the canonical label of the resulting class. */
-
-int uf_union(int x, int y) {
-  return labels[uf_find(x)] = uf_find(y);
-}
-
-/*  uf_make_set creates a new equivalence class and returns its label */
-
-int uf_make_set(void) {
-  labels[0] ++;
-  assert(labels[0] < n_labels);
-  labels[labels[0]] = labels[0];
-  return labels[0];
-}
-
-/*  uf_intitialize sets up the data structures needed by the union-find implementation. */
-
-void uf_initialize(int max_labels) {
-  n_labels = max_labels;
-  labels = calloc(sizeof(int), n_labels);
-  labels[0] = 0;
-}
-
-/*  uf_done frees the memory used by the union-find data structures */
-
-void uf_done(void) {
-  n_labels = 0;
-  free(labels);
-  labels = 0;
-}
-
-/* End Union-Find implementation */
-
-#define max(a,b) (a>b?a:b)
-#define min(a,b) (a>b?b:a)
-
-/* Label the clusters in "matrix".  Return the total number of clusters found. */
-
-int hoshen_kopelman(int **matrix, int m, int n) {
-  
-  uf_initialize(m * n / 2);
-  
-  /* scan the matrix */
-  
-  for (int i=0; i<m; i++)
-    for (int j=0; j<n; j++)
-      if (matrix[i][j]) {                        // if occupied ...
-
-	int up = (i==0 ? 0 : matrix[i-1][j]);    //  look up  
-	int left = (j==0 ? 0 : matrix[i][j-1]);  //  look left
-	
-	switch (!!up + !!left) {
-	  
-	case 0:
-	  matrix[i][j] = uf_make_set();      // a new cluster
-	  break;
-	  
-	case 1:                              // part of an existing cluster
-	  matrix[i][j] = max(up,left);       // whichever is nonzero is labelled
-	  break;
-	  
-	case 2:                              // this site binds two clusters
-	  matrix[i][j] = uf_union(up, left);
-	  break;
-	}
-	
-      }
-  
-  /* apply the relabeling to the matrix */
-
-  /* This is a little bit sneaky.. we create a mapping from the canonical labels
-     determined by union/find into a new set of canonical labels, which are 
-     guaranteed to be sequential. */
-  
-  int *new_labels = calloc(sizeof(int), n_labels); // allocate array, initialized to zero
-  
-  for (int i=0; i<m; i++)
-    for (int j=0; j<n; j++)
-      if (matrix[i][j]) {
-	int x = uf_find(matrix[i][j]);
-	if (new_labels[x] == 0) {
-	  new_labels[0]++;
-	  new_labels[x] = new_labels[0];
-	}
-	matrix[i][j] = new_labels[x];
-      }
- 
-  int total_clusters = new_labels[0];
-
-  free(new_labels);
-  uf_done();
-
-  return total_clusters;
-}
-    }
-    apc code {
-        typedef struct {
-            int id;
-
-            // The center of the detection in image pixel coordinates.
-            double c[2];
-
-            // The corners of the tag in image pixel coordinates. These always
-            // wrap counter-clock wise around the tag.
-            // TL BL BR TR
-            double p[4][2];
-
-            int size;
-        } detected_blob_t;
-
-        zarray_t *blob_detector_detect(image_u8_t *im_orig)
-        {
-            zarray_t *detections = zarray_create(sizeof(detected_blob_t*));
-
-            // m = rows, n = columns
-            int m = im_orig->height;
-            int n = im_orig->width;
-            int **matrix;
-            matrix = (int **)malloc(m * sizeof(int *));
-            for(int i = 0; i < m; i++)
-                matrix[i] = (int *)malloc(n * sizeof(int));
-
-            // for(int i = 0; i < rows; i++)
-            //     memset(matrix[i], 0, cols * sizeof(int));
-            
-            for (int y = 0; y < im_orig->height; y++) {
-                for (int x = 0; x < im_orig->width; x++) {
-                    int i = y * im_orig->stride + x;
-                    int v = im_orig->buf[i];
-
-                    // threshold
-                    if (v > 128) {
-                        v = 1;
-                    } else {
-                        v = 0;
-                    }
-                    matrix[y][x] = v;
-                }
-            }
-
-            int clusters = hoshen_kopelman(matrix,m,n);
-            // printf("clusters: %d\n", clusters);
-
-            for (int i=0; i<clusters; i++) {
-                detected_blob_t *det = calloc(1, sizeof(detected_blob_t));
-                det->id = i;
-                det->c[0] = 0;
-                det->c[1] = 0;
-                det->p[0][0] = 0;
-                det->p[0][1] = 0;
-                det->p[1][0] = 0;
-                det->p[1][1] = 0;
-                det->p[2][0] = 0;
-                det->p[2][1] = 0;
-                det->p[3][0] = 0;
-                det->p[3][1] = 0;
-                det->size = 0;
-                zarray_add(detections, &det);
-            }
-
-            for (int i=0; i<m; i++) {
-                for (int j=0; j<n; j++) {
-                    // printf("%d ",matrix[i][j]); 
-                    if (matrix[i][j]) {
-                        detected_blob_t *det;
-                        zarray_get(detections, matrix[i][j]-1, &det);
-                        det->c[0] += j;
-                        det->c[1] += i;
-                        det->size += 1;
-                    }
-                }
-                // printf("\n");
-            }
-
-            for (int i=0; i<clusters; i++) {
-                detected_blob_t *det;
-                zarray_get(detections, i, &det);
-                det->id = i;
-                det->c[0] = det->c[0] / det->size;
-                det->c[1] = det->c[1] / det->size;
-            }
-
-            for (int i=0; i<m; i++)
-                free(matrix[i]);
-            free(matrix);
-
-            return detections;
-        }
-
-        void blob_detection_destroy(detected_blob_t *det)
-        {
-            if (det == NULL)
-                return;
-
-            free(det);
-        }
-
-        void blob_detections_destroy(zarray_t *detections)
-        {
-            for (int i = 0; i < zarray_size(detections); i++) {
-                detected_blob_t *det;
-                zarray_get(detections, i, &det);
-
-                blob_detection_destroy(det);
-            }
-
-            zarray_destroy(detections);
-        }
-    }
-    defineImageType apc
-
-    apc proc detect {image_t gray} Tcl_Obj* {
-        assert(gray.components == 1);
-        image_u8_t im = (image_u8_t) { .width = gray.width, .height = gray.height, .stride = gray.width, .buf = gray.data };
-
-        zarray_t *detections = blob_detector_detect(&im);
-        int detectionCount = zarray_size(detections);
-
-        Tcl_Obj* detectionObjs[detectionCount];
-        for (int i = 0; i < detectionCount; i++) {
-            detected_blob_t *det;
-            zarray_get(detections, i, &det);
-
-            // printf("detection %3d: id %-4d\n cx %f cy %f size %d\n", i, det->id, det->c[0], det->c[1], det->size);
-
-            // int size = sqrt((det->p[0][0] - det->p[1][0])*(det->p[0][0] - det->p[1][0]) + (det->p[0][1] - det->p[1][1])*(det->p[0][1] - det->p[1][1]));
-            int size = det->size;
-            detectionObjs[i] = Tcl_ObjPrintf("id %d center {%f %f} corners {{%f %f} {%f %f} {%f %f} {%f %f}} size %d",
-                                             det->id,
-                                             det->c[0], det->c[1],
-                                             det->p[0][0], det->p[0][1],
-                                             det->p[1][0], det->p[1][1],
-                                             det->p[2][0], det->p[2][1],
-                                             det->p[3][0], det->p[3][1],
-                                             size);
-        }
-
-        blob_detections_destroy(detections);
-        
-        Tcl_Obj* result = Tcl_NewListObj(detectionCount, detectionObjs);
-        return result;
-    }
-
-    apc compile
-}
\ No newline at end of file
diff --git a/pi/pi.tcl b/pi/pi.tcl
index a582913c..9fe83fd2 100644
--- a/pi/pi.tcl
+++ b/pi/pi.tcl
@@ -79,8 +79,7 @@ namespace eval Camera {
             }
             set cameraTime [time {
                 set grayFrame [Camera::grayFrame]
-                # set tags [AprilTags::detect $grayFrame]
-                set tags [LaserBlobTracker::detect $grayFrame]
+                set tags [AprilTags::detect $grayFrame]
                 lappend grayFrames $grayFrame
             }]
             set statements [list]