~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/visualiser/dispxyz.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-20 22:12:16 UTC
  • mfrom: (0.5.3 mrol)
  • Revision ID: vikasdhi@buffalo.edu-20121120221216-nunda3pv0n8jsf4e
MergingĀ lp:~wecacuee/mrol/mrol-devĀ 

Show diffs side-by-side

added added

removed removed

Lines of Context:
1
 
#! /usr/bin/python
2
 
'''Displays the points in an xyz file the name of which is supplied on the 
3
 
command line. The file should have a format like
4
 
0 0 0
5
 
0.2 1 0.2
6
 
1 2 3
7
 
1 2 3.3
8
 
 
9
 
Author: Julian Ryde
10
 
'''
11
 
#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
12
 
#
13
 
#This program is distributed in the hope that it will be useful,
14
 
#but WITHOUT ANY WARRANTY; without even the implied warranty of
15
 
#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
 
#GNU Lesser General Public License for more details.
17
 
#
18
 
#You should have received a copy of the GNU Lesser General Public License
19
 
#along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
 
 
21
 
# TODO circular references between dispxyz and robotvisualiser
22
 
# fixed for now with delayed import statements, however should really fix 
23
 
# properly
24
 
 
25
 
 
26
 
import os
27
 
import time
28
 
import numpy as np
29
 
import visual
30
 
import sys
31
 
import matplotlib.cm
32
 
 
33
 
import mrol_mapping.pointcloud as pointcloud
34
 
import mrol_mapping.poseutil as poseutil
35
 
 
36
 
from threading import Thread
37
 
 
38
 
class EnableMouseThread(Thread):
39
 
    def __init__(self, scene):
40
 
        Thread.__init__(self)
41
 
        self.mouse_thread_enabled = True
42
 
        self.scene = scene
43
 
        
44
 
    def run(self):
45
 
        delta = 0
46
 
        self.dragging = False
47
 
        while self.mouse_thread_enabled:
48
 
            if self.scene.mouse.events:
49
 
                m1 = self.scene.mouse.getevent()
50
 
                if m1.click == 'left':
51
 
                    if m1.pickpos != None:
52
 
                        self.scene.center = m1.pickpos
53
 
                if m1.drag == 'left' and self.dragging == False:
54
 
                    self.drag_pos = np.asarray(self.scene.mouse.pos)
55
 
                    self.dragging = True
56
 
                if m1.drop == 'left':
57
 
                    self.dragging = False
58
 
            if self.dragging:
59
 
                new_pos = np.asarray(self.scene.mouse.pos)
60
 
                if (new_pos + delta != self.drag_pos).all(): # if mouse has moved
61
 
                    delta = (self.drag_pos - new_pos)
62
 
                    self.scene.center = self.scene.center + delta
63
 
                    self.drag_pos = new_pos + delta # update drag position
64
 
            time.sleep(0.01)
65
 
    def stop(self):
66
 
        self.mouse_thread_enabled = False
67
 
 
68
 
 
69
 
def rotating(xyzs):
70
 
    """ function used externally by other programs to show continuosly rotating 
71
 
    plot of points """
72
 
    plotgrid(xyzs)
73
 
 
74
 
    az = 0
75
 
    daz = np.radians(1)
76
 
    visual.scene.up = (0, 0, 1)
77
 
    while True:
78
 
        time.sleep(0.05)
79
 
        az += daz
80
 
        visual.scene.forward = (np.sin(az), np.cos(az), -1)
81
 
 
82
 
def colors(X, bounds=None):
83
 
    assert len(X) > 0
84
 
    if bounds == None:
85
 
        minx = min(X)
86
 
        maxx = max(X)
87
 
    else:
88
 
        minx = bounds[0]
89
 
        maxx = bounds[1]
90
 
 
91
 
    mcm = matplotlib.cm.jet
92
 
    #mcm = matplotlib.cm.GnBu
93
 
    #mcm = matplotlib.cm.gray
94
 
    return mcm((X-minx)/(maxx-minx))[:, :3]
95
 
 
96
 
def showpts(xyzs, pose=None, size=2, color=None):
97
 
    if pose is not None:
98
 
        xyzs = pose.transformPoints(xyzs)
99
 
        # draw origin
100
 
        import mrol_mapping.visualiser.robotvisualiser as robotvisualiser
101
 
        robotvisualiser._draw_axes((0,0,0))
102
 
 
103
 
    if color is None:
104
 
        C = colors(xyzs[:, 2])
105
 
    else:
106
 
        C = color
107
 
    P = visual.points(pos=xyzs, color=C, shape='square', size=size)
108
 
    #P.visible = False
109
 
    #visual.scene.visible = False
110
 
    #del P
111
 
    return P
112
 
 
113
 
def plotgrid(xyzs, separation=1, pointsize=2, grid_pt_size=1):
114
 
    global vis_gridpts
115
 
    mins = np.amin(xyzs, 0).astype(int)
116
 
    maxs = np.amax(xyzs, 0).astype(int)
117
 
    minz = np.amin(xyzs[:, 2])
118
 
    d = separation
119
 
    XX, YY = np.meshgrid(range(mins[0]/d, maxs[0]/d), range(mins[1]/d, maxs[1]/d)) 
120
 
    XX = d*XX.ravel()
121
 
    YY = d*YY.ravel()
122
 
    ZZ = minz*np.ones_like(XX)
123
 
    gridpts = np.vstack((XX, YY, ZZ)).T
124
 
    visual.scene.background = (1, 1, 1)
125
 
    vis_gridpts = visual.points(pos=gridpts, size=grid_pt_size, color=(0, 0, 0))
126
 
    vis_pts = showpts(xyzs, size=pointsize)
127
 
    return vis_pts
128
 
    
129
 
def plot2ptlines(pts1, pts2, color=None):
130
 
    pts1 = np.asarray(pts1)
131
 
    pts2 = np.asarray(pts2)
132
 
    assert pts1.shape == pts2.shape, 'pts1 and pts2 must have the sime dimensions'
133
 
    assert pts1.shape[1] == 3, 'pts must be Nx3'
134
 
    assert pts2.shape[1] == 3, 'pts must be Nx3'
135
 
    
136
 
    dist = np.zeros(pts1.shape[0])
137
 
    handles = []
138
 
    for i, p1 in enumerate(pts1):
139
 
        p2 = pts2[i,:]
140
 
        dist[i] = np.sqrt(np.abs(np.sum(p2**2 - p1**2)))
141
 
        assert not np.isnan(dist[i])
142
 
    if color == None:    
143
 
        C = colors(dist)
144
 
    else:
145
 
        C = np.asarray(color)
146
 
    for i in range(pts1.shape[0]):
147
 
        handles.append(visual.curve(pos=[pts1[i,:],pts2[i,:]], color=C[i,:]))
148
 
    return handles
149
 
    
150
 
def plotaxis(length=1.):
151
 
    return plot2ptlines([(0,0,0),(0,0,0),(0,0,0)],[(length,0,0),(0,length,0),(0,0,length)],color=[(1,0,0),(0,1,0),(0,0,1)])
152
 
 
153
 
# TODO merge the rotating code below with the rotating function
154
 
if __name__ == '__main__':
155
 
    if len(sys.argv) == 1:
156
 
        print __doc__
157
 
        sys.exit()
158
 
    fname = sys.argv[1]
159
 
    rotate = '--rotating' in sys.argv[1:]
160
 
    grid = '--grid' in sys.argv[1:]
161
 
    az = 0
162
 
    daz = np.radians(1)
163
 
    visual.scene.select()
164
 
    visual.scene.up = (0, 0, 1)
165
 
    visual.scene.forward = (np.sin(az), np.cos(az), -1)
166
 
    M = pointcloud.load(fname)
167
 
 
168
 
    # approximately center the view on the point cloud
169
 
    visual.scene.center = M.points.mean(axis=0) 
170
 
    if not rotate:
171
 
        MT = EnableMouseThread(visual.scene)
172
 
        MT.start()
173
 
 
174
 
    while True:
175
 
        pc = pointcloud.load(fname)
176
 
        M = pc.points
177
 
        if grid:
178
 
            vis_pts = plotgrid(M)
179
 
        else:
180
 
            vis_pts = showpts(M)
181
 
 
182
 
    # wait for file to change
183
 
        lastmodified = os.path.getmtime(fname)
184
 
        print 'Displaying ', len(M), 'points'
185
 
        while lastmodified == os.path.getmtime(fname):
186
 
            time.sleep(0.05)
187
 
            if rotate:
188
 
                az += daz 
189
 
                visual.scene.forward = (np.sin(az), np.cos(az), -1)
190
 
        vis_pts.visible = False
191
 
        vis_gridpts.visible=False