Visualization for SemanticKITTI demo

[Alexander-Yao]

Hi @anhquancao, thanks for your excellent repo.

I met a problem when I was trying to generate the demo for SemanticKITTI dataset. That is, I have generated all .pkl files for validation (sequence 08) set, but I am not sure where is the camera pose trajectory of images in each sequence.

Could you share where can we find the camera matrix? I have tried to find it in https://www.cvlibs.net/datasets/kitti/eval_odometry.php and #16 , but still does not work.

[anhquancao]

Hello @Alexander-Yao,
Thank you for your interest! I didn't use the camera pose. Instead, I generate scenes for all images within the camera/lidar coordinate system throughout the sequence, and then compile them into a video.

[Alexander-Yao]

Thanks for your reply. If it is convenient, could you please share the scripts for generating SemanticKITTI demo?

[anhquancao]

Hello,
Here is the code I used

# from operator import gt
import pickle
import os
import numpy as np
import shutil
import glob
from lib.helper_kitti import draw
from tqdm import tqdm

# input_folder = "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*.pkl"
input_folders = [
    # "/home/acao/jeanzay/scratch/outputs/monoscene/kitti/no_scal_loss/epoch_23/08/*.pkl"
    "/home/acao/jeanzay/scratch/outputs/monoscene/kitti/no_fp/epoch_23/08/*.pkl"
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*000085*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*000465*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*001500*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*001530*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*000790*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*003420*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*000290*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*003190*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*003790*.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_all_baselines/*000010*.pkl"
]
# input_folder = "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti_360/*.pkl"
# outfolder = "images/kitti_no_scal"   
outfolder = "images/kitti_no_fp"    
our_2d_folder = os.path.join(outfolder, "2d")
our_3d_keep_unknown_folder = os.path.join(outfolder, "3d_keep_unknown")
our_3d_remove_unknown_folder = os.path.join(outfolder, "3d_remove_unknown")
our_3d_gt_folder = os.path.join(outfolder, "3d_gt")
os.makedirs(our_2d_folder, exist_ok=True)
os.makedirs(our_3d_keep_unknown_folder, exist_ok=True)
os.makedirs(our_3d_remove_unknown_folder, exist_ok=True)
os.makedirs(our_3d_gt_folder, exist_ok=True)

voxel_size = 0.2
# scans = [
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti/001385_quality=11.9654_nclasses=15_fov13.4812_outfov13.4812.pkl",
    # "/home/anhquancao/jeanzay/scratch/temp/draw_output/kitti/002375_quality=12.2898_nclasses=16_fov14.9898_outfov14.9898.pkl",
# ]
scans = []
for input_folder in input_folders:
    scans += glob.glob(input_folder)
scans = [scans[0]] + scans + [scans[-1]]
scans.sort()


for scan in tqdm(scans):    
    frame_id = os.path.basename(scan)[:6]
    img_save_path = os.path.join(our_2d_folder, "{}.png".format(frame_id))
    # if os.path.exists(img_save_path):
    #     continue
    with open(scan, 'rb') as handle:
        b = pickle.load(handle)
    print(list(b.keys()))
    

    valid_pix = b['fov_mask_1']
    gt_scene = b['target']
    
    T_velo_2_cam = b['T_velo_2_cam']    
    cam_k = b['cam_k']
    vox_origin = np.array([0, -25.6, -2])
    max_depth = 7    
    pred_scenes = {
        "our": b['y_pred'],
        # "lmscnet": b["y_preds"][0],
        # "aicnet": b["y_preds"][1],
        # "sketch": b["y_preds"][2],
        # "js3cnet": b["y_preds"][3]
    }
    
    for view in [1]:        
        gt_save_path = os.path.join(our_3d_gt_folder, frame_id + "_{}_gt.png".format(view))
        if not os.path.isfile(gt_save_path):
            draw(
                gt_scene, gt_save_path, 
                T_velo_2_cam, vox_origin, valid_pix, voxel_size, 
                max_depth=max_depth, view=view, draw_coords=False)
                
        for key in pred_scenes.keys():
            pred_scene = pred_scenes[key]
            folder = our_3d_keep_unknown_folder + "_" + key
            os.makedirs(folder, exist_ok=True)
            
            pred_save_path = os.path.join(folder, frame_id + "_{}_{}.png".format(view, key))
            if not os.path.isfile(pred_save_path):
                draw(
                    pred_scene, pred_save_path, 
                    T_velo_2_cam, vox_origin, 
                    valid_pix, voxel_size, 
                    max_depth=max_depth, view=view)

            pred_scene[(gt_scene == 255)] = 255
            folder = our_3d_remove_unknown_folder + "_" + key
            os.makedirs(folder, exist_ok=True)
            pred_save_path = os.path.join(folder, frame_id + "_{}_{}.png".format(view, key))
            if not os.path.isfile(pred_save_path):
                draw(
                    pred_scene, pred_save_path, 
                    T_velo_2_cam, vox_origin, 
                    valid_pix, voxel_size, 
                    max_depth=max_depth, view=view)
        
    
    if not os.path.exists(img_save_path):            
        source_path = os.path.join("/home/acao/jeanzay/work/data/semantic_kitti/dataset/sequences/08/image_2/{}.png".format(frame_id))
        shutil.copyfile(source_path, img_save_path)

And file helper_kitti.py

import numpy as np
from mayavi import mlab
from lib.view import first_view
try:
    engine = mayavi.engine
except NameError:
    from mayavi.api import Engine
    engine = Engine()
    engine.start()


def position_scene_view(scene, view=1):
    scene.x_minus_view()
    if view == 2:
        first_view(engine)
        return
    # scene.show_axes = True        
    if view == 1:
        
        scene.x_minus_view()
        # print(scene.light_manager)
        scene.camera.position = [-97.18838148891899, 25.27090203470396, 70.00664831457846]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.4823255045744394, 0.0016575841503473478, 0.8759905022611815]
        scene.camera.clipping_range = [91.72640178375556, 201.4917697448874]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-75.87800135232737, 25.3280180886833, 58.272920037629824]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.4823255045744394, 0.0016575841503473478, 0.8759905022611815]
        scene.camera.clipping_range = [67.64240697767127, 176.79959325178078]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-58.2661169419211, 25.375221439079453, 48.57562394097806]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.4823255045744394, 0.0016575841503473478, 0.8759905022611815]
        scene.camera.clipping_range = [47.73827903875866, 156.39283581946125]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-43.71084056968449, 25.4142324724647, 40.56132964622454]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.4823255045744394, 0.0016575841503473478, 0.8759905022611815]
        scene.camera.clipping_range = [31.288586527260613, 139.52774703242034]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-36.95458640117663, 25.633876158900584, 50.84787002664494]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.6123346958955982, 0.0018207963729262866, 0.7905965500196758]
        scene.camera.clipping_range = [35.115277828333944, 134.71073109815393]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-50.091049398026556, 25.64099029966687, 61.022337801286746]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.6123346958955982, 0.0018207963729262866, 0.7905965500196758]
        scene.camera.clipping_range = [51.5649703398319, 151.57581988519473]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-50.091049398026556, 25.64099029966687, 61.022337801286746]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.6123346958955982, 0.0018207963729262866, 0.7905965500196758]
        scene.camera.clipping_range = [51.5649703398319, 151.57581988519473]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-65.98616962421498, 25.649598409994077, 73.33344380860332]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.6123346958955982, 0.0018207963729262866, 0.7905965500196758]
        scene.camera.clipping_range = [71.81160329572957, 172.98126769123564]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-85.21926509790298, 25.66001422349, 88.22988207745638]
        scene.camera.focal_point = [25.59999929810874, 25.59999929810874, 2.398023480731615]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.6123346958955982, 0.0018207963729262866, 0.7905965500196758]
        scene.camera.clipping_range = [95.89559810181375, 197.67344418434215]
        scene.camera.compute_view_plane_normal()
        scene.render()

        return
    if view == 0:             
        scene.camera.position = [-65.43857073323758, 24.753878146005814, 108.42343867174978]
        scene.camera.focal_point = [25.59999929810874, 25.79999929666519, 2.285443199792602]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.7589366237074909, 0.010794522938871592, 0.6510750183120196]
        scene.camera.clipping_range = [100.19838934306762, 190.07116454286256]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-52.54298986806082, -34.28785291000525, 101.46968059155097]
        scene.camera.focal_point = [25.59999929810874, 25.79999929666519, 2.285443199792602]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.5364746523729315, 0.4649048133903089, 0.7043141783663485]
        scene.camera.clipping_range = [83.8648190186262, 210.6317643733783]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-36.65172241012593, -43.538805847052316, 108.38530817500168]
        scene.camera.focal_point = [41.49126675604364, 16.549046359618096, 9.201070783243155]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.5364746523729315, 0.4649048133903089, 0.7043141783663485]
        scene.camera.clipping_range = [83.8648190186262, 210.6317643733783]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-59.10631300492522, -54.139364315463865, 75.81687392398277]
        scene.camera.focal_point = [41.49126675604364, 16.549046359618096, 9.201070783243155]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.35065773056859567, 0.3277209568592174, 0.8772902201824442]
        scene.camera.clipping_range = [70.06621964756395, 219.73053946183586]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-59.500420087235426, -51.76186910177997, 77.74457175755188]
        scene.camera.focal_point = [41.09715967373343, 18.926541573301957, 11.128768616812202]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.35065773056859567, 0.3277209568592174, 0.8772902201824442]
        scene.camera.clipping_range = [70.06621964756395, 219.73053946183586]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-62.362743197809486, -47.826697636611, 77.41936552477684]
        scene.camera.focal_point = [41.09715967373343, 18.926541573301957, 11.128768616812202]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.3494789592153525, 0.329400117106326, 0.8771317004396162]
        scene.camera.clipping_range = [69.93337130746471, 218.69877544655384]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-54.87400301970664, -46.602336617531776, 88.90981862219175]
        scene.camera.focal_point = [41.09715967373343, 18.926541573301957, 11.128768616812202]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.4202177534009102, 0.3687229081554059, 0.8291323517558049]
        scene.camera.clipping_range = [74.11032473077172, 216.74786657006638]
        scene.camera.compute_view_plane_normal()
        scene.render()
        scene.camera.position = [-55.28311545209242, -47.57870008101362, 87.58246448812248]
        scene.camera.focal_point = [40.688047241347654, 17.950178109820115, 9.801414482742981]
        scene.camera.view_angle = 30.0
        scene.camera.view_up = [0.4202177534009102, 0.3687229081554059, 0.8291323517558049]
        scene.camera.clipping_range = [74.11032473077172, 216.74786657006638]
        scene.camera.compute_view_plane_normal()
        scene.render()
        return
    
  
    return


def get_grid_coords(dims, resolution):
  '''
  :param dims: the dimensions of the grid [x, y, z] (i.e. [256, 256, 32])
  :return coords_grid: is the center coords of voxels in the grid
  '''

  # The sensor in centered in X (we go to dims/2 + 1 for the histogramdd)
  g_xx = np.arange(0, dims[0] + 1)
  # The sensor is in Y=0 (we go to dims + 1 for the histogramdd)
  g_yy = np.arange(0, dims[1] + 1)
  # The sensor is in Z=1.73. I observed that the ground was to voxel levels above the grid bottom, so Z pose is at 10
  # if bottom voxel is 0. If we want the sensor to be at (0, 0, 0), then the bottom in z is -10, top is 22
  # (we go to 22 + 1 for the histogramdd)
  # ATTENTION.. Is 11 for old grids.. 10 for new grids (v1.1) (https://github.com/PRBonn/semantic-kitti-api/issues/49)
  sensor_pose = 10
  g_zz = np.arange(0, dims[2] + 1)

  # Obtaining the grid with coords...
  xx, yy, zz = np.meshgrid(g_xx[:-1], g_yy[:-1], g_zz[:-1])
  coords_grid = np.array([xx.flatten(), yy.flatten(), zz.flatten()]).T
  coords_grid = coords_grid.astype(np.float)

  coords_grid = (coords_grid * resolution) + resolution/2

  temp = np.copy(coords_grid)
  temp[:, 0] = coords_grid[:, 1]
  temp[:, 1] = coords_grid[:, 0]
  coords_grid = np.copy(temp)

  return coords_grid, g_xx, g_yy, g_zz

def draw_tsdf(scans, voxel_size=0.08):
    figure = mlab.figure(size = (1400,1400),\
                    bgcolor = (1,1,1))
    t = 0
    for voxels in scans:
        t += 1
        grid_coords, _, _, _ = get_grid_coords([voxels.shape[0], voxels.shape[2], voxels.shape[1]], voxel_size)    
        grid_coords = np.vstack((grid_coords.T, np.moveaxis(voxels, [0, 1, 2], [0, 2, 1]).reshape(-1))).T
    
        # Obtaining voxels with semantic class
        # occupied_voxels = grid_coords[((grid_coords[:, 3]) < 0.0) & ((grid_coords[:, 3]) > -10) ]
        # occupied_voxels = grid_coords[((grid_coords[:, 3]) > 0)  ]
        occupied_voxels = grid_coords[((grid_coords[:, 3]) == 1)  ]
        # occupied_voxels = grid_coords
        opa = 1.0
        if t == 2:
            opa = 1.0

        plt_plot = mlab.points3d(
            occupied_voxels[:, 0], occupied_voxels[:, 1], occupied_voxels[:, 2], 
            occupied_voxels[:, 3],
            colormap='magma', scale_factor=0.075,
            mode='cube', opacity=opa,
            vmin=-1, vmax=0)

        plt_plot.glyph.scale_mode = 'scale_by_vector'

    # mlab.show()
    mlab.savefig(out_filepath, figure=figure)
    mlab.clf()

def draw_camcoords(
    voxels, out_filepath, T_velo_2_cam, vox_origin, valid_pix, 
    voxel_size=0.8, max_depth=51.2, 
    img_size=(1220, 370),
    f=707.0912,
    show=False,
    view=1):    
    # if not show:
    #     mlab.options.offscreen = True
    d = max_depth
    x = d * img_size[0]/ (2 * f)
    y = d * img_size[1]/ (2 * f)    
    tri_points = np.array([
        [0, 0, 0],
        [d, x, y],
        [d, -x, y],
        [d, -x, -y],
        [d, x, -y],
    ])

    x = tri_points[:, 0] - vox_origin[0]
    y = tri_points[:, 1] - vox_origin[1]
    z = tri_points[:, 2] - vox_origin[2]
    triangles = [
        (0, 1, 2),
        (0, 1, 4),
        (0, 3, 4),
        (0, 2, 3),
    ]
    down_scale = 1.0
    voxel_size = voxel_size / down_scale
    x /= down_scale
    y /= down_scale
    z /= down_scale


    grid_coords, _, _, _ = get_grid_coords([voxels.shape[0], voxels.shape[1], voxels.shape[2]], voxel_size)    

    # print(grid_coords.shape)
    grid_coords = np.vstack([grid_coords.T, voxels.reshape(-1)]).T
    # print(grid_coords.shape, valid_pix.shape)
    fov_grid_coords = grid_coords[valid_pix, :]
    outfov_grid_coords = grid_coords[~valid_pix, :]
    
    # Obtaining voxels with semantic class
    fov_voxels = fov_grid_coords[(fov_grid_coords[:, 3] > 0) & (fov_grid_coords[:, 3] < 255)]
    outfov_voxels = outfov_grid_coords[(outfov_grid_coords[:, 3] > 0) & (outfov_grid_coords[:, 3] < 255)]
    figure = mlab.figure(size = (1400,1400), bgcolor = (1,1,1), engine=engine)

    # mlab.axes(xlabel='', ylabel='', zlabel='')

    mlab.triangular_mesh(x, y, z, triangles, representation='wireframe', color=(0, 0, 0), line_width=5)

    plt_plot_fov = mlab.points3d(fov_voxels[:, 0], fov_voxels[:, 1], fov_voxels[:, 2], fov_voxels[:, 3],
                            colormap='viridis', scale_factor=voxel_size - 0.05 * voxel_size, mode='cube', opacity=1.,
                            vmin=1, vmax=19)

    plt_plot_outfov = mlab.points3d(outfov_voxels[:, 0], outfov_voxels[:, 1], outfov_voxels[:, 2], outfov_voxels[:, 3],
                            colormap='viridis', scale_factor=voxel_size - 0.05 * voxel_size, mode='cube', opacity=1.,
                            vmin=1, vmax=19)
    if view == 0:
        position_scene_view(figure.scene, view)
    else:
        first_view(engine)

    colors = np.array([
        # [0  , 0  , 0, 255],
        [100, 150, 245, 255],
        [100, 230, 245, 255],
        [30, 60, 150, 255],
        [80, 30, 180, 255],
        [100, 80, 250, 255],
        [255, 30, 30, 255],
        [255, 40, 200, 255],
        [150, 30, 90, 255],
        [255, 0, 255, 255],
        [255, 150, 255, 255],
        [75, 0, 75, 255],
        [175, 0, 75, 255],
        [255, 200, 0, 255],
        [255, 120, 50, 255],
        [0, 175, 0, 255],
        [135, 60, 0, 255],
        [150, 240, 80, 255],
        [255, 240, 150, 255],
        [255, 0, 0, 255]]).astype(np.uint8)

    plt_plot_fov.glyph.scale_mode = 'scale_by_vector'
    plt_plot_outfov.glyph.scale_mode = 'scale_by_vector'

    plt_plot_fov.module_manager.scalar_lut_manager.lut.table = colors
    glyph1 = engine.scenes[0].children[2].children[0].children[0]

    # glyph1 = figure.scene.children[2].children[0].children[0]
    glyph1.actor.mapper.resolve_coincident_topology = 'off'
    
    outfov_colors = colors
    outfov_colors[:, :3] = outfov_colors[:, :3] // 3 * 2
    # print(outfov_colors)
    plt_plot_outfov.module_manager.scalar_lut_manager.lut.table = outfov_colors

    # if show:
    #     mlab.show()
    # else:
    mlab.savefig(out_filepath, figure=figure)
    mlab.close()

def draw(
    voxels, out_filepath, T_velo_2_cam, vox_origin, valid_pix, 
    voxel_size=0.8, max_depth=51.2, 
    img_size=(1220, 370),
    f=707.0912,
    draw_coords=False,
    show=False,
    view=1):    
    # mlab.options.offscreen = True
    # if not show:
    #     mlab.options.offscreen = True
    d = max_depth
    x = d * img_size[0]/ (2 * f)
    y = d * img_size[1]/ (2 * f)    
    tri_points = np.array([
        [0, 0, 0],
        [x, y, d],
        [-x, y, d],
        [-x, -y, d],
        [x, -y, d],
    ])
    tri_points = np.hstack([tri_points, np.ones((5, 1))])
    tri_points = (np.linalg.inv(T_velo_2_cam) @ tri_points.T).T
    x = tri_points[:, 0] - vox_origin[0]
    y = tri_points[:, 1] - vox_origin[1]
    z = tri_points[:, 2] - vox_origin[2]
    triangles = [
        (0, 1, 2),
        (0, 1, 4),
        (0, 3, 4),
        (0, 2, 3),
    ]
    down_scale = 1.0
    voxel_size = voxel_size / down_scale
    x /= down_scale
    y /= down_scale
    z /= down_scale


    grid_coords, _, _, _ = get_grid_coords([voxels.shape[0], voxels.shape[1], voxels.shape[2]], voxel_size)    

    # print(grid_coords.shape)
    grid_coords = np.vstack([grid_coords.T, voxels.reshape(-1)]).T
    # print(grid_coords.shape, valid_pix.shape)
    fov_grid_coords = grid_coords[valid_pix, :]
    outfov_grid_coords = grid_coords[~valid_pix, :]
    
    # Obtaining voxels with semantic class
    fov_voxels = fov_grid_coords[(fov_grid_coords[:, 3] > 0) & (fov_grid_coords[:, 3] < 255)]
    outfov_voxels = outfov_grid_coords[(outfov_grid_coords[:, 3] > 0) & (outfov_grid_coords[:, 3] < 255)]
    figure = mlab.figure(size = (1400,1400), bgcolor = (1,1,1), engine=engine)
    

    

    mlab.triangular_mesh(x, y, z, triangles, representation='wireframe', color=(0, 0, 0), line_width=5)

    plt_plot_fov = mlab.points3d(fov_voxels[:, 0], fov_voxels[:, 1], fov_voxels[:, 2], fov_voxels[:, 3],
                            colormap='viridis', scale_factor=voxel_size - 0.05 * voxel_size, mode='cube', opacity=1.,
                            vmin=1, vmax=19)

    plt_plot_outfov = mlab.points3d(outfov_voxels[:, 0], outfov_voxels[:, 1], outfov_voxels[:, 2], outfov_voxels[:, 3],
                            colormap='viridis', scale_factor=voxel_size - 0.05 * voxel_size, mode='cube', opacity=1.,
                            vmin=1, vmax=19)

    

    colors = np.array([
        # [0  , 0  , 0, 255],
        [100, 150, 245, 255],
        [100, 230, 245, 255],
        [30, 60, 150, 255],
        [80, 30, 180, 255],
        [100, 80, 250, 255],
        [255, 30, 30, 255],
        [255, 40, 200, 255],
        [150, 30, 90, 255],
        [255, 0, 255, 255],
        [255, 150, 255, 255],
        [75, 0, 75, 255],
        [175, 0, 75, 255],
        [255, 200, 0, 255],
        [255, 120, 50, 255],
        [0, 175, 0, 255],
        [135, 60, 0, 255],
        [150, 240, 80, 255],
        [255, 240, 150, 255],
        [255, 0, 0, 255]]).astype(np.uint8)

    plt_plot_fov.glyph.scale_mode = 'scale_by_vector'
    plt_plot_outfov.glyph.scale_mode = 'scale_by_vector'

    plt_plot_fov.module_manager.scalar_lut_manager.lut.table = colors
    glyph1 = engine.scenes[0].children[2].children[0].children[0]

    # glyph1 = figure.scene.children[2].children[0].children[0]
    glyph1.actor.mapper.resolve_coincident_topology = 'off'
    # if draw_coords:
    #     mlab.axes(xlabel='', ylabel='', zlabel='')
    # else:
    #     figure.scene.show_axes = False

    outfov_colors = colors
    outfov_colors[:, :3] = outfov_colors[:, :3] // 3 * 2
    # print(outfov_colors)
    plt_plot_outfov.module_manager.scalar_lut_manager.lut.table = outfov_colors
    position_scene_view(figure.scene, view)
    # if show:
    #     mlab.show()
    # else:    
    mlab.savefig(out_filepath, figure=figure)    
    mlab.close()


def draw_tsdf(voxels, voxel_size=0.2):
    grid_coords, _, _, _ = get_grid_coords([voxels.shape[0], voxels.shape[1], voxels.shape[2]], voxel_size)    
    print(grid_coords.shape)
    # print(grid_coords.shape)
    grid_coords = np.vstack([grid_coords.T, voxels.reshape(-1)]).T
    # grid_coords = np.vstack((grid_coords.T, voxels.reshape(-1))).T
    # print(grid_coords.shape)

    # Obtaining voxels with semantic class
    occupied_voxels = grid_coords[grid_coords[:, 3] > 0]
    figure = mlab.figure(size = (1400,1000),\
                bgcolor = (1,1,1))

    plt_plot = mlab.points3d(occupied_voxels[:, 0], occupied_voxels[:, 1], occupied_voxels[:, 2], occupied_voxels[:, 3],
                            colormap='Reds', scale_factor=voxel_size - 0.03 * voxel_size, mode='cube', opacity=1.)
                            # vmin=-1, vmax=0)

    # position_scene_view(figure.scene)

    plt_plot.glyph.scale_mode = 'scale_by_vector'

    mlab.show()
    # mlab.clf()

def draw_probs(voxels, voxel_size=0.08):
    grid_coords, _, _, _ = get_grid_coords([voxels.shape[0], voxels.shape[2], voxels.shape[1]], voxel_size)

    grid_coords = np.vstack((grid_coords.T, np.moveaxis(voxels, [0, 1, 2], [0, 2, 1]).reshape(-1))).T

    # Obtaining voxels with semantic class
    occupied_voxels = grid_coords[(grid_coords[:, 3] > 0) & (grid_coords[:, 3] < 255)]
    figure = mlab.figure(size = (1400,1400),\
                bgcolor = (1,1,1))

    plt_plot = mlab.points3d(occupied_voxels[:, 0], occupied_voxels[:, 1], occupied_voxels[:, 2], occupied_voxels[:, 3],
                            colormap='Reds', 
                            scale_factor=voxel_size - 0.03 * voxel_size, mode='cube', opacity=0.2, 
                            vmin=0, vmax=1)

    plt_plot.glyph.scale_mode = 'scale_by_vector'

    

    # mlab.show()
    mlab.savefig(out_filepath, figure=figure)
    mlab.clf()

[Alexander-Yao]

Many thanks! Visualization issue has been sovled.