implementation/registration/PredatorRegistration-demo.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "b879834a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<Figure size 640x480 with 0 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import os, torch, time, shutil, json,glob,sys,copy, argparse\n",
    "import numpy as np\n",
    "from easydict import EasyDict as edict\n",
    "from torch.utils.data import Dataset\n",
    "from torch import optim, nn\n",
    "import open3d as o3d\n",
    "cwd = os.getcwd()\n",
    "sys.path.append(cwd)\n",
    "from datasets.indoor import IndoorDataset\n",
    "from datasets.dataloader import get_dataloader\n",
    "from models.architectures import KPFCNN\n",
    "from lib.utils import load_obj, setup_seed,natural_key, load_config\n",
    "from lib.benchmark_utils import ransac_pose_estimation, to_o3d_pcd, get_blue, get_yellow, to_tensor\n",
    "from lib.trainer import Trainer\n",
    "from lib.loss import MetricLoss\n",
    "from numpy.linalg import inv\n",
    "import shutil\n",
    "setup_seed(0)\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.subplots_adjust(left=0,\n",
    "                    bottom=0,\n",
    "                    right=1.0,\n",
    "                    top=1.0,\n",
    "                    wspace=0.5,\n",
    "                    hspace=0.2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5cb6e66f",
   "metadata": {},
   "source": [
    "# 1. PREDATOR Module "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "c17e52e8",
   "metadata": {},
   "outputs": [],
   "source": [
    "class ThreeDMatchDemo(Dataset):\n",
    "    \"\"\"\n",
    "    Load subsampled coordinates, relative rotation and translation\n",
    "    Output(torch.Tensor):\n",
    "        src_pcd:        [N,3]\n",
    "        tgt_pcd:        [M,3]\n",
    "        rot:            [3,3]\n",
    "        trans:          [3,1]\n",
    "    \"\"\"\n",
    "    def __init__(self,config, src, tgt):\n",
    "        super(ThreeDMatchDemo,self).__init__()\n",
    "        self.config = config\n",
    "        self.src = src\n",
    "        self.tgt = tgt\n",
    "\n",
    "    def __len__(self):\n",
    "        return 1\n",
    "\n",
    "    def __getitem__(self,item): \n",
    "        # get pointcloud\n",
    "        #print (type(torch.load(self.src_path)))\n",
    "        #src_pcd = torch.load(self.src_path).astype(np.float32)\n",
    "        #tgt_pcd = torch.load(self.tgt_path).astype(np.float32)   \n",
    "        src_pcd = self.src.astype(np.float32)\n",
    "        tgt_pcd = self.tgt.astype(np.float32)    \n",
    "        \n",
    "        \n",
    "        #src_pcd = o3d.io.read_point_cloud(self.src_path)\n",
    "        #tgt_pcd = o3d.io.read_point_cloud(self.tgt_path)\n",
    "        #src_pcd = src_pcd.voxel_down_sample(0.025)\n",
    "        #tgt_pcd = tgt_pcd.voxel_down_sample(0.025)\n",
    "        #src_pcd = np.array(src_pcd.points).astype(np.float32)\n",
    "        #tgt_pcd = np.array(tgt_pcd.points).astype(np.float32)\n",
    "\n",
    "\n",
    "        src_feats=np.ones_like(src_pcd[:,:1]).astype(np.float32)\n",
    "        tgt_feats=np.ones_like(tgt_pcd[:,:1]).astype(np.float32)\n",
    "\n",
    "        # fake the ground truth information\n",
    "        rot = np.eye(3).astype(np.float32)\n",
    "        trans = np.ones((3,1)).astype(np.float32)\n",
    "        correspondences = torch.ones(1,2).long()\n",
    "\n",
    "        return src_pcd,tgt_pcd,src_feats,tgt_feats,rot,trans, correspondences, src_pcd, tgt_pcd, torch.ones(1)\n",
    "\n",
    "\n",
    "def predict(config, demo_loader):\n",
    "    config.model.eval()\n",
    "    c_loader_iter = demo_loader.__iter__()\n",
    "    with torch.no_grad():\n",
    "        inputs = c_loader_iter.next()\n",
    "        ##################################\n",
    "        # load inputs to device.\n",
    "        for k, v in inputs.items():  \n",
    "            if type(v) == list:\n",
    "                inputs[k] = [item.to(config.device) for item in v]\n",
    "            else:\n",
    "                inputs[k] = v.to(config.device)\n",
    "\n",
    "        ###############################################\n",
    "        # forward pass\n",
    "        feats, scores_overlap, scores_saliency = config.model(inputs)  #[N1, C1], [N2, C2]\n",
    "        pcd = inputs['points'][0]\n",
    "        len_src = inputs['stack_lengths'][0][0]\n",
    "        c_rot, c_trans = inputs['rot'], inputs['trans']\n",
    "        correspondence = inputs['correspondences']\n",
    "        \n",
    "        src_pcd, tgt_pcd = pcd[:len_src], pcd[len_src:]\n",
    "        src_raw = copy.deepcopy(src_pcd)\n",
    "        tgt_raw = copy.deepcopy(tgt_pcd)\n",
    "        src_feats, tgt_feats = feats[:len_src].detach().cpu(), feats[len_src:].detach().cpu()\n",
    "        src_overlap, src_saliency = scores_overlap[:len_src].detach().cpu(), scores_saliency[:len_src].detach().cpu()\n",
    "        tgt_overlap, tgt_saliency = scores_overlap[len_src:].detach().cpu(), scores_saliency[len_src:].detach().cpu()\n",
    "\n",
    "        ########################################\n",
    "        # do probabilistic sampling guided by the score\n",
    "        src_scores = src_overlap * src_saliency\n",
    "        tgt_scores = tgt_overlap * tgt_saliency\n",
    "\n",
    "        if(src_pcd.size(0) > config.n_points):\n",
    "            idx = np.arange(src_pcd.size(0))\n",
    "            probs = (src_scores / src_scores.sum()).numpy().flatten()\n",
    "            idx = np.random.choice(idx, size= config.n_points, replace=False, p=probs)\n",
    "            src_pcd, src_feats = src_pcd[idx], src_feats[idx]\n",
    "        if(tgt_pcd.size(0) > config.n_points):\n",
    "            idx = np.arange(tgt_pcd.size(0))\n",
    "            probs = (tgt_scores / tgt_scores.sum()).numpy().flatten()\n",
    "            idx = np.random.choice(idx, size= config.n_points, replace=False, p=probs)\n",
    "            tgt_pcd, tgt_feats = tgt_pcd[idx], tgt_feats[idx]\n",
    "\n",
    "        ########################################\n",
    "        # run ransac and draw registration\n",
    "        tsfm = ransac_pose_estimation(src_pcd, tgt_pcd, src_feats, tgt_feats, mutual=False)\n",
    "        return tsfm\n",
    "        #draw_registration_result(src_raw, tgt_raw, src_overlap, tgt_overlap, src_saliency, tgt_saliency, tsfm)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "a73a4648",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "len(dataset):3720\n",
      "[0] Calib Neighbors 00000000: timings 0.20s\n",
      "[1] Calib Neighbors 00000001: timings 0.40s\n",
      "[2] Calib Neighbors 00000002: timings 0.57s\n",
      "[3] Calib Neighbors 00000003: timings 0.77s\n",
      "[4] Calib Neighbors 00000004: timings 0.97s\n",
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# load configs\n",
    "config = load_config(r'configs/test/indoor.yaml')\n",
    "config = edict(config)\n",
    "if config.gpu_mode:\n",
    "    config.device = torch.device('cuda')\n",
    "else:\n",
    "    config.device = torch.device('cpu')\n",
    "\n",
    "# model initialization\n",
    "config.architecture = [\n",
    "    'simple',\n",
    "    'resnetb',\n",
    "]\n",
    "for i in range(config.num_layers-1):\n",
    "    config.architecture.append('resnetb_strided')\n",
    "    config.architecture.append('resnetb')\n",
    "    config.architecture.append('resnetb')\n",
    "for i in range(config.num_layers-2):\n",
    "    config.architecture.append('nearest_upsample')\n",
    "    config.architecture.append('unary')\n",
    "config.architecture.append('nearest_upsample')\n",
    "config.architecture.append('last_unary')\n",
    "config.model = KPFCNN(config).to(config.device)\n",
    "\n",
    "# create dataset and dataloader\n",
    "info_train = load_obj(config.train_info)\n",
    "train_set = IndoorDataset(info_train,config,data_augmentation=True)\n",
    "_, neighborhood_limits = get_dataloader(dataset=train_set,\n",
    "                                    batch_size=config.batch_size,\n",
    "                                    shuffle=True,\n",
    "                                    num_workers=config.num_workers,\n",
    "                                    )\n",
    "# load pretrained weights\n",
    "assert config.pretrain != None\n",
    "state = torch.load(config.pretrain)\n",
    "config.model.load_state_dict(state['state_dict'])\n",
    "\n",
    "def PredatorRegistration(src, tgt):\n",
    "    #demo_set = ThreeDMatchDemo(config, config.src_pcd, config.tgt_pcd)\n",
    "    start = time.time()\n",
    "    demo_set = ThreeDMatchDemo(config, src, tgt)\n",
    "    demo_loader, _ = get_dataloader(dataset=demo_set,\n",
    "                                        batch_size=config.batch_size,\n",
    "                                        shuffle=False,\n",
    "                                        num_workers=3,\n",
    "                                        neighborhood_limits=neighborhood_limits)\n",
    "\n",
    "    # do pose estimation\n",
    "    return predict(config, demo_loader), time.time() - start\n",
    "\n",
    "def ICP_registration(src, tgt, threshold = 0.1, trans_init = np.identity(4)):\n",
    "    if isinstance(src, np.ndarray):\n",
    "        source = o3d.geometry.PointCloud()\n",
    "        source.points = o3d.utility.Vector3dVector(src)\n",
    "    if isinstance(tgt, np.ndarray):\n",
    "        target = o3d.geometry.PointCloud()\n",
    "        target.points = o3d.utility.Vector3dVector(tgt)\n",
    "    \n",
    "    start_time = time.time()\n",
    "    \n",
    "    reg_p2p = o3d.registration.registration_icp(\n",
    "        source, target, threshold, trans_init,\n",
    "        o3d.registration.TransformationEstimationPointToPoint(),\n",
    "        o3d.registration.ICPConvergenceCriteria(max_iteration=1000))\n",
    "    #print (reg_p2p)\n",
    "    return reg_p2p.transformation, (time.time() - start_time)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7ac0eb3b",
   "metadata": {},
   "source": [
    "# 2. Functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "159dff9d",
   "metadata": {},
   "outputs": [],
   "source": [
    "to_reg_m = np.array ([\n",
    "    [5.0,0.0,0.0,0.0],\n",
    "    [0.0,5.0,0.0,0.0],\n",
    "    [0.0,0.0,5.0,0.0],\n",
    "    [0.0,0.0,0.0,1.0]\n",
    "])\n",
    "\n",
    "def filter_point_cloud(points, voxel_size=0.015):\n",
    "    pcd = o3d.geometry.PointCloud()\n",
    "    pcd.points = o3d.utility.Vector3dVector(points)\n",
    "    size_before = points.shape[0]\n",
    "    \n",
    "    #np.savetxt('temp1.txt',points)\n",
    "    \n",
    "    filter_pcd = pcd.voxel_down_sample(voxel_size)\n",
    "#     filter_pcd = pcd.uniform_down_sample(every_k_points=3)\n",
    "#     filter_pcd, ind = filter_pcd.remove_statistical_outlier(nb_neighbors=10, std_ratio=1.0)\n",
    "    #filter_pcd, ind = filter_pcd.remove_radius_outlier(nb_points=16, radius=0.03)\n",
    "    \n",
    "    filtered_points = np.asarray(filter_pcd.points)\n",
    "    #np.savetxt('temp2.txt',filtered_points)\n",
    "    \n",
    "    #after_size = filtered_points.shape[0]\n",
    "    #print (f\"{size_before}->{after_size} (-{(size_before-after_size) * 100/size_before:.4f}%)\")\n",
    "    return filtered_points\n",
    "\n",
    "def transform_point_cloud(T, pc):\n",
    "    # PC:N*3 T:4*4\n",
    "    if pc.shape[1] == 3:\n",
    "        new_pc = np.c_[ pc, np.ones(pc.shape[0]) ]\n",
    "        transformed_pc = T.dot(new_pc.T)\n",
    "    else:\n",
    "        print (\"TODO!\")\n",
    "    return transformed_pc[0:3,].T\n",
    "\n",
    "def mpprint(tsfm):\n",
    "    for a in tsfm:\n",
    "        print (*a, end='\\n')\n",
    "\n",
    "def normalize(v):\n",
    "    norm = np.linalg.norm(v)\n",
    "    if norm == 0: \n",
    "       return v\n",
    "    return v / norm\n",
    "\n",
    "def get_Base_Cam_Calibration(tsfm, end2Raw = to_reg_m):\n",
    "    new_cs = np.dot(inv(end2Raw) , inv(tsfm))\n",
    "    #new_cs = inv(tsfm)\n",
    "\n",
    "    # Transformation from Camera to Base\n",
    "    # It should be applied to the @model data directly, and the result is shown with @scanned raw data\n",
    "    #mpprint (new_cs)\n",
    "\n",
    "    # same as the translation in @new_cs\n",
    "    model_original_p = np.array([0,0,0,1]).reshape(4,1)\n",
    "    #print (f'translate value is over here:\\n', np.dot(new_cs, model_original_p))\n",
    "    translate_T = np.dot(new_cs, model_original_p)\n",
    "    \n",
    "    R_Base_Cam = np.array([\n",
    "        normalize(new_cs[0:3, 0:3][0]),\n",
    "        normalize(new_cs[0:3, 0:3][1]),\n",
    "        normalize(new_cs[0:3, 0:3][2])\n",
    "    ])\n",
    "    #print (R_Base_Cam,'\\n', normalize((inv(new_cs[0:3, 0:3]).T)[0]))\n",
    "    #print (R_Base_Cam, '\\n', modelInScanedCS.as_matrix())\n",
    "    R_Base_Cam_4by4 = np.identity(4)\n",
    "    R_Base_Cam_4by4[0:3, 0:3] = R_Base_Cam\n",
    "\n",
    "    Translation_Base_Cam = np.identity(4)\n",
    "    Translation_Base_Cam[0:4,3:4] = translate_T\n",
    "    #print (Translation_Base_Cam)\n",
    "\n",
    "    # T_Base_Cam_Calibration = np.dot(R_Base_Cam_4by4,Translation_Base_Cam)  \n",
    "    T_Base_Cam_Calibration = np.identity(4)\n",
    "    T_Base_Cam_Calibration[0:4,3:4] = translate_T\n",
    "    T_Base_Cam_Calibration[0:3, 0:3] = R_Base_Cam\n",
    "    \n",
    "    return T_Base_Cam_Calibration\n",
    "\n",
    "# calibration_T_TCP_Base = calDHParameters)_\n",
    "def get_Cam_TCP_Calibration(T_Base_Cam_Calibration, calibration_T_TCP_Base):\n",
    "    T_Base_TCP_Calibration = inv(calibration_T_TCP_Base)\n",
    "    T_Cam_Base_Calibration = inv(T_Base_Cam_Calibration)\n",
    "    T_Cam_TCP_Calibration = np.dot(T_Base_TCP_Calibration, T_Cam_Base_Calibration)\n",
    "    #print (T_Cam_TCP_Calibration, T_Cam_TCP_Calibration.shape)\n",
    "\n",
    "    #Cam_TCP_Calibration_R = T_Cam_TCP_Calibration[0:3, 0:3]\n",
    "    #print (1000*T_Cam_TCP_Calibration[0:3, 3:4].reshape(1,3), \n",
    "    #       list(map(np.rad2deg, RotationMatrixToRXYZ(Cam_TCP_Calibration_R))))\n",
    "    return T_Cam_TCP_Calibration\n",
    "\n",
    "def RotationMatrixToRXYZ(matrix):\n",
    "    beta = np.arctan2(-matrix[2,0], np.sqrt(matrix[0,0]*matrix[0,0] + matrix[1,0]*matrix[1,0]))\n",
    "    alpha = np.arctan2(matrix[1,0]/np.cos(beta), matrix[0,0]/np.cos(beta))\n",
    "    r = np.arctan2(matrix[2,1]/np.cos(beta), matrix[2,2]/np.cos(beta))\n",
    "    return r, beta, alpha\n",
    "\n",
    "def reject_outliers(data, m = 1.5):\n",
    "    d = np.abs(data - np.median(data))\n",
    "    mdev = np.median(d)\n",
    "    s = d/mdev if mdev else 0.\n",
    "    return data[s<m]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "ec849130",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(15781, 3)\n"
     ]
    }
   ],
   "source": [
    "ROBOT_TYPE = {\n",
    "    'UR3e': False,\n",
    "    'UR5e': True,\n",
    "    'UR5': False,\n",
    "}\n",
    "\n",
    "if ROBOT_TYPE['UR3e']:\n",
    "    tgt_data = np.loadtxt(r'./demo_test/UR3e_base_m_x5.txt').astype(np.float32)[:,0:3]\n",
    "elif ROBOT_TYPE['UR5e']:\n",
    "    tgt_data = np.loadtxt(r'./demo_test/UR5e_base_m_x5.txt').astype(np.float32)[:,0:3]\n",
    "elif ROBOT_TYPE['UR5']:\n",
    "    tgt_data = np.loadtxt(r'./demo_test/UR5_base_m_x5.txt').astype(np.float32)[:,0:3]\n",
    "    \n",
    "#tgt_data = transform_point_cloud(to_reg_m, tgt_data)\n",
    "#np.savetxt('./assets/UR5e_base.txt',tgt_data,fmt='%1.6f')\n",
    "tgt_data = filter_point_cloud(tgt_data, voxel_size=0.02)\n",
    "print (tgt_data.shape)\n",
    "\n",
    "# n = 1/0.9595599145956509 # 25.4.2023 ur5\n",
    "n = 1/0.96630655934641 # 26.4.2023 ur5e\n",
    "scale = np.array([\n",
    "    [n,0,0,0 ],\n",
    "    [0,n,0,0 ],\n",
    "    [0,0,n,0 ],\n",
    "    [0,0,0,1 ]\n",
    "])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "f7e37d2b",
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "16a6d01b7b484c17b8e92fbb639a3c1a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=6), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[1]src_filename_list length:20\n",
      "./demo_test/ur5e_roi_data/joints_1/roi_000000.txt \n",
      " and more ...\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "dfe08c78ccc2415790272d313e3ff7ce",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=20), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "[2]src_filename_list length:20\n",
      "./demo_test/ur5e_roi_data/joints_2/roi_000000.txt \n",
      " and more ...\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ac6cf3b1760e4b408c514d93abc05e35",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=20), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "[3]src_filename_list length:20\n",
      "./demo_test/ur5e_roi_data/joints_3/roi_000000.txt \n",
      " and more ...\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "80113d10cc284d9bb511397a8854f2c4",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=20), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "[4]src_filename_list length:20\n",
      "./demo_test/ur5e_roi_data/joints_4/roi_000000.txt \n",
      " and more ...\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "9b07189a35724e9bb64c264b0955744f",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=20), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "[5]src_filename_list length:20\n",
      "./demo_test/ur5e_roi_data/joints_5/roi_000000.txt \n",
      " and more ...\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "8beb7cca105140d5bf0fac079763368f",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=20), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "[6]src_filename_list length:20\n",
      "./demo_test/ur5e_roi_data/joints_6/roi_000000.txt \n",
      " and more ...\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "70e09c04647743f7b034e71db3a14f2f",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntProgress(value=0, max=20), HTML(value='')))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "from tqdm.notebook import tqdm\n",
    "for joints_num in tqdm(range(1, 6 +1)):\n",
    "    #joints_num = 11\n",
    "    #src_filename_list = glob.glob('../OpenPCDet/OpenPCDet2/OpenPCDetME/tools/repeatability_object_points2/*.txt')\n",
    "    if ROBOT_TYPE['UR3e']:\n",
    "        src_filename_list = glob.glob(r'./demo_test/ur3e_roi_data/test2.txt')\n",
    "    elif ROBOT_TYPE['UR5e']:\n",
    "        src_filename_list = glob.glob(f'./demo_test/ur5e_roi_data/joints_{joints_num}/roi*.txt')\n",
    "    elif ROBOT_TYPE['UR5']:\n",
    "        src_filename_list = glob.glob(f'./demo_test/ur5_roi_data/joints_{joints_num}/roi*.txt')\n",
    "        #src_filename_list = glob.glob('./demo_test/makeCompletePoints/*.txt')\n",
    "\n",
    "    src_filename_list.sort()\n",
    "    print (f'[{joints_num}]src_filename_list length:{len(src_filename_list)}')\n",
    "    print (src_filename_list[0], '\\n', 'and more ...')\n",
    "    \n",
    "    tsfm_T_list, tsfm_R_list = [], []\n",
    "    for index, src_file in tqdm(enumerate(src_filename_list), total=len(src_filename_list)):\n",
    "        src_data = np.loadtxt(src_file).astype(np.float32)[:,0:3]\n",
    "        src_data = transform_point_cloud(np.dot(to_reg_m, scale), src_data)\n",
    "#         src_data = transform_point_cloud(to_reg_m, src_data)\n",
    "        #src_data = transform_point_cloud(to_reg_m, src_data)\n",
    "        \n",
    "        points_filter = filter_point_cloud(src_data, voxel_size=0.02)\n",
    "        src_data = points_filter\n",
    "\n",
    "        #print (src_data.shape)\n",
    "\n",
    "        #np.savetxt('temp1_src.txt', src_data)\n",
    "        #np.savetxt('temp2_tgt.txt', tgt_data)\n",
    "\n",
    "        tsfm, consuming_time = PredatorRegistration(src_data, tgt_data)\n",
    "        #mpprint(tsfm)\n",
    "        tsfm, consuming_time = ICP_registration(src_data, tgt_data, trans_init=tsfm)\n",
    "        if ROBOT_TYPE['UR3e']:\n",
    "            save_filename = f'./demo_test/ur3e_tsfm/joints_{joints_num}/tsfm_' + '{:06n}'.format(index) + '.txt'\n",
    "        elif ROBOT_TYPE['UR5e']:\n",
    "            save_filename = f'./demo_test/ur5e_tsfm/joints_{joints_num}/tsfm_' + '{:06n}'.format(index) + '.txt'\n",
    "        elif ROBOT_TYPE['UR5']:\n",
    "            save_filename = f'./demo_test/ur5_tsfm/joints_{joints_num}/tsfm_' + '{:06n}'.format(index) + '.txt'\n",
    "        \n",
    "#         if index == 2:\n",
    "#             temp = transform_point_cloud(tsfm, src_data)\n",
    "#             np.savetxt('temp.txt', temp)\n",
    "#             adfasdf\n",
    "#         mpprint(tsfm)\n",
    "#         dfadfadf\n",
    "        #tsfm_T = (tsfm[0:3, 3:4].reshape(1,3)*1000)[0].tolist()\n",
    "        #print (list(map(np.rad2deg, RotationMatrixToRXYZ(Cam_TCP_Calibration_R))))\n",
    "        #tsfm_R = list(map(np.rad2deg, RotationMatrixToRXYZ(tsfm[0:3, 0:3])))\n",
    "        #tsfm_T_list.append([tsfm_T[0], tsfm_T[1], tsfm_T[2]])\n",
    "        #tsfm_R_list.append([tsfm_R[0], tsfm_R[1], tsfm_R[2]])\n",
    "        \n",
    "        np.savetxt(save_filename,tsfm,fmt='%1.6f')\n",
    "        #tsfm_list.append(tsfm)\n",
    "        #break\n",
    "        #if index == 3:\n",
    "#         break\n",
    "    \n",
    "#     tsfm_T_list = np.asarray(tsfm_T_list)\n",
    "#     tsfm_R_list = np.asarray(tsfm_R_list)\n",
    "    \n",
    "    #if joints_num == 2:\n",
    "   #     break\n",
    "#     break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b777b31f",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b804d93c",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "pcr",
   "language": "python",
   "name": "pcr"
  },
  "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}