2Visualize3DScenes.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Intel MKL WARNING: Support of Intel(R) Streaming SIMD Extensions 4.2 (Intel(R) SSE4.2) enabled only processors has been deprecated. Intel oneAPI Math Kernel Library 2025.0 will require Intel(R) Advanced Vector Extensions (Intel(R) AVX) instructions.\n",
      "Intel MKL WARNING: Support of Intel(R) Streaming SIMD Extensions 4.2 (Intel(R) SSE4.2) enabled only processors has been deprecated. Intel oneAPI Math Kernel Library 2025.0 will require Intel(R) Advanced Vector Extensions (Intel(R) AVX) instructions.\n"
     ]
    }
   ],
   "source": [
    "import os \n",
    "import numpy as np\n",
    "import open3d as o3d\n",
    "import pickle as pkl \n",
    "from einops import rearrange\n",
    "from utils.base_utils import  *\n",
    "from collections import OrderedDict\n",
    "from matplotlib import pyplot as plt"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## KITTI-360"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# # Congig of semantic kitti scenes(from monoScene)\n",
    "scene_size = np.array((64., 64, 16))\n",
    "vox_size = np.array((0.25, 0.25,0.25))\n",
    "vox_origin = np.array([0, -32, -2])\n",
    "\n",
    "\n",
    "KITTI360_Stuff= {\n",
    "    # Major\n",
    "    'vegetation',\n",
    "    'terrain',\n",
    "    'ground',\n",
    "    'road', \n",
    "    'sidewalk',\n",
    "    'parking',\n",
    "    'rail track',\n",
    "    'building',\n",
    "    'gate',\n",
    "    'garage', \n",
    "    'bridge',\n",
    "    'tunnel',\n",
    "    # Vehicle\n",
    "    'wall',\n",
    "    'car',\n",
    "    'truck',\n",
    "    'train','caravan',\n",
    "    'bus',\n",
    "    # Minor\n",
    "    'trailer',\n",
    "    'fence',\n",
    "    'guard rail',\n",
    "    'trash bin',\n",
    "    'box',\n",
    "    'lamp',\n",
    "    'smallpole',\n",
    "    'polegroup',\n",
    "    'stop',\n",
    "    'pole',\n",
    "    'traffic sign',\n",
    "    'traffic light'\n",
    "}\n",
    "\n",
    "# H, W = 376, 1408\n",
    "# K = np.array([[552.554261,   0.      , 682.049453],\n",
    "#                  [  0.      , 552.554261, 238.769549],\n",
    "#                  [0., 0., 1.]])\n",
    "# cam_T = np.array((0,0,1.55))\n",
    "# cam_R = create_R((0,5 / 180 * pi,0,))\n",
    "# c2w = create_c2w(cam_R_world=cam_R, cam_T_world=cam_T, cam_type='opencv')\n",
    "# c2w_gl = create_c2w(cam_R_world=cam_R, cam_T_world=cam_T, cam_type='opengl')\n",
    "# w2c = np.linalg.inv(c2w)\n",
    "# rays = get_rays_np(H=H, W=W, K=K, c2w=c2w, cam_type='opencv')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def convert_legacy_layout(layout_path):\n",
    "    with open(layout_path, 'rb') as fp:\n",
    "        layout_raw = pkl.load(fp)\n",
    "    layout = OrderedDict()\n",
    "    \n",
    "    opencv2world = np.array([\n",
    "            [0,0,1,0],\n",
    "            [-1,0,0,0],\n",
    "            [0,-1,0,0],\n",
    "            [0,0,0,1]])\n",
    "    for globalId,obj in layout_raw.items():\n",
    "        # skip dynamic objects\n",
    "\n",
    "        tr = np.eye(4)\n",
    "        tr[:3,:3], tr[:3,3] = obj.R, obj.T\n",
    "        tr = tr @ opencv2world.T\n",
    "\n",
    "        vertices = np.concatenate((obj.vertices, np.ones_like(obj.vertices[:,0:1])), axis=-1)\n",
    "        vertices = vertices @ opencv2world.T\n",
    "\n",
    "        layout[globalId] = OrderedDict({\n",
    "            'R' : tr[:3,:3],\n",
    "            'T' : tr[:3,3],\n",
    "            'vertices' : vertices[:,:3],\n",
    "            'faces' : obj.faces,\n",
    "            'name' : obj.name,\n",
    "            'color' : np.array(name2label[obj.name].color),\n",
    "            'semanticId' : obj.semanticId,\n",
    "            'instanceId' : obj.instanceId,\n",
    "            'annotationId' : obj.annotationId})\n",
    "            \n",
    "    return layout\n",
    "\n",
    "def convert_legacy_stuff_semantic(semantic_voxel_path,semantic_list):\n",
    "    '''\n",
    "    Load old version seamntic voxel\n",
    "    '''\n",
    "    with open(semantic_voxel_path, 'rb') as fp:\n",
    "        stuff_semantic_idx= pkl.load(fp)\n",
    "\n",
    "    point_num = (scene_size / vox_size).astype(int) # X, Y, Z\n",
    "    H, W, D = 64, 64, 64\n",
    "    H_o, W_o, D_o = point_num[2], point_num[1], point_num[0]\n",
    "    stuff_semantic = np.zeros(H * W * D)\n",
    "    for s in semantic_list:\n",
    "        if stuff_semantic_idx[s].shape[0] == 0:\n",
    "            continue\n",
    "        stuff_semantic[stuff_semantic_idx[s]] = name2label[s].id\n",
    "        \n",
    "    semantic_voxel = stuff_semantic.reshape(H, W, D)\n",
    "    semantic_voxel = semantic_voxel.transpose(1, 2, 0) # W D H\n",
    "    semantic_voxel = np.flip(semantic_voxel, axis=0)\n",
    "    semantic_voxel = np.flip(semantic_voxel, axis=2)\n",
    "    semantic_voxel = np.ascontiguousarray(semantic_voxel)\n",
    "\n",
    "    import torch.nn.functional as F\n",
    "\n",
    "    semantic_voxel = F.interpolate(torch.tensor(semantic_voxel, dtype=torch.uint8)[None,None,:,:,:], (W_o, D_o, H_o), mode = 'nearest')[0,0].numpy()\n",
    "\n",
    "\n",
    "    vertices_gridx,  vertices_gridy, vertices_gridz= np.meshgrid(np.linspace(0, scene_size[0], point_num[0]), np.linspace(0, scene_size[1], point_num[1]), np.linspace(0, scene_size[2], point_num[2]), indexing='xy') \n",
    "    loc_voxel = np.concatenate((vertices_gridx[:,:,:,None], vertices_gridy[:,:,:,None], vertices_gridz[:,:,:,None]), axis = -1)\n",
    "    loc_voxel += vox_origin[None,None,None]\n",
    "\n",
    "    return semantic_voxel, loc_voxel\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load Instance Layout \n",
    "data_root = 'data/kitti-360'\n",
    "frame_id = 484\n",
    "legacy_layout_path = os.path.join(data_root, 'layout', '2013_05_28_drive_0000_sync', '%010d.pkl'%frame_id)\n",
    "legacy_semantic_voxel_path = os.path.join(data_root, 'voxel', '2013_05_28_drive_0000_sync', '%010d.pkl'%frame_id)\n",
    "\n",
    "semantic_voxel, loc_voxel = convert_legacy_stuff_semantic(legacy_semantic_voxel_path , KITTI360_Stuff) # H W D \n",
    "layout = convert_legacy_layout(legacy_layout_path)\n",
    "# Save all-in-one Panoptic prior ?\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def vis_camera_o3d(instrinsic, extrinsic, img_size = (256,256),z_revrse = True, vis = False):\n",
    "    frustums = []\n",
    "    camera_size = 1\n",
    "    K = np.eye(4)\n",
    "    K[:3,:3] = instrinsic\n",
    "    W2C = extrinsic\n",
    "    C2W = np.linalg.inv(W2C)\n",
    "    # img_size = (256,256)\n",
    "    frustums.append(get_camera_frustum(img_size, K, W2C, frustum_length=camera_size, color=(0,0,0.)))\n",
    "    # cnt += 1\n",
    "    camera_group = [frustums2lineset(frustums)]\n",
    "    if vis:\n",
    "        o3d.visualization.draw_geometries(camera_group)\n",
    "\n",
    "    return camera_group\n",
    "\n",
    "\n",
    "def costum_visualizer_o3d(geo_group, set_camera = True, instrinsic =None, extrinsic = None, visible= False):\n",
    "    vis = o3d.visualization.Visualizer()\n",
    "    W = 2 * round(instrinsic[0,2] + 0.5)\n",
    "    H = 2 * round(instrinsic[1,2] + 0.5)\n",
    "    vis.create_window(width= W, height= H, visible= visible)\n",
    "    for g in geo_group:\n",
    "        vis.add_geometry(g)\n",
    "    if set_camera:\n",
    "        instrinsic[0,2] = W / 2 - 0.5\n",
    "        instrinsic[1,2] = H / 2 - 0.5\n",
    "        ctr = vis.get_view_control()\n",
    "        cam = ctr.convert_to_pinhole_camera_parameters()\n",
    "        cam.extrinsic = extrinsic\n",
    "        cam.intrinsic.intrinsic_matrix = instrinsic\n",
    "        is_su = ctr.convert_from_pinhole_camera_parameters(cam)\n",
    "\n",
    "        cam_ = vis.get_view_control().convert_to_pinhole_camera_parameters()\n",
    "\n",
    "    rd = vis.get_render_option()\n",
    "    rd.light_on = False\n",
    "    rd.background_color = np.array([0,0,0])\n",
    "\n",
    "    # vis.update_geometry()\n",
    "    vis.poll_events()\n",
    "    vis.update_renderer()\n",
    "    # vis.run()\n",
    "    return vis\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n",
      "\u001b[1;33m[Open3D WARNING] invalid color in PaintUniformColor, clipping to [0, 1]\u001b[0;m\n"
     ]
    }
   ],
   "source": [
    "\n",
    "# Congig of semantic kitti scenes(from monoScene)\n",
    "H, W = 376, 1408\n",
    "K = np.array([[552.554261,   0.      , 682.049453],\n",
    "                 [  0.      , 552.554261, 238.769549],\n",
    "                 [0., 0., 1.]])\n",
    "cam_T = np.array((0,0,1.55))\n",
    "cam_R = create_R((0,5 / 180 * pi,0,))\n",
    "c2w = create_c2w(cam_R_world=cam_R, cam_T_world=cam_T, cam_type='opencv')\n",
    "w2c = np.linalg.inv(c2w)\n",
    "rays = get_rays_np(H=H, W=W, K=K, c2w=c2w, cam_type='opencv')\n",
    "\n",
    "\n",
    "# Load Instance Layout \n",
    "data_root = 'data/kitti-360'\n",
    "frame_id = 7337\n",
    "legacy_layout_path = os.path.join(data_root, 'layout', '2013_05_28_drive_0000_sync', '%010d.pkl'%frame_id)\n",
    "legacy_semantic_voxel_path = os.path.join(data_root, 'voxel', '2013_05_28_drive_0000_sync', '%010d.pkl'%frame_id)\n",
    "image_path = os.path.join(data_root, 'data_2d_raw', '2013_05_28_drive_0000_sync','image_00', '%010d.png'%frame_id)\n",
    "\n",
    "semantic_voxel, loc_voxel = convert_legacy_stuff_semantic(legacy_semantic_voxel_path , KITTI360_Stuff) # H W D \n",
    "layout = convert_legacy_layout(legacy_layout_path)\n",
    "\n",
    "\n",
    "bounds = np.array([64, 0, 32, -32, 14, -2])\n",
    "# bounds = np.array([20, 0, 5, -5, 4, -2])\n",
    "\n",
    "vis_3d = True\n",
    "\n",
    "geo_group = []\n",
    "geo_group += vis_world_bounds_o3d(bounds=bounds)\n",
    "coordinate_world = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1)\n",
    "coordinate_cam = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1).transform(c2w)\n",
    "geo_group += [coordinate_world, coordinate_cam]\n",
    "geo_group += vis_voxel_world_o3d(semantic_voxel, loc_voxel, KITTI360_Stuff, voxelization=True)\n",
    "geo_group += vis_layout_o3d(layout)\n",
    "geo_group += vis_camera_o3d(instrinsic=K, extrinsic=w2c, z_revrse=True)\n",
    "# o3d.visualization.draw_plotly(geo_group)\n",
    "vis = costum_visualizer_o3d(geo_group=geo_group, set_camera=True, instrinsic=K, extrinsic=w2c, visible = True)\n",
    "\n",
    "if vis_3d:\n",
    "    vis.run()\n",
    "else:\n",
    "    project_img = np.asarray(vis.capture_screen_float_buffer())\n",
    "    H_show , W_show = min(project_img.shape[0], H), min(project_img.shape[1], W)\n",
    "    rgb = plt.imread(image_path)\n",
    "    plt.imshow(project_img[:H_show,:W_show] +rgb[:H_show,:W_show])\n",
    "vis.close()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'c2w_gl' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[7], line 7\u001b[0m\n\u001b[1;32m      4\u001b[0m os\u001b[39m.\u001b[39mmakedirs(panoptic_prior_dir, exist_ok\u001b[39m=\u001b[39m\u001b[39mTrue\u001b[39;00m)\n\u001b[1;32m      5\u001b[0m panoptic_prior_path \u001b[39m=\u001b[39m os\u001b[39m.\u001b[39mpath\u001b[39m.\u001b[39mjoin(panoptic_prior_dir, \u001b[39m'\u001b[39m\u001b[39m%010d\u001b[39;00m\u001b[39m.pkl\u001b[39m\u001b[39m'\u001b[39m\u001b[39m%\u001b[39mframe_id)\n\u001b[1;32m      6\u001b[0m panoptic_prior \u001b[39m=\u001b[39m {\n\u001b[0;32m----> 7\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mc2w_opengl\u001b[39m\u001b[39m'\u001b[39m : c2w_gl,\n\u001b[1;32m      8\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mc2w_opencv\u001b[39m\u001b[39m'\u001b[39m : c2w,\n\u001b[1;32m      9\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mK\u001b[39m\u001b[39m'\u001b[39m : K,\n\u001b[1;32m     10\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mHW\u001b[39m\u001b[39m'\u001b[39m: (H, W), \n\u001b[1;32m     11\u001b[0m         \u001b[39m'\u001b[39m\u001b[39msemantic_voxel\u001b[39m\u001b[39m'\u001b[39m: semantic_voxel,\n\u001b[1;32m     12\u001b[0m         \u001b[39m'\u001b[39m\u001b[39minstance_layout\u001b[39m\u001b[39m'\u001b[39m: layout,\n\u001b[1;32m     13\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mvoxel_size\u001b[39m\u001b[39m'\u001b[39m: vox_size ,\n\u001b[1;32m     14\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mvox_origin\u001b[39m\u001b[39m'\u001b[39m: vox_origin,\n\u001b[1;32m     15\u001b[0m         \u001b[39m'\u001b[39m\u001b[39mscene_size\u001b[39m\u001b[39m'\u001b[39m: scene_size}\n\u001b[1;32m     17\u001b[0m \u001b[39mwith\u001b[39;00m \u001b[39mopen\u001b[39m(panoptic_prior_path, \u001b[39m'\u001b[39m\u001b[39mwb\u001b[39m\u001b[39m'\u001b[39m) \u001b[39mas\u001b[39;00m fp:\n\u001b[1;32m     18\u001b[0m     pkl\u001b[39m.\u001b[39mdump(panoptic_prior, fp)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'c2w_gl' is not defined"
     ]
    }
   ],
   "source": [
    "# Save Panoptic Prior\n",
    "panoptic_prior_dir = semantic_voxel_path = os.path.join(data_root, 'panoptic_prior','2013_05_28_drive_0000_sync')\n",
    "\n",
    "os.makedirs(panoptic_prior_dir, exist_ok=True)\n",
    "panoptic_prior_path = os.path.join(panoptic_prior_dir, '%010d.pkl'%frame_id)\n",
    "panoptic_prior = {\n",
    "        'c2w_opengl' : c2w_gl,\n",
    "        'c2w_opencv' : c2w,\n",
    "        'K' : K,\n",
    "        'HW': (H, W), \n",
    "        'semantic_voxel': semantic_voxel,\n",
    "        'instance_layout': layout,\n",
    "        'voxel_size': vox_size ,\n",
    "        'vox_origin': vox_origin,\n",
    "        'scene_size': scene_size}\n",
    "\n",
    "with open(panoptic_prior_path, 'wb') as fp:\n",
    "    pkl.dump(panoptic_prior, fp)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "urbangiraffe",
   "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.8.16"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}