python_tools_3Dpicture/draw_points.py at master · fangdefuo/python_tools_3Dpicture · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
# -*- coding: utf-8 -*-
"""
Created on Thu Jun 15 18:18:24 2017

@author: hyj
"""

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from GeometryLib import  drawCoordinateFrame, euler2Rbn,euler2Rnb
import transformations as tf

import os
#filepath=os.path.abspath('.')  #表示当前所处的文件夹的绝对路径
filepath=os.path.abspath('..')+"/bin"  #表示当前所处的文件夹上一级文件夹的绝对路径

point_id=[]
x=[]
y=[]
z=[]

with open(filepath + '/all_points.txt', 'r') as f:
    data = f.readlines()  #txt中所有字符串读入data

    for line in data:
        odom = line.split()        #将单个数据分隔开存好
        numbers_float = map(float, odom) #转化为浮点数
        x.append( numbers_float[0] )
        y.append( numbers_float[1] )
        z.append( numbers_float[2] )


position = []
quaterntions = []
timestamp = []
qw_index = 1
with open(filepath + '/cam_pose.txt', 'r') as f:   #   imu_circle   imu_spline

    data = f.readlines()  #txt中所有字符串读入data
    for line in data:
        odom = line.split()        #将单个数据分隔开存好
        numbers_float = map(float, odom) #转化为浮点数

        #timestamp.append( numbers_float[0])
        quaterntions.append( [numbers_float[qw_index], numbers_float[qw_index+1],numbers_float[qw_index+2],numbers_float[qw_index+3]   ] )   # qw,qx,qy,qz
        position.append( [numbers_float[qw_index+4], numbers_float[qw_index+5],numbers_float[qw_index+6] ] )


## plot 3d
fig = plt.figure()
plt.ion()
ax = fig.gca(projection='3d')

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
rpy = []
t = []
for i in range(0,400,5):
    ax.clear()
    ax.scatter(x, y, z,c='g')

    x1=[]
    y1=[]
    z1=[]
    rpy.append( tf.euler_from_quaternion(quaterntions[i]) )
    t.append( position[i] )
    p = position[i]
    for j in range(len(rpy)):
        drawCoordinateFrame(ax, rpy[j], t[j])

    #s = filepath + '/keyframe/all_points_' +str(i)+'.txt'
   # with open(s, 'r') as f:
    #    data = f.readlines()  #txt中所有字符串读入data
    #    for line in data:
     #       odom = line.split()        #将单个数据分隔开存好
      #      numbers_float = map(float, odom) #转化为浮点数
      #      x1.append( numbers_float[0] )
       #     y1.append( numbers_float[1] )
        #    z1.append( numbers_float[2] )
         #
          #  ax.plot( [ numbers_float[0],   p[0]  ] , [ numbers_float[1], p[1] ] , zs=[ numbers_float[2], p[2] ] )

    s = filepath + '/house_model/house.txt'
    with open(s, 'r') as f:
        data = f.readlines()  # txt中所有字符串读入data
        for line in data:
            odom = line.split()  # 将单个数据分隔开存好
            numbers_float = map(float, odom)  # 转化为浮点数
            ax.plot([numbers_float[0], numbers_float[3]], [numbers_float[1], numbers_float[4]],'b' ,zs=[numbers_float[2], numbers_float[5]])

    ax.scatter(x1, y1, z1,c='r',marker='^')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_xlim(-15, 20)
    ax.set_ylim(-15, 20)
    ax.set_zlim(0, 20)
    ax.legend()
    plt.show()
    plt.pause(0.01)