def handle_monocular(self, msg): if self.c == None: if self._camera_name: self.c = MonoCalibrator( self._boards, self._calib_flags, self._pattern, name=self._camera_name, detection=self._detection, output=self._output, checkerboard_flags=self._checkerboard_flags, min_good_enough=self._min_good_enough) else: self.c = MonoCalibrator( self._boards, self._calib_flags, self._pattern, detection=self._detection, output=self._output, checkerboard_flags=self.checkerboard_flags, min_good_enough=self._min_good_enough) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) self.displaywidth = drawable.scrib.shape[1] self.redraw_monocular(drawable)
def test_monocular(self): # Run the calibrator, produce a calibration, check it for i, setup in enumerate(self.setups): board = ChessboardInfo() board.n_cols = setup.cols board.n_rows = setup.rows board.dim = self.board_width_dim mc = MonoCalibrator([board], flags=cv2.CALIB_FIX_K3, pattern=setup.pattern) if 0: # display the patterns viewed by the camera for pattern_warped in self.limages[i]: cv2.imshow("toto", pattern_warped) cv2.waitKey(0) mc.cal(self.limages[i]) self.assert_good_mono(mc, self.limages[i], setup.lin_err) # Make sure the intrinsics are similar err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.k, ord=numpy.inf) self.assertTrue( err_intrinsics < setup.K_err, 'intrinsics error is %f for resolution i = %d' % (err_intrinsics, i)) print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.k, ord=numpy.inf))
def main(args): from optparse import OptionParser parser = OptionParser() parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") options, args = parser.parse_args() size = tuple([int(c) for c in options.size.split('x')]) dim = float(options.square) images = [] for fname in args: if os.path.isfile(fname): img = cv.LoadImage(fname) if img is None: print("[WARN] Couldn't open image " + fname + "!", file=sys.stderr) sys.exit(1) else: print("[INFO] Loaded " + fname + " (" + str(img.width) + "x" + str(img.height) + ")") images.append(img) cboard = ChessboardInfo() cboard.dim = dim cboard.n_cols = size[0] cboard.n_rows = size[1] mc = MonoCalibrator([cboard]) mc.cal(images) print(mc.as_message())
def __init__(self, chess_size, dim, approximate=0): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] if approximate <= 0: sync = message_filters.TimeSynchronizer else: sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate) tsm = sync([ message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono ], 10) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name( "stereo") + "/right/camera_info" tosync_stereo = [(left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo)] tss = sync([ message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo ], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board])
def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] cols, rows = n_corners self.boards.append(ChessboardInfo(cols, rows, float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg # Extract chessboard corners using ROS camera_calibration package drawable = self.c.handle_msg(img_msg) if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters # Length of a chessboard square self.d_square = square_length self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions # Number of examined images self.n_chessboards = len(self.c.good_corners) # Dimensions of extracted corner grid self.n_corners_y, self.n_corners_x = n_corners self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def handle_monocular(self, msg): if self.c == None: if self._camera_name: self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags) else: self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) self.displaywidth = drawable.scrib.shape[1] self.redraw_monocular(drawable)
def test_nochecker(self): # Run with same images, but looking for an incorrect chessboard size (8, 7). # Should raise an exception because of lack of input points. new_board = copy.deepcopy(board) new_board.n_cols = 8 new_board.n_rows = 7 sc = StereoCalibrator([new_board]) self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages)) mc = MonoCalibrator([new_board]) self.assertRaises(CalibrationException, lambda: mc.cal(self.limages))
def handle_monocular(self, msg): if self.c == None: self.c = MonoCalibrator(self._boards, self._calib_flags) #print 'BBBBBBBBOOOOOOOOOOOOOOARRRRRRDS',self._boards.gridsize # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) #print drawable.scrib.shape[1] self.displaywidth = drawable.scrib.shape[1] print(drawable) print(drawable.scrib) self.redraw_monocular(drawable)
def cal_from_tarfile(boards, tarname, mono = False, upload = False): if mono: calibrator = MonoCalibrator(boards) else: calibrator = StereoCalibrator(boards) calibrator.do_tarfile_calibration(tarname) print calibrator.ost() if upload: info = calibrator.as_message() if mono: set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) response = set_camera_info_service(info) if not response.success: raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") else: set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) response1 = set_left_camera_info_service(info[0]) response2 = set_right_camera_info_service(info[1]) if not (response1.success and response2.success): raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
def handle_monocular(self, msg): if self.c == None: if self._camera_name: self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) else: self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, checkerboard_flags=self.checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) self.displaywidth = drawable.scrib.shape[1] self.redraw_monocular(drawable)
def handle_monocular(self, msg): if self.c == None: if self._camera_name: self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name) else: self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) self.displaywidth = drawable.scrib.cols self.redraw_monocular(drawable)
def test_monocular(self): # Run the calibrator, produce a calibration, check it mc = MonoCalibrator([board], flags=cv2.CALIB_FIX_K3) for dim in self.sizes: mc.cal(self.l[dim]) self.assert_good_mono(mc, dim) # Make another calibration, import previous calibration as a message, # and assert that the new one is good. mc2 = MonoCalibrator([board]) mc2.from_message(mc.as_message()) self.assert_good_mono(mc2, dim)
def test_monocular(self): # Run the calibrator, produce a calibration, check it mc = MonoCalibrator([board], cv2.CALIB_FIX_K3) max_errs = [0.1, 0.2, 0.4, 0.7] for i, dim in enumerate(self.sizes): mc.cal(self.l[dim]) self.assert_good_mono(mc, dim, max_errs[i]) # Make another calibration, import previous calibration as a message, # and assert that the new one is good. mc2 = MonoCalibrator([board]) mc2.from_message(mc.as_message()) self.assert_good_mono(mc2, dim, max_errs[i])
def cal_from_tarfile(boards, tarname, mono = False, upload = False): if mono: calibrator = MonoCalibrator(boards) else: calibrator = StereoCalibrator(boards) calibrator.do_tarfile_calibration(tarname) print calibrator.ost() if upload: info = calibrator.as_message() if mono: set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) response = set_camera_info_service(info) if not response.success: raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") else: set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) response1 = set_camera_info_service(info[0]) response2 = set_camera_info_service(info[1]) if not (response1.success and response2.success): raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
def cal_from_tarfile(boards, tarname, mono=False): if mono: calibrator = MonoCalibrator(boards) else: calibrator = StereoCalibrator(boards) calibrator.do_tarfile_calibration(tarname) print calibrator.ost()
def test_rational_polynomial_model(self): """Test that the distortion coefficients returned for a rational_polynomial model are not empty.""" for i, setup in enumerate(self.setups): board = ChessboardInfo() board.n_cols = setup.cols board.n_rows = setup.rows board.dim = self.board_width_dim mc = MonoCalibrator([board], flags=cv2.CALIB_RATIONAL_MODEL, pattern=setup.pattern) mc.cal(self.limages[i]) self.assertEqual( len(mc.distortion.flat), 8, 'length of distortion coefficients is %d' % len(mc.distortion.flat)) self.assertTrue( all(mc.distortion.flat != 0), 'some distortion coefficients are zero: %s' % str(mc.distortion.flatten())) self.assertEqual(mc.as_message().distortion_model, 'rational_polynomial') self.assert_good_mono(mc, self.limages[i], setup.lin_err) yaml = mc.yaml() # Issue #278 self.assertIn('cols: 8', yaml)
def test_monocular(self): # Run the calibrator, produce a calibration, check it for i, setup in enumerate(self.setups): board = ChessboardInfo() board.n_cols = setup.cols board.n_rows = setup.rows board.dim = self.board_width_dim mc = MonoCalibrator([ board ], flags=cv2.CALIB_FIX_K3, pattern=setup.pattern) if 0: # display the patterns viewed by the camera for pattern_warped in self.limages[i]: cv2.imshow("toto", pattern_warped) cv2.waitKey(0) mc.cal(self.limages[i]) self.assert_good_mono(mc, self.limages[i], setup.lin_err) # Make sure the intrinsics are similar err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf) self.assert_(err_intrinsics < setup.K_err, 'intrinsics error is %f' % err_intrinsics) print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf))
def cal_from_tarfile(boards, tarname, mono = False): if mono: calibrator = MonoCalibrator(boards) else: calibrator = StereoCalibrator(boards) calibrator.do_tarfile_calibration(tarname) print calibrator.ost()
def test_monocular(self): # Run the calibrator, produce a calibration, check it mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3) for dim in self.sizes: mc.cal(self.l[dim]) self.assert_good_mono(mc, dim) # Make another calibration, import previous calibration as a message, # and assert that the new one is good. mc2 = MonoCalibrator([board]) mc2.from_message(mc.as_message()) self.assert_good_mono(mc2, dim)
def test_monocular(self): # Run the calibrator, produce a calibration, check it mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3) max_errs = [0.1, 0.2, 0.4, 0.7] for i, dim in enumerate(self.sizes): mc.cal(self.l[dim]) self.assert_good_mono(mc, dim, max_errs[i]) # Make another calibration, import previous calibration as a message, # and assert that the new one is good. mc2 = MonoCalibrator([board]) mc2.from_message(mc.as_message()) self.assert_good_mono(mc2, dim, max_errs[i])
def __init__(self, chess_size, dim): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" tosync_stereo = [ (left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo) ] tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board])
def main(args): from optparse import OptionParser parser = OptionParser() parser.add_option( "-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") parser.add_option( "-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") options, args = parser.parse_args() size = tuple([int(c) for c in options.size.split('x')]) dim = float(options.square) images = [] for fname in args: if os.path.isfile(fname): img = cv.LoadImage(fname) if img is None: print >> sys.stderr, "[WARN] Couldn't open image " + fname + "!" sys.exit(1) else: print "[INFO] Loaded " + fname + " (" + str( img.width) + "x" + str(img.height) + ")" images.append(img) cboard = ChessboardInfo() cboard.dim = dim cboard.n_cols = size[0] cboard.n_rows = size[1] mc = MonoCalibrator([cboard]) mc.cal(images) print mc.as_message()
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append( ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg( img_msg ) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len( self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0] * n_corners[1] def genCornerCoordinates(self, u_meas, v_meas): ''' Inputs: u_meas: a list of arrays where each array are the u values for each board. v_meas: a list of arrays where each array are the v values for each board. Output: corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board. HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end HINT: our solution does not use the u_meas and v_meas values HINT: it does not matter where your frame it, as long as you are consistent! ''' ########## Code starts here ########## # using the left bottom corner as orgin. x-axis pointing right and y-axis pointing upward row_idx = np.linspace(0, self.d_square * (self.n_corners_x - 1), self.n_corners_x) col_idx = np.linspace(0, self.d_square * (self.n_corners_y - 1), self.n_corners_y).reshape(self.n_corners_y, 1) col_idx = col_idx[::-1] X_world = np.broadcast_to( row_idx, (self.n_corners_y, self.n_corners_x)).reshape(-1) Y_world = np.broadcast_to( col_idx, (self.n_corners_y, self.n_corners_x)).reshape(-1) Xg = [X_world] * self.n_chessboards Yg = [Y_world] * self.n_chessboards corner_coordinates = (Xg, Yg) ########## Code ends here ########## return corner_coordinates def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A ''' Inputs: u_meas: an array of the u values for a board. v_meas: an array of the v values for a board. X: an array of the X values for a board. (from genCornerCoordinates) Y: an array of the Y values for a board. (from genCornerCoordinates) Output: H: the homography matrix. its size is 3x3 HINT: What is the size of the matrix L? HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live? HINT: np.stack and/or np.hstack may come in handy here. ''' ########## Code starts here ########## num_points = u_meas.size # 63 points # size of matrix L (2n x 9) L = np.zeros((2 * num_points, 9)) # note, write the world coordinates in homogeneous form for i in range(num_points): L[2 * i] = [ X[i], Y[i], 1, 0, 0, 0, -u_meas[i] * X[i], -u_meas[i] * Y[i], -u_meas[i] ] L[2 * i + 1] = [ 0, 0, 0, X[i], Y[i], 1, -v_meas[i] * X[i], -v_meas[i] * Y[i], -v_meas[i] ] # x is the right singular vector of L, which is the third output of svd P, D, x = np.linalg.svd(L) # associated with the smallest singular value is the last one , descending order H = np.reshape(x[-1, :], (3, 3)) ########## Code ends here ########## return H def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B ''' Input: H: a list of homography matrices for each board Output: A: the camera intrinsic matrix HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT HINT: What is the definition of h_ij? HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function) HINT: What is the size of V? ''' ########## Code starts here ########## # get number of images n_images = len(H) # Solve Vb = 0 V = np.zeros((2 * n_images, 6)) # get hi and v_ij first for n in range(n_images): h = H[n].T v_ij = lambda i, j: np.array([ h[i - 1, 0] * h[j - 1, 0], h[i - 1, 0] * h[j - 1, 1] + h[i - 1, 1] * h[j - 1, 0], h[i - 1, 1] * h[j - 1, 1], h[ i - 1, 2] * h[j - 1, 0] + h[i - 1, 0] * h[j - 1, 2], h[ i - 1, 2] * h[j - 1, 1] + h[i - 1, 1] * h[j - 1, 2], h[ i - 1, 2] * h[j - 1, 2] ]) # note, v_ij is not transpose, so don't need transpose in V V[2 * n] = v_ij(1, 2) V[2 * n + 1] = v_ij(1, 1) - v_ij(2, 2) P, Q, solution = np.linalg.svd(V) # b is associated with the smallest singular value b = solution[-1, :] B11, B12, B22, B13, B23, B33 = b v0 = (B12 * B13 - B11 * B23) / (B11 * B22 - B12 * B12) lam = B33 - (B13**2 + v0 * (B12 * B13 - B11 * B23)) / B11 alpha = np.sqrt(lam / B11) beta = np.sqrt(lam * B11 / (B11 * B22 - B12**2)) gamma = -B12 * alpha**2 * beta / lam u0 = gamma * v0 / beta - B13 * alpha**2 / lam A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]]) ########## Code ends here ########## return A def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C ''' Inputs: H: a single homography matrix A: the camera intrinsic matrix Outputs: R: the rotation matrix t: the translation vector ''' ########## Code starts here ########## h1 = H[:, 0] h2 = H[:, 1] h3 = H[:, 2] lam = 1 / np.linalg.norm(np.linalg.inv(A).dot(h1)) r1 = lam * np.linalg.inv(A).dot(h1) r2 = lam * np.linalg.inv(A).dot(h2) r3 = np.cross(r1, r2) t = lam * np.linalg.inv(A).dot(h3) Q = np.hstack((r1.reshape(np.size(r1), 1), r2.reshape(np.size(r2), 1), r3.reshape(np.size(r3), 1))) U, S, V_T = np.linalg.svd(Q) R = np.matmul(U, V_T) ########## Code ends here ########## return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: x, y: the coordinates in the ideal normalized image plane ''' ########## Code starts here ########## num_points = np.size(X) # 63 points # get points in world frame in homogeneous form P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1) for i in range(1, num_points): P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1))) Rt = np.hstack((R, t.reshape(3, 1))) # 3x4 p = np.matmul(Rt, P) # 3x63 p = np.divide(p, p[-1, :]) # transform to homogenous form x = p[0, :] y = p[1, :] ########## Code ends here ########## return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. A: the camera intrinsic parameters R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: u, v: the coordinates in the ideal pixel image plane ''' ########## Code starts here ########## num_points = np.size(X) # 63 points # get points in world frame in homogeneous form P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1) for i in range(1, num_points): P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1))) Rt = np.hstack((R, t.reshape(3, 1))) # 3x4 M = np.matmul(A, Rt) # 3x63 p = np.matmul(M, P) p = np.divide(p, p[-1, :]) # transform to homogeneous form u = p[0, :] v = p[1, :] ########## Code ends here ########## return u, v def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') x, y = self.transformWorld2NormImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p]) u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([ box.x0, box.y0 + box.height * 0.15, box.width, box.height * 0.85 ]) ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p + 1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [ 0, self.n_corners_x - 1, self.n_corners_x * self.n_corners_y - 1, self.n_corners_x * (self.n_corners_y - 1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([ zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]]) ]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot( np.array( [X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p + 1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2 * view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title( 'Estimated Board Locations (Chessboard {0})'.format(i + 1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape( (1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1] ) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append(ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg(img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg(img_msg) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title('Corner Extraction (Chessboard {0})'.format(i+1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len(self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0]*n_corners[1] def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack([k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6*n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title('Image Correction (Chessboard {0})'.format(i+1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([box.x0, box.y0 + box.height * 0.15, box.width, box.height*0.85]) if k[0] != 0: u_br, v_br = self.transformWorld2PixImageDist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k) ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration') ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p+1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [0, self.n_corners_x-1, self.n_corners_x*self.n_corners_y-1, self.n_corners_x*(self.n_corners_y-1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]])]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot(np.array([X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p+1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2*view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title('Estimated Board Locations (Chessboard {0})'.format(i+1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape((1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard) def genCornerCoordinates(self, u_meas, v_meas): # TODO - part (i) raise NotImplementedError return X, Y def estimateHomography(self, u_meas, v_meas, X, Y): # TODO - part (ii) raise NotImplementedError return H def getCameraIntrinsics(self, H): # TODO - part (iii) raise NotImplementedError return A def getExtrinsics(self, H, A): # TODO - part (iv) raise NotImplementedError return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): """ Note: The transformation functions should only process one chessboard at a time! This means X, Y, Z, R, t should be individual arrays """ # TODO - part (v) raise NotImplementedError return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # TODO - part (v) raise NotImplementedError return u, v def transformWorld2NormImageDist(self, X, Y, R, t, k): # TODO: test # TODO - part (vi) raise NotImplementedError return x_br, y_br def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k): # TODO - part (vi) raise NotImplementedError return u_br, v_br
class CameraCheckerNode: def __init__(self, chess_size, dim): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" tosync_stereo = [ (left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo) ] tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board]) def queue_monocular(self, msg, cmsg): self.q_mono.put((msg, cmsg)) def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg): self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg)) def mkgray(self, msg): return self.mc.mkgray(msg) def image_corners(self, im): (ok, corners, b) = self.mc.get_corners(im) if ok: return corners else: return None def handle_monocular(self, msg): (image, camera) = msg gray = self.mkgray(image) C = self.image_corners(gray) if C is not None: linearity_rms = self.mc.linear_error(C, self.board) # Add in reprojection check image_points = C object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0] dist_coeffs = numpy.zeros((4, 1)) camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ], [ camera.P[4], camera.P[5], camera.P[6] ], [ camera.P[8], camera.P[9], camera.P[10] ] ] ) ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs) # Convert rotation into a 3x3 Rotation Matrix rot3x3, _ = cv2.Rodrigues(rot) # Reproject model points into image object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans) reprojected_h = camera_matrix * object_points_world reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :]) reprojection_errors = image_points.squeeze().T - reprojected reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape)) # Print the results print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms)) else: print('no chessboard') def handle_stereo(self, msg): (lmsg, lcmsg, rmsg, rcmsg) = msg lgray = self.mkgray(lmsg) rgray = self.mkgray(rmsg) L = self.image_corners(lgray) R = self.image_corners(rgray) if L and R: epipolar = self.sc.epipolar_error(L, R) dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg)) print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension)) else: print("no chessboard")
class CameraCheckerNode: def __init__(self, chess_size, dim): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] tsm = message_filters.TimeSynchronizer([ message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono ], 10) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name( "stereo") + "/right/camera_info" tosync_stereo = [(left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo)] tss = message_filters.TimeSynchronizer([ message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo ], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue.Queue() self.q_stereo = Queue.Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) def queue_monocular(self, msg, cmsg): self.q_mono.put((msg, cmsg)) def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg): self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg)) def mkgray(self, msg): return self.br.imgmsg_to_cv(msg, "bgr8") def image_corners(self, im): (ok, corners, b) = self.mc.get_corners(im) if ok: return list( cvmat_iterator( cv.Reshape(self.mc.mk_image_points([(corners, b)]), 2))) else: return None def handle_monocular(self, msg): def pt2line(x0, y0, x1, y1, x2, y2): """ point is (x0, y0), line is (x1, y1, x2, y2) """ return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1)**2 + (y2 - y1)**2) (image, camera) = msg rgb = self.mkgray(image) C = self.image_corners(rgb) if C: cc = self.board.n_cols cr = self.board.n_rows errors = [] for r in range(cr): (x1, y1) = C[(cc * r) + 0] (x2, y2) = C[(cc * r) + cc - 1] for i in range(1, cc - 1): (x0, y0) = C[(cc * r) + i] errors.append(pt2line(x0, y0, x1, y1, x2, y2)) linearity_rms = math.sqrt( sum([e**2 for e in errors]) / len(errors)) # Add in reprojection check A = cv.CreateMat(3, 3, 0) image_points = cv.fromarray(numpy.array(C)) object_points = cv.fromarray(numpy.zeros([cc * cr, 3])) for i in range(cr): for j in range(cc): object_points[i * cc + j, 0] = j * self.board.dim object_points[i * cc + j, 1] = i * self.board.dim object_points[i * cc + j, 2] = 0.0 dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]])) camera_matrix = numpy.array( [[camera.P[0], camera.P[1], camera.P[2]], [camera.P[4], camera.P[5], camera.P[6]], [camera.P[8], camera.P[9], camera.P[10]]]) rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans) # Convert rotation into a 3x3 Rotation Matrix rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot3x3) # Reproject model points into image object_points_world = numpy.asmatrix(rot3x3) * ( numpy.asmatrix(object_points).T) + numpy.asmatrix(trans) reprojected_h = camera_matrix * object_points_world reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :]) reprojection_errors = image_points - reprojected.T reprojection_rms = numpy.sqrt( numpy.sum(numpy.array(reprojection_errors)**2) / numpy.product(reprojection_errors.shape)) # Print the results print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % ( linearity_rms, reprojection_rms) else: print 'no chessboard' def handle_stereo(self, msg): (lmsg, lcmsg, rmsg, rcmsg) = msg lrgb = self.mkgray(lmsg) rrgb = self.mkgray(rmsg) sc = StereoCalibrator([self.board]) L = self.image_corners(lrgb) R = self.image_corners(rrgb) scm = image_geometry.StereoCameraModel() scm.fromCameraInfo(lcmsg, rcmsg) if L and R: d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)] epipolar = math.sqrt(sum([i**2 for i in d]) / len(d)) disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)] pt3d = [ scm.projectPixelTo3d((x, y), d) for ((x, y), d) in zip(L, disparities) ] def l2(p0, p1): return math.sqrt( sum([(c0 - c1)**2 for (c0, c1) in zip(p0, p1)])) # Compute the length from each horizontal and vertical lines, and return the mean cc = self.board.n_cols cr = self.board.n_rows lengths = ([ l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr) ] + [ l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc) ]) dimension = sum(lengths) / len(lengths) print "epipolar error: %f pixels dimension: %f m" % (epipolar, dimension) else: print "no chessboard"
def __init__(self, name, chess_size, dim, approximate=0): super().__init__(name) self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim # make sure n_cols is not smaller than n_rows, otherwise error computation will be off if self.board.n_cols < self.board.n_rows: self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols image_topic = "monocular/image_rect" camera_topic = "monocular/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] if approximate <= 0: sync = message_filters.TimeSynchronizer else: sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate) tsm = sync([ message_filters.Subscriber(self, type, topic) for (topic, type) in tosync_mono ], 10) tsm.registerCallback(self.queue_monocular) left_topic = "stereo/left/image_rect" left_camera_topic = "stereo/left/camera_info" right_topic = "stereo/right/image_rect" right_camera_topic = "stereo/right/camera_info" tosync_stereo = [(left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo)] tss = sync([ message_filters.Subscriber(self, type, topic) for (topic, type) in tosync_stereo ], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board])
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append(ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg(img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg(img_msg) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title('Corner Extraction (Chessboard {0})'.format(i+1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len(self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0]*n_corners[1] def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack([k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6*n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title('Image Correction (Chessboard {0})'.format(i+1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([box.x0, box.y0 + box.height * 0.15, box.width, box.height*0.85]) if k[0] != 0: u_br, v_br = self.transformWorld2PixImageDist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k) ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration') ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p+1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [0, self.n_corners_x-1, self.n_corners_x*self.n_corners_y-1, self.n_corners_x*(self.n_corners_y-1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]])]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot(np.array([X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p+1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2*view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title('Estimated Board Locations (Chessboard {0})'.format(i+1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape((1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard) def genCornerCoordinates(self, u_meas, v_meas): # part (i) - World coordinates for each corner x_line = self.d_square*np.array(range(0, self.n_corners_x, 1)) y_line = self.d_square*np.array(range(self.n_corners_y-1, -1, -1)) # Flip Y-axis to traditional direction # Form 2-D array of xy-coordinates x_coord = np.tile(x_line, (self.n_corners_y, 1)) y_coord = np.tile(y_line, (self.n_corners_x, 1)).T # Flatten and replicate for all chessboards X = self.n_chessboards*[x_coord.flatten()] Y = self.n_chessboards*[y_coord.flatten()] return X, Y def estimateHomography(self, u_meas, v_meas, X, Y): # part (ii) - Homography of single chessboard n = self.n_corners_per_chessboard M_tld_T = np.vstack((X, Y, np.ones(n))).T uM_tld_T = np.multiply(np.tile(u_meas, (3,1)).T, M_tld_T) vM_tld_T = np.multiply(np.tile(v_meas, (3,1)).T, M_tld_T) # Homography equation: L*x = 0 L = np.empty((2*n,9)) L[::2,:] = np.hstack((M_tld_T, np.zeros((n,3)), -uM_tld_T)) L[1::2,:] = np.hstack((np.zeros((n,3)), M_tld_T, -vM_tld_T)) # Solve for x and reshape into H U, s, V_T = np.linalg.svd(L, full_matrices=True, compute_uv=True) x = V_T[np.argmin(s),:] # x = [h1, h2, h3] (stacked columns of H) H = np.reshape(x, (3,3)) return H def getCameraIntrinsics(self, H): # part (iii) - Linear intrinsic parameters of camera def v_row(i, j, H_obs): hi = H_obs[:,i] # i-th column vector of H hj = H_obs[:,j] # j-th column vector of H return np.array([hi[0]*hj[0], hi[0]*hj[1] + hi[1]*hj[0], hi[1]*hj[1], hi[2]*hj[0] + hi[0]*hj[2], hi[2]*hj[1] + hi[1]*hj[2], hi[2]*hj[2]]) # Homography constraints: V_hom*b = 0 V_hom = [] for H_obs in H: V_hom.append(v_row(0, 1, H_obs)) V_hom.append(v_row(0, 0, H_obs) - v_row(1, 1, H_obs)) if len(H) == 2: # Impose skewness constraint: gamma = 0 V_hom.append([0, 1, 0, 0, 0, 0]) if len(H) == 1: raise NotImplementedError # Need to know (u0, v0) and set gamma = 0 V_hom = np.asarray(V_hom) U, s, V_T = np.linalg.svd(V_hom, full_matrices=True, compute_uv=True) b = V_T[np.argmin(s),:] # b = [B11, B12, B22, B13, B23, B33] # Extract intrinsic parameters from B v0 = (b[1]*b[2] - b[0]*b[4])/(b[0]*b[2] - b[1]**2) lamb = b[5] - (b[3]**2 + v0*(b[1]*b[3] - b[0]*b[4]))/b[0] alpha = np.sqrt(lamb/b[0]) beta = np.sqrt(lamb*b[0]/(b[0]*b[2] - b[1]**2)) gamma = -b[1]*(alpha**2)*beta/lamb u0 = gamma*v0/beta - b[3]*alpha**2/lamb A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]]) return A def getExtrinsics(self, H, A): # part (iv) - Rotation and translation of single chessboard A_inv = np.linalg.inv(A) lamb = 1.0/np.linalg.norm(A_inv.dot(H[:,0])) r1 = lamb*A_inv.dot(H[:,0]) r2 = lamb*A_inv.dot(H[:,1]) r3 = np.cross(r1, r2) t = lamb*A_inv.dot(H[:,2]) # Approximate rotation matrix Q = np.vstack((r1, r2, r3)).T U, s, V_T = np.linalg.svd(Q, full_matrices=False, compute_uv=True) R = U.dot(V_T) return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): """ Note: The transformation functions should only process one chessboard at a time! This means X, Y, Z, R, t should be individual arrays """ # part (v) - World coordinate (X,Y,Z) to normalized image coordinate (x,y) in undistorted frame n = self.n_corners_per_chessboard P_hW = np.vstack((X, Y, Z, np.ones(n))) Rt = np.column_stack((R, t)) P_C = Rt.dot(P_hW) # [X_C, Y_C, Z_C] # Camera to image coordinate (assume f = 1, origin = bottom left corner) x = P_C[0,:]/P_C[2,:] # X_C/Z_C y = P_C[1,:]/P_C[2,:] # Y_C/Z_C return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # part (v) - World coordinate (X,Y,Z) to pixel image coordinate (u,v) in undistorted frame n = self.n_corners_per_chessboard P_hW = np.vstack((X, Y, Z, np.ones(n))) Rt = np.column_stack((R, t)) p_h = A.dot(Rt).dot(P_hW) # [u, v, w] # Transform to inhomogeneous coordinates u = p_h[0,:]/p_h[2,:] v = p_h[1,:]/p_h[2,:] return u, v def transformWorld2NormImageDist(self, X, Y, Z, R, t, k): # part (vi) - World coordinate (X,Y,Z) to normalized image coordinate (x,y) in distorted frame x, y = self.transformWorld2NormImageUndist(X, Y, Z, R, t) x_br = x + x*(k[0]*(x**2 + y**2) + k[1]*(x**2 + y**2)**2) y_br = y + y*(k[0]*(x**2 + y**2) + k[1]*(x**2 + y**2)**2) return x_br, y_br def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k): # part (vi) - World coordinate (X,Y,Z) to pixel image coordinate (u,v) in distorted frame n = self.n_corners_per_chessboard x_br, y_br = self.transformWorld2NormImageDist(X, Y, Z, R, t, k) p_br = np.vstack((x_br, y_br, np.ones(n))) u_br = A[0,:].dot(p_br) v_br = A[1,:].dot(p_br) return u_br, v_br
def find_intrinsics(visualize=False): fatal = False good = [] xys = [] if 1: with open('setup.json', mode='r') as fd: setup = json.loads(fd.read()) globber = '*-corners*.data' files = glob.glob(globber) files.sort() print 'all data files matching "%s"' % (globber, ) print files for f in files: this_fatal = False parts = f.split('-') board = None for p in parts: if p.startswith('board'): boardname = p with open(boardname + '.json', mode='r') as fd: buf = fd.read() boardd = json.loads(buf) board = ChessboardInfo() board.n_cols = boardd['n_cols'] board.n_rows = boardd['n_rows'] board.dim = boardd['dim'] assert board is not None xy = np.loadtxt(f) xys.append(xy) if len(xy) != board.n_cols * board.n_rows: rospy.logfatal('Error: %d points in %s. Expected %d.' % (len(xy), f, board.n_cols * board.n_rows)) this_fatal = True fatal = True if visualize: if 0: plt.figure() fmt = 'o-' label = f if this_fatal: fmt = 'kx:' label = 'BAD: ' + f plt.plot(xy[:, 0], xy[:, 1], fmt, mfc='none', label=label) if this_fatal: for i in range(len(xy)): plt.text(xy[i, 0], xy[i, 1], str(i)) corners = [(x, y) for (x, y) in xy] good.append((corners, board)) if visualize: plt.legend() plt.show() if fatal: sys.exit(1) mc = MonoCalibrator( [board] ) #, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST ) mc.size = tuple(setup['size']) mc.cal_fromcorners(good) msg = mc.as_message() cam = pymvg.CameraModel.from_ros_like(intrinsics=msg) cam_mirror = cam.get_mirror_camera() if 1: # The intrinsics are valid for the whole physical display and # each virtual display. names = setup['names'] for name in names: name = str( name) # remove unicode -- ROS bag fails on unicode topic name if 'mirror' in name: c = cam_mirror else: c = cam msg = c.get_intrinsics_as_msg() fname = 'display-intrinsic-cal-%s.bag' % (name.replace('/', '-'), ) bagout = rosbag.Bag(fname, 'w') topic = '/%s/camera_info' % name bagout.write(topic, msg) bagout.close() print 'saved to', fname print 'You can play this calibration with:\n' print 'rosbag play %s -l' % (fname, ) print
def find_intrinsics(visualize=False): fatal = False good = [] xys = [] if 1: with open('setup.json',mode='r') as fd: setup = json.loads(fd.read()) globber = '*-corners*.data' files = glob.glob(globber) files.sort() print 'all data files matching "%s"'%(globber,) print files for f in files: this_fatal = False parts = f.split('-') board = None for p in parts: if p.startswith('board'): boardname = p with open(boardname+'.json',mode='r') as fd: buf = fd.read() boardd = json.loads(buf) board = ChessboardInfo() board.n_cols = boardd['n_cols'] board.n_rows = boardd['n_rows'] board.dim = boardd['dim'] assert board is not None xy = np.loadtxt(f) xys.append(xy) if len(xy) != board.n_cols * board.n_rows: rospy.logfatal('Error: %d points in %s. Expected %d.'%( len(xy), f, board.n_cols * board.n_rows)) this_fatal = True fatal = True if visualize: if 0: plt.figure() fmt = 'o-' label = f if this_fatal: fmt = 'kx:' label = 'BAD: '+ f plt.plot( xy[:,0], xy[:,1], fmt, mfc='none', label=label ) if this_fatal: for i in range(len(xy)): plt.text( xy[i,0], xy[i,1], str(i) ) corners = [ (x,y) for (x,y) in xy ] good.append( (corners, board) ) if visualize: plt.legend() plt.show() if fatal: sys.exit(1) mc = MonoCalibrator([ board ])#, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST ) mc.size = tuple(setup['size']) mc.cal_fromcorners(good) msg = mc.as_message() cam = pymvg.CameraModel.from_ros_like( intrinsics=msg ) cam_mirror = cam.get_mirror_camera() if 1: # The intrinsics are valid for the whole physical display and # each virtual display. names = setup['names'] for name in names: name = str(name) # remove unicode -- ROS bag fails on unicode topic name if 'mirror' in name: c = cam_mirror else: c = cam msg = c.get_intrinsics_as_msg() fname = 'display-intrinsic-cal-%s.bag'%( name.replace('/','-'), ) bagout = rosbag.Bag(fname, 'w') topic = '/%s/camera_info'%name bagout.write(topic, msg) bagout.close() print 'saved to',fname print 'You can play this calibration with:\n' print 'rosbag play %s -l'%(fname,) print
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append(ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) # n_rows, n_cols, dims self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg(img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg(img_msg) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title('Corner Extraction (Chessboard {0})'.format(i+1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions (480 x 640) self.n_chessboards = len(self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0]*n_corners[1] def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack([k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6*n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title('Image Correction (Chessboard {0})'.format(i+1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([box.x0, box.y0 + box.height * 0.15, box.width, box.height*0.85]) if k[0] != 0: u_br, v_br = self.transformWorld2PixImageDist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k) ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration') ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p+1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [0, self.n_corners_x-1, self.n_corners_x*self.n_corners_y-1, self.n_corners_x*(self.n_corners_y-1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]])]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot(np.array([X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p+1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2*view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title('Estimated Board Locations (Chessboard {0})'.format(i+1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape((1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) # Flip Y-axis to traditional direction (aka NOT CV coordinate system) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]) return u_meas, v_meas # Lists of arrays (one per chessboard) def genCornerCoordinates(self, u_meas, v_meas): # part (i) ''' u_meas and v_meas are not necessarily needed in this function since the world frame is NOT relative to pixel/camera frame. Here, we define the top left corner as the origin. ''' X = [] Y = [] # set the bottom left corner as the origin, x-axis pointing right, y-axis pointing up row_coords = np.linspace(0., self.d_square*(self.n_corners_x-1), num=self.n_corners_x) col_coords = np.linspace(0., self.d_square*(self.n_corners_y-1), num=self.n_corners_y).reshape(self.n_corners_y, 1) col_coords = col_coords[-1, :] - col_coords corner_world_x = np.broadcast_to(row_coords, (self.n_corners_y, self.n_corners_x)).reshape(-1) corner_world_y = np.broadcast_to(col_coords, (self.n_corners_y, self.n_corners_x)).reshape(-1) X = [corner_world_x] * self.n_chessboards Y = [corner_world_y] * self.n_chessboards return X, Y # Lists of arrays (length: 23) def estimateHomography(self, u_meas, v_meas, X, Y): # part (ii) # u_meas/v_meas: (63,) # X/Y: (63,) assert u_meas.shape == X.shape num_points = u_meas.shape[0] # 63 points # each point: 2 constraints (9 items) --> P: 2nx9 matrix # minimize Ph = 0 subject to 2-norm(h) = 1 P = np.zeros((2*num_points, 9)) for i in range(num_points): P[2*i] = [X[i], Y[i], 1, 0, 0, 0, -u_meas[i]*X[i], -u_meas[i]*Y[i], -u_meas[i]] P[2*i+1] = [0, 0, 0, X[i], Y[i], 1, -v_meas[i]*X[i], -v_meas[i]*Y[i], -v_meas[i]] _, _, V_T = np.linalg.svd(P) h = V_T[-1, :] H = np.reshape(h, (3, 3)) return H def getCameraIntrinsics(self, H): # part (iii): n images of the model plane, construct (2n, 6) V matrix num_images = len(H) # solve Vb = 0 V = np.zeros((2*num_images, 6)) for n in range(num_images): _H = H[n].T # follow the notation of literature v_ij = lambda i, j: np.array([_H[i-1, 0]*_H[j-1, 0], _H[i-1, 0]*_H[j-1, 1]+_H[i-1, 1]*_H[j-1, 0], _H[i-1, 1]*_H[j-1, 1], _H[i-1, 2]*_H[j-1, 0]+_H[i-1, 0]*_H[j-1, 2], _H[i-1, 2]*_H[j-1, 1]+_H[i-1, 1]*_H[j-1, 2], _H[i-1, 2]*_H[j-1, 2]]) V[2*n] = v_ij(1, 2) V[2*n+1] = v_ij(1, 1) - v_ij(2, 2) _, _, V_T = np.linalg.svd(V) b = V_T[-1, :] B11, B12, B22, B13, B23, B33 = b # extract intrinsic parameters v0 = (B12*B13 - B11*B23) / (B11*B22 - B12**2) _lambda = B33 - (B13**2 + v0*(B12*B13 - B11*B23)) / B11 alpha = np.sqrt(_lambda / B11) beta = np.sqrt(_lambda*B11 / (B11*B22 - B12**2)) gamma = -B12*(alpha**2)*beta / _lambda u0 = gamma*v0 / beta - B13*(alpha**2) / _lambda # sanity check # print("u0: {}".format(u0/self.w_pixels)) # print("v0: {}".format(v0/self.h_pixels)) # print("gamma << alpha: {}".format(abs(gamma)/alpha)) # construct intrinsic matrix A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]]) return A def getExtrinsics(self, H, A): # part (iv) h1, h2, h3 = H[:, 0], H[:, 1], H[:, 2] lambda1 = 1. / np.linalg.norm(np.linalg.inv(A).dot(h1)) lambda2 = 1. / np.linalg.norm(np.linalg.inv(A).dot(h2)) # sanity check (lambda1 = lambda2 theoretically) _lambda = 0.5 * (lambda1 + lambda2) if abs(lambda1 - lambda2) > 1e-5 else lambda1 r1 = _lambda * np.linalg.inv(A).dot(h1) r2 = _lambda * np.linalg.inv(A).dot(h2) r3 = np.cross(r1, r2) t = _lambda * np.linalg.inv(A).dot(h3) # initial R Q = np.hstack((r1.reshape(-1, 1), r2.reshape(-1, 1), r3.reshape(-1, 1))) U, _, V_T = np.linalg.svd(Q) R = np.matmul(U, V_T) return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): """ Note: The transformation functions should only process one chessboard at a time! This means X, Y, Z, R, t should be individual arrays """ # part (v) num_points = X.shape[0] P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1) for i in range(1, num_points): P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1))) assert P.shape == (4, 63) Rt = np.hstack((R, t.reshape(-1, 1))) # 3x4 p = np.matmul(Rt, P) # 3x63 p = np.divide(p, p[-1, :]) x = p[0, :].tolist() y = p[1, :].tolist() return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # part (v) num_points = X.shape[0] P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1) for i in range(1, num_points): P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1))) T = np.matmul(A, np.hstack((R, t.reshape(-1, 1)))) # 3x4 p = np.matmul(T, P) # 3x63 p = np.divide(p, p[-1, :]) u = p[0, :].tolist() v = p[1, :].tolist() return u, v def transformWorld2NormImageDist(self, X, Y, R, t, k): # part (vi) x, y = self.transformWorld2NormImageUndist(X, Y, np.zeros_like(X), R, t) x, y = np.array(x), np.array(y) x_br = x + x * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2)) y_br = y + y * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2)) return x_br, y_br def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k): # part (vi) u, v = self.transformWorld2PixImageUndist(X, Y, np.zeros_like(Z), R, t, A) u_0, v_0 = A[0, 2], A[1, 2] x, y = self.transformWorld2NormImageUndist(X, Y, np.zeros_like(Z), R, t) u, v = np.array(u), np.array(v) x, y = np.array(x), np.array(y) u_0, v_0 = np.array([u_0]*u.shape[0]), np.array([v_0]*v.shape[0]) u_br = u + (u - u_0) * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2)) v_br = v + (v - v_0) * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2)) return u_br, v_br
def cal_from_tarfile(boards, tarname, mono=False, upload=False, calib_flags=0, visualize=False, alpha=1.0): if mono: calibrator = MonoCalibrator(boards, calib_flags) else: calibrator = StereoCalibrator(boards, calib_flags) calibrator.do_tarfile_calibration(tarname) print(calibrator.ost()) if upload: info = calibrator.as_message() if mono: set_camera_info_service = rospy.ServiceProxy( "%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) response = set_camera_info_service(info) if not response.success: raise RuntimeError( "connected to set_camera_info service, but failed setting camera_info" ) else: set_left_camera_info_service = rospy.ServiceProxy( "%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) set_right_camera_info_service = rospy.ServiceProxy( "%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) response1 = set_left_camera_info_service(info[0]) response2 = set_right_camera_info_service(info[1]) if not (response1.success and response2.success): raise RuntimeError( "connected to set_camera_info service, but failed setting camera_info" ) if visualize: #Show rectified images calibrator.set_alpha(alpha) archive = tarfile.open(tarname, 'r') if mono: for f in archive.getnames(): if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')): filedata = archive.extractfile(f).read() file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) im = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR) bridge = cv_bridge.CvBridge() try: msg = bridge.cv2_to_imgmsg(im, "bgr8") except cv_bridge.CvBridgeError as e: print(e) #handle msg returns the recitifed image with corner detection once camera is calibrated. drawable = calibrator.handle_msg(msg) vis = numpy.asarray(drawable.scrib[:, :]) #Display. Name of window:f display(f, vis) else: limages = [ f for f in archive.getnames() if (f.startswith('left') and ( f.endswith('pgm') or f.endswith('png'))) ] limages.sort() rimages = [ f for f in archive.getnames() if (f.startswith('right') and ( f.endswith('pgm') or f.endswith('png'))) ] rimages.sort() if not len(limages) == len(rimages): raise RuntimeError( "Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) for i in range(len(limages)): l = limages[i] r = rimages[i] if l.startswith('left') and ( l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and ( r.endswith('.pgm') or r.endswith('png')): # LEFT IMAGE filedata = archive.extractfile(l).read() file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) im_left = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR) bridge = cv_bridge.CvBridge() try: msg_left = bridge.cv2_to_imgmsg(im_left, "bgr8") except cv_bridge.CvBridgeError as e: print(e) #RIGHT IMAGE filedata = archive.extractfile(r).read() file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) im_right = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR) try: msg_right = bridge.cv2_to_imgmsg(im_right, "bgr8") except cv_bridge.CvBridgeError as e: print(e) drawable = calibrator.handle_msg([msg_left, msg_right]) h, w = numpy.asarray(drawable.lscrib[:, :]).shape[:2] vis = numpy.zeros((h, w * 2, 3), numpy.uint8) vis[:h, :w, :] = numpy.asarray(drawable.lscrib[:, :]) vis[:h, w:w * 2, :] = numpy.asarray(drawable.rscrib[:, :]) display(l + " " + r, vis)
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, display_flag): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append( ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(os.listdir(self.cal_img_path)): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg( img_msg ) # Extract chessboard corners using ROS camera_calibration package if display_flag: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len( self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0] * n_corners[1] def undistortImages(self, A, k=np.zeros(2), scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(os.listdir(self.cal_img_path)): img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(self.n_chessboards): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([ box.x0, box.y0 + box.height * 0.15, box.width, box.height * 0.85 ]) if k[0] != 0: u_br, v_br = self.transformWorld2PixImageDist( X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k) ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration') ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p + 1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [ 0, self.n_corners_x - 1, self.n_corners_x * self.n_corners_y - 1, self.n_corners_x * (self.n_corners_y - 1), ] s_cam = 0.02 d_cam = 0.1 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([ zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]]) ]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(1, 2, 1) ax3d = fig.add_subplot(1, 2, 2, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot( np.array( [X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) M_tld *= np.sign(M_tld[2]) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(os.listdir(self.cal_img_path)): img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_color('green') cam.set_alpha(0.2) ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_color('blue') boards[p].set_alpha(1.0) else: boards[p].set_color('red') boards[p].set_alpha(0.1) ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p + 1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-5 * view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') plt.tight_layout() fig.canvas.set_window_title( 'Estimated Board Locations (Chessboard {0})'.format(i + 1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def writeCalibrationYaml(self, A, k): self.c.intrinsics = A self.c.distortion = np.hstack((k, np.zeros(3))).reshape((5, 1)) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1] ) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard) def genCornerCoordinates(self, u_meas, v_meas): X = [] Y = [] # DEFINED FROM BOTTOM LEFT # for i in range(self.n_corners_y): # line = np.multiply(self.d_square,np.arange(self.n_corners_x)) # reversedLine = line[::-1] # X.append(reversedLine) # for i in range(self.n_corners_y): # Y.append(np.multiply(i*self.d_square,np.ones(self.n_corners_x))) # DEFINED FROM TOP LEFT for i in range(self.n_corners_y): line = np.multiply(self.d_square, np.arange(self.n_corners_x)) X.append(line) for i in range(self.n_corners_y): Y.append(np.multiply(i * self.d_square, np.ones(self.n_corners_x))) X = np.concatenate(X) Y = np.concatenate(Y) return X, Y def estimateHomography(self, u_meas, v_meas, X, Y): # Generate L Matrix L = np.zeros((2 * len(u_meas), 9)) iter = np.arange(len(u_meas)) M_i_T = np.column_stack((X, Y, np.ones((len(X))))) u_meas_mat = np.column_stack((u_meas, u_meas, u_meas)) v_meas_mat = np.column_stack((v_meas, v_meas, v_meas)) FirstStack = np.column_stack((M_i_T, np.zeros( (M_i_T.shape[0], 3)), -np.multiply(u_meas_mat, M_i_T))) SecondStack = np.column_stack((np.zeros( (M_i_T.shape[0], 3)), M_i_T, -np.multiply(v_meas_mat, M_i_T))) L = np.vstack((FirstStack, SecondStack)) # Use SVD to find lowest singular vector - solution to min norm (Lx) U, s, V = np.linalg.svd(L, full_matrices=False) # Extract solution vector smallestV = V[-1] # Reconstruct H H = np.zeros((3, 3)) H[0] = smallestV[0:3] H[1] = smallestV[3:6] H[2] = smallestV[6:9] return H def getCameraIntrinsics(self, H): V_LENGTH = 6 # Length of the V concatenated from every image V_mat = np.empty( (0, V_LENGTH)) # Initialize to empty matrix for concatenation # Loop over all images for i in range(self.n_chessboards): # Get the current H H_cur = H[i] # Generate V Matrix V_11 = self.getVij(H_cur, 1, 1) V_12 = self.getVij(H_cur, 1, 2) V_22 = self.getVij(H_cur, 2, 2) # Concatenate results onto V_mat V_mat = np.vstack((V_mat, V_12, (V_11 - V_22))) # Use SVD to find lowest singular vector - solution to min norm (Vx) U, s, V = np.linalg.svd(V_mat, full_matrices=False) # Extract solution vector b = V[-1] # NOTE: b = (B11;B12;B22;B13;B23;B33)^T : A = self.solveForIntrinsics(b) return A # Helper function for getCameraIntrinsics() # Gets the Vij term of each homography matrix. i and j are in form of equation, NOT for indexes # Usage: V_11 = self.getVij(H_cur,1,1) def getVij(self, H, i, j): # Adjust i and j to account for indexing i -= 1 j -= 1 H = H.T V_ij = np.array([H[i,0]*H[j,0] , \ H[i,0]*H[j,1] + H[i,1]*H[j,0], \ H[i,1]*H[j,1], \ H[i,2]*H[j,0] + H[i,0]*H[j,2], \ H[i,2]*H[j,1] + H[i,1]*H[j,2], \ H[i,2]*H[j,2]]) return V_ij # Helper function for getCameraIntrinsics() def solveForIntrinsics(self, b_sol): # NOTE: b = (B11;B12;B22;B13;B23;B33)^T : B = np.zeros((4, 4)) B[1, 1] = b_sol[0] B[1, 2] = b_sol[1] B[2, 2] = b_sol[2] B[1, 3] = b_sol[3] B[2, 3] = b_sol[4] B[3, 3] = b_sol[5] A = np.zeros((3, 3)) # Solve for intrinsic parameters v0 = (B[1, 2] * B[1, 3] - B[1, 1] * B[2, 3]) / (B[1, 1] * B[2, 2] - B[1, 2]**2) lam = B[3, 3] - (B[1, 3]**2 + v0 * (B[1, 2] * B[1, 3] - B[1, 1] * B[2, 3])) / B[1, 1] Beta = np.sqrt(lam * B[1, 1] / (B[1, 1] * B[2, 2] - B[1, 2]**2)) alpha = np.sqrt(lam / B[1, 1]) gamma = -B[1, 2] * alpha**2 * Beta / lam u0 = gamma * v0 / alpha - B[1, 3] * alpha**2 / lam A[0, 0] = alpha A[0, 1] = gamma A[0, 2] = u0 #u01 A[1, 1] = Beta A[1, 2] = v0 # A[2, 2] = 1 return A def getExtrinsics(self, H, A): lam = 1 / np.linalg.norm(np.dot(np.linalg.inv(A), H[:, 0])) r1 = lam * np.dot(np.linalg.inv(A), H[:, 0]) r2 = lam * np.dot(np.linalg.inv(A), H[:, 1]) r3 = np.cross(r1, r2) t = lam * np.dot(np.linalg.inv(A), H[:, 2]) R = np.column_stack((r1, r2, r3)) # Use SVD to find lowest singular vector - solution to min norm (Vx) U, s, V = np.linalg.svd(R, full_matrices=False) R_best = np.dot(U, V) return R_best, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): """ Note: The transformation functions should only process one chessboard at a time! This means X, Y, Z, R, t should be individual arrays """ worldCoords = np.row_stack((X, Y, Z, np.ones((len(X))))) cameraCoords = np.dot(np.column_stack((R, t)), worldCoords) X_C = cameraCoords[0] Y_C = cameraCoords[1] Z_C = cameraCoords[2] x = X_C / Z_C y = Y_C / Z_C return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): worldCoords = np.row_stack((X, Y, Z, np.ones((len(X))))) pixCoords = np.dot(np.dot(A, np.column_stack((R, t))), worldCoords) # Convert back to homogenous coordinates u = pixCoords[0] / pixCoords[2] v = pixCoords[1] / pixCoords[2] return u, v def transformWorld2NormImageDist(self, X, Y, Z, R, t, k): x, y = self.transformWorld2NormImageUndist(X, Y, Z, R, t) # Apply radial distortion correction x_br = x + x * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2) y_br = y + y * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2) return x_br, y_br def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k): u, v = self.transformWorld2PixImageUndist(X, Y, Z, R, t, A) x, y = self.transformWorld2NormImageUndist(X, Y, Z, R, t) u0 = A[0, 2] v0 = A[1, 2] # Apply radial distortion correction u_br = u + (u - u0) * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2) v_br = v + (v - v0) * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2) return u_br, v_br def estimateLensDistortion(self, u_meas, v_meas, X, Y, Z, R, t, A): u0 = A[0, 2] v0 = A[1, 2] u = [] v = [] x = [] y = [] # loop over all images and predict u,v for each (assumed undistorted) for i in range(self.n_chessboards): u_cur, v_cur = self.transformWorld2PixImageUndist( X[i], Y[i], Z[i], R[i], t[i], A) u.append(u_cur) v.append(v_cur) x_cur, y_cur = self.transformWorld2NormImageUndist( X[i], Y[i], Z[i], R[i], t[i]) x.append(x_cur) y.append(y_cur) u = np.concatenate(u) v = np.concatenate(v) x = np.concatenate(x) y = np.concatenate(y) u_breve = np.concatenate(u_meas) v_breve = np.concatenate(v_meas) # Form D-matrix d11 = (u - u0) * (x**2 + y**2) d12 = (u - u0) * ((x**2 + y**2)**2) d21 = (v - v0) * (x**2 + y**2) d22 = (v - v0) * ((x**2 + y**2)**2) D = np.row_stack((np.column_stack( (d11, d12)), np.column_stack((d21, d22)))) d = np.concatenate((u_breve - u, v_breve - v)) k = np.dot(np.linalg.pinv(D), d) return k
def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0): if mono: calibrator = MonoCalibrator(boards, calib_flags) else: calibrator = StereoCalibrator(boards, calib_flags) calibrator.do_tarfile_calibration(tarname) print(calibrator.ost()) if upload: info = calibrator.as_message() if mono: set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) response = set_camera_info_service(info) if not response.success: raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") else: set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) response1 = set_left_camera_info_service(info[0]) response2 = set_right_camera_info_service(info[1]) if not (response1.success and response2.success): raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") if visualize: #Show rectified images calibrator.set_alpha(alpha) archive = tarfile.open(tarname, 'r') if mono: for f in archive.getnames(): if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')): filedata = archive.extractfile(f).read() file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) im=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR) bridge = cv_bridge.CvBridge() try: msg=bridge.cv2_to_imgmsg(im, "bgr8") except cv_bridge.CvBridgeError as e: print(e) #handle msg returns the recitifed image with corner detection once camera is calibrated. drawable=calibrator.handle_msg(msg) vis=numpy.asarray( drawable.scrib[:,:]) #Display. Name of window:f display(f, vis) else: limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ] limages.sort() rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ] rimages.sort() if not len(limages) == len(rimages): raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) for i in range(len(limages)): l=limages[i] r=rimages[i] if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')): # LEFT IMAGE filedata = archive.extractfile(l).read() file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) im_left=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR) bridge = cv_bridge.CvBridge() try: msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8") except cv_bridge.CvBridgeError as e: print(e) #RIGHT IMAGE filedata = archive.extractfile(r).read() file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) im_right=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR) try: msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8") except cv_bridge.CvBridgeError as e: print(e) drawable=calibrator.handle_msg([ msg_left,msg_right] ) h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2] vis = numpy.zeros((h, w*2, 3), numpy.uint8) vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:]) vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:]) display(l+" "+r,vis)
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append( ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg( img_msg ) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len( self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0] * n_corners[1] def genCornerCoordinates(self, u_meas, v_meas): ''' Inputs: u_meas: a list of arrays where each array are the u values for each board. v_meas: a list of arrays where each array are the v values for each board. Output: corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board. HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end HINT: our solution does not use the u_meas and v_meas values HINT: it does not matter where your frame it, as long as you are consistent! ''' ########## Code starts here ########## # u_meas, v_meas returns the corners in pixel coordinates, where # bottom-left is (0,0). Points are ordered from blue to pink. # Slanted lines show where the start of the next row is. # This is all good, but # the Pset wants World coordinates (X,Y) attached to the # checkerboards, so we just generate these accordingly # based on what we know about the checkerboards. # In world coordinates (X,Y) in meters. The world reference frame # is centered on our arbitrary object. The way we're generating it, # X-axis goes from left-> right # Y-axis goes from top -> down # because that's how the points in u/v_meas are ordered in the array. N = self.n_chessboards nx, ny = self.n_corners_x, self.n_corners_y d = self.d_square # u_meas, v_meas have shapes (N, nx*ny) X_list = [] Y_list = [] # We don't assume nx, ny to be constant, so we need a loop for _ in range(N): Xs = np.multiply(d, np.arange(nx)) Xs = np.tile(Xs, ny) # Shape (nx*ny,) 1,2,3,4,1,2,3,4,1,2,3,4,... Ys = np.multiply(d, np.arange(ny)) Ys = np.repeat(Ys, nx) # Shape (nx*ny,) 1,1,1,2,2,2,3,3,3,4,4,4,... X_list.append(Xs) Y_list.append(Ys) corner_coordinates = (X_list, Y_list) ########## Code ends here ########## return corner_coordinates def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A ''' Inputs: u_meas: an array of the u values for a board. v_meas: an array of the v values for a board. X: an array of the X values for a board. (from genCornerCoordinates) Y: an array of the Y values for a board. (from genCornerCoordinates) Output: H: the homography matrix. its size is 3x3 HINT: What is the size of the matrix L? HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live? HINT: np.stack and/or np.hstack may come in handy here. ''' ########## Code starts here ########## # Zhang 1998. See also lecture 9 notes, especially section 9.2.1 # Lecture 9 (2019) 54 minute mark onwards (Step 1) # Equation number (x) refer to lecture 9 notes. # We follow the notation in the lecture notes. # We need to reconstruct the homography matrix M mapping (homogeneous) world # coordinates to pixel coordinates. nx, ny = self.n_corners_x, self.n_corners_y u, v = u_meas, v_meas # shapes (nx*ny, ) # X, Y as given, Z = 0 as we know the points are co-planar. P_W = np.array([X, Y, np.ones(nx * ny)]) # shape (k, nx*ny) = (3, 63) k = P_W.shape[0] # P_tilde has shape (2*nx*ny, 3k) # Each 'block' is [-1 0 ui ] * P_{W,i}^T # [ 0 -1 vi ] # and there are N of these blocks as we have N points per image. # P_{W,i} is a col vector of length 3. # This is not strictly matrix P_tilde as written in eq (18). # We blocked all rows with u and all the rows with v together. # Since each row is just a different constraint, this works just fine for SVD. P_tilde = np.block([[-1 * P_W.T, 0 * P_W.T, (u * P_W).T], [0 * P_W.T, -1 * P_W.T, (v * P_W).T]]) U, s, VT = np.linalg.svd(P_tilde, full_matrices=False) # We don't need padding. # Eigenvalues s sorted from largest to smallest. # We want the eigenvectors of P.T dot P, which corrresponds to V transpose. # Grab the smallest one in the last row, and reshape to M, which is 3x3. M = np.reshape(VT[-1, :], (k, k)) H = M # Convert back to Zhang's terminology. ########## Code ends here ########## return H def compute_Vij(self, H_T, i, j): """ Helper for getCameraIntrinsics. Expects H to already be transposed. arguments i and j should be cardinal (1-indexed) """ i, j = i - 1, j - 1 # Translate to ordinal array index Hi, Hj = H_T[i], H_T[j] V_ij = np.array([ Hi[0] * Hj[0], Hi[0] * Hj[1] + Hi[1] * Hj[0], Hi[1] * Hj[1], Hi[2] * Hj[0] + Hi[0] * Hj[2], Hi[2] * Hj[1] + Hi[1] * Hj[2], Hi[2] * Hj[2] ]) return V_ij def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B ''' Input: H: a list of homography matrices for each board Output: A: the camera intrinsic matrix HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT HINT: What is the definition of h_ij? HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function) HINT: What is the size of V? ''' ########## Code starts here ########## # Equation numbers (x) refer to lecture 9 notes. H_list = H # Rebind variable name N = self.n_chessboards k = 3 # Likewise, we will not replicate the matrix V exactly as in eq (27) # since it's just rows of SVD constraints. # First let's get rid of the list. (why use a list ?) # Each H is a 3x3, so let's turn the list into a tensor of shape (3, 3, N), # such that H[i, j] has shape (N, ). H_T = np.zeros((k, k, N)) for n in range(N): H_T[:, :, n] = H_list[n].T # Note the transpose just under eq (26) V_11 = self.compute_Vij(H_T, 1, 1) # shape (2k, N) = (6, 23) V_12 = self.compute_Vij(H_T, 1, 2) V_22 = self.compute_Vij(H_T, 2, 2) V = np.vstack((V_12.T, (V_11 - V_22).T)) # shape (2N, 2k) = (46, 6) U, s, VT_ = np.linalg.svd(V, full_matrices=False) b = VT_[-1, :] # We don't actually end up using the matrix B. # B = np.array([ # This is symmetric. We don't need the lower triangle. # [b[0], b[1], b[3]], # [b[1], b[2], b[4]], # [b[3], b[4], b[5]] # ]) # Then just plug in eq (30) (B11, B12, B22, B13, B23, B33) = b t1 = B12 * B13 - B11 * B23 t2 = B11 * B22 - B12 * B12 v0 = t1 / t2 lmb = B33 - (B13 * B13 + v0 * t1) / B11 alp = np.sqrt(lmb / B11) bet = np.sqrt(lmb * B11 / t2) gam = -B12 * alp * alp * bet / lmb u0 = gam * v0 / bet - B13 * alp * alp / lmb A = np.array([[alp, gam, u0], [0, bet, v0], [0, 0, 1]]) ########## Code ends here ########## return A def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C ''' Inputs: H: a single homography matrix A: the camera intrinsic matrix Outputs: R: the rotation matrix t: the translation vector ''' ########## Code starts here ########## # Just follow equation (31) of lecture 9 notes A_inv = np.linalg.inv(A) c1, c2, c3 = H[:, 0], H[:, 1], H[:, 2] # The cs are columns of H q = 1 / np.linalg.norm(np.dot(A_inv, c1)) # common recurring term r1 = q * np.dot(A_inv, c1) r2 = q * np.dot(A_inv, c2) r3 = np.cross(r1, r2) t = q * np.dot(A_inv, c3) R = np.column_stack((r1, r2, r3)) # The rs are columns of R. U, s, VT = np.linalg.svd(R, full_matrices=False) R_sol = np.dot(U, VT) R = R_sol ########## Code ends here ########## return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: x, y: the coordinates in the ideal normalized image plane ''' ########## Code starts here ########## d = X.shape[0] P_Wh = np.array([X, Y, Z, np.ones(d) ]) # shape (4, 63) # Homogeneous world coords Rt = np.column_stack((R, t)) # shape (3, 4) P_Ch = np.dot(Rt, P_Wh) # shape (3, 63) # Homogeneous camera coords # Transform back to nonhomogeneous coords P_Ch = P_Ch / P_Ch[2] x, y, _ = P_Ch ########## Code ends here ########## return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. A: the camera intrinsic parameters R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: u, v: the coordinates in the ideal pixel image plane ''' ########## Code starts here ########## d = X.shape[0] P_Wh = np.array([X, Y, Z, np.ones(d) ]) # shape (4, 63) # Homogeneous world coords Rt = np.column_stack((R, t)) # shape (3, 4) P_Ch = np.matmul(Rt, P_Wh) # shape (3, 63) # Homogeneous camera coords ph = np.matmul(A, P_Ch) # shape (3, 63) # Homogeneous pixel coords # Transform to nonhomogeneous coords ph = ph / ph[2] u, v, _ = ph ########## Code ends here ########## return u, v def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([ box.x0, box.y0 + box.height * 0.15, box.width, box.height * 0.85 ]) ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p + 1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [ 0, self.n_corners_x - 1, self.n_corners_x * self.n_corners_y - 1, self.n_corners_x * (self.n_corners_y - 1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([ zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]]) ]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot( np.array( [X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p + 1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2 * view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title( 'Estimated Board Locations (Chessboard {0})'.format(i + 1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape( (1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) # v_meas.append(chessboards[0][:, 0][:, 1]) # This keeps Y's original direction instead. v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1] ) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCheckerNode: def __init__(self, chess_size, dim): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" tosync_stereo = [ (left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo) ] tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board]) def queue_monocular(self, msg, cmsg): self.q_mono.put((msg, cmsg)) def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg): self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg)) def mkgray(self, msg): return self.mc.mkgray(msg) def image_corners(self, im): (ok, corners, b) = self.mc.get_corners(im) if ok: return corners else: return None def handle_monocular(self, msg): (image, camera) = msg gray = self.mkgray(image) C = self.image_corners(gray) if C: linearity_rms = self.mc.linear_error(C, self.board) # Add in reprojection check image_points = C object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0] dist_coeffs = numpy.zeros((4, 1)) camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ], [ camera.P[4], camera.P[5], camera.P[6] ], [ camera.P[8], camera.P[9], camera.P[10] ] ] ) ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs) # Convert rotation into a 3x3 Rotation Matrix rot3x3, _ = cv2.Rodrigues(rot) # Reproject model points into image object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans)) reprojected_h = camera_matrix * object_points_world reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :]) reprojection_errors = image_points.squeeze().T - reprojected.T reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape)) # Print the results print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms)) else: print('no chessboard') def handle_stereo(self, msg): (lmsg, lcmsg, rmsg, rcmsg) = msg lgray = self.mkgray(lmsg) rgray = self.mkgray(rmsg) L = self.image_corners(lgray) R = self.image_corners(rgray) if L and R: epipolar = self.sc.epipolar_error(L, R) dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg)) print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension)) else: print("no chessboard")
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append( ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg( img_msg ) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len( self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0] * n_corners[1] def genCornerCoordinates(self, u_meas, v_meas): ''' Inputs: u_meas: a list of arrays where each array are the u values for each board. v_meas: a list of arrays where each array are the v values for each board. Output: corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board. HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end HINT: our solution does not use the u_meas and v_meas values HINT: it does not matter where your frame it, as long as you are consistent! ''' ########## Code starts here ########## Xg = [] Yg = [] for i in range(len(u_meas)): Xg_single_board = np.zeros(self.n_corners_x * self.n_corners_y) Yg_single_board = np.zeros(self.n_corners_x * self.n_corners_y) X_origin = 0 Y_origin = 6 * self.d_square for rows in range(self.n_corners_y): for cols in range(self.n_corners_x): Xg_single_board[rows * self.n_corners_x + cols] = X_origin + cols * self.d_square Yg_single_board[rows * self.n_corners_x + cols] = Y_origin Y_origin -= self.d_square Xg.append(Xg_single_board) Yg.append(Yg_single_board) corner_coordinates = (Xg, Yg) ########## Code ends here ########## return corner_coordinates def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A ''' Inputs: u_meas: an array of the u values for a board. v_meas: an array of the v values for a board. X: an array of the X values for a board. (from genCornerCoordinates) Y: an array of the Y values for a board. (from genCornerCoordinates) Output: H: the homography matrix. its size is 3x3 HINT: What is the size of the matrix L? HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigenvalue live? HINT: np.stack and/or np.hstack may come in handy here. ''' ########## Code starts here ########## for n in range(self.n_corners_per_chessboard): M_h_T = np.array([[X[n], Y[n], 1]]) Line_1 = np.hstack((M_h_T, np.zeros( (1, 3)), (-1) * u_meas[n] * M_h_T)) Line_2 = np.hstack((np.zeros( (1, 3)), M_h_T, (-1) * v_meas[n] * M_h_T)) if n == 0: L = Line_1 L = np.vstack((L, Line_2)) else: L = np.vstack((L, Line_1, Line_2)) u, s, vh = np.linalg.svd(L, full_matrices=True) H = vh[-1].reshape(3, 3) ########## Code ends here ########## return H def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B ''' Input: H: a list of homography matrices for each board Output: A: the camera intrinsic matrix HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT HINT: What is the definition of h_ij? HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function) HINT: What is the size of V? ''' ########## Code starts here ########## for h in H: h = np.transpose(h) v12_T = np.array([[ h[0, 0] * h[1, 0], h[0, 0] * h[1, 1] + h[0, 1] * h[1, 0], h[0, 1] * h[1, 1], h[0, 2] * h[1, 0] + h[0, 0] * h[1, 2], h[0, 2] * h[1, 1] + h[0, 1] * h[1, 2], h[0, 2] * h[1, 2] ]]) v11_T = np.array([[ h[0, 0] * h[0, 0], h[0, 0] * h[0, 1] + h[0, 1] * h[0, 0], h[0, 1] * h[0, 1], h[0, 2] * h[0, 0] + h[0, 0] * h[0, 2], h[0, 2] * h[0, 1] + h[0, 1] * h[0, 2], h[0, 2] * h[0, 2] ]]) v22_T = np.array([[ h[1, 0] * h[1, 0], h[1, 0] * h[1, 1] + h[1, 1] * h[1, 0], h[1, 1] * h[1, 1], h[1, 2] * h[1, 0] + h[1, 0] * h[1, 2], h[1, 2] * h[1, 1] + h[1, 1] * h[1, 2], h[1, 2] * h[1, 2] ]]) if (h == np.transpose(H[0])).all(): V = v12_T V = np.vstack((V, v11_T - v22_T)) else: V = np.vstack((V, v12_T, v11_T - v22_T)) u, s, vh = np.linalg.svd(V, full_matrices=True) B = vh[-1] A = np.zeros((3, 3)) v0 = (B[1] * B[3] - B[0] * B[4]) / (B[0] * B[2] - B[1] * B[1]) lamda = B[5] - (B[3]**2 + v0 * (B[1] * B[3] - B[0] * B[4])) / B[0] alpha = (lamda / B[0])**0.5 beta = (lamda * B[0] / (B[0] * B[2] - B[1]**2))**0.5 gamma = (-1) * B[1] * alpha**2 * beta / lamda u0 = gamma * v0 / beta - B[3] * alpha**2 / lamda A[0, 0], A[0, 1], A[0, 2], A[1, 1], A[1, 2], A[2, 2] = alpha, gamma, u0, beta, v0, 1 ########## Code ends here ########## return A def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C ''' Inputs: H: a single homography matrix A: the camera intrinsic matrix Outputs: R: the rotation matrix t: the translation vector ''' ########## Code starts here ########## A_inv = np.linalg.inv(A) lamda = 1 / np.linalg.norm(A_inv.dot(H[:, 0])) r1_r2_t = A_inv.dot(H) t = lamda * r1_r2_t[:, -1] r1_est = lamda * r1_r2_t[:, 0] r2_est = lamda * r1_r2_t[:, 1] r3_est = np.cross(r1_est, r2_est) R_est = np.c_[r1_est, r2_est, r3_est] u, s, vh = np.linalg.svd(R_est, full_matrices=True) R = np.matmul(u, vh) ########## Code ends here ########## return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: x, y: the coordinates in the ideal normalized image plane ''' ########## Code starts here ########## x = np.zeros(63) y = np.zeros(63) Transform_Matrix = np.vstack((np.c_[R, t], np.array([0, 0, 0, 1]))) for i in range(len(x)): Pw_h = np.array([X[i], Y[i], Z[i], 1]) Pc_h = Transform_Matrix.dot(Pw_h) x[i] = Pc_h[0] / Pc_h[2] y[i] = Pc_h[1] / Pc_h[2] ########## Code ends here ########## return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. A: the camera intrinsic parameters R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: u, v: the coordinates in the ideal pixel image plane ''' ########## Code starts here ########## u = np.zeros(63) v = np.zeros(63) T_M_WtoPix = A.dot(np.c_[R, t]) for i in range(len(u)): Pw_h = np.array([X[i], Y[i], Z[i], 1]) Pp_h = T_M_WtoPix.dot(Pw_h) u[i] = Pp_h[0] / Pp_h[2] v[i] = Pp_h[1] / Pp_h[2] ########## Code ends here ########## return u, v def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([ box.x0, box.y0 + box.height * 0.15, box.width, box.height * 0.85 ]) ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p + 1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [ 0, self.n_corners_x - 1, self.n_corners_x * self.n_corners_y - 1, self.n_corners_x * (self.n_corners_y - 1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([ zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]]) ]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot( np.array( [X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p + 1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2 * view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title( 'Estimated Board Locations (Chessboard {0})'.format(i + 1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape( (1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1] ) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard)
import cv2 from camera_calibration.calibrator import MonoCalibrator, ChessboardInfo numImages = 30 images = [ cv2.imread('../Images/frame{:04d}.jpg'.format(i)) for i in range(numImages) ] board = ChessboardInfo() board.n_cols = 7 board.n_rows = 5 board.dim = 0.050 mc = MonoCalibrator([board], cv2.CALIB_FIX_K3) mc.cal(images) print(mc.as_message()) mc.do_save()
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append( ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg( img_msg ) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len( self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0] * n_corners[1] def genCornerCoordinates(self, u_meas, v_meas): ''' Inputs: u_meas: a list of arrays where each array are the u values for each board. v_meas: a list of arrays where each array are the v values for each board. Output: corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board. HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end HINT: our solution does not use the u_meas and v_meas values HINT: it does not matter where your frame it, as long as you are consistent! ''' ########## Code starts here ########## x_coor, y_coor = [], [] # make the range of real world squares in the x and y directions for i in range(self.n_corners_x): x_coor.append(i * self.d_square) for j in range(self.n_corners_y): y_coor.append(j * self.d_square) corner_coordinates = ([], []) # for each chessboard add each x (n_corners_x*n_corners_y,) and y to the lists for _ in range(self.n_chessboards): corner_coordinates[0].append(np.array(x_coor * self.n_corners_y)) corner_coordinates[1].append((np.array(y_coor) * np.ones( (self.n_corners_x, self.n_corners_y))).T.reshape( (self.n_corners_x * self.n_corners_y, ))) ########## Code ends here ########## return corner_coordinates def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A ''' Inputs: u_meas: an array of the u values for a board. v_meas: an array of the v values for a board. X: an array of the X values for a board. (from genCornerCoordinates) Y: an array of the Y values for a board. (from genCornerCoordinates) Output: H: the homography matrix. its size is 3x3 HINT: What is the size of the matrix L? HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live? HINT: np.stack and/or np.hstack may come in handy here. ''' ########## Code starts here ########## # reshapes to be able to multiply X = X.reshape(X.shape[0], 1) Y = Y.reshape(Y.shape[0], 1) u_meas = u_meas.reshape(u_meas.shape[0], 1) v_meas = v_meas.reshape(v_meas.shape[0], 1) # stacking up the values to be able to get L M_tilde = np.hstack((X, Y, np.ones(X.shape))) a = np.hstack((M_tilde, np.zeros(M_tilde.shape), -u_meas * M_tilde)) b = np.hstack((np.zeros(M_tilde.shape), M_tilde, -v_meas * M_tilde)) L = np.vstack((a, b)) # calculate the svd and reshape the singular values into the H matrix svd = np.linalg.svd(L) x = svd[2][-1] H = np.array([[x[0], x[1], x[2]], [x[3], x[4], x[5]], [x[6], x[7], x[8]]]) ########## Code ends here ########## return H def getvij(self, i, j, H): a = H[0][i] * H[0][j] b = H[0][i] * H[1][j] + H[1][i] * H[0][j] c = H[1][i] * H[1][j] d = H[2][i] * H[0][j] + H[0][i] * H[2][j] e = H[2][i] * H[1][j] + H[1][i] * H[2][j] f = H[2][i] * H[2][j] v_ij = np.array([a, b, c, d, e, f]).reshape((6, 1)) return v_ij def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B ''' Input: H: a list of homography matrices for each board Output: A: the camera intrinsic matrix HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT HINT: What is the definition of h_ij? HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function) HINT: What is the size of V? ''' ########## Code starts here ########## V_12 = self.getvij(0, 1, H[0]) V_11 = self.getvij(0, 0, H[0]) V_22 = self.getvij(1, 1, H[0]) V = np.vstack((V_12.T, (V_11 - V_22).T)) for x in range(1, len(H)): V_12 = self.getvij(0, 1, H[x]) V_11 = self.getvij(0, 0, H[x]) V_22 = self.getvij(1, 1, H[x]) V_cur = np.vstack((V_12.T, (V_11 - V_22).T)) V = np.vstack((V, V_cur)) u, s, vh = np.linalg.svd(V) b = vh[-1] A = np.zeros((3, 3)) # b = {B11, B12, B22, B13, B23, B33} v0 = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1]**2) lamb = b[5] - ((b[3]**2) + v0 * (b[1] * b[3] - b[0] * b[4])) / b[0] alpha = np.sqrt(lamb / b[0]) beta = np.sqrt((lamb * b[0]) / (b[0] * b[2] - b[1]**2)) gamma = -b[1] * (alpha**2) * beta / lamb u0 = gamma * v0 / beta - b[3] * (alpha**2) / lamb A[0, 0] = alpha A[0, 1] = gamma A[0, 2] = u0 A[1, 1] = beta A[1, 2] = v0 A[2, 2] = 1 ########## Code ends here ########## return A def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C ''' Inputs: H: a single homography matrix A: the camera intrinsic matrix Outputs: R: the rotation matrix t: the translation vector ''' ########## Code starts here ########## h1 = H[:, 0].reshape((3, 1)) h2 = H[:, 1].reshape((3, 1)) h3 = H[:, 2].reshape((3, 1)) A_inv = np.linalg.inv(A) lamb = 1 / np.linalg.norm(np.dot(A_inv, h1)) r1 = lamb * np.dot(A_inv, h1) r2 = lamb * np.dot(A_inv, h2) r3 = np.cross(r1.T, r2.T).T R = np.hstack((r1, r2, r3)) t = lamb * np.dot(A_inv, h3).reshape((3, )) svd = np.linalg.svd(R) R_new = np.dot(svd[0], svd[2]) ########## Code ends here ########## return R_new, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: x, y: the coordinates in the ideal normalized image plane ''' ########## Code starts here ########## t = t.reshape((3, 1)) X = X.reshape(len(X), 1) Y = Y.reshape(len(Y), 1) Z = Z.reshape(len(Z), 1) M_tilde = np.hstack((X, Y, Z, np.ones(X.shape))).T transform = np.hstack((R, t)) rotated = np.dot(transform, M_tilde) return rotated[0] / rotated[2], rotated[1] / rotated[2] def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. A: the camera intrinsic parameters R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: u, v: the coordinates in the ideal pixel image plane ''' ########## Code starts here ########## t = t.reshape((3, 1)) X = X.reshape(len(X), 1) Y = Y.reshape(len(Y), 1) Z = Z.reshape(len(Z), 1) M_tilde = np.hstack((X, Y, Z, np.ones(X.shape))).T transform = np.hstack((R, t)) m = np.dot(A, np.dot(transform, M_tilde)) u = m[0] / m[2] v = m[1] / m[2] ########## Code ends here ########## return u, v def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([ box.x0, box.y0 + box.height * 0.15, box.width, box.height * 0.85 ]) ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p + 1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [ 0, self.n_corners_x - 1, self.n_corners_x * self.n_corners_y - 1, self.n_corners_x * (self.n_corners_y - 1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([ zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]]) ]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot( np.array( [X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p + 1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2 * view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title( 'Estimated Board Locations (Chessboard {0})'.format(i + 1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape( (1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1] ) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCalibrator: def __init__(self): self.calib_flags = 0 self.pattern = Patterns.Chessboard def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True): self.name = name self.cal_img_path = cal_img_path self.boards = [] self.boards.append( ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern) if display_flag: fig = plt.figure('Corner Extraction', figsize=(12, 5)) gs = gridspec.GridSpec(1, 2) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image img_msg = self.c.br.cv2_to_imgmsg( img, 'mono8') # Convert to ROS Image msg drawable = self.c.handle_msg( img_msg ) # Extract chessboard corners using ROS camera_calibration package if display_flag and i < n_disp_img: ax = plt.subplot(gs[0, 0]) plt.imshow(img, cmap='gray') plt.axis('off') ax = plt.subplot(gs[0, 1]) plt.imshow(drawable.scrib) plt.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Corner Extraction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() # Useful parameters self.d_square = square_length # Length of a chessboard square self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions self.n_chessboards = len( self.c.good_corners) # Number of examined images self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid self.n_corners_per_chessboard = n_corners[0] * n_corners[1] def genCornerCoordinates(self, u_meas, v_meas): ''' Inputs: u_meas: a list of arrays where each array are the u values for each board. v_meas: a list of arrays where each array are the v values for each board. Output: corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board. HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end HINT: our solution does not use the u_meas and v_meas values HINT: it does not matter where your frame it, as long as you are consistent! ''' # 3D points from world corner X = [] Y = [] # Defining the world 2D coordinates wcoordinates = np.zeros((self.n_corners_y * self.n_corners_x, 3), np.float32) wcoordinates[:, :2] = (np.mgrid[0:self.n_corners_x, 0:self.n_corners_y].T.reshape( -1, 2)) * self.d_square for ii in range(len(os.listdir(self.cal_img_path))): X.append(wcoordinates[:, 0]) Y.append(wcoordinates[:, 1]) corner_coordinates = (X, Y) return corner_coordinates def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A ''' Inputs: u_meas: an array of the u values for a board. v_meas: an array of the v values for a board. X: an array of the X values for a board. (from genCornerCoordinates) Y: an array of the Y values for a board. (from genCornerCoordinates) Output: H: the homography matrix. its size is 3x3 HINT: What is the size of the matrix L? HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live? HINT: np.stack and/or np.hstack may come in handy here. ''' ########## Code starts here ########## n_points = self.n_corners_x * self.n_corners_y z_T = np.array([0, 0, 0]).reshape(1, 3) L = np.zeros((0, 9)) for i in range(n_points): M_tilde_T = np.stack([X[i], Y[i], 1]).T.reshape(1, 3) row1 = np.hstack([M_tilde_T, z_T, -u_meas[i] * M_tilde_T]) row2 = np.hstack([z_T, M_tilde_T, -v_meas[i] * M_tilde_T]) L_i = np.vstack((row1, row2)) L = np.vstack((L, L_i)) U, S, vh = np.linalg.svd(L) H = vh.T[:, 8].reshape(3, 3) return H def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B ''' Input: H: a list of homography matrices for each board Output: A: the camera intrinsic matrix HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT HINT: What is the definition of h_ij? HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function) HINT: What is the size of V? ''' def v(i, j, H): #note all indices are subtracted by 1 because matrices index at 1 i = i - 1 j = j - 1 v1 = h(i, H)[0] * h(j, H)[0] v2 = h(i, H)[0] * h(j, H)[1] + h(i, H)[1] * h(j, H)[0] v3 = h(i, H)[1] * h(j, H)[1] v4 = h(i, H)[2] * h(j, H)[0] + h(i, H)[0] * h(j, H)[2] v5 = h(i, H)[2] * h(j, H)[1] + h(i, H)[1] * h(j, H)[2] v6 = h(i, H)[2] * h(j, H)[2] v = np.array([v1, v2, v3, v4, v5, v6]).reshape(6, 1) return v def h(i, H): return H[:, i] V = np.zeros((0, 6)) for i in range(len(H)): V_i = np.vstack((v(1, 2, H[i]).T, (v(1, 1, H[i]) - v(2, 2, H[i])).T)) V = np.vstack((V, V_i)) U, S, vh = np.linalg.svd(V) print('V shape:', V.shape) print('vh shape:', vh.shape) b = vh.T[:, 5] B11 = b[0] B12 = b[1] B22 = b[2] B13 = b[3] B23 = b[4] B33 = b[5] v0 = (B12 * B13 - B11 * B23) / (B11 * B22 - B12**2) lam = B33 - (B13**2 + v0 * (B12 * B13 - B11 * B23)) / B11 alpha = np.sqrt(lam / B11) beta = np.sqrt(lam * B11 / (B11 * B22 - B12**2)) gamma = -B12 * alpha**2 * beta / lam u0 = gamma * v0 / beta - B13 * alpha**2 / lam A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]]) return A def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C ''' Inputs: H: a single homography matrix A: the camera intrinsic matrix Outputs: R: the rotation matrix t: the translation vector ''' ########## Code starts here ########## A_inv = np.linalg.inv(A) lam = 1 / np.linalg.norm(np.matmul(A_inv, H[:, 0])) r1 = lam * np.matmul(A_inv, H[:, 0]).reshape(3, 1) r2 = lam * np.matmul(A_inv, H[:, 1]).reshape(3, 1) r3 = np.cross(r1, r2, axis=0) t = lam * np.matmul(A_inv, H[:, 2]) Q = np.hstack((np.hstack((r1, r2)), r3)) U, S, vh = np.linalg.svd(Q) R = np.matmul(U, vh) ########## Code ends here ########## return R, t def transformWorld2NormImageUndist(self, X, Y, Z, R, t): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: x, y: the coordinates in the ideal normalized image plane ''' ########## Code starts here ########## M_tilde = np.vstack((np.vstack((np.vstack( (X, Y)), Z)), np.ones(len(X)))) Rt = np.hstack((R, t.reshape(3, 1))) m_tilde = np.matmul(Rt, M_tilde) x = m_tilde[0, :] / m_tilde[2, :] y = m_tilde[1, :] / m_tilde[2, :] ########## Code ends here ########## return x, y def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A): # Zhang 2.1, Eq. (1) ''' Inputs: X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros. A: the camera intrinsic parameters R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board. Outputs: u, v: the coordinates in the ideal pixel image plane ''' ########## Code starts here ########## M_tilde = np.vstack((np.vstack((np.vstack( (X, Y)), Z)), np.ones(len(X)))) Rt = np.hstack((R, t.reshape(3, 1))) m_tilde = np.matmul(A, np.matmul(Rt, M_tilde)) u = m_tilde[0, :] / m_tilde[2, :] v = m_tilde[1, :] / m_tilde[2, :] ########## Code ends here ########## return u, v def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)): # Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6)) plt.clf() for p in range(min(self.n_chessboards, n_disp_img)): plt.clf() ax = plt.subplot(111) ax.plot(u_meas[p], v_meas[p], 'r+', label='Original') u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A) ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration') box = ax.get_position() ax.set_position([ box.x0, box.y0 + box.height * 0.15, box.width, box.height * 0.85 ]) ax.axis([0, self.w_pixels, 0, self.h_pixels]) plt.gca().set_aspect('equal', adjustable='box') plt.title('Chessboard {0}'.format(p + 1)) ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True) plt.show(block=False) plt.waitforbuttonpress() def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5): # Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas ind_corners = [ 0, self.n_corners_x - 1, self.n_corners_x * self.n_corners_y - 1, self.n_corners_x * (self.n_corners_y - 1), ] s_cam = 0.02 d_cam = 0.05 xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam], [0, -s_cam, -s_cam, s_cam, s_cam], [0, -d_cam, -d_cam, -d_cam, -d_cam]] ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]] verts_cam = [] for i in range(len(ind_cam)): verts_cam.append([ zip([xyz_cam[0][j] for j in ind_cam[i]], [xyz_cam[1][j] for j in ind_cam[i]], [xyz_cam[2][j] for j in ind_cam[i]]) ]) fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5)) axim = fig.add_subplot(121) ax3d = fig.add_subplot(122, projection='3d') boards = [] verts = [] for p in range(self.n_chessboards): M = [] W = np.column_stack((R[p], t[p])) for i in range(4): M_tld = W.dot( np.array( [X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1])) if np.sign(M_tld[2]) == 1: Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) M_tld = Rz.dot(M_tld) M_tld[2] *= -1 M.append(M_tld[0:3]) M = (np.array(M).T).tolist() verts.append([zip(M[0], M[1], M[2])]) boards.append(Poly3DCollection(verts[p])) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img = cv2.imread(self.cal_img_path + '/' + file, 0) axim.imshow(img, cmap='gray') axim.axis('off') ax3d.clear() for j in range(len(ind_cam)): cam = Poly3DCollection(verts_cam[j]) cam.set_alpha(0.2) cam.set_color('green') ax3d.add_collection3d(cam) for p in range(self.n_chessboards): if p == i: boards[p].set_alpha(1.0) boards[p].set_color('blue') else: boards[p].set_alpha(0.1) boards[p].set_color('red') ax3d.add_collection3d(boards[p]) ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p + 1)) plt.show(block=False) view_max = 0.2 ax3d.set_xlim(-view_max, view_max) ax3d.set_ylim(-view_max, view_max) ax3d.set_zlim(-2 * view_max, 0) ax3d.set_xlabel('X axis') ax3d.set_ylabel('Y axis') ax3d.set_zlabel('Z axis') if i == 0: ax3d.view_init(azim=90, elev=120) plt.tight_layout() fig.canvas.set_window_title( 'Estimated Board Locations (Chessboard {0})'.format(i + 1)) plt.show(block=False) raw_input('<Hit Enter To Continue>') def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0): Anew_no_k, roi = cv2.getOptimalNewCameraMatrix( A, np.zeros(4), (self.w_pixels, self.h_pixels), scale) mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap( A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack( [k, 0, 0]), (self.w_pixels, self.h_pixels), scale) mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap( A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2) if k[0] != 0: n_plots = 3 else: n_plots = 2 fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5)) gs = gridspec.GridSpec(1, n_plots) gs.update(wspace=0.025, hspace=0.05) for i, file in enumerate(sorted(os.listdir(self.cal_img_path))): if i < n_disp_img: img_dist = cv2.imread(self.cal_img_path + '/' + file, 0) img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k) img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k) ax = plt.subplot(gs[0, 0]) ax.imshow(img_dist, cmap='gray') ax.axis('off') ax = plt.subplot(gs[0, 1]) ax.imshow(img_undist_no_k, cmap='gray') ax.axis('off') if k[0] != 0: ax = plt.subplot(gs[0, 2]) ax.imshow(img_undist_w_k, cmap='gray') ax.axis('off') plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02) fig.canvas.set_window_title( 'Image Correction (Chessboard {0})'.format(i + 1)) plt.show(block=False) plt.waitforbuttonpress() def writeCalibrationYaml(self, A, k): self.c.intrinsics = np.array(A) self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape( (1, 5)) #self.c.distortion = np.zeros(5) self.c.name = self.name self.c.R = np.eye(3) self.c.P = np.column_stack((np.eye(3), np.zeros(3))) self.c.size = [self.w_pixels, self.h_pixels] filename = self.name + '_calibration.yaml' with open(filename, 'w') as f: f.write(self.c.yaml()) print('Calibration exported successfully to ' + filename) def getMeasuredPixImageCoord(self): u_meas = [] v_meas = [] for chessboards in self.c.good_corners: u_meas.append(chessboards[0][:, 0][:, 0]) v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1] ) # Flip Y-axis to traditional direction return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCheckerNode: def __init__(self, chess_size, dim): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim image_topic = rospy.resolve_name("monocular") + "/image_rect" camera_topic = rospy.resolve_name("monocular") + "/camera_info" tosync_mono = [(image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo)] tsm = message_filters.TimeSynchronizer( [message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10 ) tsm.registerCallback(self.queue_monocular) left_topic = rospy.resolve_name("stereo") + "/left/image_rect" left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" right_topic = rospy.resolve_name("stereo") + "/right/image_rect" right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" tosync_stereo = [ (left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo), ] tss = message_filters.TimeSynchronizer( [message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10 ) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue.Queue() self.q_stereo = Queue.Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) def queue_monocular(self, msg, cmsg): self.q_mono.put((msg, cmsg)) def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg): self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg)) def mkgray(self, msg): return self.br.imgmsg_to_cv(msg, "bgr8") def image_corners(self, im): (ok, corners, b) = self.mc.get_corners(im) if ok: return list(cvmat_iterator(cv.Reshape(self.mc.mk_image_points([(corners, b)]), 2))) else: return None def handle_monocular(self, msg): def pt2line(x0, y0, x1, y1, x2, y2): """ point is (x0, y0), line is (x1, y1, x2, y2) """ return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) (image, camera) = msg rgb = self.mkgray(image) C = self.image_corners(rgb) if C: cc = self.board.n_cols cr = self.board.n_rows errors = [] for r in range(cr): (x1, y1) = C[(cc * r) + 0] (x2, y2) = C[(cc * r) + cc - 1] for i in range(1, cc - 1): (x0, y0) = C[(cc * r) + i] errors.append(pt2line(x0, y0, x1, y1, x2, y2)) linearity_rms = math.sqrt(sum([e ** 2 for e in errors]) / len(errors)) # Add in reprojection check A = cv.CreateMat(3, 3, 0) image_points = cv.fromarray(numpy.array(C)) object_points = cv.fromarray(numpy.zeros([cc * cr, 3])) for i in range(cr): for j in range(cc): object_points[i * cc + j, 0] = j * self.board.dim object_points[i * cc + j, 1] = i * self.board.dim object_points[i * cc + j, 2] = 0.0 dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]])) camera_matrix = numpy.array( [ [camera.P[0], camera.P[1], camera.P[2]], [camera.P[4], camera.P[5], camera.P[6]], [camera.P[8], camera.P[9], camera.P[10]], ] ) rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2( object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans ) # Convert rotation into a 3x3 Rotation Matrix rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot3x3) # Reproject model points into image object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points).T) + numpy.asmatrix(trans) reprojected_h = camera_matrix * object_points_world reprojected = reprojected_h[0:2, :] / reprojected_h[2, :] reprojection_errors = image_points - reprojected.T reprojection_rms = numpy.sqrt( numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape) ) # Print the results print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % ( linearity_rms, reprojection_rms, ) else: print "no chessboard" def handle_stereo(self, msg): (lmsg, lcmsg, rmsg, rcmsg) = msg lrgb = self.mkgray(lmsg) rrgb = self.mkgray(rmsg) sc = StereoCalibrator([self.board]) L = self.image_corners(lrgb) R = self.image_corners(rrgb) scm = image_geometry.StereoCameraModel() scm.fromCameraInfo(lcmsg, rcmsg) if L and R: d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)] epipolar = math.sqrt(sum([i ** 2 for i in d]) / len(d)) disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)] pt3d = [scm.projectPixelTo3d((x, y), d) for ((x, y), d) in zip(L, disparities)] def l2(p0, p1): return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)])) # Compute the length from each horizontal and vertical lines, and return the mean cc = self.board.n_cols cr = self.board.n_rows lengths = [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + [ l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc) ] dimension = sum(lengths) / len(lengths) print "epipolar error: %f pixels dimension: %f m" % (epipolar, dimension) else: print "no chessboard"