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()
