def gentubes(self, homomat, elearray=None, type="dumbbell"): """ :param elearray: :param tubestand_homomat: :param eleconfidencearray: None by default :param alpha: only works when eleconfidencearray is None, it renders the array transparently :return: author: weiwei date: 20191229osaka """ if elearray is None: elearray = self.pattern tubecmlist = [] for i in range(elearray.shape[0]): for j in range(elearray.shape[1]): if elearray[i,j] == 1: if type is "dumbbell": tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 100]))) tubecm.setColor(.5,0,0,1) elif type is "cad": tubecm = self.tubebigcm elif elearray[i,j] == 2: if type is "dumbbell": tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 80]))) tubecm.setColor(0,.5,0,1) elif type is "cad": tubecm = self.tubesmall1cm elif elearray[i,j] == 3: if type is "dumbbell": tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70]))) tubecm.setColor(0,0,.5,1) elif type is "cad": tubecm = self.tubesmall2cm elif elearray[i,j] == -1: tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70]))) tubecm.setColor(1,1,1,1) elif elearray[i,j] == -2: tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70]))) tubecm.setColor(0,0,0,1) else: continue newtubecm = copy.deepcopy(tubecm) tubemat = copy.deepcopy(homomat) tubepos_normalized = np.array([self.tubeholecenters[i,j][0], self.tubeholecenters[i,j][1], 5]) tubepos = rm.homotransformpoint(homomat, tubepos_normalized) tubemat[:3, 3] = tubepos newtubecm.set_homomat(tubemat) tubecmlist.append(newtubecm) return tubecmlist
def gentubes(self, elearray, tubestand_homomat, eleconfidencearray=None, alpha=.3): """ :param elearray: :param tubestand_homomat: :param eleconfidencearray: None by default :param alpha: only works when eleconfidencearray is None, it renders the array transparently :return: author: weiwei date: 20191229osaka """ if eleconfidencearray is None: eleconfidencearray = np.ones_like(elearray) * alpha tubecmlist = [] for i in range(5): for j in range(10): if elearray[i, j] == 1: tubecm = self.tubebigcm rgba = np.array([.7, .7, 0, eleconfidencearray[i, j]]) elif elearray[i, j] == 2: tubecm = self.tubesmallcm rgba = np.array([.7, 0, .7, eleconfidencearray[i, j]]) else: continue newtubecm = copy.deepcopy(tubecm) tubemat = copy.deepcopy(tubestand_homomat) tubepos_normalized = np.array([ self.tubeholecenters[i, j][0], self.tubeholecenters[i, j][1], 5 ]) tubepos = rm.homotransformpoint(tubestand_homomat, tubepos_normalized) tubemat[:3, 3] = tubepos newtubecm.set_homomat(tubemat) newtubecm.setColor(rgba[0], rgba[1], rgba[2], rgba[3]) tubecmlist.append(newtubecm) return tubecmlist
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