class MyApp(ShowBase): def __init__(self, markerImage='marker.jpg', calib_file='test.npz'): ShowBase.__init__(self) base.disableMouse() self.marker = cv2.imread(markerImage) self.marker = cv2.flip(self.marker, 0) self.kp_marker, self.des_marker = getDes(self.marker) if useCamera: self.cap = cv2.VideoCapture(0) ret, frame = self.cap.read() else: ret, frame = True, cv2.imread("sample_0.jpg") if ret: self.frame = frame self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) with np.load(calib_file) as calib_File: self.K = calib_File['mtx'] self.D = calib_File['coef'] (h, w) = frame.shape[0:2] print w, h far = 100 near = 0.1 fovx, fovy, f, (cx, cy), a = cv2.calibrationMatrixValues( self.K, (w, h), w, h) print fovx, fovy, f, cx, cy base.camLens.setFilmSize(w, h) base.camLens.setFilmOffset(w * 0.5 - cx, h * 0.5 - cy) base.camLens.setFocalLength(f) base.camLens.setFov(fovx, fovy) base.camLens.setNearFar(near, far) #base.camLens.setCoordinateSystem(4) base.camLens.setCoordinateSystem(4) #base.camLens.setViewVector(Vec3(0,0,1), Vec3(0,1,0)) #self.render.setAttrib(CullFaceAttrib.make(CullFaceAttrib.MCullCounterClockwise)) self.tex = Texture("detect") #self.buff.getTexture() self.tex.setCompression(Texture.CMOff) self.tex.setup2dTexture(w, h, Texture.TUnsignedByte, Texture.FRgb) self.b = OnscreenImage(parent=render2d, image=self.tex) base.cam.node().getDisplayRegion(0).setSort(20) self.taskMgr.add(self.updateFrameTask, "UpdateCameraFrameTask") self.modelroot = NodePath('ARRootNode') self.modelroot.reparentTo(self.render) ''' self.x = self.loader.loadModel("models/box") self.x.reparentTo(self.modelroot) self.x.setScale(3, 0.1, 0.1) self.x.setPos(0, -0.05, -0.05) self.x.setColor(1,0,0,1,1) self.y = self.loader.loadModel("models/box") self.y.reparentTo(self.modelroot) self.y.setScale(0.1, 3, 0.1) self.y.setPos(-0.05, 0, -0.05) self.y.setColor(0,1,0,1,1) self.z = self.loader.loadModel("models/box") self.z.reparentTo(self.modelroot) self.z.setScale(0.1, 0.1, 3) self.z.setPos(-0.05, -0.05, 0) self.z.setColor(0,0,1,1,1) ''' self.panda = NodePath('PandaRoot') self.panda.reparentTo(self.modelroot) # Load and transform the panda actor. self.pandaActor = Actor("models/panda-model", {"walk": "models/panda-walk4"}) self.pandaActor.setScale(0.003, 0.003, 0.003) self.pandaActor.reparentTo(self.panda) self.pandaActor.loop("walk") self.pandaActor.setH(180) #self.pandaMotion = MopathInterval("Panda Path", self.panda, "Interval Name") self.pathCurve = createNurbsCurve() for i in range(0, 30): self.pathCurve.addPoint( (random.uniform(1, 7), random.uniform(1, 7), 0)) ''' self.pathCurve.addPoint((1, 5, 0)) self.pathCurve.addPoint((5, 5, 0)) self.pathCurve.addPoint((5, 1, 0)) self.pathCurve.addPoint((1, 1, 0)) ''' curveNode = self.pathCurve.getNodepath() self.myMopath = Mopath() self.myMopath.loadNodePath(curveNode) self.myMopath.fFaceForward = True myInterval = MopathInterval(self.myMopath, self.panda, duration=100, name="Name") myInterval.loop() # This task runs for two seconds, then prints done def updateFrameTask(self, task): if useCamera: ret, frame = self.cap.read() else: ret, frame = True, self.frame if ret: frame = cv2.flip(frame, 0) kp_frame, des_frame = getDes(frame) matches = getMatches(self.des_marker, des_frame) if not matches or len(matches) < 5: return task.cont pattern_points = [self.kp_marker[pt.trainIdx].pt for pt in matches] pattern_points = np.array([(x / 50.0, y / 50.0, 0) for x, y in pattern_points], dtype=np.float32) image_points = np.array( [kp_frame[pt.queryIdx].pt for pt in matches], dtype=np.float32) #ret, rvecs, tvecs = cv2.solvePnP(pattern_points, image_points, self.K, None) rvecs, tvecs, inliers = cv2.solvePnPRansac(pattern_points, image_points, self.K, None) #print "Tadam!",rvecs, tvecs #img_pts, jac = cv2.projectPoints(axis, rvecs, tvecs, camera_matrix, dist_coefs) #draw_lines(image, img_pts) T = tvecs.ravel() R = rvecs.ravel() RotM, _ = cv2.Rodrigues(R) RotM = RotM.T RotM = Mat3( RotM[0, 0], RotM[0, 1], RotM[0, 2], RotM[1, 0], RotM[1, 1], RotM[1, 2], # RotM[2,0],RotM[2,1],RotM[2,2]) -RotM[2, 0], -RotM[2, 1], -RotM[2, 2]) self.modelroot.setMat(Mat4(RotM, Vec3(T[0], T[1], T[2]))) self.tex.setRamImage(frame) self.b.setImage(image=self.tex, parent=render2d) self.camera.setPos(0, 0, 0) self.camera.setHpr(0, 0, 0) self.camera.setScale(1, 1, 1) return task.cont