pcd_rgb.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "# 3D Drivable Map Generation"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import os\n",
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import cv2 as cv\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# switch to cpu in case of error\n",
    "# from open3d.cuda.pybind.utility import Vector3dVector\n",
    "from open3d.cuda.pybind.utility import Vector3dVector\n",
    "\n",
    "from tqdm import tqdm\n",
    "from scipy.spatial.transform import Rotation as R"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "## Read Image, PointCloud, Pose Data"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "outputs": [],
   "source": [
    "# sequence 00\n",
    "\"\"\"\n",
    "velodyne_path = \"/media/hty/T7/Kitti/lidar/sequences/00/pcd/\"\n",
    "velodyne_seg_path = \"/media/hty/T7/Kitti/lidar/sequences/00/pcd_seg/\"\n",
    "camera_path = \"/media/hty/T7/Kitti/dataset/sequences/00/image_seg/seg/\"\n",
    "pcd_paths = os.listdir(velodyne_path)\n",
    "pcd_paths.sort()\n",
    "png_paths = os.listdir(camera_path)\n",
    "png_paths.sort()\n",
    "\n",
    "with open('samples/poses/00.txt') as f:\n",
    "    pose_txt = f.readlines()\n",
    "\n",
    "poses = []\n",
    "pose_index =  []\n",
    "for j in range(0,1000):\n",
    "    pose = np.array([float(i) for i in pose_txt[j].strip('\\n').split(' ')]).reshape(3,4)\n",
    "    pose_index.append(j)\n",
    "    poses.append(pose)\n",
    "\n",
    "with open(\"samples/calib/00/calib.txt\") as f:\n",
    "    iPx = f.readlines()\n",
    "\"\"\""
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "outputs": [],
   "source": [
    "# sequence 09\n",
    "velodyne_path = \"/media/hty/T7/Kitti/lidar/sequences/00/pcd/\"\n",
    "velodyne_seg_path = \"/media/hty/T7/Kitti/lidar/sequences/09/pcd_seg/\"\n",
    "camera_path = \"/media/hty/T7/Kitti/dataset/sequences/09/image_seg/seg/\"\n",
    "pcd_paths = os.listdir(velodyne_seg_path)\n",
    "pcd_paths.sort()\n",
    "png_paths = os.listdir(camera_path)\n",
    "png_paths.sort()\n",
    "\n",
    "with open('samples/poses/09.txt') as f:\n",
    "    pose_txt = f.readlines()\n",
    "\n",
    "poses = []\n",
    "pose_index =  []\n",
    "for j in range(0,790):\n",
    "    pose = np.array([float(i) for i in pose_txt[j].strip('\\n').split(' ')]).reshape(3,4)\n",
    "    pose_index.append(j)\n",
    "    poses.append(pose)\n",
    "\n",
    "with open(\"samples/calib/09/calib.txt\") as f:\n",
    "    iPx = f.readlines()\n"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "outputs": [],
   "source": [
    "# homography matrix\n",
    "iP3 = np.array([float(i) for i in iPx[3].strip('\\n').split(' ')[1:]]).reshape(3,4)\n",
    "iP2 = np.array([float(i) for i in iPx[2].strip('\\n').split(' ')[1:]]).reshape(3,4)\n",
    "\n",
    "# transformation matrix\n",
    "Tr = np.array([float(i) for i in iPx[4].strip('\\n').split(' ')[1:]]).reshape(4,3)\n",
    "\n",
    "R0_rect = np.array([[1, 0.009791,  -0.0029253,0],\n",
    "                    [-0.00980, 0.999938, -0.0052387,0],\n",
    "                    [0.002873828, 0.005267, 0.999982,0],\n",
    "                    [0,0,0,1]])\n",
    "\n",
    "Tr_velo_to_cam = np.array([[0.0077554, -0.9999694, -0.001014303, -0.007275538],\n",
    "                           [0.002294056, 0.001032122, -0.9999968, -0.06324057],\n",
    "                           [0.9999673, 0.007753097,  0.00230199, -0.2670414],\n",
    "                           [0,0,0,1]])"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "# colorize point cloud with images"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "for i in tqdm(range(4500)):\n",
    "    pcd = o3d.io.read_point_cloud(kitti_dir + velodyne_path + pcd_paths[i])\n",
    "    pcd.paint_uniform_color([0, 0, 0])\n",
    "    pts = np.asarray(pcd.points)\n",
    "    pts = np.hstack([pts,np.ones((len(pts),1))])\n",
    "    clrs = np.asarray(pcd.colors)\n",
    "    image = cv.imread(kitti_dir + camera_path + png_paths[i])\n",
    "    image = cv.cvtColor(image, cv.COLOR_BGR2RGB)\n",
    "    h, w, c = image.shape\n",
    "    trans = iP2 @  R0_rect @ Tr_velo_to_cam\n",
    "    pixel = pts@trans.T\n",
    "    for idx, pix in enumerate(pixel) :\n",
    "        px = int(pix[1]/pix[2])\n",
    "        py = int(pix[0]/pix[2])\n",
    "        if px > 0 and py > 0 and px < h and py < w and pix[2] > 0:\n",
    "            clrs[idx] = image[px][py] / 255.0\n",
    "    pcd.colors = Vector3dVector(clrs)\n",
    "    o3d.io.write_point_cloud(kitti_dir + velodyne_path + pcd_paths[i], pcd)\n",
    "    # o3d.visualization.draw(pcd)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 1590/1590 [04:30<00:00,  5.88it/s]\n"
     ]
    }
   ],
   "source": [
    "# colorization\n",
    "output_path = \"/Volumes/T7/Kitti/lidar/sequences/09/pcd_seg/\"\n",
    "\n",
    "for i in tqdm(range(1590)):\n",
    "    pcd = o3d.io.read_point_cloud(velodyne_path + pcd_paths[i])\n",
    "    pcd.paint_uniform_color([0, 0, 0])\n",
    "    pts = np.asarray(pcd.points)\n",
    "    pts = np.hstack([pts,np.ones((len(pts),1))])\n",
    "    clrs = np.asarray(pcd.colors)\n",
    "    image = cv.imread(camera_path + str(i) +\"_seg.jpg\")\n",
    "    image = cv.resize(image,(1241, 376))\n",
    "    image = cv.cvtColor(image, cv.COLOR_BGR2RGB)\n",
    "    h, w, c = image.shape\n",
    "\n",
    "    trans = iP2 @  R0_rect @ Tr_velo_to_cam\n",
    "    pixel = pts@trans.T\n",
    "    for idx, pix in enumerate(pixel) :\n",
    "        px = int(pix[1]/pix[2])\n",
    "        py = int(pix[0]/pix[2])\n",
    "        if px > 0 and py > 0 and px < h and py < w and pix[2] > 0:\n",
    "            clrs[idx] = image[px][py] / 255.0\n",
    "    pcd.colors = Vector3dVector(clrs)\n",
    "    o3d.io.write_point_cloud(output_path + pcd_paths[i], pcd)\n",
    "    # o3d.visualization.draw(pcd)\n"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "# Visualize colored point clouds"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 790/790 [00:26<00:00, 29.97it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[Open3D INFO] Window window_0 created.\n",
      "[Open3D INFO] EGL headless mode enabled.\n",
      "[Open3D INFO] ICE servers: {\"stun:stun.l.google.com:19302\", \"turn:user:password@34.69.27.100:3478\", \"turn:user:password@34.69.27.100:3478?transport=tcp\"}\n",
      "[Open3D INFO] Set WEBRTC_STUN_SERVER environment variable add a customized WebRTC STUN server.\n",
      "[Open3D INFO] WebRTC Jupyter handshake mode enabled.\n"
     ]
    },
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
      "\u001B[0;31mKeyboardInterrupt\u001B[0m                         Traceback (most recent call last)",
      "Cell \u001B[0;32mIn[14], line 30\u001B[0m\n\u001B[1;32m     28\u001B[0m     vis\u001B[38;5;241m.\u001B[39mupdate_renderer()\n\u001B[1;32m     29\u001B[0m vis\u001B[38;5;241m.\u001B[39mdestroy_window()\n\u001B[0;32m---> 30\u001B[0m \u001B[43mo3d\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mvisualization\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mdraw\u001B[49m\u001B[43m(\u001B[49m\u001B[43mbase\u001B[49m\u001B[43m)\u001B[49m\n",
      "File \u001B[0;32m~/PycharmProjects/HybridNets/venv/lib/python3.8/site-packages/open3d/visualization/draw.py:127\u001B[0m, in \u001B[0;36mdraw\u001B[0;34m(geometry, title, width, height, actions, lookat, eye, up, field_of_view, bg_color, bg_image, ibl, ibl_intensity, show_skybox, show_ui, raw_mode, point_size, line_width, animation_time_step, animation_duration, rpc_interface, on_init, on_animation_frame, on_animation_tick, non_blocking_and_return_uid)\u001B[0m\n\u001B[1;32m    125\u001B[0m     \u001B[38;5;28;01mreturn\u001B[39;00m w\u001B[38;5;241m.\u001B[39muid\n\u001B[1;32m    126\u001B[0m \u001B[38;5;28;01melse\u001B[39;00m:\n\u001B[0;32m--> 127\u001B[0m     \u001B[43mgui\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mApplication\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43minstance\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mrun\u001B[49m\u001B[43m(\u001B[49m\u001B[43m)\u001B[49m\n",
      "\u001B[0;31mKeyboardInterrupt\u001B[0m: "
     ]
    }
   ],
   "source": [
    "# Visualization\n",
    "\n",
    "base = o3d.io.read_point_cloud(velodyne_seg_path + pcd_paths[0])\n",
    "base.transform(np.vstack((poses[0],np.array([0,0,0,1]))))\n",
    "filter = np.mean(np.asarray(base.colors) != (0,0,0), axis = 1,dtype=bool)\n",
    "base.points = Vector3dVector(np.asarray(base.points)[filter])\n",
    "base.colors = Vector3dVector(np.asarray(base.colors)[filter])\n",
    "\n",
    "vis = o3d.visualization.Visualizer()\n",
    "vis.create_window()\n",
    "vis.add_geometry(base)\n",
    "\n",
    "for i in tqdm(range(len(poses[0:1000]))):\n",
    "    idx = pose_index[i]\n",
    "    pcd = o3d.io.read_point_cloud(velodyne_seg_path+ pcd_paths[idx])\n",
    "    filter = np.mean(np.asarray(pcd.colors) != (0,0,0), axis = 1,dtype=bool)\n",
    "    pcd.points = Vector3dVector(np.asarray(pcd.points)[filter])\n",
    "    pcd.colors = Vector3dVector(np.asarray(pcd.colors)[filter])\n",
    "    r = R.from_matrix(poses[i][0:3,0:3])\n",
    "    ax, ay, az = r.as_euler('xyz', degrees=False)\n",
    "    r = o3d.geometry.get_rotation_matrix_from_xyz(np.array([0,0,-ay]))\n",
    "    T = np.hstack((r, np.array([poses[i][2][3],-poses[i][0][3],-poses[i][1][3]]).reshape((3,1))))\n",
    "    pcd = pcd.transform(np.vstack((T,np.array([0,0,0,1]))))\n",
    "    base += pcd.voxel_down_sample(voxel_size=1)\n",
    "    # base.voxel_down_sample(voxel_size=1)\n",
    "    vis.update_geometry(base)\n",
    "    vis.poll_events()\n",
    "    vis.update_renderer()\n",
    "vis.destroy_window()\n",
    "# o3d.visualization.draw(base)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}