measuring_cuboid.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Measuring cuboids\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# You can only fill in a file with a .ply extension\n",
    "# see data.ipynb to use the data from the dataset\n",
    "fileName = 'pointcloud1'\n",
    "\n",
    "# Coordinates of point in the top plane of the cuboid\n",
    "top_plane_point_x = -0.060090084\n",
    "top_plane_point_y = -0.15732084\n",
    "top_plane_point_z = -0.2964331"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Importing libraries and data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "\n",
    "# remove outliers\n",
    "cloud = o3d.io.read_point_cloud(fileName + \".ply\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Statistical Outlier Filter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cloud, ind = cloud.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_1_statOutlierRemoval.ply\", cloud)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Radius Outlier Removal"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cloud, ind = cloud.remove_radius_outlier(nb_points=10, radius=0.01)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_2_removeRadiusOutlier.ply\", cloud)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Voxel Down Sample"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cloud = cloud.voxel_down_sample(voxel_size=0.005)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_3_voxelDownSample.ply\", cloud)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 5. Floor removal"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "normal = np.array([0, 1, 0])\n",
    "top_plane_point = np.array([top_plane_point_x, top_plane_point_y, top_plane_point_z])\n",
    "top_plane_point_product = -np.dot(normal, top_plane_point)\n",
    "points = np.asarray(cloud.points)\n",
    "\n",
    "threshold = 0.02\n",
    "\n",
    "points_not_in_plane = points[~((np.abs(np.dot(normal, points.T) + top_plane_point_product)) < threshold)]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize\n",
    "v_points_in_plane = np.arange(points.shape[0])[(np.abs(np.dot(normal, points.T) + top_plane_point_product)) < threshold]\n",
    "v_points_not_in_plane = np.arange(points.shape[0])[~((np.abs(np.dot(normal, points.T) + top_plane_point_product)) < threshold)]\n",
    "\n",
    "\n",
    "visual = cloud.select_by_index(v_points_not_in_plane, invert=False)\n",
    "visual.paint_uniform_color([0, 1, 0])\n",
    "visual2 = cloud.select_by_index(v_points_in_plane, invert=False)\n",
    "visual2.paint_uniform_color([0, 0, 1])\n",
    "o3d.io.write_point_cloud(f\"{fileName}_x_pointsNotIP.ply\", visual)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_x_pointsIP.ply\", visual2)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_x_combined.ply\", visual + visual2)\n",
    "\n",
    "# draw tap point\n",
    "# mesh = o3d.geometry.TriangleMesh.create_sphere(radius=0.02)\n",
    "# mesh.translate(top_plane_point)\n",
    "\n",
    "# o3d.visualization.draw_geometries([mesh, cloud])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pyransac3d as pyrsc\n",
    "# convert cloud to numpy array\n",
    "floor_plane = pyrsc.Plane()\n",
    "floor_cutting = 0.02\n",
    "\n",
    "best_eq, best_inliers = floor_plane.fit(points_not_in_plane, floor_cutting, 1000)\n",
    "\n",
    "# Points that do not belong to the floor found floorplane\n",
    "points_no_floor = points[~((np.abs(np.dot(best_eq[0:3], points.T) + best_eq[3])) < threshold)]\n",
    "# Points that do not belong to the floor found floorplane and are not below the floor\n",
    "points_not_below_floor = points_no_floor[~(points_no_floor[:, 1] < (-best_eq[3] / best_eq[1]))] \n",
    "\n",
    "newCloud = o3d.geometry.PointCloud()\n",
    "newCloud.points = o3d.utility.Vector3dVector(np.array(points_not_below_floor))\n",
    "# o3d.visualization.draw_geometries([newCloud])\n",
    "o3d.io.write_point_cloud(f\"{fileName}_4_floorPlane.ply\", newCloud)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<!-- \\texttt{floor\\_plane.get\\_oriented\\_bounding\\_box():} Deze methode berekent de georiënteerde bounding box die past rond de punten die behoren tot \\texttt{floor\\_plane} object en slaat het resultaat op in het obb object. \\texttt{obb.color = [0, 0, 1]} zet de kleur van de boundingbox op blauw. \\\\\n",
    "\n",
    "\\texttt{obb.get\\_point\\_indices\\_within\\_bounding\\_box(cloud.points)} geeft een lijst van indices van punten in de puntewolk terug die zich binnen de bounding box bevinden. -->"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6. Isolating the cuboid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Alle punten vinden die in de vorm van een kubus zijn\n",
    "cuboid = pyrsc.Cuboid()\n",
    "# best_eq is een numpy matrix van 3 beste vlakkenvergelijking\n",
    "# best_inliers is een lijst van de indexen van de punten die in de kubus passen\n",
    "best_eq, best_inliers = cuboid.fit(np.asarray(points_not_below_floor), thresh=0.005)\n",
    "\n",
    "# VISUALISATIE START\n",
    "# cuboid_pcl is een pointcloud van de punten die in de kubus passen\n",
    "# not_cuboid_pcl is een pointcloud van de punten die niet in de kubus passen\n",
    "cuboid_pcl = newCloud.select_by_index(best_inliers).paint_uniform_color([1, 0, 0])\n",
    "not_cuboid_pcl = newCloud.select_by_index(best_inliers, invert=True).paint_uniform_color([0, 1, 0])\n",
    "plane = newCloud.select_by_index(best_inliers).paint_uniform_color([1, 0, 0])\n",
    "\n",
    "# Join  plane and not plane togheter \n",
    "o3d.io.write_point_cloud(f\"{fileName}_6_cuboidPcl.ply\", cuboid_pcl)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_7_nonCuboidPcl.ply\", not_cuboid_pcl)\n",
    "o3d.io.write_point_cloud(f\"{fileName}_8_combinedCuboidAndNonCuboidPcl.ply\", cuboid_pcl + not_cuboid_pcl)\n",
    "# VISUALISATIE EINDE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "horizontal_plane_idx = 0\n",
    "left_plane_idx = 1\n",
    "right_plane_idx = 2\n",
    "\n",
    "# Haal de beste vlakkenvergelijkingen op voor de horizontale, linker en rechter vlakken\n",
    "top_plane = best_eq[horizontal_plane_idx]\n",
    "left_plane = best_eq[left_plane_idx]\n",
    "right_plane = best_eq[right_plane_idx]\n",
    "\n",
    "# VISUALISATIE START\n",
    "\n",
    "# VISUALISATIE EINDE"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 7. Calculating vectors\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plane_intersect(a, b):\n",
    "    \"\"\"\n",
    "    a, b   4-tuples/lists\n",
    "           Ax + By +Cz + D = 0\n",
    "           A,B,C,D in order  \n",
    "\n",
    "    output is een punt en een vector\n",
    "\n",
    "p_inter is punt op d eintersectielijn en aXb_vec is de richtingsvector\n",
    "    \"\"\"\n",
    "    a_vec, b_vec = np.array(a[:3]), np.array(b[:3])\n",
    "\n",
    "    aXb_vec = np.cross(a_vec, b_vec)\n",
    "\n",
    "    A = np.array([a_vec, b_vec, aXb_vec])\n",
    "    d = np.array([-a[3], -b[3], 0.]).reshape(3,1)\n",
    "\n",
    "    p_inter = np.linalg.solve(A, d).T\n",
    "\n",
    "    return p_inter[0], aXb_vec"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "_, new_y_vector = plane_intersect(left_plane, right_plane)\n",
    "_, new_x_vector = plane_intersect(top_plane, right_plane)\n",
    "new_z_vector = np.cross(new_x_vector, new_y_vector)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# normalize vectors\n",
    "new_x_vector = new_x_vector / np.linalg.norm(new_x_vector)\n",
    "new_y_vector = new_y_vector / np.linalg.norm(new_y_vector)\n",
    "new_z_vector = new_z_vector / np.linalg.norm(new_z_vector)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 8. Rotating the cuboid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cuboid_pcl_points = np.asarray(cuboid_pcl.points)\n",
    "\n",
    "base_change_matrix = np.array([new_x_vector, new_y_vector, new_z_vector]).reshape(3,3)\n",
    "new_points = np.dot(base_change_matrix, cuboid_pcl_points.T).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# to pointcloud\n",
    "new_cuboid_pcl = o3d.geometry.PointCloud()\n",
    "new_cuboid_pcl.points = o3d.utility.Vector3dVector(new_points)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def selectClosestCluster(pcd, top_plane_point):\n",
    "    pcdPoints = np.asarray(pcd.points)\n",
    "    clusterLabels = np.asarray(pcd.cluster_dbscan(eps=0.01, min_points=10))\n",
    "\n",
    "    nonNoisePoints = pcdPoints[clusterLabels != -1]\n",
    "    nonNoiseClusterLabels = clusterLabels[clusterLabels != -1]\n",
    "    pointDistances = np.linalg.norm(nonNoisePoints - np.asarray(top_plane_point), axis=1)\n",
    "\n",
    "    closestPointIdx = np.argmin(pointDistances)\n",
    "    closestPointLabel = nonNoiseClusterLabels[closestPointIdx]\n",
    "\n",
    "    new_cuboid_pcl = o3d.geometry.PointCloud()\n",
    "    new_cuboid_pcl.points = o3d.utility.Vector3dVector(pcdPoints[clusterLabels == closestPointLabel])\n",
    "\n",
    "    return new_cuboid_pcl\n",
    "\n",
    "new_cuboid_pcl = selectClosestCluster(new_cuboid_pcl, top_plane_point)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 9. Fitting the bounding box and returing the measurements\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# get axis aligned bounding box\n",
    "bb = new_cuboid_pcl.get_axis_aligned_bounding_box()\n",
    "bb.color = [0, 1, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# translate bounding box to original coordinate system\n",
    "bb_points = np.asarray(bb.get_box_points())\n",
    "bb_points = np.dot(np.linalg.inv(base_change_matrix), bb_points.T).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# Visualize the oriented bounding box\n",
    "# Create an oriented bounding box from the transformed points\n",
    "new_bb = o3d.geometry.OrientedBoundingBox.create_from_points(o3d.utility.Vector3dVector(bb_points))\n",
    "new_bb.color = [0, 1, 0]\n",
    "\n",
    "\n",
    "width = bb.max_bound[0] - bb.min_bound[0]\n",
    "height = bb.max_bound[1] - bb.min_bound[1] + 0.02\n",
    "depth = bb.max_bound[2] - bb.min_bound[2]\n",
    "\n",
    "print(f\"Width: {width}, Height: {height}, Depth: {depth}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "o3d.visualization.draw_geometries([cloud, new_bb])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "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.9.12"
  },
  "vscode": {
   "interpreter": {
    "hash": "9c989041d992532e0b91a48157c549286f92417413d85e1f44b78593fb847874"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}