def genPointsnp(self, point, height, color = [], size = 2.0): verts3d = [[point.x, point.y, height]] return pg.genPntsnp(verts3d, color, size)
if __name__ == '__main__': import copy base = pandactrl.World(camp=[0, 0, 700], lookatp=[0, 0, 0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") ppfpath = os.path.join(this_dir, "models", "ttubegmd.pickle") # ppfmatch = PPFMatch(objpath, ppfpath = None, bsave = True) ppfmatch = PPFMatch(objpath, ppfpath=ppfpath) perceivedpnts, perceivednormals = ppfmatch.perceivePoints(1) # plotperceived pnts pntsnp = pg.genPntsnp(perceivedpnts) pntsnp.reparentTo(base.render) for i, normal in enumerate(perceivednormals): pg.plotArrow(base.render, spos=perceivedpnts[i], epos=normal + perceivedpnts[i], thickness=.2, length=10) rotmat4list, silist, milist = ppfmatch.match(perceivedpnts, perceivednormals, ddist=5.0, dangle=30.0) try: for i in range(len(rotmat4list)):
pointsnormals.append(eigvec) return pointsnormals if __name__=='__main__': import os import pandaplotutils.pandactrl as pandactrl import pandaplotutils.pandageom as pg import CADTemp base = pandactrl.World(camp=[0,500,3000], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "calibtable.stl") cadtemp = CADTemp.CADTemp(ompath=objpath, numpointsoververts=200) objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') temppnts = cadtemp.pcdtemp normals = estimatenormals(temppnts) temppntsnp = pg.genPntsnp(temppnts) temppntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos = temppnts[i], epos = normal+temppnts[i], thickness = 5, length = 100) base.run()
# transform objectpntscenter = np.mean(objectpnts, axis=0) for i in range(objectpnts.shape[0]): objectpnts[i] = objectpnts[i] - objectpntscenter colors = [] color = [ np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand() ] for pnt in objectpnts: colors.append(color) pntsnp = pg.genPntsnp(objectpnts, colors=colors) pntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos=objectpnts[i], epos=normal + objectpnts[i], thickness=.2, length=10) # import robotsim.nextage.nxt as nxt # import robotsim.nextage.nxtplot as nxtplot # import pandaplotutils.pandageom as pg # from manipulation.grip.robotiq85 import rtq85nm # handpkg = rtq85nm # nxtrobot = nxt.NxtRobot() # nxtmnp = nxtplot.genmnp(nxtrobot, handpkg)
def genPointsnp(point, height, color = [], size = 2.0): verts3d = [[point.x, point.y, height]] return pg.genPntsnp(verts3d, color, size)
# points objectpnts = kface.getObjectPcd() print objectpnts # # normals normals = tools.estimatenormals(objectpnts) # transform objectpntscenter = np.mean(objectpnts, axis=0) for i in range(objectpnts.shape[0]): objectpnts[i] = objectpnts[i]-objectpntscenter colors = [] color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()] for pnt in objectpnts: colors.append(color) pntsnp = pg.genPntsnp(objectpnts, colors=colors) pntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos = objectpnts[i], epos = normal+objectpnts[i], thickness = .2, length = 2) # import robotsim.nextage.nxt as nxt # import robotsim.nextage.nxtplot as nxtplot # import pandaplotutils.pandageom as pg # from manipulation.grip.robotiq85 import rtq85nm # handpkg = rtq85nm # nxtrobot = nxt.NxtRobot() # nxtmnp = nxtplot.genmnp(nxtrobot, handpkg) # nxtmnp.reparentTo(base.render) # pg.plotAxisSelf(base.render) # objarray = kface.getAllObjectPcd()
while True: if kinect.has_new_depth_frame(): cframe = kinect.get_last_color_frame() cwidth = kinect.color_frame_desc.Width cheight = kinect.color_frame_desc.Height dframe = kinect.get_last_depth_frame() dwidth = kinect.depth_frame_desc.Width dheight = kinect.depth_frame_desc.Height tmpvertslist = getRotMat(depthToXYZ(dframe, dwidth, dheight)) for tmpverts in tmpvertslist: dverts= kinxyzToRobXYZ(tmpverts) color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()] colors = [] for dvert in dverts: colors.append(color) pntsnp = pg.genPntsnp(dverts, colors = colors) pntsnp.reparentTo(base.render) break import robotsim.nextage.nxt as nxt import robotsim.nextage.nxtplot as nxtplot import pandaplotutils.pandageom as pg from manipulation.grip.robotiq85 import rtq85nm handpkg = rtq85nm nxtrobot = nxt.NxtRobot() nxtmnp = nxtplot.genmnp(nxtrobot, handpkg) nxtmnp.reparentTo(base.render) pg.plotAxisSelf(base.render) base.run()
if __name__ == '__main__': import os import pandaplotutils.pandactrl as pandactrl import pandaplotutils.pandageom as pg import CADTemp base = pandactrl.World(camp=[0, 500, 3000], lookatp=[0, 0, 0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "calibtable.stl") cadtemp = CADTemp.CADTemp(ompath=objpath, numpointsoververts=200) objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') temppnts = cadtemp.pcdtemp normals = estimatenormals(temppnts) temppntsnp = pg.genPntsnp(temppnts) temppntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos=temppnts[i], epos=normal + temppnts[i], thickness=5, length=100) base.run()
self.__newsamplesnoverts.append(sample) self.__newsamplesnovertsnormals.append(normal) else: continue if __name__=='__main__': base = pandactrl.World(camp=[0,0,300], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") cadtemp = CADTemp(ompath = objpath) objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') # objnp.reparentTo(base.render) colors = [] color = [1,0,0,1] for pnt in cadtemp.pcdtempnovertsinner: colors.append(color) pntsnp = pg.genPntsnp(cadtemp.pcdtempnovertsinner, colors=colors) pntsnp.reparentTo(base.render) for i, normal in enumerate(cadtemp.pcdtempnovertsnormalsinner): pg.plotArrow(base.render, spos = cadtemp.pcdtempnovertsinner[i], epos = normal+cadtemp.pcdtempnovertsinner[i], thickness = .2, length =10) base.run()
if __name__=='__main__': import copy base = pandactrl.World(camp=[0,0,700], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") ppfpath = os.path.join(this_dir, "models", "ttubegmd.pickle") # ppfmatch = PPFMatch(objpath, ppfpath = None, bsave = True) ppfmatch = PPFMatch(objpath, ppfpath = ppfpath) perceivedpnts, perceivednormals = ppfmatch.perceivePoints(1) # plotperceived pnts pntsnp = pg.genPntsnp(perceivedpnts) pntsnp.reparentTo(base.render) for i, normal in enumerate(perceivednormals): pg.plotArrow(base.render, spos = perceivedpnts[i], epos = normal+perceivedpnts[i], thickness = .2, length = 10) rotmat4list, silist, milist = ppfmatch.match(perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0) try: for i in range(len(rotmat4list)): # for i in range(1): rotmat4 = rotmat4list[i] si = silist[i] mi = milist[i] #object # ppfmatch.objnp.setMat(rotmat4) # ppfmatch.objnp.reparentTo(base.render)
return [point3d.x, point3d.y, point3d.z] else: return [] if __name__ == '__main__': base = pandactrl.World(camp=[0, 0, 3000], lookatp=[0, 0, 0]) kapi = KinectAPI() # points pntclds = kapi.getPointCloud() colors = [] color = [ np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand() ] for pnt in pntclds: colors.append(color) pntsnp = pg.genPntsnp(pntclds, colors=colors) pntsnp.reparentTo(base.render) from plyfile import PlyData, PlyElement verts = np.array(pntclds, dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')]) el = PlyElement.describe(verts, 'vertex') PlyData([el], text=True).write('pythoncloud.ply') base.run()
if __name__=='__main__': base = pandactrl.World(camp=[0,0,300], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "calibtable.stl") caliber = Calibrate(objpath) tablepnts = caliber.getTablePcd(sampleradius=5) colors = [] color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()] for pnt in tablepnts: colors.append(color) pntsnp = pg.genPntsnp(tablepnts, colors=colors) pntsnp.reparentTo(base.render) #normals # normals = tools.estimatenormals(caliber.temppnt) # for i, normal in enumerate(normals): # pg.plotArrow(base.render, spos = caliber.temppnt[i], epos = normal+caliber.temppnt[i], thickness = 2, length = 100) # normals # normals = tools.estimatenormals(tablepnts, method = 'ransac') # for i, normal in enumerate(normals): # pg.plotArrow(base.render, spos = tablepnts[i], epos = normal+tablepnts[i], thickness = 2, length = 100) # # points # objectpnts = caliber.getObjectPcd() # print objectpnts # # # normals
def getMakerXYZ(self, markercolumnid, markerrowid, markerdepth): if markerdepth > 0: point3d = self.kinect.mapper().MapDepthPointToCameraSpace( PyKinectV2._DepthSpacePoint(ctypes.c_float(markercolumnid), ctypes.c_float(markerrowid)), ctypes.c_ushort(markerdepth)) return [point3d.x, point3d.y, point3d.z] else: return [] if __name__=='__main__': base = pandactrl.World(camp=[0,0,3000], lookatp=[0,0,0]) kapi = KinectAPI() # points pntclds = kapi.getPointCloud() colors = [] color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()] for pnt in pntclds: colors.append(color) pntsnp = pg.genPntsnp(pntclds, colors=colors) pntsnp.reparentTo(base.render) from plyfile import PlyData, PlyElement verts = np.array(pntclds, dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')]) el = PlyElement.describe(verts, 'vertex') PlyData([el], text=True).write('pythoncloud.ply') base.run()
if __name__ == '__main__': base = pandactrl.World(camp=[0, 0, 700], lookatp=[0, 0, 0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") ppfmatch = PPFMatch(objpath) # ppfmatch.computePPFwithAlpha() colors = [] color = [ np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand() ] for pnt in ppfmatch.temppnts: colors.append(color) pntsnp = pg.genPntsnp(ppfmatch.temppnts, colors=colors) for i, normal in enumerate(ppfmatch.tempnormals): pg.plotArrow(base.render, spos=ppfmatch.temppnts[i], epos=normal + ppfmatch.temppnts[i], thickness=.2, length=2) # pg.plotAxisSelf(base.render) base.run()