<filename>easyplotCV/draw.py<gh_stars>0
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
import numpy as np
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import scipy as sp
import matplotlib.colors as colors
import cv2
from geo3d import *


class Arrow3D(FancyArrowPatch):
    def __init__(self, xs, ys, zs, *args, **kwargs):
        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)
        self._verts3d = xs, ys, zs

    def draw(self, renderer):
        xs3d, ys3d, zs3d = self._verts3d
        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
        FancyArrowPatch.draw(self, renderer)

def axis_setup(ax, xyz_lim=[0,10], show_axis=True):
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')
    ax.set_xlim(xyz_lim)
    ax.set_ylim(xyz_lim)
    ax.set_zlim(xyz_lim)
    ax.view_init(elev=40, azim=40)

    if show_axis:
        vec_len = (xyz_lim[1] - xyz_lim[0])/3.0
        draw_vector(ax, (0,0,0), (vec_len, 0, 0), color = 'r')
        draw_vector(ax, (0,0,0), (0, vec_len, 0), color = 'g')
        draw_vector(ax, (0,0,0), (0, 0, vec_len), color = 'b')

        ax.text(1.1*vec_len,0,0, "x", color='r')
        ax.text(0, 1.1*vec_len, 0, "y", color = 'g')
        ax.text(0,0,1.1*vec_len, "z", color= 'b')

def draw_vector(ax, start_point, end_point, color = "black", ms = 15, lw = 2):
    xx = [start_point[0], end_point[0]]
    yy = [start_point[1], end_point[1]]
    zz = [start_point[2], end_point[2]]
    vec = Arrow3D(xx, yy,
                zz, mutation_scale=ms,
                lw=lw, arrowstyle="-|>", color=color)
    ax.add_artist(vec)

def draw_frame(ax, R, t, scale=3, name=""):
    ax.text(t[0], t[1], t[2], name)

    start_x = t
    end_x = R[:,0]*scale+t
    draw_vector(ax, start_x, end_x, color = 'r')

    start_y = t
    end_y = R[:,1]*scale+t
    draw_vector(ax, start_y, end_y, color = 'g')

    start_z = t
    end_z = R[:,2]*scale+t
    draw_vector(ax, start_z, end_z, color = 'b')


def draw_line_from_points(ax, start_point, end_point):
    p1 = start_point
    p2 = end_point
    ax.plot([p1[0], p2[0]], [p1[1], p2[1]], [p1[2], p2[2]])


def draw_point(ax, point, color='r'):
    ax.scatter(point[0], point[1], point[2], color=color)

def draw_plane_from_4_points(ax, p1, p2, p3, p4, color = 'b', alpha = 0.4):
    point_list = [p1, p2, p3, p4]
    """
    sorted = False
    while not sorted:
        sorted = True
        for i in range(3):
            edge = (point_list[i+1][i] - point_list[i][i])*(point_list[i+1][i+1] + point_list[i][i+1])
            print(edge)
            if(edge < 0):
                sorted = False
                temp = point_list[i]
                point_list[i] = point_list[i+1]
                point_list[i+1] = temp
    print(point_list)
    temp = point_list[1]
    point_list[1] = point_list[0]
    point_list[0] = temp
    """
    vstacked = np.vstack((point_list[0][0:3], point_list[1][0:3], point_list[2][0:3], point_list[3][0:3]))
    pol_col = Poly3DCollection([vstacked])
    pol_col.set_color(color)
    pol_col.set_edgecolor(color)
    pol_col.set_alpha(alpha)
    ax.add_collection3d(pol_col)

def draw_plane(ax, vstack_points, color = [0.5,0.5,0.5]):
    pol_col = Poly3DCollection([vstack_points])
    pol_col.set_color(colors.rgb2hex(color))
    pol_col.set_edgecolor('k')
    ax.add_collection3d(pol_col)


def draw_camera(ax, R, t):

    p1 = np.array([0.5, 0.5, 0.0, 1.0])
    p2 = np.array([0.5, -0.5, 0.0, 1.0])
    p3 = np.array([-0.5, -0.5, 0.0, 1.0])
    p4 = np.array([-0.5, 0.5, 0.0, 1.0])

    p5 = np.array([0.5, 0.5, -1.0, 1.0])
    p6 = np.array([0.5, -0.5, -1.0, 1.0])
    p7 = np.array([-0.5, -0.5, -1.0, 1.0])
    p8 = np.array([-0.5, 0.5, -1.0, 1.0])

    p9 = np.array([0.0, 0.0, 0.0, 1.0])
    p10 = np.array([0.5, 0.5, 0.5, 1.0])
    p11 = np.array([0.5, -0.5, 0.5, 1.0])
    p12 = np.array([-0.5, -0.5, 0.5, 1.0])
    p13 = np.array([-0.5, 0.5, 0.5, 1.0])

    T = make_transf(R,t)

    p1_s = T @ p1
    p2_s = T @ p2
    p3_s = T @ p3
    p4_s = T @ p4

    p5_s = T @ p5
    p6_s = T @ p6
    p7_s = T @ p7
    p8_s = T @ p8

    p9_s = T @ p9
    p10_s = T @ p10
    p11_s = T @ p11
    p12_s = T @ p12
    p13_s = T @ p13

    top = np.vstack((p1_s[0:3], p2_s[0:3], p3_s[0:3], p4_s[0:3]))
    bot = np.vstack((p5_s[0:3], p6_s[0:3], p7_s[0:3], p8_s[0:3]))
    side1 = np.vstack((p1_s[0:3], p2_s[0:3], p6_s[0:3], p5_s[0:3]))
    side2 = np.vstack((p2_s[0:3], p3_s[0:3], p7_s[0:3], p6_s[0:3]))
    side3 = np.vstack((p3_s[0:3], p4_s[0:3], p8_s[0:3], p7_s[0:3]))
    side4 = np.vstack((p4_s[0:3], p1_s[0:3], p5_s[0:3], p8_s[0:3]))

    front = np.vstack((p10_s[0:3], p11_s[0:3], p12_s[0:3], p13_s[0:3]))
    cside1 = np.vstack((p9_s[0:3], p10_s[0:3], p11_s[0:3]))
    cside2 = np.vstack((p9_s[0:3], p11_s[0:3], p12_s[0:3]))
    cside3 = np.vstack((p9_s[0:3], p12_s[0:3], p13_s[0:3]))
    cside4 = np.vstack((p9_s[0:3], p13_s[0:3], p10_s[0:3]))

    draw_plane(ax, top)
    draw_plane(ax, bot)
    draw_plane(ax, side1)
    draw_plane(ax, side2)
    draw_plane(ax, side3)
    draw_plane(ax, side4)
    #draw_plane(ax, front)
    draw_plane(ax, cside1)
    draw_plane(ax, cside2)
    draw_plane(ax, cside3)
    draw_plane(ax, cside4)



<filename>easyplotCV/draw.py<gh_stars>0
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
import numpy as np
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import scipy as sp
import matplotlib.colors as colors
import cv2
from geo3d import *


class Arrow3D(FancyArrowPatch):
    def __init__(self, xs, ys, zs, *args, **kwargs):
        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)
        self._verts3d = xs, ys, zs

    def draw(self, renderer):
        xs3d, ys3d, zs3d = self._verts3d
        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
        FancyArrowPatch.draw(self, renderer)

def axis_setup(ax, xyz_lim=[0,10], show_axis=True):
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')
    ax.set_xlim(xyz_lim)
    ax.set_ylim(xyz_lim)
    ax.set_zlim(xyz_lim)
    ax.view_init(elev=40, azim=40)

    if show_axis:
        vec_len = (xyz_lim[1] - xyz_lim[0])/3.0
        draw_vector(ax, (0,0,0), (vec_len, 0, 0), color = 'r')
        draw_vector(ax, (0,0,0), (0, vec_len, 0), color = 'g')
        draw_vector(ax, (0,0,0), (0, 0, vec_len), color = 'b')

        ax.text(1.1*vec_len,0,0, "x", color='r')
        ax.text(0, 1.1*vec_len, 0, "y", color = 'g')
        ax.text(0,0,1.1*vec_len, "z", color= 'b')

def draw_vector(ax, start_point, end_point, color = "black", ms = 15, lw = 2):
    xx = [start_point[0], end_point[0]]
    yy = [start_point[1], end_point[1]]
    zz = [start_point[2], end_point[2]]
    vec = Arrow3D(xx, yy,
                zz, mutation_scale=ms,
                lw=lw, arrowstyle="-|>", color=color)
    ax.add_artist(vec)

def draw_frame(ax, R, t, scale=3, name=""):
    ax.text(t[0], t[1], t[2], name)

    start_x = t
    end_x = R[:,0]*scale+t
    draw_vector(ax, start_x, end_x, color = 'r')

    start_y = t
    end_y = R[:,1]*scale+t
    draw_vector(ax, start_y, end_y, color = 'g')

    start_z = t
    end_z = R[:,2]*scale+t
    draw_vector(ax, start_z, end_z, color = 'b')


def draw_line_from_points(ax, start_point, end_point):
    p1 = start_point
    p2 = end_point
    ax.plot([p1[0], p2[0]], [p1[1], p2[1]], [p1[2], p2[2]])


def draw_point(ax, point, color='r'):
    ax.scatter(point[0], point[1], point[2], color=color)

def draw_plane_from_4_points(ax, p1, p2, p3, p4, color = 'b', alpha = 0.4):
    point_list = [p1, p2, p3, p4]
    """
    sorted = False
    while not sorted:
        sorted = True
        for i in range(3):
            edge = (point_list[i+1][i] - point_list[i][i])*(point_list[i+1][i+1] + point_list[i][i+1])
            print(edge)
            if(edge < 0):
                sorted = False
                temp = point_list[i]
                point_list[i] = point_list[i+1]
                point_list[i+1] = temp
    print(point_list)
    temp = point_list[1]
    point_list[1] = point_list[0]
    point_list[0] = temp
    """
    vstacked = np.vstack((point_list[0][0:3], point_list[1][0:3], point_list[2][0:3], point_list[3][0:3]))
    pol_col = Poly3DCollection([vstacked])
    pol_col.set_color(color)
    pol_col.set_edgecolor(color)
    pol_col.set_alpha(alpha)
    ax.add_collection3d(pol_col)

def draw_plane(ax, vstack_points, color = [0.5,0.5,0.5]):
    pol_col = Poly3DCollection([vstack_points])
    pol_col.set_color(colors.rgb2hex(color))
    pol_col.set_edgecolor('k')
    ax.add_collection3d(pol_col)


def draw_camera(ax, R, t):

    p1 = np.array([0.5, 0.5, 0.0, 1.0])
    p2 = np.array([0.5, -0.5, 0.0, 1.0])
    p3 = np.array([-0.5, -0.5, 0.0, 1.0])
    p4 = np.array([-0.5, 0.5, 0.0, 1.0])

    p5 = np.array([0.5, 0.5, -1.0, 1.0])
    p6 = np.array([0.5, -0.5, -1.0, 1.0])
    p7 = np.array([-0.5, -0.5, -1.0, 1.0])
    p8 = np.array([-0.5, 0.5, -1.0, 1.0])

    p9 = np.array([0.0, 0.0, 0.0, 1.0])
    p10 = np.array([0.5, 0.5, 0.5, 1.0])
    p11 = np.array([0.5, -0.5, 0.5, 1.0])
    p12 = np.array([-0.5, -0.5, 0.5, 1.0])
    p13 = np.array([-0.5, 0.5, 0.5, 1.0])

    T = make_transf(R,t)

    p1_s = T @ p1
    p2_s = T @ p2
    p3_s = T @ p3
    p4_s = T @ p4

    p5_s = T @ p5
    p6_s = T @ p6
    p7_s = T @ p7
    p8_s = T @ p8

    p9_s = T @ p9
    p10_s = T @ p10
    p11_s = T @ p11
    p12_s = T @ p12
    p13_s = T @ p13

    top = np.vstack((p1_s[0:3], p2_s[0:3], p3_s[0:3], p4_s[0:3]))
    bot = np.vstack((p5_s[0:3], p6_s[0:3], p7_s[0:3], p8_s[0:3]))
    side1 = np.vstack((p1_s[0:3], p2_s[0:3], p6_s[0:3], p5_s[0:3]))
    side2 = np.vstack((p2_s[0:3], p3_s[0:3], p7_s[0:3], p6_s[0:3]))
    side3 = np.vstack((p3_s[0:3], p4_s[0:3], p8_s[0:3], p7_s[0:3]))
    side4 = np.vstack((p4_s[0:3], p1_s[0:3], p5_s[0:3], p8_s[0:3]))

    front = np.vstack((p10_s[0:3], p11_s[0:3], p12_s[0:3], p13_s[0:3]))
    cside1 = np.vstack((p9_s[0:3], p10_s[0:3], p11_s[0:3]))
    cside2 = np.vstack((p9_s[0:3], p11_s[0:3], p12_s[0:3]))
    cside3 = np.vstack((p9_s[0:3], p12_s[0:3], p13_s[0:3]))
    cside4 = np.vstack((p9_s[0:3], p13_s[0:3], p10_s[0:3]))

    draw_plane(ax, top)
    draw_plane(ax, bot)
    draw_plane(ax, side1)
    draw_plane(ax, side2)
    draw_plane(ax, side3)
    draw_plane(ax, side4)
    #draw_plane(ax, front)
    draw_plane(ax, cside1)
    draw_plane(ax, cside2)
    draw_plane(ax, cside3)
    draw_plane(ax, cside4)



sorted = False     while not sorted:         sorted = True         for i in range(3):             edge = (point_list[i+1][i] - point_list[i][i])*(point_list[i+1][i+1] + point_list[i][i+1])             print(edge)             if(edge < 0):                 sorted = False                 temp = point_list[i]                 point_list[i] = point_list[i+1]                 point_list[i+1] = temp     print(point_list)     temp = point_list[1]     point_list[1] = point_list[0]     point_list[0] = temp #draw_plane(ax, front)