def frame_rel(self,frame_rel,*args):
if len(args)==1:
scale = args[0]
else:
if frame_rel == None:
scale = 1
else:
scale = frame_rel.axis.scale
visible = self.visible
visible_label = self.visible_label
visible_frame = self.axis.visible
visible_frame_label = self.axis.visible_label
color = self.color
self.visible = False
del self.__frame_rel
del self.__frame_obj
if frame_rel == None:
self.__frame_rel = None
self.__frame_obj = visual.frame(dis=self.__dis,visible=visible)
else:
self.__frame_rel = frame_rel
self.__frame_obj = visual.frame(frame=self.__frame_rel.__frame_obj,pos=self.__dis,visible=visible)
self.__frame_obj.axis = self.__rot[:,0]
self.__frame_obj.up = self.__rot[:,1]
self.__point_obj = visual.points(frame=self.__frame_obj,pos=(0,0,0),color=color,size=4,size_units="pixels",shape="round")
self.__label_obj = visual.label(frame=self.__frame_obj,text=self.__label,visible=visible_label,
pos=(0,0,0),xoffset=5*scale,yoffset=1*scale,space=5*scale,line=0,height=16*scale,border=0,font='sans',color=color,box=False,opacity=0)
self.axis = axes(frame_obj=self.__frame_obj,visible=visible_frame,visible_label=visible_frame_label,scale=scale)
def fitICP(model,target):
Tf=np.eye(4,4)
dif=100
nIter=0
while nIter<MAX_ITER and dif>MIN_DIF:
T1,pit1=ICPstep(model,target)
Tf=Tf.dot(T1)
dif=distData(model,pit1)
#saDif=vector(sum(abs(model-pit1)))
#dif=saDif.mag #difference with respect to the anterior model
print nIter,dif
points(pos=pit1,size=2,color=(1,0,1))
model=pit1
nIter+=1
#print nIter,dif
return Tf,pit1
def fitICP(model, target):
Tf = np.eye(4, 4)
dif = 100
nIter = 0
while nIter < MAX_ITER and dif > MIN_DIF:
T1, pit1 = ICPstep(model, target)
Tf = Tf.dot(T1)
dif = distData(model, pit1)
#saDif=vector(sum(abs(model-pit1)))
#dif=saDif.mag #difference with respect to the anterior model
print nIter, dif
points(pos=pit1, size=2, color=(1, 0, 1))
model = pit1
nIter += 1
#print nIter,dif
return Tf, pit1
def __init__(self, npts=100000, Rotate=False, title=''):
ptsize = 3
mapptsize = 1
self.scene_angle = 0
self.localscene = visual.display(title=title + ' Local Coordinate System')
self.globalscene = visual.display(title=title + ' Global Coordinate System')
#self.globalscene.background = (1, 1, 1)
#self.globalscene.up = (0, 0, 1)
#self.globalscene.forward = (0, 1, -0.4)
self.currentrow = 0
self.auto_colour = False
# Set up the global coordinate frame visualiser
self.globalscene.select()
self.globalscene.up =(0, 0, 1.0)
#self.visualrobot = visual.box(length=3, height=2, width=1.5)
w = 1
wid = 0.2
self.robotpts = np.array(( (0, -wid, 0, w), (0, wid, 0, w), (3*wid, 0, 0, w), (0, -wid, 0, w) ))
#self.visualrobot = visual.points(pos=self.robotpts[:, :3], size=4, shape='round', color=(0, 1, 1))
self.visualrobot = visual.curve(pos=self.robotpts[:, :3], color=(0, 1, 1))
X = np.array([(-1, -1), (-1, 1), (1, 1), (1, -1), (-1, -1)])
square = visual.curve(pos=50*X)
self.leftmappts = visual.points(size=ptsize, shape='square', color=(1, 0, 0))
self.rightmappts = visual.points(size=ptsize, shape='square', color=(0, 1, 0))
self.spinmappts = visual.points(size=ptsize, shape='square', color=(0, 0, 1))
X = np.zeros((npts, 3))
self.mappts = visual.points(pos=X, size=mapptsize, shape='square', color=(1, 1, 1))
self.trajpts_ind = 0
self.trajpts = visual.points(pos=np.zeros((10000, 3)), color=(0, 0, 1), size=2)
visual.scene.show_rendertime = True
if Rotate:
# Enable continuous rotation
RV = RotateVisual(self.globalscene)
RV.start()
else:
# Enable mouse panning
MT = dispxyz.EnableMouseThread(self.globalscene)
MT.start()
# Set up the local coordinate frame visualiser
if showlocal:
self.localscene.select()
self.leftpts = visual.points(color=(1, 0, 0), size=ptsize, shape='square')
self.rightpts = visual.points(color=(0, 1, 0), size=ptsize, shape='square')
self.spinpts = visual.points(color=(0, 0, 1), size=ptsize, shape='square')
visual.scene.show_rendertime = True
self.colourmin = -4
self.colourmax = 4
def RawRead(fname, pt_size):
f = open(fname,"r")
tuples = []
for line in f:
line = line.strip()
arr = line.split("\t")
tuples.append((float(arr[2]),float(arr[1]),float(arr[0])))
f.close()
return visual.points(pos=tuples, size=pt_size, color=visual.color.black)
def InitVisuals():
from visual import display, points, box
window = display(title="F16 Simulation",
width=Resolution[0],
height=Resolution[1])
window.up = np.identity(3)[2]
window.forward = -np.ones(3)
window.center = pos
if FPV:
window.range = 15 * 3 #2
if stereo is not None:
window.stereo = stereo
D = 2500 #5000#2500
#--- CS ---
for i in range(3):
arrow(axis=np.identity(3)[i] * 1000,
color=np.identity(3)[i],
opacity=0.75)
#--- Point Cloud ---
if 1:
Points = []
d = 500 #100
n = int(D / d)
for i in range(-n, n + 1):
for j in range(-n, n + 1):
for k in range(0, n + 1):
p = np.array([i, j, k]).astype(float) * d
Points.append(tuple(p)) #+=list(p)
points(pos=Points)
if 1:
box(width=D * 2,
height=D * 2,
depth=10,
axis=-np.identity(3)[2],
color=[0.6, 0.3, 0.15])
return window
def c_trail(i, Count, Nlenght, Nupdate):
if Count[1] % Nupdate == 0 and i == obj[0]:
Count[0] += 1
if Count[1] % Nupdate == 0:
if Nlenght < Count[0] <= 2 * Nlenght + 1:
i.trail[Count[0] % Nlenght - 1].pos = i.pos
elif Count[0] > 2 * Nlenght + 1:
Count[0] = Nlenght + 1
else:
i.trail.append(v.points(pos=i.pos, size=1,
color=v.color.white))
def plasma(x, y, width, height, c1, c2, c3, c4):
newWidth = width / 2
newHeight = height / 2
global idx, dots
if (width > gridSize or height > gridSize):
#Randomly displace the midpoint!
midPoint = (c1 + c2 + c3 + c4) / 4 + Displace(rand)
#Calculate the edges by averaging the two corners of each edge.
edge1 = (c1 + c2) / 2
edge2 = (c2 + c3) / 2
edge3 = (c3 + c4) / 2
edge4 = (c4 + c1) / 2
#Do the operation over again for each of the four new grids.
plasma(x, y, newWidth, newHeight, c1, edge1, midPoint, edge4)
plasma(x + newWidth, y, newWidth, newHeight, edge1, c2, edge2,
midPoint)
plasma(x + newWidth, y + newHeight, newWidth, newHeight,
midPoint, edge2, c3, edge3)
plasma(x, y + newHeight, newWidth, newHeight, edge4, midPoint,
edge3, c4)
else:
#This is the "base case," where each grid piece is less than the size of a pixel.
c = (c1 + c2 + c3 + c4) / 4
# dots[idx] = c
# idx = idx + 1
# print(c)
if (c > 0.5):
c = 0
if (draw == True):
visual.points(pos=[x - (100), c, y - (100)],
color=(1, 0.31, 0.1))
dots[idx] = c
idx = idx + 1
else:
c = 5
if (draw == True):
visual.points(pos=[x - (100), c, y - (100)],
color=(0.8, 1, 1))
dots[idx] = c
idx = idx + 1
def showpts(xyzs, pose=None, size=2, color=None):
if pose is not None:
xyzs = pose.transformPoints(xyzs)
if color is None:
C = colors(xyzs[:, 2])
else:
C = color
P = visual.points(pos=xyzs, color=C, shape='square', size=size)
#P.visible = False
#visual.scene.visible = False
#del P
return P
def showpts(xyzs, pose=None, size=2, color=None):
if pose is not None:
xyzs = pose.transformPoints(xyzs)
if color is None:
C = colors(xyzs[:, 2])
else:
C = color
P = visual.points(pos=xyzs, color=C, shape='square', size=size)
#P.visible = False
#visual.scene.visible = False
#del P
return P
def __init__(self, **kwargs):
self.container = visual.frame(**kwargs)
self.box = visual.box(frame=self.container,
length=self.size[0],
width=self.size[1],
height=self.size[2],
color=(0, 0, 0.7))
self.lidarframe = visual.frame(frame=self.container,
pos=(self.size[0] / 2.0,
self.size[1] / 2.0, self.size[2]))
self.lidarpoints = visual.points(pos=[(0, 0, 0) for i in range(360)],
frame=self.lidarframe,
size=5,
color=(0, 1, 0))
print(repr(self.lidarpoints))
def plotgrid(xyzs, separation=1, pointsize=2, grid_pt_size=1):
global vis_gridpts
mins = np.amin(xyzs, 0).astype(int)
maxs = np.amax(xyzs, 0).astype(int)
minz = np.amin(xyzs[:, 2])
d = separation
XX, YY = np.meshgrid(range(mins[0]/d, maxs[0]/d), range(mins[1]/d, maxs[1]/d))
XX = d*XX.ravel()
YY = d*YY.ravel()
ZZ = minz*np.ones_like(XX)
gridpts = np.vstack((XX, YY, ZZ)).T
visual.scene.background = (1, 1, 1)
vis_gridpts = visual.points(pos=gridpts, size=grid_pt_size, color=(0, 0, 0))
vis_pts = showpts(xyzs, size=pointsize)
return vis_pts
def plotgrid(xyzs, separation=1, pointsize=2, grid_pt_size=1):
global vis_gridpts
mins = np.amin(xyzs, 0).astype(int)
maxs = np.amax(xyzs, 0).astype(int)
minz = np.amin(xyzs[:, 2])
d = separation
XX, YY = np.meshgrid(range(mins[0] / d, maxs[0] / d),
range(mins[1] / d, maxs[1] / d))
XX = d * XX.ravel()
YY = d * YY.ravel()
ZZ = minz * np.ones_like(XX)
gridpts = np.vstack((XX, YY, ZZ)).T
visual.scene.background = (1, 1, 1)
vis_gridpts = visual.points(pos=gridpts,
size=grid_pt_size,
color=(0, 0, 0))
vis_pts = showpts(xyzs, size=pointsize)
return vis_pts
def main():
if len(argv) < 2:
raise Exception('>>> ERROR! Please supply a data file name <<<')
dataFile = open(argv[1], 'r')
# scene basics
scene.center = (0,0,0)
scene.width = 1800
scene.height = 1000
# scene.range = (10.0, 10.0, 10.0)
scene.range = (1.0e9, 1.0e9, 1.0e9)
# get data dimensions
line = dataFile.readline()
bodies = loads(line)
pRange = range(len(bodies))
# set up the balls
colours = [ (1.0, 1.0, 0.0), (1.0, 1.0, 1.0), (0.0, 1.0, 0.0), (0.0, 0.5, 0.5), (1.0, 0.0, 0.0), (0.5, 1.0, 0.0), (1.0, 0.0, 1.0), (1.0, 0.5, 0.0), (0.0, 1.0, 1.0), (1.0, 1.0, 1.0) ]
spheres = []
for j in pRange:
p = bodies[j]
r = 1000000.0 * log10(p['mass']**(1.0 / 3.0))
# ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = 0.1 * p['mass']**(1.0 / 3.0), color = colours[j])
ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = r, color = colours[j])
# ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = 10000000.0, color = colours[j])
ball.trail = points(color = ball.color, size = 1)
spheres.append(ball)
while line:
rate(60)
bodies = loads(line)
X = Y = Z = mT = 0.0;
for j in pRange: # COG correction
p = bodies[j]
X += p['qX'] * p['mass']
Y += p['qY'] * p['mass']
Z += p['qZ'] * p['mass']
mT += p['mass']
for j in pRange:
p = bodies[j]
ball = spheres[j]
position = (p['qX'] - X / mT, p['qY'] - Y / mT, p['qZ'] - Z / mT)
ball.pos = position
ball.trail.append(pos = position, retain = 1000)
line = dataFile.readline()
def PlotIsingSystem2D(filename):
try:
f = open(filename,'r')
data = json.loads(f.read())
f.close()
except Exception as e:
return str(e)
print("Beta: %f" % data["Beta"])
scene = vs.display(title='3D representation',
x=0, y=0, width=1440, height=1080,background=(0,0,0)
)
spin_up_color = array([0.1, 0.4, 0.8])
spin_down_color = array([0.7, 0.7, 0.7])
p_pos = cartesian([arange(0,data["Size"]) for i in range(2)])
system_array = array(data["System"][0]).reshape(data["Size"], data["Size"])
color_array = ((system_array == 1).reshape(data["Size"],data["Size"],1) * spin_up_color + (system_array == 0).reshape(data["Size"],data["Size"],1) * spin_down_color).reshape(data["TotalSize"], 3)
p = vs.points(pos=p_pos-array([data["Size"]*0.5, data["Size"]*0.5]), size=15, shape="square", size_units="pixels", color=color_array)
del p_pos
del color_array
del system_array
nt = 0
while True:
vs.rate(60)
system_array = array(data["System"][nt]).reshape(data["Size"], data["Size"])
color_array = ((system_array == 1).reshape(data["Size"],data["Size"],1) * spin_up_color + (system_array == 0).reshape(data["Size"],data["Size"],1) * spin_down_color).reshape(data["TotalSize"], 3)
p.color = color_array
del system_array
del color_array
if nt + 1 >= data["SavedSteps"]:
nt = 0
else:
nt += 1
def go():
y = [1.0, 0.0, 0.0, ma.pi*1.8] # initial x,y and vx,vy
# draw the scene, planet earth/path, sun/sunlight
scene = vp.display(title='Planetary motion',
background=(.2,.5,1), forward=(0,2,-1))
planet= vp.sphere(pos=(y[0],y[1]), radius=0.1,
material=vp.materials.earth,up=(0,0,1))
path = vp.points(pos=(y[0],y[1]), size=2)
sun = vp.sphere(pos=(0,0),radius=0.2,color=vp.color.yellow,
material=vp.materials.emissive)
sunlight = vp.local_light(pos=(0,0), color=vp.color.yellow)
t, h = 0.0, 0.002
while True:
vp.rate(200) # limit animation speed
y = dansode.RK4n(earth, 4, y, t, h) # integrate
t = t + h
planet.pos=(y[0],y[1]) # move planet
path.append(pos=(y[0],y[1])) # draw path
def main():
print "N-Body Plotter: {}".format(argv)
# scene basics
scene.center = (0,0,0)
scene.width = scene.height = 1024
#scene.range = (10.0, 10.0, 10.0)
scene.range = (1.0e9, 1.0e9, 1.0e9)
# get data dimensions
line = stdin.readline()
bodies = loads(line)
p_range = range(len(bodies))
# set up the balls
colours = [ (1.0, 1.0, 0.0), (1.0, 1.0, 1.0), (0.0, 1.0, 0.0), (0.0, 0.5, 0.5), (1.0, 0.0, 0.0), (0.5, 1.0, 0.0), (1.0, 0.0, 1.0), (1.0, 0.5, 0.0), (0.0, 1.0, 1.0), (1.0, 1.0, 1.0) ]
spheres = []
for j in p_range:
body = bodies[j]
r = 1000000.0 * log10(body['mass']**(1.0 / 3.0))
#ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = 0.1 * p['mass']**(1.0 / 3.0), color = colours[j])
ball = sphere(pos = (body['qX'], body['qY'], body['qZ']), radius = r, color = colours[j])
#ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = 10000000.0, color = colours[j])
ball.trail = points(color = ball.color, size = 1)
spheres.append(ball)
counter = 0
while line:
rate(60)
#if counter % 1000 == 0:
# ball.visible = False
# ball = sphere(radius = 0.2) # Particle
# ball.trail = curve(size = 1) # trail
bodies = loads(line)
for j in p_range:
body = bodies[j]
ball = spheres[j]
position = (body['qX'], body['qY'], body['qZ'])
ball.pos = position
ball.trail.append(pos = position, retain = 1000)
#popen('import -window 0x3200003 -compress None VPythonOutput/' + str(counter).zfill(4) + '.png')
counter += 1
line = stdin.readline()
def main():
# scene basics
scene.center = (0,0,0)
scene.width = 1800
scene.height = 1000
# scene.range = (10.0, 10.0, 10.0)
scene.range = (1.0e9, 1.0e9, 1.0e9)
# get data dimensions
line = stdin.readline()
bodies = loads(line)
pRange = range(len(bodies))
# set up the balls
colours = [ (1.0, 1.0, 0.0), (1.0, 1.0, 1.0), (0.0, 1.0, 0.0), (0.0, 0.5, 0.5), (1.0, 0.0, 0.0), (0.5, 1.0, 0.0), (1.0, 0.0, 1.0), (1.0, 0.5, 0.0), (0.0, 1.0, 1.0), (1.0, 1.0, 1.0) ]
spheres = []
for j in pRange:
p = bodies[j]
r = 1000000.0 * log10(p['mass']**(1.0 / 3.0))
# ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = 0.1 * p['mass']**(1.0 / 3.0), color = colours[j])
ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = r, color = colours[j])
# ball = sphere(pos = (p['qX'], p['qY'], p['qZ']), radius = 10000000.0, color = colours[j])
ball.trail = points(color = ball.color, size = 1)
spheres.append(ball)
while line:
rate(60)
bodies = loads(line)
X = Y = Z = mT = 0.0;
for j in pRange: # COG correction
p = bodies[j]
X += p['qX'] * p['mass']
Y += p['qY'] * p['mass']
Z += p['qZ'] * p['mass']
mT += p['mass']
for j in pRange:
p = bodies[j]
ball = spheres[j]
position = (p['qX'] - X / mT, p['qY'] - Y / mT, p['qZ'] - Z / mT)
ball.pos = position
ball.trail.append(pos = position, retain = 1000)
line = stdin.readline()
def __init__(self,dis=np.zeros((3,1)),rot=np.eye(3),label="",frame_rel=None,color=visual.color.text,
visible=True,visible_frame=True,visible_label=True,visible_frame_label=True,scale=1.0):
self.__dis = np.array(dis).reshape(3,1)
self.__rot = np.array(rot).reshape(3,3)
self.__hom = np.vstack((np.hstack((self.__rot,self.__dis)),np.array([0,0,0,1])))
self.__hom_inv = np.vstack((np.hstack((self.__rot.T,-np.dot(self.__rot.T,self.__dis))),np.array([0,0,0,1])))
self.__label = label
self.__color = color
if frame_rel == None:
self.__frame_rel = None
self.__frame_obj = visual.frame(pos=self.__dis,visible=visible)
else:
self.__frame_rel = frame_rel
self.__frame_obj = visual.frame(frame=self.__frame_rel.__frame_obj,pos=self.__dis,visible=visible)
self.__frame_obj.axis = self.__rot[:,0]
self.__frame_obj.up = self.__rot[:,1]
self.__point_obj = visual.points(frame=self.__frame_obj,pos=(0,0,0),color=color,size=4,size_units="pixels",shape="round")
self.__label_obj = visual.label(frame=self.__frame_obj,text=self.__label,visible=visible_label,
pos=(0,0,0),xoffset=5*scale,yoffset=1*scale,space=5*scale,line=0,height=16*scale,border=0,font='sans',color=color,box=False,opacity=0)
self.axis = axes(frame_obj=self.__frame_obj,visible=visible_frame,visible_label=visible_frame_label,scale=scale)
def make_on_screen(self, min_x, max_x, min_y, max_y, min_z, max_z):
# Convert it to coordinates between 0 and 1 so everything fits properly and we know where everything is
# Clear our point if it exists
self.clear_point()
# Check if each axis is used by the view
use_x=min_x!=max_x
use_y=min_y!=max_y
use_z=min_z!=max_z
#Check if we are visible
visible = True
if not (use_x or min_x==self.x):
visible=False
if not (use_y or min_y==self.y):
visible=False
if not (use_y or min_y==self.y):
visible=False
if not visible:
return
if use_x:
box_x = (self.x - min_x) / (max_x - min_x)
else:
box_x = 0
if use_y:
box_y = (self.y - min_y) / (max_y - min_y)
else:
box_y = 0
if use_z:
box_z = (self.z - min_z) / (max_z - min_z)
else:
box_z = 0
# Make our point
self.visual_point = visual.points(pos=[box_x, box_y, box_z], size=self.size, shape="round", color=(self.r, self.g, self.b))
rright = r[:, 0:-1] - r[:, 1:] # rel pos to right neighbor
ftop, fright = force(rtop), force(rright) # forces from top, right
f[0:-1, :] = ftop # force from top
f[:, 0:-1] += fright # force from right
f[1:, :] -= ftop # below, left: use 3rd law
f[:, 1:] -= fright
a = (f - damp*v)/mass + gvec
v[0,0], v[0,-1], v[-1,0], v[-1,-1]=0, 0, 0, 0 # fixed coners
return np.array([v,a])
L, M, N = 2.0, 15, 15 # size, (M,N) particle array
h, mass, damp = 0.01, 0.004, 0.01 # keep damp between [.01,.1]
x, y = np.linspace(0,L,M), np.linspace(0,L,N) # particle grid
r, v = np.zeros((N,M,3)), np.zeros((N,M,3))
spring_k, spring_l = 50.0, x[1]-x[0] # spring const., relaxed length
r[:,:, 0], r[:,:, 1] = np.meshgrid(x,y) # initialize pos
Y, gvec = np.array([r, v]), np.array([0,0,-9.8]) # [v,a], g vector
scene = vp.display(title='Tablecloth', background=(.2,.5,1),
up=(0,0,1), center=(L/2,L/2,-L/4), forward=(1,2,-1))
vp.points(pos=[(0,0,0),(0,L,0),(L,L,0),(L,0,0)], size=50) # corners
x, y, z = r[:,:,0], r[:,:,1], r[:,:,2] # mesh points
net = vpm.net(x, y, z, vp.color.yellow, 0.005) # mesh net
mesh = vpm.mesh(x, y, z, vp.color.red, vp.color.yellow)
while (1):
vp.rate(100), vpm.wait(scene) # pause if key pressed
Y = ode.RK4(cloth, Y, 0, h)
x, y, z = Y[0,:,:,0], Y[0,:,:,1], Y[0,:,:,2]
net.move(x, y, z), mesh.move(x, y, z)
global running
running = not running
pButton.text = "Pause" if running else "Resume"
# Function to update temperature readout when slider is moved:
def tSliderAdjust():
global T
T = tSlider.value
tLabel.text = "%1.2f" % T # display temperature to two decimal places
# We represent the square 2D array of lattice sites using a vpython "points"
# object, which is basically a 1D array of squares (or circles) placed in the
# 3D space. The code below arranges them on a grid in the xy plane.
thePoints = visual.points(size=1, size_units="world",
shape="square") # can also try "round"
for i in range(size):
for j in range(size):
thePoints.append(pos=(i - size / 2 + 0.5, j - size / 2 + 0.5, 0))
# Function to color the "point" representing site (i,j):
def colorSquare(i, j):
thePoints.color[i * size +
j] = (0.5, 0,
1) if s[i, j] == 1 else (1, 1, 1) # purple and white
# Initialize the lattice to a random array, and draw it as we go:
for i in range(size):
for j in range(size):
return pts,mn,vn,ro
fnBase='/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_desk'
#fnBase='/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_xyz'
fnAsso=fnBase+'/association.txt'
with open(fnAsso) as f:
asso = f.readlines()
assoFn=[] #array of associated file name tuples (rgb,depth)
for l in asso:
ts1,fnRgb,ts2,fnDep=l.rstrip().split(" ")
assoFn.append((fnRgb,fnDep))
ptsTf,clrTf,ptsT,clrT,imgT,depT=getPointCloudFromRangeAsso(assoFn,150,10)
meanPtsT=np.mean(ptsTf,0)
points(pos=ptsTf-meanPtsT,size=1,color=clrTf)
print ptsT.shape,ptsTf.shape
p=get2DGridOrganizedFromAsso(assoFn,150,5)
npf=np.array(p)
print npf.shape
npfp=npf[:,:,:,0,:]
npfc=npf[:,:,:,1,:]
print "npfp",npfp.shape
pts=np.vstack(npfp[:,:,0])
print "pts",pts.shape
#points(pos=pts,size=2,color=(1,0,0))
npp=np.array(npfp)
ptsPlane=[]
ptsNp=[]
arrPlane=[]
#fnBase='/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_desk'
fnBase = '/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_xyz'
fnAsso = fnBase + '/association.txt'
with open(fnAsso) as f:
asso = f.readlines()
assoFn = [] #array of associated file name tuples (rgb,depth)
for l in asso:
ts1, fnRgb, ts2, fnDep = l.rstrip().split(" ")
assoFn.append((fnRgb, fnDep))
ptsTf, clrTf, ptsT, clrT, imgT, depT = getPointCloudFromRangeAsso(assoFn, 0, 1)
ptsMf, clrMf, ptsM, clrM, imgM, depM = getPointCloudFromRangeAsso(assoFn, 1, 1)
points(pos=ptsT, size=1, color=clrT)
# TT=np.eye(4,4)
# ptsTT=ptsT
# clrTT=clrT
# for i in range(1):
# ptsMf,clrMf,ptsM,clrM,imgM,depM=getPointCloudFromRangeAsso(assoFn,i+1)
# ptsM=icp.transformDataSetUsingTransform(ptsM,TT)
# print "ICP start %d" % i
# #Tf,pitM=icp.fitICPkdTree(ptsM,ptsT)
# Tf,pitM=icp.fitICP(ptsM,ptsT)
# #points(pos=pitM,size=4,color=clrM)
# ptsT=pitM
# TT=TT.dot(Tf)
# pitMf=icp.transformDataSetUsingTransform(ptsMf,TT)
# ptsTT=np.vstack((ptsTT,pitMf))
# clrTT=np.vstack((clrTT,clrMf))
# Ask the user the location where we want to show the contributions to B
xp = input("Choose a point (enter x)")
yp = input("Choose a point (enter y)")
zp = input("Choose a point (enter z)")
P = (xp, yp, zp) # Location where we want to show the contributions to B
# Choose two infinitesimal points on the ring
sourceR = (0.5, 0, 0)
sourceL = (-0.5, 0, 0)
# Show the two infinitesimal points (right and left)
pieceR = points(pos=sourceR, size=10, color=color.red)
pieceL = points(pos=sourceL, size=10, color=color.blue)
# Show the r hat vectors for the right and left infinitesimal pieces of ring at P
rhatR = arrow(pos = P, axis = (P[0]-sourceR[0],P[1]-sourceR[1],P[2]-sourceR[2]),\
length=0.2, shaftwidth = 0.02, color = color.red)
rhatL = arrow(pos= P, axis = (P[0]-sourceL[0],P[1]-sourceL[1],P[2]-sourceL[2]), \
length = 0.2 , shaftwidth = 0.02, color=color.blue)
# Decide the direction of the current dl (CCW from the top)- at the location of the right infinitestimal
# it is dlR
dlR = vector(0, 0, -1)
RVec = vector(P[0] - sourceR[0], P[1] - sourceR[1], P[2] - sourceR[2])
def __init__(self, npts=100000, Rotate=False, title=''):
ptsize = 3
mapptsize = 1
self.scene_angle = 0
self.localscene = visual.display(title=title +
' Local Coordinate System')
self.globalscene = visual.display(title=title +
' Global Coordinate System')
#self.globalscene.background = (1, 1, 1)
#self.globalscene.up = (0, 0, 1)
#self.globalscene.forward = (0, 1, -0.4)
self.currentrow = 0
self.auto_colour = False
# Set up the global coordinate frame visualiser
self.globalscene.select()
self.globalscene.up = (0, 0, 1.0)
#self.visualrobot = visual.box(length=3, height=2, width=1.5)
w = 1
wid = 0.2
self.robotpts = np.array(((0, -wid, 0, w), (0, wid, 0, w),
(3 * wid, 0, 0, w), (0, -wid, 0, w)))
#self.visualrobot = visual.points(pos=self.robotpts[:, :3], size=4, shape='round', color=(0, 1, 1))
self.visualrobot = visual.curve(pos=self.robotpts[:, :3],
color=(0, 1, 1))
X = np.array([(-1, -1), (-1, 1), (1, 1), (1, -1), (-1, -1)])
square = visual.curve(pos=50 * X)
self.leftmappts = visual.points(size=ptsize,
shape='square',
color=(1, 0, 0))
self.rightmappts = visual.points(size=ptsize,
shape='square',
color=(0, 1, 0))
self.spinmappts = visual.points(size=ptsize,
shape='square',
color=(0, 0, 1))
X = np.zeros((npts, 3))
self.mappts = visual.points(pos=X,
size=mapptsize,
shape='square',
color=(1, 1, 1))
self.trajpts_ind = 0
self.trajpts = visual.points(pos=np.zeros((10000, 3)),
color=(0, 0, 1),
size=2)
visual.scene.show_rendertime = True
if Rotate:
# Enable continuous rotation
RV = RotateVisual(self.globalscene)
RV.start()
else:
# Enable mouse panning
MT = dispxyz.EnableMouseThread(self.globalscene)
MT.start()
# Set up the local coordinate frame visualiser
if showlocal:
self.localscene.select()
self.leftpts = visual.points(color=(1, 0, 0),
size=ptsize,
shape='square')
self.rightpts = visual.points(color=(0, 1, 0),
size=ptsize,
shape='square')
self.spinpts = visual.points(color=(0, 0, 1),
size=ptsize,
shape='square')
visual.scene.show_rendertime = True
self.colourmin = -4
self.colourmax = 4
T = 2.27 # temperature in natural units (adjusted by GUI slider)
running = False # will be true when simulation is running
# Set up the main graphics window (see www.vpython.org/contents/docs/visual/display.html):
#windowSize = 500 # size in screen pixels
#visual.scene.title = "Ising Model"
#visual.scene.x = 50 # move window away from left edge of screen
#visual.scene.width = windowSize
#visual.scene.height = windowSize - 20 # height in pixels includes title bar; 20 is a guess
#visual.scene.fov = 0.1 # small field of view eliminates 3D perspective
#visual.scene.userzoom = visual.scene.userspin = False # no changing the view!
# We represent the square 2D array of lattice sites using a vpython "points"
# object, which is basically a 1D array of squares (or circles) placed in the
# 3D space. The code below arranges them on a grid in the xy plane.
thePoints = visual.points(size=1, size_units="world", shape="round") # "square" or "round"
for i in range(size):
for j in range(size):
thePoints.append(pos=(i-size/2+0.5, j-size/2+0.5, 0))
# Function to color the "point" representing site (i,j):
def colorSite(i, j):
thePoints.color[i*size + j] = (0.5,0,1) if s[i,j]==1 else (1,1,1) # purple and white
# Initialize the lattice to a random array, and draw it as we go:
for i in range(size):
for j in range(size):
s[i,j] = 1 if random.random()<0.5 else -1
colorSite(i,j)
# Function to calculate energy change upon hypothetical flip (with pbc):
return p-m
#fnBase='/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_desk'
fnBase='/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_xyz'
fnAsso=fnBase+'/association.txt'
with open(fnAsso) as f:
asso = f.readlines()
assoFn=[] #array of associated file name tuples (rgb,depth)
for l in asso:
ts1,fnRgb,ts2,fnDep=l.rstrip().split(" ")
assoFn.append((fnRgb,fnDep))
ptsTf,clrTf,ptsT,clrT,imgT,depT=getPointCloudFromRangeAsso(assoFn,0,1)
ptsMf,clrMf,ptsM,clrM,imgM,depM=getPointCloudFromRangeAsso(assoFn,1,1)
points(pos=ptsT,size=1,color=clrT)
# TT=np.eye(4,4)
# ptsTT=ptsT
# clrTT=clrT
# for i in range(1):
# ptsMf,clrMf,ptsM,clrM,imgM,depM=getPointCloudFromRangeAsso(assoFn,i+1)
# ptsM=icp.transformDataSetUsingTransform(ptsM,TT)
# print "ICP start %d" % i
# #Tf,pitM=icp.fitICPkdTree(ptsM,ptsT)
# Tf,pitM=icp.fitICP(ptsM,ptsT)
# #points(pos=pitM,size=4,color=clrM)
# ptsT=pitM
# TT=TT.dot(Tf)
# pitMf=icp.transformDataSetUsingTransform(ptsMf,TT)
# ptsTT=np.vstack((ptsTT,pitMf))
# clrTT=np.vstack((clrTT,clrMf))
model2=getPickleModel('lidarJ2.dat')
#vmodel=points(pos=model2,color=(1,0,1))#purple points
#Grandfather house data not twisted. ICP doesn't work on it
# m0c=getPickleModel('lidar0c.dat')
# points(pos=m0c,color=(0,1,0))
# m1c=getPickleModel('lidar1c.dat')
# points(pos=m1c,color=(1,0,1))
# m2c=getPickleModel('lidar2c.dat')
# points(pos=m2c,color=(1,1,0))
# m3c=getPickleModel('lidar3c.dat')
# points(pos=m3c,color=(0,1,1))
#dataset to test ICP in 3D
model3D =np.array([(0,1,0),(1,1,0),(0,0,-1),(0,1,-1)])*500
points(pos=model3D ,color=(0,0,1))
Ti=np.array([[0.5,0.1,0.8],
[0.1,0.5,0.3],
[0.8,0.3,0.5]])
target3D=np.array([(0.1,1,0),(1.1,1.1,0),(0.1,0.1,-1),(0.1,1,-1)])*500
# target3D=np.array([[-103.47318056, -4.73010521, 489.24986969],
# [ -81.20086263 , 494.66670749, 499.58269659],
# [-487.54454916 , 24.66799255, -103.72983964],
# [-591.94480715 , 19.20743331 ,385.21879614]])
#target3D=Ti.dot(model3D.transpose()).transpose()+np.array([(1,1,1),(1,1,1),(1,1,1),(1,1,1)])*500
#dataset to fit the model in this target usually is a 2D map or a 3D geometric point cloud
target=model0
vtarget=points(pos=target,color=(0,1,0))
if __name__ == '__main__':
def __init__(self, *a):
super().__init__(*a)
self.cone = v.cone(pos=self.pos, axis=(self.get_end(10)-self.pos))
self.trail = v.points(pos=[(0, 0, 0)])
mean = (0.1 + 0.8*random.rand(3)) * img_size
a = (random.rand(3, 3)-0.5)*img_size*0.1
cov = np.dot(a.T, a) + img_size*0.05*np.eye(3)
n = 100 + random.randint(900)
pts = random.multivariate_normal(mean, cov, n)
apts.append( pts )
ref_distrs.append( (mean, cov) )
nppoints = np.float32( np.vstack(apts) )
#points(pos=nppoints)
puntos=[[10,0,0],
[-10,-1,0],
[5,0,-5],
[-5,0,5],
[2,2,2],
[-2,2,-2]]
npts=np.float32(puntos)/len(puntos)
#npts=np.float32(nppoints)/nppoints.shape[0]
cov=npts.T.dot(npts)
print cov
w, u, vt = cv2.SVDecomp(cov)
print "w",w
print "u",u
print "vt",vt
l=np.sqrt(w)
ro=l[2]/np.sum(l)
print ro
points(pos=npts)
if __name__ == '__main__':
pass
fnBase = '/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_desk'
#fnBase='/media/francisco/Packard Bell/Users/paco/Pictures/datasets/RGBD/cvg/rgbd_dataset_freiburg1_xyz'
fnAsso = fnBase + '/association.txt'
with open(fnAsso) as f:
asso = f.readlines()
assoFn = [] #array of associated file name tuples (rgb,depth)
for l in asso:
ts1, fnRgb, ts2, fnDep = l.rstrip().split(" ")
assoFn.append((fnRgb, fnDep))
ptsTf, clrTf, ptsT, clrT, imgT, depT = getPointCloudFromRangeAsso(
assoFn, 150, 10)
meanPtsT = np.mean(ptsTf, 0)
points(pos=ptsTf - meanPtsT, size=1, color=clrTf)
print ptsT.shape, ptsTf.shape
p = get2DGridOrganizedFromAsso(assoFn, 150, 5)
npf = np.array(p)
print npf.shape
npfp = npf[:, :, :, 0, :]
npfc = npf[:, :, :, 1, :]
print "npfp", npfp.shape
pts = np.vstack(npfp[:, :, 0])
print "pts", pts.shape
#points(pos=pts,size=2,color=(1,0,0))
npp = np.array(npfp)
ptsPlane = []
ptsNp = []
arrPlane = []
def visualize(
net,
mode="grid",
distance_between_layers=4,
draw_nodes=True,
draw_edges=True,
edge_size=0.01,
ignore_weights_below=2,
exaggerate_edges=False,
node_size=0.2,
column_size=2,
auto_clear=True,
):
modes = ["grid", "column", "randomcolumn"]
if mode not in modes:
raise ValueError("mode has to be one of {0}".format(modes))
vis.scene.visible = True # In case the window was closed
if auto_clear:
clear()
pos_per_layer = []
z0 = -len(net.sizes) * distance_between_layers / 2.0 # Center network at (0, 0, 0)
color_cyle = itertools.cycle([vis.color.red, vis.color.yellow, vis.color.blue])
for level, size in enumerate(net.sizes):
if mode in ["grid", "column"]:
curr_pos = []
nodes_per_edge = np.round(
np.sqrt(size)
) # This just determines one edge, the other one is restricted directly by the number of elements
if mode == "grid":
x0 = -nodes_per_edge / 2.0
y0 = -nodes_per_edge / 2.0
elif mode == "column":
x0 = y0 = -column_size / 2.0
x = 0
y = 0
for i in range(size):
curr_pos.append((x0 + x, y0 + y, z0 + level * distance_between_layers))
if mode == "grid":
if x >= nodes_per_edge:
x = 0
y += 1
else:
x += 1
elif mode == "column":
if x >= column_size:
x = 0
y += column_size / nodes_per_edge
else:
x += column_size / nodes_per_edge
elif mode == "randomcolumn":
curr_pos = np.random.rand(size, 3) - 0.5
curr_pos[:, :2] *= column_size
curr_pos[:, 2] *= distance_between_layers * 0.9 # Leave a short margin between the layers
curr_pos[:, 2] += z0 + level * distance_between_layers
pos_per_layer.append(curr_pos)
# Draw nodes.
if draw_nodes:
# all_pos = []
# for pos in pos_per_layer:
# all_pos.extend(pos)
# global nodes
for pos in pos_per_layer:
nodes.append(vis.points(pos=pos, color=color_cyle.next(), size=node_size, size_units="world"))
# Draw edges, with radius proportional to connection weight.
if draw_edges:
for i in range(len(pos_per_layer) - 1):
curr_edges = []
for from_index, from_pos in enumerate(pos_per_layer[i]):
for to_index, to_pos in enumerate(pos_per_layer[i + 1]):
abs_weight = np.abs(net.weights[i][to_index, from_index])
if exaggerate_edges:
radius = edge_size * abs_weight ** 2
else:
radius = edge_size * abs_weight
if abs_weight >= ignore_weights_below:
dx = to_pos[0] - from_pos[0]
dy = to_pos[1] - from_pos[1]
dz = to_pos[2] - from_pos[2]
edge = vis.box(
pos=(from_pos[0] + dx / 2.0, from_pos[1] + dy / 2.0, from_pos[2] + dz / 2.0),
axis=(dx, dy, dz),
color=vis.color.white,
height=radius,
width=radius,
) # Box is way more performant than cylinder for example
curr_edges.append(edge)
edges.append(curr_edges)
import visual as v scene2 = v.display(title="Example of Tetrahedrons", x=0, y = 0, width = 600,height = 200,center=(5,0,0),backround = (0,1,1)) for obj in scene2.objects: if isinstance(obj,box): obj.color = color.red print "test" p = v.points(pos=[(-1,0,0), (1,0,0)], size = 50, color = v.color.red)
def draw_efield(V, scale): # draw electric field
Ex, Ey = np.gradient(-V)
Emag = np.sqrt(Ex*Ex + Ey*Ey)
for i in range(2, M-1, 2):
for j in range(2, N-1, 2):
vp.arrow(pos=(i,j), axis=(Ex[i,j], Ey[i,j]),
length=Emag[i,j]*scale)
vp.rate(100)
return Ex, Ey
M, N, s = 61, 61, 10 # M x N = grid dim, s = point size
w, d, h = M//3, N//6, N//2 # plates width, separation, half N
bot, top = h - d//2, h + d//2 # bottom and top plates
scene = vp.display(width=M*s, height=N*s, center=(M//2,N//2))
grid = vp.points(pos=[(i,j) for i in range(M) for j in range(N)], # grid
color=[(0,0,0)]*(M*N), size=s, shape='square')
V = np.zeros((M,N)) # initialze V on grid, apply BC
V = set_boundary(V)
V = relax(V) # solve by relaxation
Ex, Ey = draw_efield(V, scale = 16)
V, Ex, Ey = np.transpose(V), np.transpose(Ex), np.transpose(Ey)
X, Y = np.meshgrid(range(M), range(N))
plt.figure() # Fig.1, contour plot
plt.contour(V, 14)
plt.quiver(X[::2,], Y[::2,], Ex[::2,], Ey[::2,], # stride 2 in y dir
width=0.004, minshaft=1.5, minlength=0, scale=10.)
plt.xlabel('x'), plt.ylabel('y')
def drawFrames(points, frames):
visual.points(pos=points, size=5, color=visual.color.red)
for i in range(0, len(points)):
drawFrameAxes(points[i], frames[i])
drawFrameProfiles(points[i], frames[i])
#
# Program 3.1: Ball toss (balltoss.py)
# J Wang, Computational modeling and visualization with Python
#
import visual as vp # get VPython modules for animation
vp.display(background=(.2, .5, 1)) # make scene, ball, floor, and path
ball = vp.sphere(pos=(-4, -4, 0), radius=1, color=vp.color.yellow)
floor = vp.box(pos=(0, -5, 0), length=12, height=0.2, width=4)
path = vp.points(pos=ball.pos, size=4) # use make_trail in newer VP
h, g, vx, vy = 0.01, 9.8, 4.0, 9.8 # $\Delta t$, g, and initial velocity
while True:
vp.rate(400) # limit animation rate
ball.pos.x += vx * h # update position
ball.pos.y += vy * h
path.append(pos=ball.pos, retain=300) # draw path, keep 300 pts
if ball.pos.y > floor.pos.y + ball.radius:
vy = vy - g * h # above floor, update vy
else:
vx, vy = -vx, -vy # below floor, reverse vel.
# Function to run or pause the simulation:
def runPause():
global running
running = not running
pButton.text = "Pause" if running else "Resume"
# Function to update temperature readout when slider is moved:
def tSliderAdjust():
global T
T = tSlider.value
tLabel.text = "%1.2f" % T # display temperature to two decimal places
# We represent the square 2D array of lattice sites using a vpython "points"
# object, which is basically a 1D array of squares (or circles) placed in the
# 3D space. The code below arranges them on a grid in the xy plane.
thePoints = visual.points(size=1, size_units="world", shape="square") # can also try "round"
for i in range(size):
for j in range(size):
thePoints.append(pos=(i-size/2+0.5, j-size/2+0.5, 0))
# Function to color the "point" representing site (i,j):
def colorSquare(i, j):
thePoints.color[i*size + j] = (0.5,0,1) if s[i,j]==1 else (1,1,1) # purple and white
# Initialize the lattice to a random array, and draw it as we go:
for i in range(size):
for j in range(size):
s[i,j] = 1 if random.random()<0.5 else -1
colorSquare(i,j)
# Function to calculate energy change upon hypothetical flip (with pbc):
model2 = getPickleModel('lidarJ2.dat')
#vmodel=points(pos=model2,color=(1,0,1))#purple points
#Grandfather house data not twisted. ICP doesn't work on it
# m0c=getPickleModel('lidar0c.dat')
# points(pos=m0c,color=(0,1,0))
# m1c=getPickleModel('lidar1c.dat')
# points(pos=m1c,color=(1,0,1))
# m2c=getPickleModel('lidar2c.dat')
# points(pos=m2c,color=(1,1,0))
# m3c=getPickleModel('lidar3c.dat')
# points(pos=m3c,color=(0,1,1))
#dataset to test ICP in 3D
model3D = np.array([(0, 1, 0), (1, 1, 0), (0, 0, -1), (0, 1, -1)]) * 500
points(pos=model3D, color=(0, 0, 1))
Ti = np.array([[0.5, 0.1, 0.8], [0.1, 0.5, 0.3], [0.8, 0.3, 0.5]])
target3D = np.array([(0.1, 1, 0), (1.1, 1.1, 0), (0.1, 0.1, -1),
(0.1, 1, -1)]) * 500
# target3D=np.array([[-103.47318056, -4.73010521, 489.24986969],
# [ -81.20086263 , 494.66670749, 499.58269659],
# [-487.54454916 , 24.66799255, -103.72983964],
# [-591.94480715 , 19.20743331 ,385.21879614]])
#target3D=Ti.dot(model3D.transpose()).transpose()+np.array([(1,1,1),(1,1,1),(1,1,1),(1,1,1)])*500
#dataset to fit the model in this target usually is a 2D map or a 3D geometric point cloud
target = model0
vtarget = points(pos=target, color=(0, 1, 0))
if __name__ == '__main__':
Tf = np.eye(3, 3)
f[1:, :] -= ftop # below, left: use 3rd law
f[:, 1:] -= fright
a = (f - damp * v) / mass + gvec
v[0, 0], v[0, -1], v[-1, 0], v[-1, -1] = 0, 0, 0, 0 # fixed coners
return np.array([v, a])
L, M, N = 2.0, 15, 15 # size, (M,N) particle array
h, mass, damp = 0.01, 0.004, 0.01 # keep damp between [.01,.1]
x, y = np.linspace(0, L, M), np.linspace(0, L, N) # particle grid
r, v = np.zeros((N, M, 3)), np.zeros((N, M, 3))
spring_k, spring_l = 50.0, x[1] - x[0] # spring const., relaxed length
r[:, :, 0], r[:, :, 1] = np.meshgrid(x, y) # initialize pos
Y, gvec = np.array([r, v]), np.array([0, 0, -9.8]) # [v,a], g vector
scene = vp.display(title='Tablecloth',
background=(.2, .5, 1),
up=(0, 0, 1),
center=(L / 2, L / 2, -L / 4),
forward=(1, 2, -1))
vp.points(pos=[(0, 0, 0), (0, L, 0), (L, L, 0), (L, 0, 0)], size=50) # corners
x, y, z = r[:, :, 0], r[:, :, 1], r[:, :, 2] # mesh points
net = vpm.net(x, y, z, vp.color.yellow, 0.005) # mesh net
mesh = vpm.mesh(x, y, z, vp.color.red, vp.color.yellow)
while (1):
vp.rate(100), vpm.wait(scene) # pause if key pressed
Y = ode.RK4(cloth, Y, 0, h)
x, y, z = Y[0, :, :, 0], Y[0, :, :, 1], Y[0, :, :, 2]
net.move(x, y, z), mesh.move(x, y, z)
for j in range(2, N - 1, 2):
vp.arrow(pos=(i, j),
axis=(Ex[i, j], Ey[i, j]),
length=Emag[i, j] * scale)
vp.rate(100)
return Ex, Ey
M, N, s = 61, 61, 10 # M x N = grid dim, s = point size
w, d, h = M // 3, N // 6, N // 2 # plates width, separation, half N
bot, top = h - d // 2, h + d // 2 # bottom and top plates
scene = vp.display(width=M * s, height=N * s, center=(M // 2, N // 2))
grid = vp.points(
pos=[(i, j) for i in range(M) for j in range(N)], # grid
color=[(0, 0, 0)] * (M * N),
size=s,
shape='square')
V = np.zeros((M, N)) # initialze V on grid, apply BC
V = set_boundary(V)
V = relax(V) # solve by relaxation
Ex, Ey = draw_efield(V, scale=16)
V, Ex, Ey = np.transpose(V), np.transpose(Ex), np.transpose(Ey)
X, Y = np.meshgrid(range(M), range(N))
plt.figure() # Fig.1, contour plot
plt.contour(V, 14)
plt.quiver(
X[::2, ],
