예제 #1
0
def rigid_inv_transform(p, R, t):
    p_ = Point3D(p)
    q = np.matmul(R.T, p_.p - t)
    if isinstance(p, Point3D):
        return Point3D(q)
    else:
        return q
예제 #2
0
    def run(self):
        angle = Point3D(30.0, 30.0, 0.0)
        delta = Point3D(0, 0, 0)

        friction_factor = 0.98
        init_pos = None
        dragging = False
        while True:
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    sys.exit()
                elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
                    dragging = True
                    init_pos = Point2D(*event.pos)
                elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
                    dragging = False
                elif event.type == pygame.MOUSEMOTION and dragging:
                    curr_pos = Point2D(*event.pos)
                    delta.y, delta.x = (init_pos -
                                        curr_pos) * 2 * math.pi / self.width
                    angle += delta

            cube = Cube(size=100,
                        angle_x=angle.x,
                        angle_y=angle.y,
                        angle_z=angle.z)
            self.__draw_cube(cube)

            if not dragging:
                delta *= friction_factor

            angle += delta
            pygame.display.flip()
    def _generate_step(self, step_number):
        a = self._l_b + Point3D(y=-(step_number * self._step_length))
        b = a + Point3D(z=self._step_height * (step_number + 1))
        c = b + Point3D(y=-self._step_length)
        d = a + Point3D(y=-self._step_length)
        f = b + Point3D(z=-self._step_height)
        a1 = a + Point3D(x=self._width)
        b1 = b + Point3D(x=self._width)
        c1 = c + Point3D(x=self._width)
        d1 = d + Point3D(x=self._width)
        f1 = f + Point3D(x=self._width)

        self.triangles.append(Triangle(a, b, c))
        self.triangles.append(Triangle(a, d, c))

        self.triangles.append(Triangle(a1, b1, c1))
        self.triangles.append(Triangle(a1, d1, c1))

        self.triangles.append(Triangle(b, f, f1))
        self.triangles.append(Triangle(f1, b1, b))

        self.triangles.append(Triangle(c, b, b1))
        self.triangles.append(Triangle(b1, c1, c))

        self.edges.append(Edge(f, b))
        self.edges.append(Edge(f1, b1))
        self.edges.append(Edge(b, b1))
        self.edges.append(Edge(c, c1))
        self.edges.append(Edge(b, c))
        self.edges.append(Edge(b1, c1))
예제 #4
0
def rigid_transform(p, R, t):
    p_ = Point3D(p)
    q = np.matmul(R, p_.p) + t
    if isinstance(p, Point3D):
        return Point3D(q)
    else:
        return q
    def _generate_back(self):
        a = self._l_t + Point3D(z=self._step_count * self._step_height)
        b = a + Point3D(self._width)

        self.triangles.append(Triangle(a, b, self._r_t))
        self.triangles.append(Triangle(a, self._l_t, self._r_t))

        self.edges.append(Edge(a, self._l_t))
        self.edges.append(Edge(b, self._r_t))
    def _generate_base(self):
        self._l_t = self._base_point
        self._r_t = self._l_t + Point3D(self._width)
        self._r_b = self._r_t + Point3D(y=self._step_count * self._step_length)
        self._l_b = self._r_b + Point3D(x=-self._width)

        self.triangles.append(Triangle(self._l_t, self._r_t, self._r_b))
        self.triangles.append(Triangle(self._l_t, self._l_b, self._r_b))

        self.edges.append(Edge(self._l_t, self._r_t))
        self.edges.append(Edge(self._r_t, self._r_b))
        self.edges.append(Edge(self._r_b, self._l_b))
        self.edges.append(Edge(self._l_b, self._l_t))
예제 #7
0
    def __init__(self, start_point, width, height, depth):
        # from lower to upper plane, clockwise
        self.p0 = Point3D(start_point.x, start_point.y, start_point.z)
        self.p1 = Point3D(start_point.x, start_point.y, start_point.z + depth)
        self.p2 = Point3D(start_point.x + width, start_point.y,
                          start_point.z + depth)
        self.p3 = Point3D(start_point.x + width, start_point.y, start_point.z)

        self.p4 = Point3D(start_point.x, start_point.y + height, start_point.z)
        self.p5 = Point3D(start_point.x, start_point.y + height,
                          start_point.z + depth)
        self.p6 = Point3D(start_point.x + width, start_point.y + height,
                          start_point.z + depth)
        self.p7 = Point3D(start_point.x + width, start_point.y + height,
                          start_point.z)

        self.points = [
            self.p0, self.p1, self.p2, self.p3, self.p4, self.p5, self.p6,
            self.p7
        ]
        self.edges = [
            Edge(self.p0, self.p1),
            Edge(self.p1, self.p2),
            Edge(self.p2, self.p3),
            Edge(self.p3, self.p0),  # down
            Edge(self.p0, self.p4),
            Edge(self.p1, self.p5),
            Edge(self.p2, self.p6),
            Edge(self.p3, self.p7),  # between
            Edge(self.p4, self.p5),
            Edge(self.p5, self.p6),
            Edge(self.p6, self.p7),
            Edge(self.p7, self.p4)
        ]  # up
예제 #8
0
파일: data.py 프로젝트: ZouLG/py_sfm
def generate_rand_points(num=1, loc=[0, 0, 0], scale=[1, 1, 1]):
    x = np.random.normal(loc[0], scale[0], (num, ))
    y = np.random.normal(loc[1], scale[1], (num, ))
    z = np.random.normal(loc[2], scale[2], (num, ))
    points = []
    for i in range(num):
        points.append(Point3D((x[i], y[i], z[i])))
    return points
예제 #9
0
def find_beta_n1(v, ctrl_pw):
    ctrl_pc = []
    for i in range(0, len(ctrl_pw)):
        ctrl_pc.append(Point3D(v[i * 3:i * 3 + 3]))
    dist_c = calc_dist(ctrl_pc)
    dist_w = calc_dist(ctrl_pw)
    scale = calc_sign(ctrl_pc) * np.matmul(dist_w, dist_c) / np.matmul(
        dist_c, dist_c)
    return np.array([0, 0, 0, scale])
예제 #10
0
    def illum_map_elevation(elevation):
        """ Illumination map for specific elevation. """
        lamp = Lamp(light, Point3D(0.0, 0.0, LAMP_HEIGHT), LAMP_AZIMUT_DEG,
                    elevation)

        plot_illuminance_map(lamp.illuminance,
                             x_distances,
                             y_distances,
                             color_levels=np.linspace(0, 500, 25),
                             title=f"Illumination [lux] (elev={elevation})")
예제 #11
0
파일: camera.py 프로젝트: ZouLG/py_sfm
def triangulation(n1, n2, p1, p2):
    delt = p2 - p1
    N1 = np.matmul(n1, n1) * np.eye(3) - np.matmul(n1.reshape(3, 1),
                                                   n1.reshape((1, 3)))
    N2 = np.matmul(n2, n2) * np.eye(3) - np.matmul(n2.reshape(3, 1),
                                                   n2.reshape((1, 3)))
    a2 = geo.quadratic_form(n2, N1, delt)
    b2 = geo.quadratic_form(n2, N1, n2)
    t2 = -a2 / b2
    v2 = t2 * n2
    P2 = Point3D(p2 + v2)

    a1 = geo.quadratic_form(n1, N2, -delt)
    b1 = geo.quadratic_form(n1, N2, n1)
    t1 = -a1 / b1
    v1 = t1 * n1
    P1 = Point3D(p1 + v1)
    P = (P1 + P2) / 2
    return P, P1, P2
예제 #12
0
    def __init__(self, start_point, width, depth, height, color):
        self.color = color
        p = start_point
        self.bottom_0 = Point3D(p.x, p.y, p.z)
        self.bottom_1 = Point3D(p.x + width, p.y, p.z)
        self.bottom_2 = Point3D(p.x + width, p.y, p.z + depth)
        self.bottom_3 = Point3D(p.x, p.y, p.z + depth)

        self.top_0 = Point3D(p.x, p.y + height, p.z)
        self.top_1 = Point3D(p.x + width, p.y + height, p.z)
        self.top_2 = Point3D(p.x + width, p.y + height, p.z + depth)
        self.top_3 = Point3D(p.x, p.y + height, p.z + depth)

        self.edge_bottom_0 = Edge(self.bottom_0, self.bottom_1)
        self.edge_bottom_1 = Edge(self.bottom_1, self.bottom_2)
        self.edge_bottom_2 = Edge(self.bottom_2, self.bottom_3)
        self.edge_bottom_3 = Edge(self.bottom_3, self.bottom_0)
        self.edge_vertical_0 = Edge(self.bottom_0, self.top_0)
        self.edge_vertical_1 = Edge(self.bottom_1, self.top_1)
        self.edge_vertical_2 = Edge(self.bottom_2, self.top_2)
        self.edge_vertical_3 = Edge(self.bottom_3, self.top_3)
        self.edge_top_0 = Edge(self.top_0, self.top_1)
        self.edge_top_1 = Edge(self.top_1, self.top_2)
        self.edge_top_2 = Edge(self.top_2, self.top_3)
        self.edge_top_3 = Edge(self.top_3, self.top_0)

        self.points = [self.bottom_0, self.bottom_1, self.bottom_2, self.bottom_3,
                       self.top_0, self.top_1, self.top_2, self.top_3]

        self.edges = [
            self.edge_bottom_0,
            self.edge_bottom_1,
            self.edge_bottom_2,
            self.edge_bottom_3,
            self.edge_vertical_0,
            self.edge_vertical_1,
            self.edge_vertical_2,
            self.edge_vertical_3,
            self.edge_top_0,
            self.edge_top_1,
            self.edge_top_2,
            self.edge_top_3
        ]

        self.faces = [
            Face([self.edge_bottom_0, self.edge_bottom_1, self.edge_bottom_2, self.edge_bottom_3], color),
            Face([self.edge_top_0, self.edge_top_1, self.edge_top_2, self.edge_top_3], color),
            Face([self.edge_bottom_0, self.edge_vertical_1, self.edge_top_0.inversed(), self.edge_vertical_0.inversed()], color),
            Face([self.edge_bottom_1, self.edge_vertical_2, self.edge_top_1.inversed(), self.edge_vertical_1.inversed()], color),
            Face([self.edge_bottom_2, self.edge_vertical_3, self.edge_top_2.inversed(), self.edge_vertical_2.inversed()], color),
            Face([self.edge_bottom_3, self.edge_vertical_0, self.edge_top_3.inversed(), self.edge_vertical_3.inversed()], color)
        ]
예제 #13
0
파일: camera.py 프로젝트: ZouLG/py_sfm
 def project_image2camera(self, p2d):
     """
     back-project the 2d image Point3D to the homogeneous coordinate in PinHoleCamera Frame
         p2d: Nx2 array of 2d key points
         return: list of homogeneous 3d points of in the PinHoleCamera Frame
     """
     p2d = np.column_stack((p2d, np.ones((p2d.shape[0], 1))))
     p3d_tmp = np.matmul(self.K_, p2d.T)
     p3d = []
     for i in range(p3d_tmp.shape[1]):
         p3d.append(Point3D(p3d_tmp[:, i]))
     return p3d
예제 #14
0
def estimate_pose_epnp(K, pw, pi, ctrl_num=4):
    n = len(pw)
    ctrl_pw = get_control_points(pw)
    bc = calc_barycentric(ctrl_pw, pw)
    L = np.zeros((n * 2, ctrl_num * 3))
    fu, fv = K[0, 0], K[1, 1]
    uc, vc = K[0, 2], K[1, 2]
    for i in range(n):
        for j in range(ctrl_num):
            L[i * 2, j * 3] = fu * bc[i, j]
            L[i * 2, j * 3 + 2] = (uc - pi[i, 0]) * bc[i, j]
            L[i * 2 + 1, j * 3 + 1] = fv * bc[i, j]
            L[i * 2 + 1, j * 3 + 2] = (vc - pi[i, 1]) * bc[i, j]
    u, z, eig_v = np.linalg.svd(L)
    eig_v = eig_v.T[:, -4:]
    # M = np.matmul(L.T, L)
    # lamda, eig_v = np.linalg.eig(M)
    # eig_v = eig_v[:, np.argsort(-lamda)][:, -4:]

    # set the initial beta for Epnp
    data = np.split(eig_v, 4)
    target = calc_dist(ctrl_pw)**2
    beta_best = find_beta_n1(eig_v[:, -1], ctrl_pw)
    solver = EpnpSolver(beta_best, data, target)
    err_best = solver.residual
    for i in range(2, 5):
        beta = find_beta_n234(eig_v[:, -i:], ctrl_pw, N=i)
        solver.coef = beta
        err = solver.forward()
        if err < err_best:
            beta_best = beta
            err_best = err
    solver.coef = beta_best
    solver.forward()
    # print("error before fine tune: %f" % err_best)

    # fine-tune beta
    k = 0
    while solver.residual > 5e-3 and k < 15:
        solver.solve()
        k += 1
        # print("error after %d iterations: %f" % (k, solver.residual))
    # print("error after fine-tune: %f" % solver.residual)

    # recover pose
    ctrl_pc = [Point3D(np.matmul(pc, solver.coef)) for pc in data]
    sign = calc_sign(ctrl_pc)
    ctrl_pc = [pc * sign for pc in ctrl_pc]
    R, t = geo.recover_Rt(ctrl_pc, ctrl_pw)
    return R, t
예제 #15
0
파일: camera.py 프로젝트: ZouLG/py_sfm
    def show(self, ax, color='blue', s=20):
        o = geo.rigid_inv_transform(Point3D((0, 0, 0)), self.R, self.t)
        o.plot3d(ax, color=color, s=s)
        ex = self.R[0, :]
        ey = self.R[1, :]
        ez = self.R[2, :]
        ax.quiver(o.x,
                  o.y,
                  o.z,
                  ex[0],
                  ex[1],
                  ex[2],
                  normalize=True,
                  color='red')
        ax.quiver(o.x,
                  o.y,
                  o.z,
                  ey[0],
                  ey[1],
                  ey[2],
                  normalize=True,
                  color='green')
        ax.quiver(o.x,
                  o.y,
                  o.z,
                  ez[0],
                  ez[1],
                  ez[2],
                  normalize=True,
                  color='blue')

        sx, sy = [self.f / self.fx, self.f / self.fy]
        c = o + ez * self.f
        p0 = c - ex * sx * self.img_w / 2 - ey * sy * self.img_h / 2
        p1 = c + ex * sx * self.img_w / 2 - ey * sy * self.img_h / 2
        p2 = c + ex * sx * self.img_w / 2 + ey * sy * self.img_h / 2
        p3 = c - ex * sx * self.img_w / 2 + ey * sy * self.img_h / 2

        p0.plot3d(ax, color='red', marker='.', s=20)
        p1.plot3d(ax, color=color, marker='.', s=20)
        p3.plot3d(ax, color=color, marker='.', s=20)
        p2.plot3d(ax, color=color, marker='.', s=20)
        ax.plot3D([p0.x, p1.x], [p0.y, p1.y], [p0.z, p1.z], color=color)
        ax.plot3D([p1.x, p2.x], [p1.y, p2.y], [p1.z, p2.z], color=color)
        ax.plot3D([p2.x, p3.x], [p2.y, p3.y], [p2.z, p3.z], color=color)
        ax.plot3D([p3.x, p0.x], [p3.y, p0.y], [p3.z, p0.z], color=color)
        ax.plot3D([o.x, p0.x], [o.y, p0.y], [o.z, p0.z], color=color)
        ax.plot3D([o.x, p1.x], [o.y, p1.y], [o.z, p1.z], color=color)
        ax.plot3D([o.x, p2.x], [o.y, p2.y], [o.z, p2.z], color=color)
        ax.plot3D([o.x, p3.x], [o.y, p3.y], [o.z, p3.z], color=color)
예제 #16
0
    def initialize(self):
# (wysokosc, glebokosc, wysokosc)
##popuste plasterki
        # self.shapes.append(Cuboid(Point3D(2, -5, 31), 20, 0.2, 20, "green"))
        # self.shapes.append(Cuboid(Point3D(2, -7, 31.5), 20, 0.2, 20, "blue"))
        # self.shapes.append(Cuboid(Point3D(2, -9, 32), 20, 0.2, 20, "red"))
        # self.shapes.append(Cuboid(Point3D(2, -11, 32.5), 20, 0.2, 20, "yellow"))

    #oplasterki
        self.shapes.append(Cuboid(Point3D(2, -5, 31), 20, 0.2, 20, "green"))
        self.shapes.append(Cuboid(Point3D(2, -5, 31.5), 20, 0.2, 20, "blue"))
        self.shapes.append(Cuboid(Point3D(2, -5, 32), 20, 0.2, 20, "red"))
        self.shapes.append(Cuboid(Point3D(2, -5, 32.5), 20, 0.2, 20, "yellow"))
        self.shapes.append(Cuboid(Point3D(2, -5, 33), 20, 0.2, 20, "green"))
        self.shapes.append(Cuboid(Point3D(2, -5, 33.5), 20, 0.2, 20, "blue"))
        self.shapes.append(Cuboid(Point3D(2, -5, 34), 20, 0.2, 20, "red"))
        self.shapes.append(Cuboid(Point3D(2, -5, 34.5), 20, 0.2, 20, "yellow"))
예제 #17
0
    def __init__(self,
                 light: Light,
                 position: Point3D = Point3D(0.0, 0.0, 0.0),
                 azimut_deg: float = 0.0,
                 elevation_deg: float = 0.0):
        """
        :param position: [m]
        :param azimut [deg] - light azimut counterclockwise from light front direction
                       (-90 .. right, 0 .. front, 90 .. left)"
  
        :param elevation [deg] - light elevation
                          (0 - standard mounting, positive - lighten longer distance, negative - shorter)

        """
        self._light = light
        self._position = position
        self._azimut_deg = azimut_deg
        self._elevation_deg = elevation_deg
예제 #18
0
    def __init__(self, master):
        self.width = 800
        self.height = 800
        self.move_step = 2
        self.zoom_step = 5
        self.rotate_step = math.pi / 18

        self.canvas = tk.Canvas(master, width=self.width, height=self.height, bg="black")
        self.canvas.pack()

        self.painter = Painter(self)

        self.camera = Point3D(0, 0, 0)
        self.d = 200

        self.shapes = []
        self.initialize()
        self.draw()
예제 #19
0
파일: camera.py 프로젝트: ZouLG/py_sfm
 def show_projection(self, ax, pw):
     pi, pc = self.project_world2image(pw)
     for i, p in enumerate(pw):
         o = geo.rigid_inv_transform(Point3D((0, 0, 0)), self.R, self.t)
         x = pc[i] / pc[i].z
         xi = pi[i]
         p.plot3d(ax, s=10, marker='o', color='blue')
         if not self.is_out_of_bound(xi):
             ax.plot3D([o.x, p.x], [o.y, p.y], [o.z, p.z],
                       color='blue',
                       linestyle='--',
                       linewidth=1)
             xw = geo.rigid_inv_transform(x, self.R, self.t)
             xw.plot3d(ax, s=10, marker='o', color='red')
         else:
             ax.plot3D([o.x, p.x], [o.y, p.y], [o.z, p.z],
                       color='red',
                       linestyle='--',
                       linewidth=1)
예제 #20
0
def recover_Rt(pc, pw):
    n = len(pc)
    center_c = Point3D((0, 0, 0))
    center_w = center_c
    for p, q in zip(pc, pw):
        center_c += p
        center_w += q
    center_c /= n
    center_w /= n
    mc = list2mat(pc)
    mw = list2mat(pw)
    mc_c = mc - np.tile(center_c.p, (n, 1))
    mw_c = mw - np.tile(center_w.p, (n, 1))
    R = np.matmul(np.linalg.pinv(np.matmul(mw_c.T, mw_c)),
                  np.matmul(mw_c.T, mc_c))
    U, Z, V = np.linalg.svd(R)
    R = np.matmul(U, V)
    ta = mc - np.matmul(mw, R)
    t = np.mean(ta, axis=0)
    return R.T, t
예제 #21
0
파일: camera.py 프로젝트: ZouLG/py_sfm
def test_camera_func():
    import matplotlib.pyplot as plt
    plt.figure()
    ax = plt.gca(projection='3d')
    p1 = Point3D([2, 6, 7])
    p1.plot3d(ax, s=5)

    R = geo.rodriguez((1, 1, 1), np.pi / 4)
    camera1 = PinHoleCamera(R=R)
    camera2 = PinHoleCamera(R=R, t=np.array((-4, -4, 0)))
    camera1.show(ax)
    camera2.show(ax)

    pi1, _ = camera1.project_world2image([p1])
    pi2, _ = camera2.project_world2image([p1])
    pw, _, _ = camera_triangulation(camera1, camera2, pi1, pi2)
    print(pw)

    # pi = np.array([[1920, 1080]])
    # pc = camera1.project_image2camera(pi)
    # pi_ = camera1.project_camera2image(pc)
    # print(pi_)
    # print(pc)
    #
    # pw = camera1.project_camera2world(pc)
    # pc_ = camera1.project_world2camera(pw)
    # print(pc_)
    # pw[0].plot3d(ax, s=5, marker='x', color='red')

    # camera1.show_projection(ax, [p1])
    # img = camera1.get_projected_img([p1])
    # plt.figure()
    # plt.imshow(img)

    ax.set_xlim([-3, 10])
    ax.set_ylim([-3, 10])
    ax.set_zlim([-3, 10])
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_zlabel("z")
    plt.show()
    def __init__(self, step_height, width, step_count):
        if step_count < 1:
            raise Exception('Step count must be more than 0')

        self.triangles = []
        self.edges = []
        self.x_angle = 0
        self.y_angle = 0
        self.z_angle = 0
        self.dx = 0
        self.dy = 0
        self.dz = 0
        self.scale = 1

        self._step_height = step_height
        self._step_length = step_height * 2
        self._width = width
        self._step_count = step_count
        self._base_point = Point3D()

        self._generate()
예제 #23
0
파일: camera.py 프로젝트: ZouLG/py_sfm
def camera_triangulation(camera1, camera2, pi1, pi2, filter=False):
    """
    get the back-projected 3D points from the 2D key-points in two views
        camera1 & camera2: the two cameras
        pi1 & pi2: Nx2 array which stores the 2D key-points of the two cameras
        filter: ignore points which have small view angles when filter is True
        return: reconstructed points' coordinates in world-frame
    """
    assert pi1.shape == pi2.shape
    pc1 = camera1.project_image2camera(pi1)
    pc2 = camera2.project_image2camera(pi2)
    r = np.matmul(camera2.R, camera1.R.T)
    a = np.matmul(r, camera1.t) - camera2.t
    pw = []
    for p, q in zip(pc1, pc2):
        b = geo.cross_mat(q.p)
        x = np.matmul(np.matmul(b, r), p.p)
        y = np.matmul(b, a)
        s1 = np.matmul(x, y) / np.matmul(x, x)
        pw.append(Point3D(np.matmul(camera1.R.T, s1 * p.p - camera1.t)))
    return pw, None, None
예제 #24
0
파일: camera.py 프로젝트: ZouLG/py_sfm
 def place_a_camera(p,
                    ez,
                    ex,
                    f=1.0,
                    fx=500,
                    fy=500,
                    img_w=1920,
                    img_h=1080):
     ez /= np.linalg.norm(ez)
     ex = ex - np.matmul(ez, ex) * ez
     ex /= np.linalg.norm(ex)
     ey = np.cross(ez, ex)
     R = np.row_stack((ex, ey, ez))
     t = -np.matmul(R, Point3D(p).p)
     return PinHoleCamera(R=R,
                          t=t,
                          f=f,
                          fx=fx,
                          fy=fy,
                          img_w=img_w,
                          img_h=img_h)
예제 #25
0
 def calc_gravity_center(self):
     center = Point3D(0, 0, 0)
     for point in self.unique_points():
         center = center.add_point(point)
     return center.divide_by_scalar(len(self.unique_points()))
예제 #26
0
파일: file_op.py 프로젝트: ZouLG/py_sfm
def test_plot_map():
    pw = [Point3D((0, 0, 0)), Point3D((0, 0, 10)), Point3D((20, 0, 0))]
    # plot_map(pw, [])
    plt.show()
예제 #27
0
 def initialize(self):
     self.shapes.append(Cuboid(Point3D(2, -5, 20), 10, 10, 10, "red"))
     self.shapes.append(Cuboid(Point3D(-12, -5, 20), 10, 10, 10, "blue"))
     self.shapes.append(Cuboid(Point3D(2, -5, 32), 10, 10, 10, "green"))
     self.shapes.append(Cuboid(Point3D(-12, -5, 32), 10, 10, 10, "yellow"))
예제 #28
0
파일: file_op.py 프로젝트: ZouLG/py_sfm
def test_save_ply():
    file_name = "../data/test.ply"
    save_to_ply([Point3D((0, 0, 0)), Point3D((0, 1, 0))], file_name)
예제 #29
0
from datetime import time

from actuator import Actuator
import sys
from point import Point3D
import math

actuator = Actuator(['y', [80, 0., 0.], 'z', [80, 0., 0.], 'z', [120, 0., 0.]])

args = sys.argv

angles = actuator.inverse_kinematics(
    Point3D(int(args[1]), int(args[2]), int(args[3])))
for i, angle in enumerate(angles):
    angles[i] = round(angle, 2)
sys.stdout.write("{0} {1} {2}".format(angles[0], angles[1], angles[2]))
예제 #30
0
def calc_center(plist):
    center = Point3D((0, 0, 0))
    for p in plist:
        center += p
    center = center / len(plist)
    return center