add camera image calibration method

frinklai · May 27, 2019 · b902bb0 · b902bb0
1 parent c9c11a4
commit b902bb0
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 4 deletions.
diff --git 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
@@ -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: