Clouds.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "cf36f531",
   "metadata": {},
   "source": [
    "https://github.com/dimitri-yatsenko/point-cloud-registration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c38f92ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import numpy as np\n",
    "from matplotlib import pyplot as plt\n",
    "from tqdm import tqdm\n",
    "from scipy.stats import gaussian_kde"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f79a2663",
   "metadata": {},
   "outputs": [],
   "source": [
    "import point_cloud_registration as pcr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15a3dbc9",
   "metadata": {},
   "outputs": [],
   "source": [
    "points1 = np.load('data/Animal1_Day1_points.npy')\n",
    "points2 = np.load('data/Animal1_Day2_points.npy')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ba52dd64",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.scatter(*points1[:,:2].T, s=2, alpha=0.1)\n",
    "plt.scatter(*points2[:,:2].T, s=2, alpha=0.1)\n",
    "plt.axis('equal')\n",
    "plt.box(False)\n",
    "plt.title('original datasets');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78a0ebe0",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# crop and scale datasets differently\n",
    "points1 = points1[points1[:,1] < 0.6 * points1[:,1].max(), :]\n",
    "points1 *= 1.2\n",
    "\n",
    "# rotate and translate the second set\n",
    "alpha = 0.1\n",
    "rotation = [\n",
    "    [np.cos(alpha), -np.sin(alpha), 0],\n",
    "    [np.sin(alpha), np.cos(alpha), 0],\n",
    "    [0, 0, 1]]\n",
    "points2 = points2 @ rotation\n",
    "\n",
    "points2 = points2[points2[:,1] < 0.7 * points2[:,1].max(), :]\n",
    "points2 = points2[points2[:,0] < 0.6 * points2[:,0].max(),:]\n",
    "\n",
    "points2 += np.array([500, -100, 30])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e3b5feac",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.scatter(*points1[:,:2].T, s=2, alpha=0.1)\n",
    "plt.scatter(*points2[:,:2].T, s=2, alpha=0.1)\n",
    "plt.axis('equal')\n",
    "plt.box(False)\n",
    "plt.title('transformed datasets')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38578a5b",
   "metadata": {},
   "outputs": [],
   "source": [
    "tetras1 = pcr.make_normal_tetras(points1)\n",
    "tetras2 = pcr.make_normal_tetras(points2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cfb4387c",
   "metadata": {},
   "outputs": [],
   "source": [
    "pcr.compute_canonical_features(tetras1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25bc062f",
   "metadata": {},
   "outputs": [],
   "source": [
    "pcr.remove_common_tetras(tetras1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99b33b3d",
   "metadata": {},
   "outputs": [],
   "source": [
    "pcr.compute_canonical_features(tetras2)\n",
    "pcr.remove_common_tetras(tetras2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "26cdf012",
   "metadata": {},
   "outputs": [],
   "source": [
    "F1 = tetras1['features']\n",
    "F2 = tetras2['features']\n",
    "\n",
    "fig, ax = plt.subplots(1, 3, figsize=(20, 6))\n",
    "ax[0].scatter(*F1[:,[0,1]].T, s=1, alpha=0.2)\n",
    "ax[0].scatter(*F2[:,[0,1]].T, s=1, alpha=0.2)\n",
    "\n",
    "ax[1].scatter(*F1[:,[2,3]].T, s=1, alpha=0.2)\n",
    "ax[1].scatter(*F2[:,[2,3]].T, s=1, alpha=0.2)\n",
    "\n",
    "ax[2].scatter(*F1[:,[4,5]].T, s=1, alpha=0.2)\n",
    "ax[2].scatter(*F2[:,[4,5]].T, s=1, alpha=0.2)\n",
    "\n",
    "fig.suptitle('feature space')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3aa864b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "distances, matches = pcr.match_features(tetras1, tetras2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "edb3545c",
   "metadata": {},
   "outputs": [],
   "source": [
    "ix = ~np.isinf(distances)\n",
    "distances = distances[ix]\n",
    "matches = np.stack(matches[ix])[:,0]\n",
    "pcr.select_tetras(tetras2, ix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "90acaf3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# vote based on size\n",
    "N1 = tetras1['norms']\n",
    "N2 = tetras2['norms']\n",
    "\n",
    "scale = N1[matches][:,0,0] / N2[:,0,0] \n",
    "plt.hist(scale,40,density=True)\n",
    "\n",
    "density = gaussian_kde(scale)(scale)\n",
    "mode = scale[np.argmax(density)]\n",
    "\n",
    "plt.scatter(scale, density, s=0.5, c='r')\n",
    "plt.grid(True)\n",
    "plt.xlabel('relative scale')\n",
    "\n",
    "mode"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a4d8421d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# remove matches with other scales\n",
    "ix = abs(scale/mode - 1) < 0.05\n",
    "matches = matches[ix]\n",
    "pcr.select_tetras(tetras2, ix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "462af824",
   "metadata": {},
   "outputs": [],
   "source": [
    "q, r = np.unique(matches, return_inverse=True)\n",
    "matches = matches[r]\n",
    "pcr.select_tetras(tetras2, r)\n",
    "pcr.select_tetras(tetras1, matches)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c5f74e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "dd = tetras1['means'][:,0,:]-tetras2['means'][:,0,:]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1cbbeefe",
   "metadata": {},
   "outputs": [],
   "source": [
    "r = gaussian_kde(dd.T)(dd.T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "601ac468",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "plt.scatter(dd[:,0], dd[:,1], c=r, s=1)\n",
    "plt.axis('equal')\n",
    "plt.grid(True)\n",
    "plt.box(False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "efdbe7f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "tetras2['means'].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1cc7cae0",
   "metadata": {},
   "outputs": [],
   "source": [
    "C1 = tetras1['coords'].reshape((-1, 3))\n",
    "C2 = tetras2['coords'].reshape((-1, 3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c2a3ea4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.scatter(C1[:,0], C1[:,1], s=2, alpha=0.2)\n",
    "plt.scatter(C2[:,0], C2[:,1], s=2, alpha=0.2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0eb2a319",
   "metadata": {},
   "outputs": [],
   "source": [
    "M1 = C1.mean(axis=0, keepdims=True)\n",
    "M2 = C2.mean(axis=0, keepdims=True)\n",
    "C1 -= M1\n",
    "C2 -= M2\n",
    "N1 = np.linalg.norm(C1, axis=0, keepdims=True)\n",
    "N2 = np.linalg.norm(C2, axis=0, keepdims=True)\n",
    "C1 /= N1\n",
    "C2 /= N2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "772142f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "R, D = pcr.ortho_procrustes(C1, C2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d3553737",
   "metadata": {},
   "outputs": [],
   "source": [
    "R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d114012f",
   "metadata": {},
   "outputs": [],
   "source": [
    "CC2 = D * C2 @ R.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "692aa253",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.scatter(C1[:,0], C1[:,1], s=2, alpha=0.2)\n",
    "plt.scatter(C2[:,0], C2[:,1], s=2, alpha=0.2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "401a88a6",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "benv",
   "language": "python",
   "name": "benv"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}