From efaad29b7893da9db528167fda7f8282dfb9aa69 Mon Sep 17 00:00:00 2001 From: Ting-Jun Wang Date: Sat, 16 Dec 2023 19:25:58 +0800 Subject: [PATCH] feat: camera calibration --- camera_calibration.py | 216 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 216 insertions(+) create mode 100644 camera_calibration.py diff --git a/camera_calibration.py b/camera_calibration.py new file mode 100644 index 0000000..2daba32 --- /dev/null +++ b/camera_calibration.py @@ -0,0 +1,216 @@ +import numpy as np +import cv2 as cv +import glob, sys, argparse + +class Calibrator: + def __init__(self, args): + self.args = args + self.inner_w = args.w + self.inner_h = args.h + + # termination criteria + self.criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) + self.objp = np.zeros((self.inner_w * self.inner_h, 3), np.float32) + self.objp[:,:2] = np.mgrid[0:self.inner_h, 0:self.inner_w].T.reshape(-1,2) + # Arrays to store object points and image points from all the images. + self.objpoints = [] # 3d point in real world space + self.imgpoints = [] # 2d points in image plane. + self.imgs = [] + + def run(self): + self.video_reader = cv.VideoCapture(args.input) + self.load_images() + assert len(self.imgs) >= 4, print('=> Error: need a least 4 images to calibrate') + + ret, self.K, self.dist, self.rvec, self.tvecs = self.calibrate() + print('=> Overall RMS re-projection error: {}'.format(ret)) + + self.save_result() + + if args.show: + self.show_result(self.imgs, self.imgpoints, self.K, self.dist, self.rvec, self.tvecs) + + + def load_images(self): + + #for fname in images: + print('=> press to add image') + print('=> press to exit adding and start calibration') + while True: + #img = cv.imread(fname) + ret, img = self.video_reader.read() + if not ret: break + #img = img[:, ::-1] + cv.imshow('calibration video', img) + key = cv.waitKey(10) #& 0xFF + if key == ord(' '): + self.imgs.append(img) + print('save images: {}'.format(len(self.imgs))) + if key == ord('q'): + break + cv.destroyAllWindows() + + def calibrate(self): + + print('=> start corner detection and calibration') + + for index, img in enumerate(self.imgs): + # Find the chess board corners + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + ret, corners = cv.findChessboardCorners(gray, (self.inner_h, self.inner_w), None) + # If found, add object points, image points (after refining them) + if ret == True: + self.objpoints.append(self.objp) + corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), self.criteria) + self.imgpoints.append(corners2) + # Draw and display the corners + img_ = cv.UMat(img) + cv.drawChessboardCorners(img_, (self.inner_h, self.inner_w), corners2, ret) + cv.imshow('img', img_) + cv.waitKey(45) + + cv.destroyAllWindows() + + assert len(self.objpoints) > 0, print('=> Error: no corner detected') + assert len(self.objpoints) >= 4, print('=> Error: need a least 4 images to calibrate') + print('=> corners found in {} images'.format(len(self.objpoints))) + + #ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) + + return cv.calibrateCamera(self.objpoints, self.imgpoints, gray.shape[::-1], None, None) + + def save_result(self): + print('Camera Intrinsic') + print(self.K) + print('Distortion Coefficients') + print(self.dist) + np.save(self.args.output, {'K': self.K, 'dist': self.dist}) + + def show_result(self, imgs, imgpoints, mtx, dist, rvecs, tvecs): + try: + import open3d as o3d + except Exception as e: + print(e) + return + coord = np.eye(4) + coord[1, 1] = -1 + coord[2, 2] = -1 + vis = o3d.visualization.Visualizer() + vis.create_window() + for i in range(self.inner_h+1): + for j in range(self.inner_w+1): + color = [0, 0, 0] if (i + j) % 2 == 0 else [0.9, 0.9, 0.9] + mesh = o3d.geometry.TriangleMesh.create_box(width=1.0, height=1.0, depth=0.2) + mesh.translate((i-1, - j, -0.2)) + mesh.paint_uniform_color(color) + vis.add_geometry(mesh) + + def expand_batch(_m, _batch_size): + b_m = np.repeat(np.expand_dims(_m, 0), _batch_size, axis=0) + return b_m + + def create_camera(_img, _r_mat, _t_vec, _K): + _h, _w = _img.shape[:2] + verts = np.zeros((5, 3)).astype(np.float32) + verts[1:3, 0] = _w + verts[2:4, 1] = _h + verts[:, 2] = 1. + verts[4, 0] = _w / 2 + verts[4, 1] = _h / 2 + verts = (expand_batch(np.linalg.inv(_K), 5) @ np.expand_dims(verts, -1)) + verts[4, -1] = 0. + #verts = expand_batch(coord[:3, :3], 5) @ verts + verts = (expand_batch(_r_mat, 5) @ verts) + _t_vec + lines = [[0, 1], [1, 2], [2, 3], [3, 0], [0, 4], [1, 4], [2, 4], [3, 4]] + colors = [[1, 0, 0]] * len(lines) + + cam = o3d.geometry.LineSet() + cam.points = o3d.utility.Vector3dVector(verts.squeeze(-1)) + cam.lines = o3d.utility.Vector2iVector(lines) + cam.colors = o3d.utility.Vector3dVector(colors) + + extrinsic = np.concatenate([_r_mat, _t_vec], axis=-1) + extrinsic = np.concatenate([extrinsic, np.zeros([1, 4], np.float32)], axis=0) + extrinsic[-1, -1] = 1. + + img_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( + o3d.geometry.Image(_img[..., ::-1].astype(np.uint8)), + o3d.geometry.Image(np.ones((_h, _w), np.float32)), + 1.0, 2.0, False + ) + img_ = o3d.geometry.PointCloud.create_from_rgbd_image( + img_rgbd, + o3d.camera.PinholeCameraIntrinsic(_w, _h, _K[0, 0], _K[1, 1], _K[0, 2], _K[1, 2]) + ) + #img_.transform(coord) + img_.transform(extrinsic.astype(np.float64)) + return img_, cam + + num_points = self.inner_w * self.inner_h + batch_K_inv = expand_batch(np.linalg.inv(mtx), num_points) + + for i, (img, rvec, tvec, imgpoint) in enumerate(zip(imgs, rvecs, tvecs, imgpoints)): + r_mat = cv.Rodrigues(rvec)[0] + Rt = np.concatenate([r_mat, tvec], -1) + Rt = np.concatenate([Rt, np.zeros((1, 4))], 0) + Rt[-1, -1] = 1. + Rt = Rt @ coord + r_mat = Rt[:3, :3] + tvec = Rt[:3, -1] + tvec = np.expand_dims(tvec, -1) + + imgpoint_ = np.concatenate([imgpoint, np.ones([imgpoint.shape[0], 1, 1], np.float32)], -1) + + campoint = batch_K_inv @ imgpoint_.transpose(0, 2, 1) + worldpoint = expand_batch(r_mat.transpose(), num_points) @ campoint + _Rt = r_mat.transpose() @ tvec + + s = worldpoint[:, -1] / _Rt[-1] + s = np.expand_dims(np.repeat(s, 3, axis=1), -1) + corners = (worldpoint / s - _Rt).squeeze(-1) + + line = o3d.geometry.LineSet() + line.points = o3d.utility.Vector3dVector(corners) + line.lines = o3d.utility.Vector2iVector([[i, i+1] for i in range(num_points - 1)]) + LINE_COLORS = [[1, 0, 0], [1, 0.5, 0], [0.75, 0.75, 0], [0, 1, 0], [0, 0.75, 0.75], [0, 0, 1], [1, 0, 1]] + + line_colors = [] + for i in range(self.inner_w): + for j in range(self.inner_h): + k = i % len(LINE_COLORS) + line_colors.append(LINE_COLORS[k]) + + line.colors = o3d.utility.Vector3dVector(line_colors[:-1]) + #line.transform(coord) + vis.add_geometry(line) + + img_, cam = create_camera(img, r_mat, tvec, mtx) + vis.add_geometry(cam) + vis.add_geometry(img_) + + vis.run() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('input', + help='input video for calibration') + parser.add_argument('--output', + default='camera_parameters.npy', + help='npy file of camera parameters') + parser.add_argument('--w', + type=int, + default=6, + help='the width of inner corners of chessboard') + parser.add_argument('--h', + type=int, + default=9, + help='the height of inner corners of chessboard') + parser.add_argument('--show', + action='store_true', + help='to show the 3D visualization of calibration (require install open3D and only work on Linux)') + + args = parser.parse_args() + calibrator = Calibrator(args) + calibrator.run()