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 __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, ],