0_laser_homography_tool.py

import os
import glob
import time
import subprocess
import tempfile
import numpy as np
import cv2
from scipy.optimize import linear_sum_assignment

# === CONFIG ===
PROJECTOR_CMD_FILE = "constellation.txt"
BLE_JS_CMD = ["node", "../ble.js"]
CALIBRATION_FILE = os.path.expanduser("~/dev/BLE/chess/camera_calibration.npz")
PROJECTOR_COORDS = [
    [-7.19, 7.99],
    [-9.73, 10.0],
    [-5.56, 4.47],
    [-8.88, -4.95],
    [9.0, -4.86],
    [10.0, -7.91],
    [10.0, -10.0]
]

# Constellation identifiers
# This is a list of distances from each constellation point to each of it's neighbors,
# ordered by distance and normalized by the distance to the closest neighbor.
# We use these to detect which constellation point is which.
#
# The constellation was computed to have an optimally differentiable identifier list.
CONSTELLATION_IDENTIFIERS = [
    [1.0, 1.198, 4.029, 6.381, 7.229, 7.682],
    [1.0, 2.138, 4.623, 7.381, 8.227, 8.673],
    [1.0, 1.785, 2.575, 4.458, 5.126, 5.478],
    [1.0, 1.307, 1.499, 1.79, 1.913, 1.957],
    [1.0, 1.631, 5.388, 5.571, 6.44, 7.449],
    [1.0, 1.536, 9.144, 9.514, 11.204, 12.75],
    [1.0, 2.505, 9.351, 10.167, 11.905, 13.442]
]

# === Step 1: Cleanup old snapshots ===
print("🧹 Cleaning up old snapshot images...")
for f in glob.glob("snapshot_*.jpg"):
    os.remove(f)

# === Step 2: Project constellation ===
print("📡 Sending constellation command to projector...")
subprocess.run(BLE_JS_CMD + [PROJECTOR_CMD_FILE])

# === Step 3: Wait for new snapshots ===
print("📷 Waiting for new snapshot images...")
before_img = None
after_img = None
timeout = 30
poll_interval = 0.5
start_time = time.time()

while True:
    snapshot_files = sorted(glob.glob("snapshot_*.jpg"))
    if len(snapshot_files) >= 2:
        before_img_path, after_img_path = snapshot_files[-2], snapshot_files[-1]
        before_img = cv2.imread(before_img_path)
        after_img = cv2.imread(after_img_path)
        if before_img is not None and after_img is not None:
            print(f"✅ Found images: {before_img_path}, {after_img_path}")
            break
    if time.time() - start_time > timeout:
        print("❌ Timeout waiting for snapshot images.")
        exit(1)
    time.sleep(poll_interval)

# === Step 4: Spot detection ===
def find_laser_spots(before_img, after_img, expected_count=7):
    diff = cv2.absdiff(after_img, before_img)
    diff_gray = cv2.cvtColor(diff, cv2.COLOR_BGR2GRAY)
    diff_gray = cv2.GaussianBlur(diff_gray, (5, 5), 0)

    best_thresh = None
    best_centers = []
    best_thresh_img = None

    for thresh_val in range(255, 0, -1):
        _, thresh = cv2.threshold(diff_gray, thresh_val, 255, cv2.THRESH_BINARY)

        kernel = np.ones((3, 3), np.uint8)
        thresh = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel)
        thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)

        contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        if len(contours) == expected_count:
            centers = []
            for cnt in contours:
                M = cv2.moments(cnt)
                if M['m00'] != 0:
                    cx = int(M['m10']/M['m00'])
                    cy = int(M['m01']/M['m00'])
                    centers.append((cx, cy))

            best_thresh = thresh_val
            best_centers = centers
            best_thresh_img = thresh.copy()
            print(f"✅ Found expected {expected_count} contours at threshold = {thresh_val}")
            break

    if best_thresh is None:
        print("❌ Could not find the expected number of laser spots.")
        best_thresh_img = np.zeros_like(diff_gray)

    return diff, diff_gray, best_thresh_img, best_centers

def compute_normalized_distances(centers, idx):
    distances = []
    x0, y0 = centers[idx]
    for j, (x1, y1) in enumerate(centers):
        if idx == j:
            continue
        dist = np.linalg.norm([x1 - x0, y1 - y0])
        distances.append(dist)
    distances = np.array(distances)
    min_dist = np.min(distances)
    normalized = distances / min_dist
    return np.sort(normalized)

def compute_error(norm_distances, ref_distances):
    return np.mean(np.abs(np.array(norm_distances) - np.array(ref_distances)))

def assign_ids(centers, constellation_identifiers, max_error_threshold=1.0):
    num_points = len(centers)
    num_ids = len(constellation_identifiers)
    cost_matrix = np.zeros((num_points, num_ids))

    for i in range(num_points):
        norm_distances = compute_normalized_distances(centers, i)
        for j, ref_distances in enumerate(constellation_identifiers):
            cost_matrix[i, j] = compute_error(norm_distances, ref_distances)

    row_indices, col_indices = linear_sum_assignment(cost_matrix)

    assignments = [-1] * num_points
    for r, c in zip(row_indices, col_indices):
        if cost_matrix[r, c] < max_error_threshold:
            assignments[r] = c
        else:
            assignments[r] = None
    return assignments

# Load calibration
calib = np.load(CALIBRATION_FILE)
camera_matrix = calib["camera_matrix"]
dist_coeffs = calib["dist_coeffs"]

# Detect spots
diff, diff_gray, thresh, centers = find_laser_spots(before_img, after_img)
assigned_ids = assign_ids(centers, CONSTELLATION_IDENTIFIERS)

# Visualize
debug_img = before_img.copy()
for (i, (x, y)) in enumerate(centers):
    cv2.circle(debug_img, (x, y), 10, (0, 255, 0), 2)
    label = str(assigned_ids[i]) if assigned_ids[i] is not None else "?"
    cv2.putText(debug_img, label, (x - 10, y + 33), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)

cv2.imshow("Before Image", before_img)
cv2.imshow("Diff Image", diff)
cv2.imshow("Thresholded Image", thresh)
cv2.imshow("Detected Spots", debug_img)
cv2.waitKey(0)

# === Step 5: Compute homography ===
matches = []
centers_np = np.array(centers, dtype=np.float32).reshape(-1, 1, 2)
undistorted_points = cv2.undistortPoints(centers_np, camera_matrix, dist_coeffs, P=camera_matrix)
undistorted_points = undistorted_points.reshape(-1, 2)

for idx, (undist_pt, assigned_id) in enumerate(zip(undistorted_points, assigned_ids)):
    if assigned_id is not None:
        cam_x, cam_y = undist_pt
        proj_x, proj_y = PROJECTOR_COORDS[assigned_id]
        matches.append([[cam_x, cam_y], [proj_x, proj_y]])

camera_points = np.array([m[0] for m in matches], dtype=np.float32).reshape(-1, 1, 2)
projector_points = np.array([m[1] for m in matches], dtype=np.float32).reshape(-1, 1, 2)
H, _ = cv2.findHomography(camera_points, projector_points, method=0)

print("\n✅ Homography computed:")
print(H)

def click_event(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        print(f"\n🖱️ Click at pixel ({x}, {y})")

        # Undistort
        pt_np = np.array([[[x, y]]], dtype=np.float32)
        undist = cv2.undistortPoints(pt_np, camera_matrix, dist_coeffs, P=camera_matrix)
        undist_pt = undist.reshape(-1, 2)[0]

        # Map through homography
        pt_hom = np.array([[undist_pt]], dtype=np.float32)
        proj_pt = cv2.perspectiveTransform(pt_hom, H).reshape(-1, 2)[0]

        print(f"Homography result → Projector coords: ({proj_pt[0]:.2f}, {proj_pt[1]:.2f})")

        # Generate temp dot file
        with tempfile.NamedTemporaryFile("w", suffix=".txt", delete=False) as tmpfile:
            tmpfile.write(f"""lsr:stop

lsr:sc:100
lsr:hz:13000

lsr:>;{proj_pt[0]:.4f},{proj_pt[1]:.4f},r0.1;<

lsr:go
""")
            tmpfile_path = tmpfile.name

        print(f"🚀 Sending single dot command...")
        subprocess.run(BLE_JS_CMD + [tmpfile_path])
        print(f"✅ Done!")


cv2.namedWindow("Click to project")
cv2.setMouseCallback("Click to project", click_event)

print("\n🖱️ Click in the window to project a dot. Press ESC to exit.")
while True:
    cv2.imshow("Click to project", before_img)
    key = cv2.waitKey(20)
    if key == 27:  # ESC key
        break

cv2.destroyAllWindows()

ble.js

const fs = require('fs');
const path = require('path');
const os = require('os');

// Handle command line argument for commands file
if (process.argv.length < 3) {
  console.error('❌ Usage: node ble.js <path-to-commands.txt>');
  process.exit(1);
}

let commandFilePath = process.argv[2];

// Expand ~ to home directory if present
if (commandFilePath.startsWith('~')) {
  commandFilePath = path.join(os.homedir(), commandFilePath.slice(1));
}

const noble = require('@abandonware/noble');

const DEVICE_NAME = 'ESPCAM';
const SERVICE_UUID = 'ab05';
const WRITE_UUID = 'ab06';
const READ_UUID = 'ab07';

let writeChar = null;
let readChar = null;
let commandQueue = [];
let responsePending = false;
let subscriptionsReady = 0;
let pausedForImage = false;

// Image state
let imageChunks = [];
let receivingImage = false;
const JPG_START = 'ffd8';
const JPG_END = 'ffd9';
const OUTPUT_IMAGE = 'snapshot.jpg';

function containsJpgEnd(buffer) {
  return buffer.includes(Buffer.from([0xff, 0xd9]));
}

function saveImage(buffer) {
  const end = buffer.lastIndexOf(Buffer.from(JPG_END, 'hex')) + 2;
  const trimmed = buffer.slice(0, end);

  const now = new Date();
  const timestamp = now.toISOString().replace(/[-:]/g, '').replace(/\..+/, '').replace('T', '_');
  const filename = `snapshot_${timestamp}.jpg`;

  fs.writeFileSync(filename, trimmed);
  console.log(`🖼️ Image saved to ${filename} (${trimmed.length} bytes)\n`);
}

function readImageChunk() {
  if (!receivingImage || !readChar) return;

  readChar.readAsync()
    .then(data => {
      if (data.length <= 4) {
        console.warn('⚠️ Received a short or invalid chunk.');
        return readImageChunk(); // skip and try again
      }

      const chunkData = data.slice(4); // remove first 4 bytes (chunk header)
      imageChunks.push(chunkData);

      console.log(`📦 Chunk: ${data.length} bytes (stripped to ${chunkData.length})`);

      if (containsJpgEnd(chunkData)) {
        receivingImage = false;
        pausedForImage = false;
        console.log('🏁 Image transfer complete, saving...');
        saveImage(Buffer.concat(imageChunks));

        // Now that the image is done, try sending next command
        if (commandQueue.length > 0) {
          setTimeout(sendNextCommand, 150);
        } else {
          console.log('✅ All commands and image received.\n');
          process.exit(0);
        }
      } else {
        setTimeout(readImageChunk, 25); // keep reading
      }
    })
    .catch(err => {
      console.error('❌ Error during image read:', err);
      receivingImage = false;
    });
}


function subscribeToNotifications(char, label) {
  if (char.uuid === WRITE_UUID) {
    char.on('data', (data) => {
      const msg = data.toString().trim();
      console.log(`📥 Response from ${label}: ${msg}\n`);

      if (msg.toLowerCase().includes('img ready for xfer')) {
        console.log('📸 Snapshot ready — reading image data...\n');
        receivingImage = true;
        pausedForImage = true;
        imageChunks = [];
        readImageChunk();
      }

      if (responsePending) {
        responsePending = false;
        setTimeout(sendNextCommand, 150);
      }
    });
  }

  char.subscribe((err) => {
    if (err) {
      console.error(`❌ Failed to subscribe to ${label}`);
    } else {
      console.log(`📡 Subscribed to ${label}`);
      subscriptionsReady++;
      if (subscriptionsReady === 2 && commandQueue.length > 0) {
        console.log('\n🚀 Starting command sequence...\n');
        sendNextCommand();
      }
    }
  });
}

function sendNextCommand() {
  if (commandQueue.length === 0) {
    if (receivingImage || pausedForImage) {
      console.log('⏳ Waiting for image to finish before exiting...');
      return;
    }
    console.log('✅ All commands sent and processed.\n');
    process.exit(0);
  }

  if (pausedForImage) {
    console.log('⏸️ Waiting for image to finish before sending next command...');
    return;
  }

  const next = commandQueue.shift();
  console.log(`📤 Sending: ${next}`);
  responsePending = true;
  writeChar.writeAsync(Buffer.from(next), false)
    .catch(err => {
      console.error(`❌ Error sending command: ${err}`);
    });
}

noble.on('stateChange', async (state) => {
  if (state === 'poweredOn') {
    console.log('🔍 Scanning for ESPCAM...');
    noble.startScanning([], false);
  } else {
    noble.stopScanning();
  }
});

noble.on('discover', async (peripheral) => {
  if (peripheral.advertisement.localName === DEVICE_NAME || peripheral.address === 'a1:73:44:38:b6:71') {
    console.log(`✅ Found device: ${DEVICE_NAME}`);
    noble.stopScanning();

    try {
      await peripheral.connectAsync();
      console.log('🔌 Connected to ESPCAM\n');

      const { characteristics } = await peripheral.discoverSomeServicesAndCharacteristicsAsync(
        [SERVICE_UUID],
        [WRITE_UUID, READ_UUID]
      );

      // Load commands before subscriptions
      let fileContent;
      try {
        fileContent = fs.readFileSync(commandFilePath, 'utf8');
      } catch (err) {
        console.error(`❌ Failed to read command file: ${commandFilePath}`);
        console.error(err.message);
        process.exit(1);
      }

      commandQueue = fileContent
        .split('\n')
        .map(line => line.trim())
        .filter(line => line.length && !line.startsWith('#'));

      console.log(`📄 Loaded ${commandQueue.length} commands from ${commandFilePath}\n`);

      for (const char of characteristics) {
        if (char.uuid === WRITE_UUID) {
          writeChar = char;
          subscribeToNotifications(char, 'WRITE/NOTIFY (0xAB06)');
        } else if (char.uuid === READ_UUID) {
          readChar = char;
          subscribeToNotifications(char, 'READ/NOTIFY (0xAB07)');
        }
      }

      if (!writeChar || !readChar) {
        console.error('❌ Missing required characteristics. Exiting...');
        await peripheral.disconnectAsync();
        process.exit(1);
      }

    } catch (err) {
      console.error('❌ BLE Error:', err);
      process.exit(1);
    }
  }
});

constellation.txt

lsr:stop

lsr:sc:100
lsr:hz:13000

lsr:>;-7.19,7.99,r0.1;
lsr:-9.73,10.00,r0.1;
lsr:-5.56,4.47,r0.1;
lsr:-8.88,-4.95,r0.1;
lsr:9.00,-4.86,r0.1;
lsr:10.00,-7.91,r0.1;
lsr:10.00,-10.00,r0.1;<

snap

lsr:go

snap