# objpcdmerged = objpcd
# else:
# objpcdmerged = np.vstack((objpcdmerged, objpcd))
objpcd = loc.capturecorrectedpcd(yhx.pxc, ncapturetimes=1)
homomat = loc.findtubestand_matchonobb(objpcd, toggledebug=False)
yhx.startworld()
# homomat = loc.findtubestand_match(objpcdmerged, toggle_debug=True)
elearray, eleconfidencearray = loc.findtubes(homomat,
objpcd,
toggledebug=False)
yhx.p3dh.genframe(pos=homomat[:3, 3],
rotmat=homomat[:3, :3]).reparentTo(yhx.base.render)
rbtnp = yhx.rbtmesh.genmnp(yhx.rbt)
rbtnp.reparentTo(yhx.base.render)
pcdnp = p3dh.genpointcloudnodepath(objpcd, pntsize=5)
pcdnp.reparentTo(yhx.base.render)
tbscm = loc.gentubestand(homomat=homomat)
tbscm.reparentTo(yhx.base.render)
tbscm.showcn()
tubecms = loc.gentubes(elearray,
tubestand_homomat=homomat,
eleconfidencearray=eleconfidencearray)
for tbcm in tubecms:
tbcm.reparentTo(yhx.base.render)
tbcm.showcn()
yhx.base.run()
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False):
"""
:param tgtpcdnp:
:return:
author: weiwei
date: 20200317
"""
elearray = np.zeros((5, 10))
eleconfidencearray = np.zeros((5, 10))
tgtpcdnp = o3dh.remove_outlier(tgtpcdnp,
downsampling_voxelsize=None,
nb_points=90,
radius=5)
# transform back to the local frame of the tubestand
tgtpcdnp_normalized = rm.homotransformpointarray(
rm.homoinverse(tubestand_homomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
tscm2 = copy.deepcopy(self.tubestandcm)
tscm2.reparentTo(base.render)
for i in range(5):
for j in range(10):
holepos = self.tubeholecenters[i][j]
tmppcd = self._crop_pcd_overahole(tgtpcdnp_normalized,
holepos[0], holepos[1])
if len(tmppcd) > 50:
if toggledebug:
print(
"------more than 50 raw points, start a new test------"
)
tmppcdover100 = tmppcd[tmppcd[:, 2] > 100]
tmppcdbelow90 = tmppcd[tmppcd[:, 2] < 90]
tmppcdlist = [tmppcdover100, tmppcdbelow90]
if toggledebug:
print("rotate around...")
rejflaglist = [False, False]
allminstdlist = [[], []]
newtmppcdlist = [None, None]
minstdlist = [None, None]
for k in range(2):
if toggledebug:
print("checking over 100 and below 90, now: ", j)
if len(tmppcdlist[k]) < 10:
rejflaglist[k] = True
continue
for angle in np.linspace(0, 180, 10):
tmphomomat = np.eye(4)
tmphomomat[:3, :3] = rm.rodrigues(
tubestand_homomat[:3, 2], angle)
newtmppcdlist[k] = rm.homotransformpointarray(
tmphomomat, tmppcdlist[k])
minstdlist[k] = np.min(
np.std(newtmppcdlist[k][:, :2], axis=0))
if toggledebug:
print(minstdlist[k])
allminstdlist[k].append(minstdlist[k])
if minstdlist[k] < 1.5:
rejflaglist[k] = True
if toggledebug:
print("rotate round done")
print("minstd ",
np.min(np.asarray(allminstdlist[k])))
if all(item for item in rejflaglist):
continue
elif all(not item for item in rejflaglist):
print("CANNOT tell if the tube is big or small")
raise ValueError()
else:
tmppcd = tmppcdbelow90 if rejflaglist[
0] else tmppcdover100
candidatetype = 2 if rejflaglist[0] else 1
tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0])
tmpangles[
tmpangles < 0] = 360 + tmpangles[tmpangles < 0]
if toggledebug:
print(np.std(tmpangles))
print("ACCEPTED! ID: ", i, j)
elearray[i][j] = candidatetype
eleconfidencearray[i][j] = 1
if toggledebug:
# normalized
objnp = p3dh.genpointcloudnodepath(tmppcd,
pntsize=5)
rgb = np.random.rand(3)
objnp.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp.reparentTo(base.render)
stick = p3dh.gendumbbell(
spos=np.array([holepos[0], holepos[1], 10]),
epos=np.array([holepos[0], holepos[1], 60]))
stick.setColor(rgb[0], rgb[1], rgb[2], 1)
stick.reparentTo(base.render)
# original
tmppcd_tr = rm.homotransformpointarray(
tubestand_homomat, tmppcd)
objnp_tr = p3dh.genpointcloudnodepath(tmppcd_tr,
pntsize=5)
objnp_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp_tr.reparentTo(base.render)
spos_tr = rm.homotransformpoint(
tubestand_homomat,
np.array([holepos[0], holepos[1], 0]))
stick_tr = p3dh.gendumbbell(
spos=np.array([spos_tr[0], spos_tr[1], 10]),
epos=np.array([spos_tr[0], spos_tr[1], 60]))
stick_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
stick_tr.reparentTo(base.render)
# box normalized
center, bounds = rm.get_aabb(tmppcd)
boxextent = np.array([
bounds[0, 1] - bounds[0, 0],
bounds[1, 1] - bounds[1, 0],
bounds[2, 1] - bounds[2, 0]
])
boxhomomat = np.eye(4)
boxhomomat[:3, 3] = center
box = p3dh.genbox(
extent=boxextent,
homomat=boxhomomat,
rgba=np.array([rgb[0], rgb[1], rgb[2], .3]))
box.reparentTo(base.render)
# box original
center_r = rm.homotransformpoint(
tubestand_homomat, center)
boxhomomat_tr = copy.deepcopy(tubestand_homomat)
boxhomomat_tr[:3, 3] = center_r
box_tr = p3dh.genbox(
extent=boxextent,
homomat=boxhomomat_tr,
rgba=np.array([rgb[0], rgb[1], rgb[2], .3]))
box_tr.reparentTo(base.render)
if toggledebug:
print("------the new test is done------")
return elearray, eleconfidencearray
newtubecm.set_homomat(tubemat)
newtubecm.setColor(rgba[0], rgba[1], rgba[2], rgba[3])
tubecmlist.append(newtubecm)
return tubecmlist
if __name__ == '__main__':
import robothelper
import numpy as np
import environment.collisionmodel as cm
yhx = robothelper.RobotHelperX(usereal=False, startworld=True)
loc = LocatorFixed(directory=yhx.root)
standpcd_origin = p3dh.genpointcloudnodepath(
loc.tubestandcm.samplesurface()[0], pntsize=5)
standpcd_origin.reparentTo(yhx.base.render)
# bgdepth = pickle.load(open("./databackground/bgdepth.pkl", "rb"))
# bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb"))
# objpcdmerged = None
# for i in range(1):
# yhx.pxc.triggerframe()
# fgdepth = yhx.pxc.getdepthimg()
# fgpcd = yhx.pxc.getpcd()
#
# substracteddepth = bgdepth - fgdepth
# substracteddepth = substracteddepth.clip(40, 300)
# substracteddepth[substracteddepth == 40] = 0
# substracteddepth[substracteddepth == 300] = 0