← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/visualiser/dispxyz.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-14 00:20:26 UTC
  • Revision ID: vikasdhi@buffalo.du-20121114002026-e4rbu85n6ekroafk
Removed unnecessary term from compile.sh

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/visualiser/dispxyz.py
 
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