def findtubestand_matchonobb(self, tgtpcdnp, toggledebug=False):
"""
match self.tstpcd from tgtpcdnp
using the initilization by findtubestand_obb
:param tgtpcdnp:
:param toggledebug:
:return:
author: weiwei
date:20191229osaka
"""
# toggle the following command to crop the point cloud
# tgtpcdnp = tgtpcdnp[np.logical_and(tgtpcdnp[:,2]>40, tgtpcdnp[:,2]<60)]
inithomomat = self.findtubestand_obb(tgtpcdnp, toggledebug)
tgtpcdo3d = o3dh.nparray_to_o3dpcd(tgtpcdnp)
inlinnerrmse, homomat = o3dh.registration_icp_ptpt(self.tstpcdo3d, tgtpcdo3d, inithomomat, maxcorrdist=5, toggledebug=toggledebug)
inithomomatflipped = copy.deepcopy(inithomomat)
inithomomatflipped[:3,0] = -inithomomatflipped[:3,0]
inithomomatflipped[:3,1] = -inithomomatflipped[:3,1]
inlinnerrmseflipped, homomatflipped = o3dh.registration_icp_ptpt(self.tstpcdo3d, tgtpcdo3d, inithomomatflipped, maxcorrdist=5, toggledebug=toggledebug)
# print(inlinnerrmse, inlinnerrmseflipped)
if inlinnerrmseflipped < inlinnerrmse:
homomat = homomatflipped
return copy.deepcopy(homomat)
def findtubestand_matchonobb(self, tgtpcdnp, toggledebug=False):
"""
match self.tstpcd from tgtpcdnp
using the initilization by findtubestand_obb
:param tgtpcdnp:
:param toggledebug:
:return:
author: weiwei
date:20191229osaka
"""
# toggle the following command to crop the point cloud
# tgtpcdnp = tgtpcdnp[np.logical_and(tgtpcdnp[:,2]>40, tgtpcdnp[:,2]<60)]
# 20200425 cluster is further included
pcdarraylist, _ = o3dh.cluster_pcd(tgtpcdnp)
tgtpcdnp = max(pcdarraylist, key=lambda x: len(x))
# for pcdarray in pcdarraylist:
# rgb = np.random.rand(3)
# rgba = np.array([rgb[0], rgb[1], rgb[2], 1])
# pcdnp = p3dh.genpointcloudnodepath(pcdarray, pntsize=5, colors=rgba)
# pcdnp.reparentTo(base.render)
# break
# base.run()
inithomomat = self.findtubestand_obb(tgtpcdnp, toggledebug)
inlinnerrmse, homomat = o3dh.registration_icp_ptpt(
self.tstpcdnp,
tgtpcdnp,
inithomomat,
maxcorrdist=5,
toggledebug=toggledebug)
inithomomatflipped = copy.deepcopy(inithomomat)
inithomomatflipped[:3, 0] = -inithomomatflipped[:3, 0]
inithomomatflipped[:3, 1] = -inithomomatflipped[:3, 1]
inlinnerrmseflipped, homomatflipped = o3dh.registration_icp_ptpt(
self.tstpcdnp,
tgtpcdnp,
inithomomatflipped,
maxcorrdist=5,
toggledebug=toggledebug)
print(inlinnerrmse, inlinnerrmseflipped)
if inlinnerrmseflipped < inlinnerrmse:
homomat = homomatflipped
# for compatibility with locactorfixed
self.tubestandhomomat = homomat
return copy.deepcopy(homomat)
def phoxi_calib_refinewithmodel(yhx, pxc, rawamat, armname):
"""
The performance of this refining method using cad model is not good.
The reason is probably a precise mobdel is needed.
:param yhx: an instancde of YumiHelperX
:param pxc: phoxi client
:param armname:
:return:
author: weiwei
date: 20191228
"""
handpalmtemplate = pickle.load(
open(
os.path.join(yhx.root, "dataobjtemplate",
"handpalmtemplatepcd.pkl"), "rb"))
newhomomatlist = []
lastarmjnts = yhx.robot_s.initrgtjnts
eerot = np.array([[1, 0, 0], [0, 0, -1],
[0, 1, 0]]).T # horizontal, facing right
for x in [300, 360, 420]:
for y in range(-200, 201, 200):
for z in [70, 90, 130, 200]:
armjnts = yhx.movetoposrotmsc(eepos=np.array([x, y, z]),
eerot=eerot,
msc=lastarmjnts,
armname=armname)
if armjnts is not None and not yhx.pcdchecker.isSelfCollided(
yhx.robot_s):
lastarmjnts = armjnts
yhx.movetox(armjnts, armname=armname)
tcppos, tcprot = yhx.robot_s.gettcp()
initpos = tcppos + tcprot[:, 2] * 7
initrot = tcprot
inithomomat = rm.homobuild(initpos, initrot)
pxc.triggerframe()
pcd = pxc.getpcd()
realpcd = rm.homotransformpointarray(rawamat, pcd)
minx = tcppos[0] - 100
maxx = tcppos[0] + 100
miny = tcppos[1]
maxy = tcppos[1] + 140
minz = tcppos[2]
maxz = tcppos[2] + 70
realpcdcrop = o3dh.crop_nx3_nparray(
realpcd, [minx, maxx], [miny, maxy], [minz, maxz])
if len(realpcdcrop) < len(handpalmtemplate) / 2:
continue
# yhx.rbtmesh.genmnp(yhx.robot_s).reparentTo(base.render)
# yhx.p3dh.genframe(tcppos, tcprot, thickness=10). reparentTo(base.render)
# yhx.p3dh.gensphere([minx, miny, minz], radius=10).reparentTo(base.render)
# yhx.p3dh.gensphere([maxx, maxy, maxz], radius=10).reparentTo(base.render)
# yhx.p3dh.genpointcloudnodepath(realpcd).reparentTo(base.render)
# yhx.p3dh.genpointcloudnodepath(realpcdcrop, colors=[1,1,0,1]).reparentTo(base.render)
# yhx.p3dh.genpointcloudnodepath(rm.homotransformpointarray(inithomomat, handpalmtemplate), colors=[.5,1,.5,1]).reparentTo(base.render)
# yhx.base.run()
hto3d = o3dh.nparray_to_o3dpcd(
rm.homotransformpointarray(inithomomat,
handpalmtemplate))
rpo3d = o3dh.nparray_to_o3dpcd(realpcdcrop)
inlinnerrmse, newhomomat = o3dh.registration_icp_ptpt(
hto3d,
rpo3d,
np.eye(4),
maxcorrdist=2,
toggledebug=False)
print(inlinnerrmse, ", one round is done!")
newhomomatlist.append(rm.homoinverse(newhomomat))
newhomomat = rm.homomat_average(newhomomatlist, denoise=False)
refinedamat = np.dot(newhomomat, rawamat)
pickle.dump(
refinedamat,
open(os.path.join(yhx.root, "datacalibration", "refinedcalibmat.pkl"),
"wb"))
print(rawamat)
print(refinedamat)
return refinedamat