From b902bb00d2de208f7f3236ecd6ff6e4ae8e6c299 Mon Sep 17 00:00:00 2001 From: frinklai Date: Mon, 27 May 2019 18:33:51 +0800 Subject: [PATCH] add camera image calibration method --- vision/get_image/scripts/Get_Image.py | 11 +++++++++-- vision/yolo_v3/scripts/yolo.py | 12 ++++++++++-- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/vision/get_image/scripts/Get_Image.py b/vision/get_image/scripts/Get_Image.py index 28fb112..19e21b0 100755 --- a/vision/get_image/scripts/Get_Image.py +++ b/vision/get_image/scripts/Get_Image.py @@ -30,6 +30,10 @@ def __init__(self): self.bridge = CvBridge() self.image = np.zeros((0,0,3), np.uint8) self.take_picture_counter = 0 + self.mtx = np.load('/home/iclab-arm/AI_Bot_ws/src/AI_Bot/vision/get_image/scripts/'+'camera_calibration_mtx.npy') + self.dist = np.load('/home/iclab-arm/AI_Bot_ws/src/AI_Bot/vision/get_image/scripts/'+'camera_calibration_dist.npy') + self.newcameramtx = np.load('/home/iclab-arm/AI_Bot_ws/src/AI_Bot/vision/get_image/scripts/'+'camera_calibration_newcameramtx.npy') + self.dst_roi_x, self.dst_roi_y, self.dst_roi_w, self.dst_roi_h = np.load('/home/iclab-arm/AI_Bot_ws/src/AI_Bot/vision/get_image/scripts/'+'camera_calibration_roi.npy') #s = rospy.Service("request FLIR", FLIR_image, self.service_callback) rospy.Subscriber("/camera/image_color", Image, self.callback) @@ -41,11 +45,14 @@ def __init__(self): def callback(self, image): try: self.cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8") + self.un_dst_img = cv2.undistort(self.cv_image, self.mtx, self.dist, None, self.newcameramtx) + self.un_dst_img = self.un_dst_img[self.dst_roi_y:self.dst_roi_y+self.dst_roi_h, \ + self.dst_roi_x:self.dst_roi_x+self.dst_roi_w] except CvBridgeError as e: print(e) cv2.namedWindow("result", cv2.WINDOW_NORMAL) - cv2.imshow("result", self.cv_image) - self.get_image(self.cv_image) + cv2.imshow("result", self.un_dst_img) + self.get_image(self.un_dst_img) cv2.waitKey(1) def get_image(self, crop_image): diff --git a/vision/yolo_v3/scripts/yolo.py b/vision/yolo_v3/scripts/yolo.py index 098cdbe..3e2d75c 100755 --- a/vision/yolo_v3/scripts/yolo.py +++ b/vision/yolo_v3/scripts/yolo.py @@ -64,6 +64,10 @@ def __init__(self, **kwargs): self.boxes, self.scores, self.classes = self.generate() self.bridge = CvBridge() self.cv_image = np.zeros((1024, 768, 3), np.uint8) + self.mtx = np.load(pth.model_data_path + 'model_data/camera_calibration_mtx.npy') + self.dist = np.load(pth.model_data_path + 'model_data/camera_calibration_dist.npy') + self.newcameramtx = np.load(pth.model_data_path + 'model_data/camera_calibration_newcameramtx.npy') + self.dst_roi_x, self.dst_roi_y, self.dst_roi_w, self.dst_roi_h = np.load(pth.model_data_path + 'model_data/camera_calibration_roi.npy') def _get_class(self): classes_path = os.path.expanduser(self.classes_path) @@ -201,8 +205,12 @@ def detect_video(yolo, video_path, output_path=""): rospy.Subscriber("/camera/image_color", rosimage, yolo.FLIR_Callback) while not rospy.is_shutdown(): - - image = Image.fromarray(yolo.cv_image) + un_dst_img = cv.undistort(yolo.cv_image, yolo.mtx, yolo.dist, None, yolo.newcameramtx) + un_dst_img = un_dst_img[yolo.dst_roi_y:yolo.dst_roi_y+yolo.dst_roi_h, \ + yolo.dst_roi_x:yolo.dst_roi_x+yolo.dst_roi_w] + image = Image.fromarray(un_dst_img) + # image = Image.fromarray(yolo.cv_image) + image, ROI_array_recive = yolo.detect_image(image) # receieve ROI if ROI_array_recive != None: