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