def register_tube(self, tgtpcdnp, type = 1, toggledebug=False):
"""
allow user to register a tube
:param tgtpcdnp:
:param type: 1 -- large; 2 -- small
:return:
author: weiwei
date: 20200318
"""
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(self.tubestandhomomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
if self.__directory is None:
tscm2 = cm.CollisionModel("./objects/tubestand.stl")
else:
tscm2 = cm.CollisionModel(self.__directory + "/objects/tubestand.stl")
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])
return tmppcd
def trackobject_multicamfusion(camcaps, cammtxs, camdists, camrelhomos, aruco_dict, arucomarkersize = 100, nframe = 5, denoise = True, bandwidth=10):
"""
:param camcaps: a list of cv2.VideoCaptures
:param cammtxs: a list of mtx for each of the camcaps
:param camdists: as list of dist for each of the camcaps
:param camrelhomos: a list of relative homogeneous matrices
:param aruco_dict: NOTE this is not things like aruco.DICT_6x6_250, instead, it is the return value of aruco.Dictionary_get
:param nframe: number of frames used for fusion
:param denoise:
:param bandwidth: the bandwidth for meanshift, a large bandwidth leads to tracking instead of clustering, a small bandwith will be very costly
:return:
author: weiwei
date: 20190422
"""
parameters = aruco.DetectorParameters_create()
framelist = {}
for i in range(nframe):
for capid, cap in enumerate(camcaps):
ret, frame = cap.read()
corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, aruco_dict, parameters=parameters, ids=np.array([[1,2,3,4]]))
ids = ids.get()
if ids is not None:
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corners, arucomarkersize, cammtxs[capid], camdists[capid])
for i in range(ids.size):
rot = cv2.Rodrigues(rvecs[i])[0]
pos = tvecs[i][0].ravel()
if capid > 0:
matinb = np.dot(rm.homoinverse(camrelhomos[capid-1]), rm.homobuild(pos, rot))
rot = matinb[:3, :3]
pos = matinb[:3, 3]
idslist = ids.ravel().tolist()
if idslist[i] in framelist:
framelist[idslist[i]].append([pos, rot])
else:
framelist[idslist[i]] = [[pos, rot]]
import time
frameavglist = {}
for id in framelist:
posveclist = [frame[0] for frame in framelist[id]]
rotmatlist = [frame[1] for frame in framelist[id]]
if len(posveclist) >= nframe:
posvecavg = rm.posvec_average(posveclist, bandwidth, denoise)
rotmatavg = rm.rotmat_average(rotmatlist, bandwidth, denoise)
frameavglist[id] = [posvecavg, rotmatavg]
return frameavglist
def facetboundary(objtrimesh, facet, facetcenter, facetnormal):
"""
compute a boundary polygon for facet
assumptions:
1. there is only one boundary
2. the facet is convex
:param objtrimesh: a datatype defined in trimesh
:param facet: a data type defined in trimesh
:param facetcenter and facetnormal used to compute the transform, see trimesh.geometry.plane_transform
:return: [a list of 3d points, a shapely polygon, facetmat4 (the matrix that cvt the facet to 2d 4x4)]
author: weiwei
date: 20161213, tsukuba
"""
facetp = None
# use -facetnormal to let the it face downward
facetnpmat4 = trigeom.plane_transform(facetcenter, -facetnormal)
for i, faceidx in enumerate(facet):
vert0 = objtrimesh.vertices[objtrimesh.faces[faceidx][0]]
vert1 = objtrimesh.vertices[objtrimesh.faces[faceidx][1]]
vert2 = objtrimesh.vertices[objtrimesh.faces[faceidx][2]]
vert0p = rm.homotransform(facetnpmat4, vert0)
vert1p = rm.homotransform(facetnpmat4, vert1)
vert2p = rm.homotransform(facetnpmat4, vert2)
facep = Polygon([vert0p[:2], vert1p[:2], vert2p[:2]])
if facetp is None:
facetp = facep
else:
facetp = facetp.union(facep)
verts2d = list(facetp.exterior.coords)
verts3d = []
for vert2d in verts2d:
vert3d = rm.homotransform(rm.homoinverse(facetnpmat4),
np.array([vert2d[0], vert2d[1], 0]))[:3]
verts3d.append(vert3d)
return [verts3d, verts2d, facetnpmat4]
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
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False):
"""
:param tubestand_homomat:
:param tgtpcdnp:
:return:
"""
elearray = np.zeros((5, 10))
eleconfidencearray = np.zeros((5, 10))
tgtpcdo3d = o3dh.nparray_to_o3dpcd(tgtpcdnp)
tgtpcdo3d_removed = o3dh.remove_outlier(tgtpcdo3d,
downsampling_voxelsize=None,
nb_points=90,
radius=5)
tgtpcdnp = o3dh.o3dpcd_to_parray(tgtpcdo3d_removed)
# transform tgtpcdnp back
tgtpcdnp_normalized = rm.homotransformpointarray(
rm.homoinverse(tubestand_homomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
tscm2 = cm.CollisionModel("../objects/tubestand.stl")
tscm2.reparentTo(base.render)
for i in range(5):
for j in range(10):
holepos = self.tubeholecenters[i][j]
# squeeze the hole size by half
tmppcd = tgtpcdnp_normalized[
tgtpcdnp_normalized[:, 0] < holepos[0] +
self.tubeholesize[0] / 1.9]
tmppcd = tmppcd[tmppcd[:, 0] > holepos[0] -
self.tubeholesize[0] / 1.9]
tmppcd = tmppcd[tmppcd[:, 1] < holepos[1] +
self.tubeholesize[1] / 1.9]
tmppcd = tmppcd[tmppcd[:, 1] > holepos[1] -
self.tubeholesize[1] / 1.9]
tmppcd = tmppcd[tmppcd[:, 2] > 70]
if len(tmppcd) > 100:
print(
"------more than 100 raw points, start a new test------"
)
# use core tmppcd to decide tube types (avoid noises)
coretmppcd = tmppcd[tmppcd[:, 0] < holepos[0] +
self.tubeholesize[0] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 0] > holepos[0] -
self.tubeholesize[0] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 1] < holepos[1] +
self.tubeholesize[1] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 1] > holepos[1] -
self.tubeholesize[1] / 4]
print("testing the number of core points...")
print(len(coretmppcd[:, 2]))
if len(coretmppcd[:, 2]) < 10:
print("------the new test is done------")
continue
if np.max(tmppcd[:, 2]) > 100:
candidatetype = 1
tmppcd = tmppcd[tmppcd[:, 2] >
100] # crop tmppcd for better charge
else:
candidatetype = 2
tmppcd = tmppcd[tmppcd[:, 2] < 90]
if len(tmppcd) < 10:
continue
print("passed the core points test, rotate around...")
rejflag = False
for angle in np.linspace(0, 180, 20):
tmphomomat = np.eye(4)
tmphomomat[:3, :3] = rm.rodrigues(
tubestand_homomat[:3, 2], angle)
newtmppcd = rm.homotransformpointarray(
tmphomomat, tmppcd)
minstd = np.min(np.std(newtmppcd[:, :2], axis=0))
print(minstd)
if minstd < 1.3:
rejflag = True
print("rotate round done")
if rejflag:
continue
else:
tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0])
tmpangles[
tmpangles < 0] = 360 + tmpangles[tmpangles < 0]
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)
print("------the new test is done------")
return elearray, eleconfidencearray
# find_rhomo(basecamyamlpath = 'cam0_calib.yaml', relcamyamlpath = 'cam2_calib.yaml', savename = 'homo_rb20.yaml')
# find_rhomo(basecamyamlpath = 'cam0_calib.yaml', relcamyamlpath = 'cam4_calib.yaml', savename = 'homo_rb40.yaml')
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 0])
# framenp = base.pggen.genAxis()
# framenp.reparentTo(base.render)
# base.run()
base.pggen.plotAxis(base.render)
homo_rb20 = yaml.load(open('homo_rb20.yaml', 'r'))
homo_rb40 = yaml.load(open('homo_rb40.yaml', 'r'))
# draw in 3d to validate
pandamat4homo_r2 = base.pg.np4ToMat4(rm.homoinverse(homo_rb20))
base.pggen.plotAxis(base.render,
spos=pandamat4homo_r2.getRow3(3),
pandamat3=pandamat4homo_r2.getUpper3())
pandamat4homo_r4 = base.pg.np4ToMat4(rm.homoinverse(homo_rb40))
base.pggen.plotAxis(base.render,
spos=pandamat4homo_r4.getRow3(3),
pandamat3=pandamat4homo_r4.getUpper3())
# show in videos
mtx0, dist0, rvecs0, tvecs0, candfiles0 = yaml.load(
open('cam0_calib.yaml', 'r'))
mtx2, dist2, rvecs2, tvecs2, candfiles2 = yaml.load(
open('cam2_calib.yaml', 'r'))
mtx4, dist4, rvecs4, tvecs4, candfiles4 = yaml.load(
def genTpsShow(self, base, doverh=.1):
"""
getnerate free tabletop placements by
removing the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = base.pg.packpandageom_fn(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
base.pggen.plotArrow(base.render, spos=self.objcom,
epos = self.objcom+self.ocfacetnormals[i], length=100)
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = base.pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
base.pggen.plotStick(base.render, spos, epos, thickness = 1, rgba=[.5,.5,.5,1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c < doverh:
print("not stable")
# return
else:
print(dist2p/dist2c)
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array([closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3]
base.pggen.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0,0,1,1])
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
base.pggen.plotStick(base.render, spos, epos, thickness = 1.5, rgba=[0,1,0,1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter+=1
else:
self.counter=0
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
def find_rhomo(basecamyamlpath, relcamyamlpath, savename):
"""
compute the _rative transformation matrix (homogenous) between two cameras
the resulting matrix will be:
resulthomomat*p_in_rel=p_in_base
:param basecamyamlpath: results of board calibration, with (rvecs, tvecs, and candfiles) included
:param relcamyamlpath: results of board calibration, with (rvecs, tvecs, and candfiles) included
format of base and rel yaml: [mtx, dist, rvecs, tvecs, candfiles] see calibcharucoboard for example
:param savename:
:return:
author: weiwei
date: 20190421
"""
mtx_b, dist_b, rvecs_b, tvecs_b, candfiles_b = yaml.load(
open(basecamyamlpath, 'r'))
mtx_r, dist_r, rvecs_r, tvecs_r, candfiles_r = yaml.load(
open(relcamyamlpath, 'r'))
# get a list of relative mat from rel to base
homo_rb_list = []
for id_b, candimg_b in enumerate(candfiles_b):
try:
id_r = candfiles_r.index(candimg_b)
rot_b, _ = cv2.Rodrigues(rvecs_b[id_b].ravel())
pos_b = tvecs_b[id_b].ravel()
homo_b = rm.homobuild(pos_b, rot_b)
rot_r, _ = cv2.Rodrigues(rvecs_r[id_r].ravel())
pos_r = tvecs_r[id_r].ravel()
homo_r = rm.homobuild(pos_r, rot_r)
# homo_rb*homo_r = homo_b
homo_rb = np.dot(homo_b, rm.homoinverse(homo_r))
homo_rb_list.append(homo_rb)
except Exception as e:
print(e)
continue
# compute the average
pos_rb_list = []
quat_rb_list = []
angle_rb_list = []
for homo_rb in homo_rb_list:
quat_rb_list.append(rm.quaternion_from_matrix(homo_rb[:3, :3]))
angle_rb_list.append([rm.quaternion_angleaxis(quat_rb_list[-1])[0]])
pos_rb_list.append(homo_rb[:3, 3])
quat_rb_arr = np.array(quat_rb_list)
mt = cluster.MeanShift()
quat_rb_arrrefined = quat_rb_arr[np.where(
mt.fit(angle_rb_list).labels_ == 0)]
quat_rb_avg = rm.quaternion_average(quat_rb_arrrefined)
pos_rb_avg = mt.fit(pos_rb_list).cluster_centers_[0]
homo_rb_avg = rm.quaternion_matrix(quat_rb_avg)
homo_rb_avg[:3, 3] = pos_rb_avg
with open(savename, "w") as f:
yaml.dump(rm.homoinverse(homo_rb_avg), f)
arucomarkersize = int(40 * .57)
# find_rhomo(basecamyamlpath = 'cam0_calib.yaml', relcamyamlpath = 'cam2_calib.yaml', savename = 'homo_rb20.yaml')
# find_rhomo(basecamyamlpath = 'cam0_calib.yaml', relcamyamlpath = 'cam4_calib.yaml', savename = 'homo_rb40.yaml')
import math
homo_rb20 = yaml.load(open('homo_rb20.yaml', 'r'))
homo_rb40 = yaml.load(open('homo_rb40.yaml', 'r'))
# draw in 3d to validate
from pandaplotutils import pandactrl
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 0])
homo_b = rm.homobuild(pos=np.ones(3), rot=np.eye(3))
base.pggen.plotAxis(base.render)
pandamat4homo_r2 = base.pg.np4ToMat4(rm.homoinverse(homo_rb20))
base.pggen.plotAxis(base.render,
spos=pandamat4homo_r2.getRow3(3),
pandamat3=pandamat4homo_r2.getUpper3())
pandamat4homo_r4 = base.pg.np4ToMat4(rm.homoinverse(homo_rb40))
base.pggen.plotAxis(base.render,
spos=pandamat4homo_r4.getRow3(3),
pandamat3=pandamat4homo_r4.getUpper3())
# base.run()
# show in videos
mtx0, dist0, rvecs0, tvecs0, candfiles0 = yaml.load(
open('cam0_calib.yaml', 'r'))
mtx2, dist2, rvecs2, tvecs2, candfiles2 = yaml.load(
open('cam2_calib.yaml', 'r'))
