Python mayavi.mlab.points3d() Examples

def draw_lidar_simple(pc, color=None):
    ''' Draw lidar points. simplest set up. '''
    fig = mlab.figure(figure=None, bgcolor=(0,0,0), fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    #draw points
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=None, mode='point', colormap = 'gnuplot', scale_factor=1, figure=fig)
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig 

def plot(self, points=None):

        tube_rad = max(self.lattice.lengths) / 100.0

        frame, line1, line2, line3 = self.lattice.get_path()
        for i, j, k in [[frame[:, 0], frame[:, 1], frame[:, 2]],
                        [line1[:, 0], line1[:, 1], line1[:, 2]],
                        [line2[:, 0], line2[:, 1], line2[:, 2]],
                        [line3[:, 0], line3[:, 1], line3[:, 2]]]:
            mlab.plot3d(i, j, k, tube_radius=tube_rad, color=(1, 1, 1), tube_sides=24, transparent=True, opacity=0.5)

        if points is not None:
            ip = np.array(points)
            mlab.points3d(ip[:, 0], ip[:, 1], ip[:, 2], tube_rad * np.ones(len(ip)), scale_factor=1)

        return mlab.pipeline 

def viz(pc, centers, corners_3d, pc_origin):
	import mayavi.mlab as mlab
	fig = mlab.figure(figure=None, bgcolor=(0.4, 0.4, 0.4),
	                  fgcolor=None, engine=None, size=(500, 500))
	mlab.points3d(pc[:, 0], pc[:, 1], pc[:, 2], mode='sphere',
	              colormap='gnuplot', scale_factor=0.1, figure=fig)
	mlab.points3d(centers[:, 0], centers[:, 1], centers[:, 2], mode='sphere',
	              color=(1, 0, 1), scale_factor=0.3, figure=fig)
	mlab.points3d(corners_3d[:, 0], corners_3d[:, 1], corners_3d[:, 2], mode='sphere',
	              color=(1, 1, 0), scale_factor=0.3, figure=fig)
	mlab.points3d(pc_origin[:, 0], pc_origin[:, 1], pc_origin[:, 2], mode='sphere',
	              color=(0, 1, 0), scale_factor=0.05, figure=fig)
	'''
        Green points are original PC from KITTI
        White points are PC feed into the network
        Red point is the predicted center
        Yellow point the post-processed predicted bounding box corners
    '''
	raw_input("Press any key to continue") 

def draw_lidar_simple(pc, color=None):
    ''' Draw lidar points. simplest set up. '''
    fig = mlab.figure(figure=None, bgcolor=(0,0,0), fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    #draw points
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=None, mode='point', colormap = 'gnuplot', scale_factor=1, figure=fig)
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig 

def draw_lidar_simple(pc, color=None):
    ''' Draw lidar points. simplest set up. '''
    fig = mlab.figure(figure=None, bgcolor=(0,0,0), fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    #draw points
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=None, mode='point', colormap = 'gnuplot', scale_factor=1, figure=fig)
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig 

def draw_lidar_simple(pc, color=None):
    ''' Draw lidar points. simplest set up. '''
    fig = mlab.figure(figure=None, bgcolor=(0,0,0), fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    #draw points
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=None, mode='point', colormap = 'gnuplot', scale_factor=1, figure=fig)
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig 

def show_points(self, point, color='lb', scale_factor=.0005):
        if color == 'b':
            color_f = (0, 0, 1)
        elif color == 'r':
            color_f = (1, 0, 0)
        elif color == 'g':
            color_f = (0, 1, 0)
        elif color == 'lb':  # light blue
            color_f = (0.22, 1, 1)
        else:
            color_f = (1, 1, 1)
        if point.size == 3:  # vis for only one point, shape must be (3,), for shape (1, 3) is not work
            point = point.reshape(3, )
            mlab.points3d(point[0], point[1], point[2], color=color_f, scale_factor=scale_factor)
        else:  # vis for multiple points
            mlab.points3d(point[:, 0], point[:, 1], point[:, 2], color=color_f, scale_factor=scale_factor) 

def draw_lidar(pc, color=None, fig=None, bgcolor=(0, 0, 0), pts_scale=1, pts_mode='point', pts_color=None):
    """
    Add lidar points
    Args:
        pc: point cloud xyz [npoints, 3]
        color:
        fig: fig handler
    Returns:

    """
    ''' Draw lidar points
    Args:
        pc: numpy array (n,3) of XYZ
        color: numpy array (n) of intensity or whatever
        fig: mayavi figure handler, if None create new one otherwise will use it
    Returns:
        fig: created or used fig
    '''
    if fig is None:
        fig = mlab.figure(figure=None, bgcolor=bgcolor, fgcolor=None, engine=None, size=(1600, 1000))
    if color is None:
        color = pc[:, 2]

    # add points
    mlab.points3d(pc[:, 0], pc[:, 1], pc[:, 2], color, color=pts_color, mode=pts_mode, colormap='gnuplot',
                  scale_factor=pts_scale, figure=fig)

    # # draw origin
    # mlab.points3d(0, 0, 0, color=(1, 1, 1), mode='sphere', scale_factor=0.2)

    # draw axis
    axes = np.array([
        [2., 0., 0., 0.],
        [0., 2., 0., 0.],
        [0., 0., 2., 0.],
    ], dtype=np.float64)
    mlab.plot3d([0, axes[0, 0]], [0, axes[0, 1]], [0, axes[0, 2]], color=(1, 0, 0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1, 0]], [0, axes[1, 1]], [0, axes[1, 2]], color=(0, 1, 0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2, 0]], [0, axes[2, 1]], [0, axes[2, 2]], color=(0, 0, 1), tube_radius=None, figure=fig)

    return fig 

def viz_single(pc):
	import mayavi.mlab as mlab
	fig = mlab.figure(figure=None, bgcolor=(0.4, 0.4, 0.4),
	                  fgcolor=None, engine=None, size=(500, 500))
	mlab.points3d(pc[:, 0], pc[:, 1], pc[:, 2], mode='sphere',
	              colormap='gnuplot', scale_factor=0.1, figure=fig)
	'''
        Green points are original PC from KITTI
        White points are PC feed into the network
        Red point is the predicted center
        Yellow point the post-processed predicted bounding box corners
    '''
	raw_input("Press any key to continue") 

def points3d(verts, point_size=3, **kwargs):
    if 'mode' not in kwargs:
        kwargs['mode'] = 'point'
    p = mlab.points3d(verts[:, 0], verts[:, 1], verts[:, 2], **kwargs)
    p.actor.property.point_size = point_size 

def remove_table_points(points_voxel_, vis=False):
    xy_unique = np.unique(points_voxel_[:, 0:2], axis=0)
    new_points_voxel_ = points_voxel_
    pre_del = np.zeros([1])
    for i in range(len(xy_unique)):
        tmp = []
        for j in range(len(points_voxel_)):
            if np.array_equal(points_voxel_[j, 0:2], xy_unique[i]):
                tmp.append(j)
        print(len(tmp))
        if len(tmp) < 3:
            tmp = np.array(tmp)
            pre_del = np.hstack([pre_del, tmp])
    if len(pre_del) != 1:
        pre_del = pre_del[1:]
        new_points_voxel_ = np.delete(points_voxel_, pre_del, 0)
    print("Success delete [[ {} ]] points from the table!".format(len(points_voxel_) - len(new_points_voxel_)))

    if vis:
        p = points_voxel_
        mlab.points3d(p[:, 0], p[:, 1], p[:, 2], scale_factor=0.002, color=(1, 0, 0))
        p = new_points_voxel_
        mlab.points3d(p[:, 0], p[:, 1], p[:, 2], scale_factor=0.002, color=(0, 0, 1))
        mlab.points3d(0, 0, 0, scale_factor=0.01, color=(0, 1, 0))  # plot 0 point
        mlab.show()
    return new_points_voxel_ 

def show_obj(surface_points_, color='b'):
    if color == 'b':
        color_f = (0, 0, 1)
    elif color == 'r':
        color_f = (1, 0, 0)
    elif color == 'g':
        color_f = (0, 1, 0)
    else:
        color_f = (1, 1, 1)
    points = surface_points_
    mlab.points3d(points[:, 0], points[:, 1], points[:, 2], color=color_f, scale_factor=.0007) 

def show_obj(surface_points_, title, color='b'):
    mlab.figure(bgcolor=(0, 0, 0), fgcolor=(0.7, 0.7, 0.7), size=(1000, 1000))
    if color == 'b':
        color_f = (0, 0, 1)
    elif color == 'r':
        color_f = (1, 0, 0)
    elif color == 'g':
        color_f = (0, 1, 0)
    else:
        color_f = (1, 1, 1)
    points_ = surface_points_
    mlab.points3d(points_[:, 0], points_[:, 1], points_[:, 2], color=color_f, scale_factor=.0007)
    mlab.title(title, size=0.5) 

def grasp(grasp, T_obj_world=RigidTransform(from_frame='obj', to_frame='world'),
              tube_radius=0.002, endpoint_color=(0, 1, 0),
              endpoint_scale=0.004, grasp_axis_color=(0, 1, 0)):
        """ Plots a grasp as an axis and center.

        Parameters
        ----------
        grasp : :obj:`dexnet.grasping.Grasp`
            the grasp to plot
        T_obj_world : :obj:`autolab_core.RigidTransform`
            the pose of the object that the grasp is referencing in world frame
        tube_radius : float
            radius of the plotted grasp axis
        endpoint_color : 3-tuple
            color of the endpoints of the grasp axis
        endpoint_scale : 3-tuple
            scale of the plotted endpoints
        grasp_axis_color : 3-tuple
            color of the grasp axis
        """
        g1, g2 = grasp.endpoints
        center = grasp.center
        g1 = Point(g1, 'obj')
        g2 = Point(g2, 'obj')
        center = Point(center, 'obj')

        g1_tf = T_obj_world.apply(g1)
        g2_tf = T_obj_world.apply(g2)
        center_tf = T_obj_world.apply(center)
        grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])

        mlab.points3d(g1_tf.data[0], g1_tf.data[1], g1_tf.data[2], color=endpoint_color, scale_factor=endpoint_scale)
        mlab.points3d(g2_tf.data[0], g2_tf.data[1], g2_tf.data[2], color=endpoint_color, scale_factor=endpoint_scale)

        mlab.plot3d(grasp_axis_tf[:, 0], grasp_axis_tf[:, 1], grasp_axis_tf[:, 2], color=grasp_axis_color,
                    tube_radius=tube_radius) 

def draw_coordinate_frame_at_origin(fig: Figure) -> Figure:
    """
    Draw the origin and 3 vectors representing standard basis vectors to express
    a coordinate reference frame.
    Args:
       fig: Mayavi figure
    Returns:
       Updated Mayavi figure
    Based on
    --------
    https://github.com/hengck23/didi-udacity-2017/blob/master/baseline-04/kitti_data/draw.py
    https://github.com/charlesq34/frustum-pointnets/blob/master/mayavi/viz_util.py
    """
    # draw origin
    mlab.points3d(0, 0, 0, color=(1, 1, 1), mode="sphere", scale_factor=0.2)

    # Form standard basis vectors e_1, e_2, e_3
    axes = np.array([[2.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 2.0]], dtype=np.float64)
    # e_1 in red
    mlab.plot3d(
        [0, axes[0, 0]], [0, axes[0, 1]], [0, axes[0, 2]], color=(1, 0, 0), tube_radius=None, figure=fig
    )
    # e_2 in green
    mlab.plot3d(
        [0, axes[1, 0]], [0, axes[1, 1]], [0, axes[1, 2]], color=(0, 1, 0), tube_radius=None, figure=fig
    )
    # e_3 in blue
    mlab.plot3d(
        [0, axes[2, 0]], [0, axes[2, 1]], [0, axes[2, 2]], color=(0, 0, 1), tube_radius=None, figure=fig
    )

    return fig 

def show_pc_segments(pc_all, pc_all2, pc_segments):
   """
   3D visualization of segmentation result
   """
   from mayavi import mlab
   mlab.figure(bgcolor=(0.0,0.0,0.0))
   mlab.points3d(pc_all[:,0],pc_all[:,1],pc_all[:,2],scale_factor=0.1,color=(0.3,0.2,0.2),opacity=0.3)
   mlab.points3d(pc_all2[:,0],pc_all2[:,1],pc_all2[:,2],scale_factor=0.1,color=(0.4,0.4,0.4),opacity=1.0)

   for i in range(len(pc_segments)):
       color = (random.random()*0.8 + 0.2  ,random.random()*0.8 + 0.2  ,random.random()*0.8 + 0.2)
       nodes1 = mlab.points3d(pc_segments[i][:,0],pc_segments[i][:,1],pc_segments[i][:,2],scale_factor=0.1,color=color,opacity=0.8)

       nodes1.glyph.scale_mode = 'scale_by_vector'


   length_axis = 2.0
   linewidth_axis = 0.1
   color_axis = (1.0,0.0,0.0)
   pc_axis = np.array([[0,0,0], [length_axis,0,0], [0,length_axis,0],[0,0,length_axis]])
   mlab.plot3d(pc_axis[0:2,0], pc_axis[0:2,1], pc_axis[0:2,2],tube_radius=linewidth_axis, color=(1.0,0.0,0.0))
   mlab.plot3d(pc_axis[[0,2],0], pc_axis[[0,2],1], pc_axis[[0,2],2],tube_radius=linewidth_axis, color=(0.0,1.0,0.0))
   mlab.plot3d(pc_axis[[0,3],0], pc_axis[[0,3],1], pc_axis[[0,3],2],tube_radius=linewidth_axis, color=(0.0,0.0,1.0))


   mlab.show() 

def draw_lidar(pc, color=None, fig=None, bgcolor=(0,0,0), pts_scale=1, pts_mode='point', pts_color=None):
    ''' Draw lidar points
    Args:
        pc: numpy array (n,3) of XYZ
        color: numpy array (n) of intensity or whatever
        fig: mayavi figure handler, if None create new one otherwise will use it
    Returns:
        fig: created or used fig
    '''
    if fig is None: fig = mlab.figure(figure=None, bgcolor=bgcolor, fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=pts_color, mode=pts_mode, colormap = 'gnuplot', scale_factor=pts_scale, figure=fig)
    
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)

    # draw fov (todo: update to real sensor spec.)
    fov=np.array([  # 45 degree
        [20., 20., 0.,0.],
        [20.,-20., 0.,0.],
    ],dtype=np.float64)
    
    mlab.plot3d([0, fov[0,0]], [0, fov[0,1]], [0, fov[0,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
    mlab.plot3d([0, fov[1,0]], [0, fov[1,1]], [0, fov[1,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
   
    # draw square region
    TOP_Y_MIN=-20
    TOP_Y_MAX=20
    TOP_X_MIN=0
    TOP_X_MAX=40
    TOP_Z_MIN=-2.0
    TOP_Z_MAX=0.4
    
    x1 = TOP_X_MIN
    x2 = TOP_X_MAX
    y1 = TOP_Y_MIN
    y2 = TOP_Y_MAX
    mlab.plot3d([x1, x1], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x2, x2], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x1, x2], [y1, y1], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x1, x2], [y2, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    
    #mlab.orientation_axes()
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig 

def draw_lidar(pc, color=None, fig=None, bgcolor=(0,0,0), pts_scale=1, pts_mode='point', pts_color=None):
    ''' Draw lidar points
    Args:
        pc: numpy array (n,3) of XYZ
        color: numpy array (n) of intensity or whatever
        fig: mayavi figure handler, if None create new one otherwise will use it
    Returns:
        fig: created or used fig
    '''
    if fig is None: fig = mlab.figure(figure=None, bgcolor=bgcolor, fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=pts_color, mode=pts_mode, colormap = 'gnuplot', scale_factor=pts_scale, figure=fig)
    
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)

    # draw fov (todo: update to real sensor spec.)
    fov=np.array([  # 45 degree
        [20., 20., 0.,0.],
        [20.,-20., 0.,0.],
    ],dtype=np.float64)
    
    mlab.plot3d([0, fov[0,0]], [0, fov[0,1]], [0, fov[0,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
    mlab.plot3d([0, fov[1,0]], [0, fov[1,1]], [0, fov[1,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
   
    # draw square region
    TOP_Y_MIN=-20
    TOP_Y_MAX=20
    TOP_X_MIN=0
    TOP_X_MAX=40
    TOP_Z_MIN=-2.0
    TOP_Z_MAX=0.4
    
    x1 = TOP_X_MIN
    x2 = TOP_X_MAX
    y1 = TOP_Y_MIN
    y2 = TOP_Y_MAX
    mlab.plot3d([x1, x1], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x2, x2], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x1, x2], [y1, y1], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x1, x2], [y2, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    
    #mlab.orientation_axes()
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig 

def draw_lidar(pc, color=None, fig1=None, bgcolor=(0,0,0), pts_scale=1, pts_mode='point', pts_color=None):
    ''' Draw lidar points
    Args:
        pc: numpy array (n,3) of XYZ
        color: numpy array (n) of intensity or whatever
        fig: mayavi figure handler, if None create new one otherwise will use it
    Returns:
        fig: created or used fig
    '''
    #if fig1 is None: fig1 = mlab.figure(figure="point cloud", bgcolor=bgcolor, fgcolor=None, engine=None, size=(1600, 1000))
    
    mlab.clf(figure=None)
    if color is None: color = pc[:,2]
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=pts_color, mode=pts_mode, colormap = 'gnuplot', scale_factor=pts_scale, figure=fig1)
    
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
	
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig1)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig1)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig1)

    # draw fov (todo: update to real sensor spec.)
    fov=np.array([  # 45 degree
        [20., 20., 0.,0.],
        [20.,-20., 0.,0.],
    ],dtype=np.float64)
    
    mlab.plot3d([0, fov[0,0]], [0, fov[0,1]], [0, fov[0,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig1)
    mlab.plot3d([0, fov[1,0]], [0, fov[1,1]], [0, fov[1,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig1)
   
    # draw square region
    TOP_Y_MIN=-20
    TOP_Y_MAX=20
    TOP_X_MIN=0
    TOP_X_MAX=40
    TOP_Z_MIN=-2.0
    TOP_Z_MAX=0.4
    
    x1 = TOP_X_MIN
    x2 = TOP_X_MAX
    y1 = TOP_Y_MIN
    y2 = TOP_Y_MAX
    mlab.plot3d([x1, x1], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig1)
    mlab.plot3d([x2, x2], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig1)
    mlab.plot3d([x1, x2], [y1, y1], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig1)
    mlab.plot3d([x1, x2], [y2, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig1)
    
    #mlab.orientation_axes()
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=60.0, figure=fig1)
    return fig1 

def plot_points_3D_mayavi(
    points: np.ndarray,
    bird: bool,
    fig: Figure,
    per_pt_color_strengths: np.ndarray = None,
    fixed_color: Optional[Color] = (1, 0, 0),
    colormap: str = "spectral",
) -> Figure:
    """Visualize points with Mayavi. Scale factor has no influence on point size rendering
    when calling `points3d()` with the mode="point" argument, so we ignore it altogether.
    The parameter "line_width" also has no effect on points, so we ignore it also.
    Args:
       points: The points to visualize
       fig: A Mayavi figure
       per_pt_color_strengths: An array of scalar values the same size as `points`
       fixed_color: Use a fixed color instead of a colormap
       colormap: different green to red jet for 'spectral' or 'gnuplot'
    Returns:
       Updated Mayavi figure
    """
    if len(points) == 0:
        return None

    if per_pt_color_strengths is None or len(per_pt_color_strengths) != len(points):
        # Height data used for shading
        if bird:
            per_pt_color_strengths = points[:, 2]
        else:
            per_pt_color_strengths = points[:, 0]

    mlab.points3d(
        points[:, 0],  # x
        points[:, 1],  # y
        points[:, 2],  # z
        per_pt_color_strengths,
        mode="point",  # Render each point as a 'point', not as a 'sphere' or 'cube'
        colormap=colormap,
        color=fixed_color,  # Used a fixed (r,g,b) color instead of colormap
        figure=fig,
    )

    return fig 

def plot(self, figname=None, size=(300, 325), view=(30, 30), color=(1.0, 1.0, 1.0)):

        fig = mlab.figure(size=size)
        figure = mlab.gcf()
        fig.scene.disable_render = True
        figure.scene.background = (0.0, 0.0, 0.0)
        mlab.view(0, 90, distance=0.2)
        assert (self.structure.natom > 0)

        x = self.structure.positions[:, 0]
        y = self.structure.positions[:, 1]
        z = self.structure.positions[:, 2]
        cr = covalent_radius(self.structure.symbols)
        s = np.apply_along_axis(np.linalg.norm, 1, self.structure.positions)

        mlab.points3d(x, y, z, s, scale_factor=1.0, resolution=8, opacity=1.0,
                      color=color,
                      scale_mode='none')

        if self.structure.is_crystal:
            frame, line1, line2, line3 = self.structure.get_cell().get_path()

            mlab.plot3d(frame[:, 0], frame[:, 1], frame[:, 2], tube_radius=.02, color=(1, 1, 1))
            mlab.plot3d(line1[:, 0], line1[:, 1], line1[:, 2], tube_radius=.02, color=(1, 1, 1))
            mlab.plot3d(line2[:, 0], line2[:, 1], line2[:, 2], tube_radius=.02, color=(1, 1, 1))
            mlab.plot3d(line3[:, 0], line3[:, 1], line3[:, 2], tube_radius=.02, color=(1, 1, 1))
        else:
            for i in range(self.structure.natom - 1):
                for j in range(i + 1, self.structure.natom):
                    vector = self.structure.positions[i] - self.structure.positions[j]
                    mvector = np.linalg.norm(vector)
                    uvector = 1.0 / mvector * vector
                    if 2 * mvector < covalent_radius(self.structure.symbols[i]) + \
                            covalent_radius(self.structure.symbols[j]):
                        pair = np.concatenate(
                            (self.structure.positions[i] - 0.1 * uvector,
                             self.structure.positions[j] + 0.1 * uvector)).reshape((-1, 3))
                        mlab.plot3d(pair[:, 0], pair[:, 1], pair[:, 2], tube_radius=0.15, opacity=1.0, color=(1, 1, 1))

        mlab.view(distance=12.0)
        fig.scene.disable_render = False
        if figname is not None:
            mlab.savefig(figname)
        return figure 

def show_correspondence(verts1, tris1, verts2, tris2, ij, points=5, show_spheres=True, 
                        scalars=None, colormap='gist_rainbow', blend_factor=0.9, 
                        compute_blend_weights=compute_fake_weights,
                        color_only_correspondences=1, color_no_correspondence=(0,0,0),
                        offset_factor=(1.5, 0., 0.), block=True,
                       ):
    mlab.figure(bgcolor=(1,1,1))
    # select sparse points to visualize
    if type(points) is int:
        i_sel = [0]
        geo = GeodesicDistanceComputation(verts1, tris1)
        for n in xrange(points-1):
            d = geo(ij[i_sel,0])[ij[:,0]]
            i_sel.append(d.argmax())
    else:
        i_sel = points
    #i_sel = np.random.randint(0, len(ij), 10)
    ij_sel = np.column_stack((ij[i_sel,0], ij[i_sel, 1]))
    # color per marker - value between 0 and 1 which is passed through a color map later on
    color = np.linspace(0, 1, len(ij_sel))
    # prepare visualization
    offset = verts2.ptp(axis=0) * offset_factor#(verts2[:,0].ptp() * offset_factor, 0, 0)
    p1 = verts1[ij_sel[:,0]]
    p2 = verts2[ij_sel[:,1]] + offset
    v = p2 - p1
    # visualize!
    # correspondence arrows
    quiv = mlab.quiver3d(p1[:,0], p1[:,1], p1[:,2], v[:,0], v[:,1], v[:,2],
                         scale_factor=1, line_width=2, mode='2ddash', scale_mode='vector',
                         scalars=color, colormap=colormap)
    quiv.glyph.color_mode = 'color_by_scalar'
    # show sparse points as spheres
    if show_spheres:
        h = veclen(verts1[tris1[:,0]] - verts1[tris1[:,1]]).mean() * 2
        mlab.points3d(p1[:,0], p1[:,1], p1[:,2], color, scale_mode='none', 
                      scale_factor=h, colormap=colormap, resolution=32)
        mlab.points3d(p2[:,0], p2[:,1], p2[:,2], color, scale_mode='none', 
                      scale_factor=h, colormap=colormap, resolution=32)
    # make colors for the meshes
    if scalars is None:
        H = compute_blend_weights(verts1, tris1, ij_sel[:,0])
        # interpolate colors
        lut = quiv.module_manager.scalar_lut_manager.lut.table.to_array()
        lut_colors_rgb = lut[(color * (lut.shape[0]-1)).astype(np.int), :3].astype(np.float)
        scalars = ((1 - blend_factor) * lut_colors_rgb[H.argmax(axis=1)] + \
                   blend_factor * (H[:,:,np.newaxis] * lut_colors_rgb[np.newaxis,:,:]).sum(1))
        scalars = np.uint8(scalars)
        if color_only_correspondences:
            scalars_filtered = np.zeros((len(verts1), 3))
            scalars_filtered[:] = np.array(color_no_correspondence)[np.newaxis] * 255
            scalars_filtered[ij[:,0]] = scalars[ij[:,0]]
            scalars = scalars_filtered
    scalars2 = np.zeros((len(verts2), 3))
    scalars2[:] = np.array(color_no_correspondence)[np.newaxis] * 255
    scalars2[ij[:,1]] = scalars[ij[:,0]]
    # show meshes
    vismesh(verts1, tris1, scalars=scalars)
    vismesh(verts2 + offset, tris2, scalars=scalars2)
    if block:
        mlab.show() 

def CurvilinearPlotLine(mesh, TotalDisp=None, QuantityToPlot=None, plot_on_faces=True,
        ProjectionFlags=None, interpolation_degree=20, EquallySpacedPoints=False, PlotActualCurve=False,
        plot_points=False, plot_edges=True, plot_surfaces=True, point_radius=0.02, colorbar=False, color=None, figure=None,
        show_plot=True, save=False, filename=None, save_tessellation=False):

        """High order curved line mesh plots, based on high order nodal FEM.
        """

        if not isinstance(mesh,Mesh):
            raise TypeError("mesh has to be an instance of type {}".format(Mesh))
        if mesh.element_type != "line":
            raise RuntimeError("Calling line plotting function with element type {}".format(mesh.element_type))
        if TotalDisp is None:
            TotalDisp = np.zeros_like(mesh.points)


        tmesh = PostProcess.TessellateLines(mesh, TotalDisp, QuantityToPlot=QuantityToPlot,
            ProjectionFlags=ProjectionFlags, interpolation_degree=interpolation_degree,
            EquallySpacedPoints=EquallySpacedPoints, plot_points=plot_points,
            plot_edges=plot_edges, plot_on_faces=plot_on_faces)

        # UNPACK
        x_edges = tmesh.x_edges
        y_edges = tmesh.y_edges
        z_edges = tmesh.z_edges
        nnode = tmesh.nnode
        nelem = tmesh.nelem
        nsize = tmesh.nsize

        # Xplot = tmesh.points
        # Tplot = tmesh.elements
        vpoints = tmesh.vpoints
        connections = tmesh.elements


        import os
        os.environ['ETS_TOOLKIT'] = 'qt4'
        from mayavi import mlab

        if figure is None:
            figure = mlab.figure(bgcolor=(1,1,1),fgcolor=(1,1,1),size=(1000,800))

        # PLOT LINES
        if plot_points:
            h_points = mlab.points3d(vpoints[:,0],vpoints[:,1],vpoints[:,2],color=(0,0,0),mode='sphere',scale_factor=point_radius)

        # PLOT CURVED EDGES
        if plot_edges:
            src = mlab.pipeline.scalar_scatter(x_edges.T.copy().flatten(), y_edges.T.copy().flatten(), z_edges.T.copy().flatten())
            src.mlab_source.dataset.lines = connections
            lines = mlab.pipeline.stripper(src)
            h_edges = mlab.pipeline.surface(lines, color = (0,0,0), line_width=2)


        mlab.view(azimuth=0, roll=0)
        mlab.show()
        return 

def draw_lidar(pc, color=None, fig=None, bgcolor=(0,0,0), pts_scale=1, pts_mode='point', pts_color=None):
    ''' Draw lidar points
    Args:
        pc: numpy array (n,3) of XYZ
        color: numpy array (n) of intensity or whatever
        fig: mayavi figure handler, if None create new one otherwise will use it
    Returns:
        fig: created or used fig
    '''
    if fig is None: fig = mlab.figure(figure=None, bgcolor=bgcolor, fgcolor=None, engine=None, size=(1600, 1000))
    if color is None: color = pc[:,2]
    mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=pts_color, mode=pts_mode, colormap = 'gnuplot', scale_factor=pts_scale, figure=fig)
    
    #draw origin
    mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
    
    #draw axis
    axes=np.array([
        [2.,0.,0.,0.],
        [0.,2.,0.,0.],
        [0.,0.,2.,0.],
    ],dtype=np.float64)
    mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
    mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)

    # draw fov (todo: update to real sensor spec.)
    fov=np.array([  # 45 degree
        [20., 20., 0.,0.],
        [20.,-20., 0.,0.],
    ],dtype=np.float64)
    
    mlab.plot3d([0, fov[0,0]], [0, fov[0,1]], [0, fov[0,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
    mlab.plot3d([0, fov[1,0]], [0, fov[1,1]], [0, fov[1,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
   
    # draw square region
    TOP_Y_MIN=-20
    TOP_Y_MAX=20
    TOP_X_MIN=0
    TOP_X_MAX=40
    TOP_Z_MIN=-2.0
    TOP_Z_MAX=0.4
    
    x1 = TOP_X_MIN
    x2 = TOP_X_MAX
    y1 = TOP_Y_MIN
    y2 = TOP_Y_MAX
    mlab.plot3d([x1, x1], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x2, x2], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x1, x2], [y1, y1], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    mlab.plot3d([x1, x2], [y2, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=0.1, line_width=1, figure=fig)
    
    #mlab.orientation_axes()
    mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
    return fig