backgroundRemoval/backgroundremoval.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import copy\n",
    "\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# transforem the source cloud and then draw the point clouds\n",
    "def draw_registration_result(source, target, transformation):\n",
    "    #copy original point clouds\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    #set the color\n",
    "    source_temp.paint_uniform_color([1, 0.706, 0])\n",
    "    target_temp.paint_uniform_color([0, 0.651, 0.929])\n",
    "    #apply transformation\n",
    "    source_temp.transform(transformation)\n",
    "    #draw the clouds\n",
    "    o3d.visualization.draw_geometries([source_temp,target_temp])\n",
    "    #o3d.visualization.draw_geometries([target])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "#down sample the point cloud, recompute new normals, compute fpfh features\n",
    "def preprocess_point_cloud(pcd, voxel_size):\n",
    "    print(\":: Downsample with a voxel size %.3f.\" % voxel_size)\n",
    "    pcd_down = pcd.voxel_down_sample(voxel_size)\n",
    "\n",
    "    #use the neighbors recompute the normal\n",
    "    radius_normal = voxel_size * 2\n",
    "    print(\":: Estimate normal with search radius %.3f.\" % radius_normal)\n",
    "    pcd_down.estimate_normals(\n",
    "        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))\n",
    "\n",
    "    #compute the FPFH features\n",
    "    radius_feature = voxel_size * 5\n",
    "    print(\":: Compute FPFH feature with search radius %.3f.\" % radius_feature)\n",
    "    pcd_fpfh = o3d.registration.compute_fpfh_feature(\n",
    "        pcd_down,\n",
    "        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))\n",
    "    return pcd_down, pcd_fpfh\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "#load data, down sample and compute fpfh feature\n",
    "def prepare_dataset(voxel_size):\n",
    "    #load point clouds\n",
    "    print(\":: Load two point clouds and disturb initial pose.\")\n",
    "    source = o3d.io.read_point_cloud(\"/home/yunke/3dv_proj/code/badslam0427/new_bg/bg.ply\")\n",
    "    target = o3d.io.read_point_cloud(\"/home/yunke/3dv_proj/code/badslam0427/cereal_g/lie/lie.ply\")\n",
    "    \n",
    "    #apply an inital transformation\n",
    "    trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],\n",
    "                             [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])\n",
    "    source.transform(trans_init)\n",
    "    #visualize\n",
    "    draw_registration_result(source, target, np.identity(4))\n",
    "\n",
    "    #downsample and compute fpfh feature of the point clouds\n",
    "    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)\n",
    "    target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)\n",
    "    return source, target, source_down, target_down, source_fpfh, target_fpfh"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "#global registration based on RANSAC\n",
    "def execute_global_registration(source_down, target_down, source_fpfh,\n",
    "                                target_fpfh, voxel_size):\n",
    "    distance_threshold = voxel_size * 1.5\n",
    "    print(\":: RANSAC registration on downsampled point clouds.\")\n",
    "    print(\"   Since the downsampling voxel size is %.3f,\" % voxel_size)\n",
    "    print(\"   we use a liberal distance threshold %.3f.\" % distance_threshold)\n",
    "    \n",
    "    #apply RANSAC algorithms for registration:\n",
    "    #pick 4 points,\n",
    "    #pruning with edge length and distance for early stop\n",
    "    result = o3d.registration.registration_ransac_based_on_feature_matching(\n",
    "        source_down, target_down, source_fpfh, target_fpfh, distance_threshold,\n",
    "        o3d.registration.TransformationEstimationPointToPoint(False), 4, [\n",
    "            o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),\n",
    "            o3d.registration.CorrespondenceCheckerBasedOnDistance(\n",
    "                distance_threshold)\n",
    "        ], o3d.registration.RANSACConvergenceCriteria(4000000, 500))\n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "#fast global registration\n",
    "def execute_fast_global_registration(source_down, target_down, source_fpfh,\n",
    "                                     target_fpfh, voxel_size):\n",
    "    distance_threshold = voxel_size * 0.5\n",
    "    print(\":: Apply fast global registration with distance threshold %.3f\" \\\n",
    "            % distance_threshold)\n",
    "    result = o3d.registration.registration_fast_based_on_feature_matching(\n",
    "        source_down, target_down, source_fpfh, target_fpfh,\n",
    "        o3d.registration.FastGlobalRegistrationOption(\n",
    "            maximum_correspondence_distance=distance_threshold))\n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "#point to plane registration\n",
    "def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):\n",
    "    distance_threshold = voxel_size * 0.4\n",
    "    print(\":: Point-to-plane ICP registration is applied on original point\")\n",
    "    print(\"   clouds to refine the alignment. This time we use a strict\")\n",
    "    print(\"   distance threshold %.3f.\" % distance_threshold)\n",
    "    result = o3d.registration.registration_icp(\n",
    "        source, target, distance_threshold, result_fast.transformation,\n",
    "        o3d.registration.TransformationEstimationPointToPlane())\n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "#crop point cloud+outlier\n",
    "def crop_pcd_target(pcd,source,result_icp):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    cor_ind = np.asarray(result_icp.correspondence_set)\n",
    "    pcd_out = pcd.select_down_sample(cor_ind[:,1],invert=True)\n",
    "    out_dist = np.asarray(pcd_out.compute_point_cloud_distance(source_temp.transform(result_icp.transformation)))\n",
    "    out_index = np.arange(0,out_dist.shape[0],1)\n",
    "    #threshold\n",
    "    thr = 0.01\n",
    "    #select outliers\n",
    "    out_list = out_index[out_dist>thr]\n",
    "    pcd_out = pcd_out.select_down_sample(out_list)\n",
    "    #define bounding box\n",
    "    center = np.asarray([[0.0,-0.1,0.60]]).reshape((3,1))\n",
    "    R = np.eye(3)\n",
    "    extent = np.asarray([[1.0,1.0,1.0]]).reshape((3,1))\n",
    "    box = o3d.geometry.OrientedBoundingBox(center,R,extent)\n",
    "    print(box)\n",
    "    #crop\n",
    "    cropped = pcd_out.crop(box)\n",
    "    \n",
    "    return cropped"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "#crop point cloud+outlier\n",
    "def crop_pcd_source(pcd):\n",
    "    \n",
    "    #define bounding box\n",
    "    \n",
    "    center = np.asarray([[0.0,-0.1,0.60]]).reshape((3,1))\n",
    "    R = np.eye(3)\n",
    "    extent = np.asarray([[1.0,1.0,1.0]]).reshape((3,1))\n",
    "    box = o3d.geometry.OrientedBoundingBox(center,R,extent)\n",
    "    print(box)\n",
    "    #crop\n",
    "    cropped = pcd.crop(box)\n",
    "    \n",
    "    return cropped"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "#segment inlier plane\n",
    "def remove_plane(target,source,result_icp):\n",
    "    #crop\n",
    "    target_crop = crop_pcd_target(target,source,result_icp)\n",
    "    o3d.visualization.draw_geometries_with_editing([target_crop])\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    source_temp.transform(result_icp.transformation)\n",
    "    source_crop = crop_pcd_source(source_temp)\n",
    "    o3d.visualization.draw_geometries_with_editing([source_crop])\n",
    "    #combine point clouds, make the plane to be removed easily\n",
    "    tgt_src = target_crop + source_crop + source_crop\n",
    "    o3d.visualization.draw_geometries_with_editing([tgt_src])\n",
    "    #segement \n",
    "    plane,points_on_plane = tgt_src.segment_plane(0.015,4,1000)\n",
    "    #select not on plane only from target point cloud\n",
    "    full_ind = np.arange(np.asarray(target_crop.points).shape[0])\n",
    "    not_on_plane_ind = np.setdiff1d(full_ind,points_on_plane)\n",
    "    return tgt_src.select_down_sample(not_on_plane_ind)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "#cluster\n",
    "def cluster_points(target_no_plane):\n",
    "    full_index = np.arange(np.asarray(target_no_plane.points).shape[0])\n",
    "\n",
    "    #cluster\n",
    "    cluster_class = np.asarray(target_no_plane.cluster_dbscan(voxel_size*0.25, 20, print_progress=False))\n",
    "    #select cluster\n",
    "    min_dist = 1.0\n",
    "    obj_points = copy.deepcopy(target_no_plane)\n",
    "    for cluster in range(max(cluster_class)+1):\n",
    "        \n",
    "        cur_cluster_ind = full_index[cluster_class==cluster]\n",
    "        cur_cluster_points = target_no_plane.select_down_sample(cur_cluster_ind)\n",
    "        #compute center\n",
    "        cur_points = np.asarray(cur_cluster_points.points)\n",
    "        cur_mean = np.mean(cur_points,axis=0)\n",
    "        print(cur_mean)\n",
    "        cur_dist = np.linalg.norm(cur_mean-np.asarray([[0.0,0.0,0.6]]))\n",
    "        print(cur_dist)\n",
    "        if cur_dist<min_dist and cur_points.shape[0]>1000:\n",
    "            min_dist = cur_dist\n",
    "            obj_points = cur_cluster_points\n",
    "            \n",
    "    return obj_points\n",
    "            "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "#get object\n",
    "def get_object_point_cloud(target,source,result_icp):\n",
    "    #remove plane\n",
    "    target_no_plane = remove_plane(target,source,result_icp)\n",
    "    #cluster points\n",
    "    obj_points = cluster_points(target_no_plane)\n",
    "    o3d.visualization.draw_geometries([obj_points])\n",
    "    return obj_points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ":: Load two point clouds and disturb initial pose.\n",
      ":: Downsample with a voxel size 0.050.\n",
      ":: Estimate normal with search radius 0.100.\n",
      ":: Compute FPFH feature with search radius 0.250.\n",
      ":: Downsample with a voxel size 0.050.\n",
      ":: Estimate normal with search radius 0.100.\n",
      ":: Compute FPFH feature with search radius 0.250.\n",
      ":: RANSAC registration on downsampled point clouds.\n",
      "   Since the downsampling voxel size is 0.050,\n",
      "   we use a liberal distance threshold 0.075.\n",
      "Global registration took 1.762 sec.\n",
      "\n",
      "registration::RegistrationResult with fitness=9.656524e-01, inlier_rmse=3.147987e-02, and correspondence_set size of 10346\n",
      "Access transformation to get result.\n",
      ":: Apply fast global registration with distance threshold 0.025\n",
      "Fast global registration took 0.365 sec.\n",
      "\n",
      "registration::RegistrationResult with fitness=6.747247e-01, inlier_rmse=1.786993e-02, and correspondence_set size of 7229\n",
      "Access transformation to get result.\n",
      ":: Point-to-plane ICP registration is applied on original point\n",
      "   clouds to refine the alignment. This time we use a strict\n",
      "   distance threshold 0.020.\n",
      "geometry::OrientedBoundingBox\n",
      "geometry::OrientedBoundingBox\n",
      "[-0.08741084  0.48060762  0.47561607]\n",
      "0.504079063900516\n",
      "[0.03116015 0.30347854 0.5437776 ]\n",
      "0.3102114401075114\n",
      "[-0.84204113 -0.05584113  1.09822084]\n",
      "0.9799874989577614\n",
      "[-0.68290559 -0.14886193  1.4509844 ]\n",
      "1.1012240358389989\n",
      "[-0.84518286 -0.11386883  1.10134548]\n",
      "0.9892661310157925\n",
      "[-0.83745785  0.0023912   1.09725118]\n",
      "0.9739610404607764\n",
      "[-0.82174305  0.11052062  1.10680297]\n",
      "0.9717642159074892\n",
      "[0.25595533 0.48534319 0.68554373]\n",
      "0.5553277144744858\n",
      "[-0.86756431 -0.16172363  1.06885142]\n",
      "0.9993217846252967\n",
      "[-0.82055228  0.05146151  1.11422822]\n",
      "0.9697344995633526\n",
      "[-0.13579126  0.49734899  0.62087862]\n",
      "0.5159759646627504\n",
      "[-0.58117153  0.32824267  0.17158567]\n",
      "0.793121952964919\n",
      "[-0.57873666  0.51652252  0.04309324]\n",
      "0.9549223871541718\n",
      "[-0.59412544  0.62878873  0.03570075]\n",
      "1.032857181600259\n",
      "[-0.82596051  0.72018749  0.4864084 ]\n",
      "1.1017185870930495\n",
      "[-0.59725081  0.69476466  0.04514733]\n",
      "1.0711059438415793\n",
      "[0.18708145 0.4537837  0.67025892]\n",
      "0.4958381100934308\n",
      "[-0.69008146  0.87134269  0.23894927]\n",
      "1.168677943160213\n",
      "[-0.83551591  0.57964298  0.44003276]\n",
      "1.0293990167657716\n",
      "[ 0.02093589  0.83734761 -0.12109745]\n",
      "1.1052469716036029\n",
      "[ 0.0255649   0.80223105 -0.12624922]\n",
      "1.0824352856112989\n",
      "[ 0.03654083  0.77468433 -0.13802785]\n",
      "1.0705868260242748\n",
      "[-0.82719673  0.61148385  0.46332448]\n",
      "1.0377124471943178\n",
      "[-0.60265939  0.60145505  0.04922845]\n",
      "1.0140492173183007\n",
      "[-0.59374368  0.65568179  0.01543202]\n",
      "1.060268782730728\n",
      "[-0.4697507   0.64429416  0.0597811 ]\n",
      "0.9631288326546276\n",
      "[ 0.07636615  0.65691977 -0.1206251 ]\n",
      "0.9780981042810968\n",
      "[-0.68077076  0.70410435  0.21013808]\n",
      "1.0541366473677434\n",
      "[ 0.02195425  0.75272728 -0.16158273]\n",
      "1.0710222275563037\n",
      "[ 0.10852142  0.53108131 -0.1550728 ]\n",
      "0.929494050549022\n",
      "[-0.60806046  0.45607222  0.12455028]\n",
      "0.8965443820027662\n",
      "[-0.87553218  0.31511482  0.45990964]\n",
      "0.9409990718708579\n",
      "[0.76382144 0.58893528 0.44803125]\n",
      "0.9764028172699993\n",
      "[-0.67845874  0.60037102  0.23543224]\n",
      "0.9765558181604886\n",
      "[-0.13689843  0.77914677 -0.21465777]\n",
      "1.1355519157385996\n",
      "[-0.45041289  0.50520136  0.03529454]\n",
      "0.8814717440682183\n",
      "[-0.4583022   0.3586165   0.03050022]\n",
      "0.8142338143897581\n",
      "[-0.81830299  0.0260754   1.12441137]\n",
      "0.9722689913947528\n",
      "[ 0.07922951  0.48974382 -0.19753652]\n",
      "0.9392501364879932\n",
      "[-0.05068275  0.76477966 -0.16229843]\n",
      "1.080997487355124\n",
      "[-0.88314824  0.04613188  0.83012632]\n",
      "0.9138036394632725\n",
      "[-0.81041358  0.17526381  1.11365414]\n",
      "0.975360522191943\n",
      "[-0.89160047  0.01796553  0.81919997]\n",
      "0.918326077354387\n",
      "[-0.88408409  0.07943221  0.82287976]\n",
      "0.9151991801153954\n",
      "[ 0.00359933  0.54478874 -0.22981583]\n",
      "0.9926741873409097\n",
      "[ 0.16805156  0.68839039 -0.10157003]\n",
      "0.9971575419196047\n",
      "[-0.02714646  0.70849182 -0.16610259]\n",
      "1.0438442323846444\n",
      "[-0.84029922  0.50252918  0.44234276]\n",
      "0.9917127405884641\n",
      "[-0.9072072   0.05605265  0.78808892]\n",
      "0.9281940747531033\n",
      "[ 0.17885649  0.84071619 -0.18122277]\n",
      "1.1615086688309393\n",
      "[-0.8671182   0.35797829  0.46108802]\n",
      "0.9483348377286855\n",
      "[-0.87408522  0.30729187  0.49683281]\n",
      "0.9322535801783713\n",
      "[-0.48615262  0.77131516  0.09081107]\n",
      "1.0442915365721446\n",
      "[-0.5688383   0.25114179  0.08187883]\n",
      "0.8093817148109793\n",
      "[ 0.1034386   0.70628055 -0.08928094]\n",
      "0.9922902668467426\n",
      "[ 0.0353953   0.62354851 -0.13934081]\n",
      "0.967827677880855\n",
      "[-0.89887663 -0.02915322  0.81348777]\n",
      "0.9243409201974332\n",
      "[-0.69588059  0.75647114  0.22986701]\n",
      "1.092472800811605\n",
      "[-0.46064243  0.18418541  0.06163393]\n",
      "0.7320886202306969\n",
      "[0.17759055 0.51477546 0.510466  ]\n",
      "0.5518591409145924\n",
      "[-0.91837455 -0.19560432  0.96774173]\n",
      "1.0084179935400615\n",
      "[-0.82607452  0.66539408  0.4457229 ]\n",
      "1.0718907677925473\n",
      "[ 0.04392259  0.53653118 -0.20140289]\n",
      "0.9654229572020252\n",
      "[-0.60737872  0.49076932  0.17042716]\n",
      "0.8912330018558922\n",
      "[-0.62773903  0.50075006  0.13895689]\n",
      "0.9259414986369675\n",
      "[-0.02126927  0.73240803 -0.15560739]\n",
      "1.052528590007196\n",
      "[ 0.14533049  0.6069531  -0.16396212]\n",
      "0.9864842342124136\n",
      "[-0.60724391  0.72582613  0.06520689]\n",
      "1.0870015683341012\n",
      "[-0.60582262  0.70607916  0.06420348]\n",
      "1.0736138701897846\n",
      "[ 0.01924459  0.58006419 -0.1954033 ]\n",
      "0.9846376161530878\n",
      "[-0.52151879  0.4354241   0.06354471]\n",
      "0.8656559797472902\n",
      "[ 0.03021332  0.76888019 -0.16038835]\n",
      "1.0817948173951728\n",
      "[-0.5932943   0.67390438  0.02882037]\n",
      "1.0641388122612925\n",
      "[-0.06415698  0.74097654 -0.14842521]\n",
      "1.055131579143282\n",
      "[-0.08507141  0.57029967  0.60462545]\n",
      "0.5766283534387291\n",
      "[-0.58385103  0.58478719  0.02346168]\n",
      "1.007598387463348\n",
      "[-0.53035943  0.3412845   0.04883003]\n",
      "0.8375825776266276\n",
      "[-0.58667866  0.30398115  0.16341414]\n",
      "0.7919618736433652\n",
      "[-0.6214891   0.52167527  0.18588809]\n",
      "0.910978847377626\n",
      "[-0.01353622  0.77950943 -0.15393951]\n",
      "1.0845473557656788\n",
      "[-0.53333706  0.12671538  0.12843695]\n",
      "0.7231022871303764\n",
      "[ 0.0328805   0.75923197 -0.17955187]\n",
      "1.088675998817301\n",
      "[0.17259456 0.51649768 0.64587318]\n",
      "0.5465007619182919\n",
      "[0.77009322 0.6506782  0.44411578]\n",
      "1.020159585541921\n",
      "[-0.61187695  0.34498942  0.19217131]\n",
      "0.8122409422500768\n",
      "[-0.56026337  0.18156413  0.16548591]\n",
      "0.7318900680916485\n",
      "[0.18522805 0.52628613 0.67741747]\n",
      "0.5632761134194038\n",
      "[-0.50060429  0.43654518  0.06334075]\n",
      "0.8539200825616133\n",
      "[-0.64733316  0.56460929  0.17956268]\n",
      "0.9563427265298393\n",
      "[-0.07701227  0.56008197  0.50053371]\n",
      "0.5740350568221324\n",
      "[-0.4950714   0.45212414  0.06462149]\n",
      "0.85798722098093\n",
      "[-0.00399277  0.46407359 -0.24398106]\n",
      "0.963163677358724\n",
      "[-0.48337479  0.83666876  0.12512527]\n",
      "1.076648418432882\n",
      "[ 0.12771264  0.85579191 -0.14885328]\n",
      "1.1443214365813956\n",
      "[-0.70633359 -0.17826103  1.43804547]\n",
      "1.1104072824717153\n",
      "[ 0.0257779   0.87307798 -0.1041769 ]\n",
      "1.121960232202787\n",
      "[-0.64650826  0.43488673  0.22707596]\n",
      "0.8638123288377102\n",
      "[ 0.04279156  0.47743942 -0.22994832]\n",
      "0.9584329545724848\n",
      "[-0.5084823   0.48955162  0.09322115]\n",
      "0.868930288528\n",
      "[ 0.14163588  0.88271146 -0.15605457]\n",
      "1.1708367736599221\n",
      "[-0.5657495   0.4541866   0.03232962]\n",
      "0.9211990165078249\n",
      "[-0.00931014  0.8632484  -0.11403876]\n",
      "1.1203284453835798\n",
      "[0.7662634  0.62568726 0.44155019]\n",
      "1.0018734842927657\n",
      "[-0.08076448  0.55715583  0.67001712]\n",
      "0.5673164195084246\n",
      "[0.8004662  0.4677232  0.48511452]\n",
      "0.9341893828374109\n",
      "[-0.50110494  0.79085038  0.11242917]\n",
      "1.0555926291825555\n",
      "[-0.82605498  0.76331945  0.4782462 ]\n",
      "1.1313034113033456\n",
      "[-0.52597963  0.76276649  0.11516358]\n",
      "1.045721593077405\n",
      "[-0.49533288  0.69013518  0.07976923]\n",
      "0.9961331653130581\n",
      "[-0.68876795  0.66015379  0.2388236 ]\n",
      "1.0201238710302947\n",
      "[-0.44645501  0.49859347  0.05491871]\n",
      "0.8631518642674875\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "voxel_size = 0.05  # means 5cm for the dataset\n",
    "\n",
    "#down sample, compute fpfh of the data\n",
    "source, target, source_down, target_down, source_fpfh, target_fpfh = \\\n",
    "        prepare_dataset(voxel_size)\n",
    "\n",
    "#global registration using ransac\n",
    "start = time.time()\n",
    "result_ransac = execute_global_registration(source_down, target_down,\n",
    "                                            source_fpfh, target_fpfh,\n",
    "                                            voxel_size)\n",
    "print(\"Global registration took %.3f sec.\\n\" % (time.time() - start))\n",
    "print(result_ransac)\n",
    "\n",
    "\n",
    "# #draw the result\n",
    "# draw_registration_result(source_down, target_down,\n",
    "#                         result_ransac.transformation)\n",
    "\n",
    "#fast global registration\n",
    "start = time.time()\n",
    "result_fast=execute_fast_global_registration(source_down, target_down,\n",
    "                                             source_fpfh, target_fpfh,\n",
    "                                             voxel_size)\n",
    "                         \n",
    "print(\"Fast global registration took %.3f sec.\\n\" % (time.time() - start))\n",
    "print(result_fast)\n",
    "# draw_registration_result(source_down, target_down,\n",
    "#                         result_fast.transformation)\n",
    "    \n",
    "#local refinement\n",
    "result_icp = refine_registration(source, target, source_fpfh, target_fpfh,\n",
    "                                 voxel_size)\n",
    "# print(result_icp)\n",
    "# draw_registration_result(source, target, result_icp.transformation)\n",
    "\n",
    "obj_points = get_object_point_cloud(target,source,result_icp)\n",
    "#save\n",
    "o3d.io.write_point_cloud(\"objects/creal_g_lie.pcd\",obj_points)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}