def __call__(self, image, camera_info): if self._bag_capture.n_captures < 1: self.setupCamera(camera_info) self.setupStitch() try: cv_image = self.bridge.imgmsg_to_cv(image, "rgb8") pano_image = pcv.convertCvMat2Mat(cv_image) self.stitcher.addNewImage(pano_image) except CvBridgeError, e: print e
def stop(self): #allocate a cv mat, this will be stitched to inplace #2000 rows = 2000 height #4000 cols = 4000 width (this is backwards from widthxheight if(self.stitcher): blended_t = cv.CreateMat(1500,3000,cv.CV_8UC3) blended_num = numpy.asarray(blended_t) blended = pcv.convertCvMat2Mat(blended_t) self.stitcher.stitch(blended, self.stitch_cb) img_msg = self.bridge.cv2_to_imgmsg(blended_num, "rgb8") self.img_pub.publish(img_msg) self._bag_capture.stop()
def __call__(self, image, camera_info): if self._bag_capture.n_captures < 1: self.setupCamera(camera_info) self.setupStitch() try: self.img_pub.publish(image) cv_image = self.bridge.imgmsg_to_cv(image, "rgb8") pano_image = pcv.convertCvMat2Mat(cv_image) self.stitcher.addNewImage(pano_image) except CvBridgeError, e: print e
def stitch(self): bag = rosbag.Bag(self.bag_name) saved_camera_info = False img_n = 0 pano_dir = tempfile.mkdtemp("pano") image_names = [] camera = pano.Camera() blur_detector = pano.BlurDetector() for topic, msg, t in bag.read_messages(topics=['camera_info']): if ('camera_info' in topic): print "reading camera info" K = pcv.Mat(3,3,pcv.CV_32FC1) K.fromarray(msg.K) img_size = pcv.Size( msg.width, msg.height) camera.setCameraIntrinsics(K,pcv.Mat(),img_size) break options = pano.Options() options.camera = camera options.stitch_size = pcv.Size(4000,2000) options.directory = pano_dir #options.image_names.assign(image_names) params = options.fitter_params params.error_thresh = 6; params.inliers_thresh = 15; params.maxiters = 100; params.nNeeded = 2; options.stitch_output = self.stitch_file_name stitcher = pano.StitchEngine(options) for topic, msg, t in bag.read_messages(topics=['image']): if ('image' in topic): try: cv_image = self.bridge.imgmsg_to_cv(msg, "rgb8") b_prob = blur_detector.checkBlur(cv_image) print "image blur prob = %f" %b_prob pano_image = pcv.convertCvMat2Mat(cv_image) stitcher.addNewImage(pano_image) img_n += 1 except CvBridgeError, e: print e
def stitch(self): bag = rosbag.Bag(self.bag_name) saved_camera_info = False img_n = 0 pano_dir = tempfile.mkdtemp("pano") image_names = [] camera = pano.Camera() blur_detector = pano.BlurDetector() for topic, msg, t in bag.read_messages(topics=['camera_info']): if ('camera_info' in topic): print "reading camera info" K = pcv.Mat(3, 3, pcv.CV_32FC1) K.fromarray(msg.K) img_size = pcv.Size(msg.width, msg.height) camera.setCameraIntrinsics(K, pcv.Mat(), img_size) break options = pano.Options() options.camera = camera options.stitch_size = pcv.Size(4000, 2000) options.directory = pano_dir #options.image_names.assign(image_names) params = options.fitter_params params.error_thresh = 6 params.inliers_thresh = 15 params.maxiters = 100 params.nNeeded = 2 options.stitch_output = self.stitch_file_name stitcher = pano.StitchEngine(options) for topic, msg, t in bag.read_messages(topics=['image']): if ('image' in topic): try: cv_image = self.bridge.imgmsg_to_cv(msg, "rgb8") b_prob = blur_detector.checkBlur(cv_image) print "image blur prob = %f" % b_prob pano_image = pcv.convertCvMat2Mat(cv_image) stitcher.addNewImage(pano_image) img_n += 1 except CvBridgeError, e: print e