mirror of
https://github.com/opencv/opencv.git
synced 2025-08-06 14:36:36 +08:00
Merge pull request #22363 from ivashmak:multiview-calib
Add multiview calibration [GSOC 2022]
### Pull Request Readiness Checklist
- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
The usage tutorial is on Google Docs following this link: https://docs.google.com/document/d/1k6YpD0tpSVqnVnvU2nzE34K3cp_Po6mLWqXV06CUHwQ/edit?usp=sharing
This commit is contained in:
Maksym Ivashechkin
GitHub
parent
0c55ed0ca8
commit
67a3d35b4e
59
apps/python-calibration-generator/board.py
Normal file
59
apps/python-calibration-generator/board.py
Normal file
@ -0,0 +1,59 @@
|
||||
# This file is part of OpenCV project.
|
||||
# It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
# of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
import numpy as np
|
||||
|
||||
class Board:
|
||||
def __init__(self, w, h, square_len, euler_limit, t_limit, t_origin=None):
|
||||
assert w >= 0 and h >= 0 and square_len >= 0
|
||||
assert len(euler_limit) == len(t_limit) == 3
|
||||
self.w = w
|
||||
self.h = h
|
||||
self.square_len = square_len
|
||||
|
||||
self.t_limit = t_limit
|
||||
self.euler_limit = np.array(euler_limit, dtype=np.float32)
|
||||
colors = [[1,0,0], [0,1,0], [0,0,0], [0,0,1]]
|
||||
self.colors_board = np.zeros((w*h, 3))
|
||||
self.t_origin = np.array(t_origin, dtype=np.float32)[:,None] if t_origin is not None else None
|
||||
for i in range(h):
|
||||
for j in range(w):
|
||||
if j <= w // 2 and i <= h // 2: color = colors[0]
|
||||
elif j <= w // 2 and i > h // 2: color = colors[1]
|
||||
elif j > w // 2 and i <= h // 2: color = colors[2]
|
||||
else: color = colors[3]
|
||||
self.colors_board[i*w+j] = color
|
||||
for i in range(3):
|
||||
assert len(euler_limit[i]) == len(t_limit[i]) == 2
|
||||
self.euler_limit[i] *= (np.pi / 180)
|
||||
|
||||
def isProjectionValid(self, pts_proj):
|
||||
"""
|
||||
projection is valid, if x coordinate of left top corner point is smaller than x of bottom right point, ie do not allow 90 deg rotation of 2D board
|
||||
also, if x coordinate of left bottom corner is smaller than x coordinate on top right corner, ie do not allow flip
|
||||
pts_proj : 2 x N
|
||||
"""
|
||||
assert pts_proj.ndim == 2 and pts_proj.shape[0] == 2
|
||||
# pdb.set_trace()
|
||||
return pts_proj[0,0] < pts_proj[0,-1] and pts_proj[0,(self.h-1)*self.w] < pts_proj[0,self.w-1]
|
||||
|
||||
class CircleBoard(Board):
|
||||
def __init__(self, w, h, square_len, euler_limit, t_limit, t_origin=None):
|
||||
super().__init__(w, h, square_len, euler_limit, t_limit, t_origin)
|
||||
self.pattern = []
|
||||
for row in range(h):
|
||||
for col in range(w):
|
||||
if row % 2 == 1:
|
||||
self.pattern.append([(col+.5)*square_len, square_len*(row//2+.5), 0])
|
||||
else:
|
||||
self.pattern.append([col*square_len, (row//2)*square_len, 0])
|
||||
self.pattern = np.array(self.pattern, dtype=np.float32).T
|
||||
|
||||
class CheckerBoard(Board):
|
||||
def __init__(self, w, h, square_len, euler_limit, t_limit, t_origin=None):
|
||||
super().__init__(w, h, square_len, euler_limit, t_limit, t_origin)
|
||||
self.pattern = np.zeros((w * h, 3), np.float32)
|
||||
# https://stackoverflow.com/questions/37310210/camera-calibration-with-opencv-how-to-adjust-chessboard-square-size
|
||||
self.pattern[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) * square_len # only for (x,y,z=0)
|
||||
self.pattern = self.pattern.T
|
||||
336
apps/python-calibration-generator/calibration_generator.py
Normal file
336
apps/python-calibration-generator/calibration_generator.py
Normal file
@ -0,0 +1,336 @@
|
||||
# This file is part of OpenCV project.
|
||||
# It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
# of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
import argparse
|
||||
import numpy as np
|
||||
import math
|
||||
import yaml
|
||||
from drawer import animation2D, animation3D
|
||||
from utils import RandGen, insideImage, eul2rot, saveKDRT, areAllInsideImage, insideImageMask, projectCamera, export2JSON
|
||||
from pathlib import Path
|
||||
from board import CheckerBoard
|
||||
|
||||
class Camera:
|
||||
def __init__(self, idx, img_width, img_height, fx_limit, euler_limit, t_limit, is_fisheye, fy_deviation=None, skew=None,
|
||||
distortion_limit=None, noise_scale_img_diag=None):
|
||||
"""
|
||||
@skew : is either None or in radians
|
||||
@fy_deviation : is either None (that is fx=fy) or value such that fy = [fx*(1-fy_deviation/100), fx*(1+fy_deviation/100)]
|
||||
@distortion_limit : is either None or array of size (num_tangential_dist+num_radial_dist) x 2
|
||||
@euler_limit : is 3 x 2 limit of euler angles in degrees
|
||||
@t_limit : is 3 x 2 limit of translation in meters
|
||||
"""
|
||||
assert len(fx_limit) == 2 and img_width >= 0 and img_width >= 0
|
||||
if is_fisheye and distortion_limit is not None: assert len(distortion_limit) == 4 # distortion for fisheye has only 4 parameters
|
||||
self.idx = idx
|
||||
self.img_width, self.img_height = img_width, img_height
|
||||
self.fx_min = fx_limit[0]
|
||||
self.fx_max = fx_limit[1]
|
||||
self.fy_deviation = fy_deviation
|
||||
self.img_diag = math.sqrt(img_height ** 2 + img_width ** 2)
|
||||
self.is_fisheye = is_fisheye
|
||||
self.fx, self.fy = None, None
|
||||
self.px, self.py = None, None
|
||||
self.K, self.R, self.t, self.P = None, None, None, None
|
||||
self.skew = skew
|
||||
self.distortion = None
|
||||
self.distortion_lim = distortion_limit
|
||||
self.euler_limit = np.array(euler_limit, dtype=np.float32)
|
||||
self.t_limit = t_limit
|
||||
self.noise_scale_img_diag = noise_scale_img_diag
|
||||
if idx != 0:
|
||||
assert len(euler_limit) == len(t_limit) == 3
|
||||
for i in range(3):
|
||||
assert len(euler_limit[i]) == len(t_limit[i]) == 2
|
||||
self.euler_limit[i] *= (np.pi / 180)
|
||||
|
||||
def generateAll(cameras, board, num_frames, rand_gen, MAX_RAND_ITERS=10000, save_proj_animation=None, save_3d_animation=None):
|
||||
EPS = 1e-10
|
||||
"""
|
||||
output:
|
||||
points_2d: NUM_FRAMES x NUM_CAMERAS x 2 x NUM_PTS
|
||||
"""
|
||||
|
||||
for i in range(len(cameras)):
|
||||
cameras[i].t = np.zeros((3, 1))
|
||||
if cameras[i].idx == 0:
|
||||
cameras[i].R = np.identity(3)
|
||||
else:
|
||||
angles = [0, 0, 0]
|
||||
for k in range(3):
|
||||
if abs(cameras[i].t_limit[k][0] - cameras[i].t_limit[k][1]) < EPS:
|
||||
cameras[i].t[k] = cameras[i].t_limit[k][0]
|
||||
else:
|
||||
cameras[i].t[k] = rand_gen.randRange(cameras[i].t_limit[k][0], cameras[i].t_limit[k][1])
|
||||
|
||||
if abs(cameras[i].euler_limit[k][0] - cameras[i].euler_limit[k][1]) < EPS:
|
||||
angles[k] = cameras[i].euler_limit[k][0]
|
||||
else:
|
||||
angles[k] = rand_gen.randRange(cameras[i].euler_limit[k][0], cameras[i].euler_limit[k][1])
|
||||
|
||||
cameras[i].R = eul2rot(angles)
|
||||
|
||||
if abs(cameras[i].fx_min - cameras[i].fx_max) < EPS:
|
||||
cameras[i].fx = cameras[i].fx_min
|
||||
else:
|
||||
cameras[i].fx = rand_gen.randRange(cameras[i].fx_min, cameras[i].fx_max)
|
||||
if cameras[i].fy_deviation is None:
|
||||
cameras[i].fy = cameras[i].fx
|
||||
else:
|
||||
cameras[i].fy = rand_gen.randRange((1 - cameras[i].fy_deviation) * cameras[i].fx,
|
||||
(1 + cameras[i].fy_deviation) * cameras[i].fx)
|
||||
|
||||
cameras[i].px = int(cameras[i].img_width / 2.0) + 1
|
||||
cameras[i].py = int(cameras[i].img_height / 2.0) + 1
|
||||
cameras[i].K = np.array([[cameras[i].fx, 0, cameras[i].px], [0, cameras[i].fy, cameras[i].py], [0, 0, 1]], dtype=float)
|
||||
if cameras[i].skew is not None: cameras[i].K[0, 1] = np.tan(cameras[i].skew) * cameras[i].K[0, 0]
|
||||
cameras[i].P = cameras[i].K @ np.concatenate((cameras[i].R, cameras[i].t), 1)
|
||||
|
||||
if cameras[i].distortion_lim is not None:
|
||||
cameras[i].distortion = np.zeros((1, len(cameras[i].distortion_lim))) # opencv using 5 values distortion as default
|
||||
for k, lim in enumerate(cameras[i].distortion_lim):
|
||||
cameras[i].distortion[0,k] = rand_gen.randRange(lim[0], lim[1])
|
||||
else:
|
||||
cameras[i].distortion = np.zeros((1, 5)) # opencv is using 5 values distortion as default
|
||||
|
||||
origin = None
|
||||
box = np.array([[0, board.square_len * (board.w - 1), 0, board.square_len * (board.w - 1)],
|
||||
[0, 0, board.square_len * (board.h - 1), board.square_len * (board.h - 1)],
|
||||
[0, 0, 0, 0]])
|
||||
|
||||
if board.t_origin is None:
|
||||
try:
|
||||
import torch, pytorch3d, pytorch3d.transforms
|
||||
has_pytorch = True
|
||||
except:
|
||||
has_pytorch = False
|
||||
|
||||
if has_pytorch:
|
||||
rot_angles = torch.zeros(3, requires_grad=True)
|
||||
origin = torch.ones((3,1), requires_grad=True)
|
||||
optimizer = torch.optim.Adam([rot_angles, origin], lr=5e-3)
|
||||
Ps = torch.tensor(np.stack([cam.K @ np.concatenate((cam.R, cam.t), 1) for cam in cameras]), dtype=torch.float32)
|
||||
rot_conv = 'XYZ'
|
||||
board_pattern = torch.tensor(box, dtype=Ps.dtype)
|
||||
corners = torch.tensor([[[0, 0], [0, cam.img_height], [cam.img_width, 0], [cam.img_width, cam.img_height]] for cam in cameras], dtype=Ps.dtype).transpose(-1,-2)
|
||||
loss_fnc = torch.nn.HuberLoss()
|
||||
lr_scheduler = torch.optim.lr_scheduler.ReduceLROnPlateau(optimizer, 'min', min_lr=1e-4, factor=0.8, patience=10)
|
||||
prev_loss = 1e10
|
||||
torch.autograd.set_detect_anomaly(True)
|
||||
MAX_DEPTH = 4
|
||||
for it in range(500):
|
||||
pts_board = pytorch3d.transforms.euler_angles_to_matrix(rot_angles, rot_conv) @ board_pattern + origin
|
||||
pts_proj = Ps[:,:3,:3] @ pts_board[None,:] + Ps[:,:,[-1]]
|
||||
pts_proj = pts_proj[:, :2] / (pts_proj[:, [2]]+1e-15)
|
||||
|
||||
loss = num_wrong = 0
|
||||
for i, proj in enumerate(pts_proj):
|
||||
if not areAllInsideImage(pts_proj[i], cameras[i].img_width, cameras[i].img_height):
|
||||
loss += loss_fnc(corners[i], pts_proj[i])
|
||||
num_wrong += 1
|
||||
if num_wrong > 0:
|
||||
loss /= num_wrong
|
||||
loss.backward()
|
||||
optimizer.step()
|
||||
lr_scheduler.step(loss)
|
||||
if origin[2] < 0:
|
||||
with torch.no_grad(): origin[2] = 2.0
|
||||
if it % 5 == 0:
|
||||
print('iter', it, 'loss %.2E' % loss)
|
||||
if abs(prev_loss - loss) < 1e-10:
|
||||
break
|
||||
prev_loss = loss.item()
|
||||
else:
|
||||
print('all points inside')
|
||||
break
|
||||
print(origin)
|
||||
points_board = (torch.tensor(board.pattern, dtype=Ps.dtype) + origin).detach().numpy()
|
||||
else:
|
||||
max_sum_diag = 0.0
|
||||
total_tested = 0
|
||||
for z in np.arange(0.25, 50, .5):
|
||||
if origin is not None: break # will not update
|
||||
min_x1, max_x1 = -z * cameras[0].px / cameras[0].fx, (cameras[0].img_width * z - z * cameras[0].px) / cameras[0].fx
|
||||
min_y1, max_y1 = -z * cameras[0].py / cameras[0].fy, (cameras[0].img_height * z - z * cameras[0].py) / cameras[0].fy
|
||||
min_x2, max_x2 = -z * cameras[0].px / cameras[0].fx - box[0, 1], (cameras[0].img_width * z - z * cameras[0].px) / cameras[0].fx - box[0, 1]
|
||||
min_y2, max_y2 = -z * cameras[0].py / cameras[0].fy - box[1, 2], (cameras[0].img_height * z - z * cameras[0].py) / cameras[0].fy - box[1, 2]
|
||||
min_x = max(min_x1, min_x2)
|
||||
min_y = max(min_y1, min_y2)
|
||||
max_x = min(max_x1, max_x2)
|
||||
max_y = min(max_y1, max_y2)
|
||||
if max_x < min_x or max_y < min_y: continue
|
||||
for x in np.linspace(min_x, max_x, 40):
|
||||
for y in np.linspace(min_y, max_y, 40):
|
||||
total_tested += 1
|
||||
pts = box + np.array([[x], [y], [z]])
|
||||
sum_diag = 0.0
|
||||
all_visible = True
|
||||
for i in range(len(cameras)):
|
||||
pts_proj = projectCamera(cameras[i], pts)
|
||||
visible_pts = insideImage(pts_proj, cameras[i].img_width, cameras[i].img_height)
|
||||
if visible_pts != pts_proj.shape[1]:
|
||||
# print(i,')',x, y, z, 'not visible, total', visible_pts, '/', pts_proj.shape[1])
|
||||
all_visible = False
|
||||
break
|
||||
sum_diag += np.linalg.norm(pts_proj[:, 0] - pts_proj[:, -1])
|
||||
if not all_visible: continue
|
||||
if max_sum_diag < sum_diag:
|
||||
max_sum_diag = sum_diag
|
||||
origin = np.array([[x], [y], [z]])
|
||||
points_board = board.pattern + origin
|
||||
else:
|
||||
points_board = board.pattern + board.t_origin
|
||||
|
||||
points_2d, points_3d = [], []
|
||||
valid_frames_per_camera = np.zeros(len(cameras))
|
||||
MIN_FRAMES_PER_CAM = int(num_frames * 0.1)
|
||||
for frame in range(MAX_RAND_ITERS):
|
||||
R_board = eul2rot([ rand_gen.randRange(board.euler_limit[0][0], board.euler_limit[0][1]),
|
||||
rand_gen.randRange(board.euler_limit[1][0], board.euler_limit[1][1]),
|
||||
rand_gen.randRange(board.euler_limit[2][0], board.euler_limit[2][1])])
|
||||
t_board = np.array([[rand_gen.randRange(board.t_limit[0][0], board.t_limit[0][1])],
|
||||
[rand_gen.randRange(board.t_limit[1][0], board.t_limit[1][1])],
|
||||
[rand_gen.randRange(board.t_limit[2][0], board.t_limit[2][1])]])
|
||||
|
||||
points_board_mean = points_board.mean(-1)[:,None]
|
||||
pts_board = R_board @ (points_board - points_board_mean) + points_board_mean + t_board
|
||||
cam_points_2d = [projectCamera(cam, pts_board) for cam in cameras]
|
||||
|
||||
"""
|
||||
# plot normals
|
||||
board_normal = 10*np.cross(pts_board[:,board.w] - pts_board[:,0], pts_board[:,board.w-1] - pts_board[:,0])
|
||||
ax = plotCamerasAndBoardFig(pts_board, cameras, pts_color=board.colors_board)
|
||||
pts = np.stack((pts_board[:,0], pts_board[:,0]+board_normal))
|
||||
ax.plot(pts[:,0], pts[:,1], pts[:,2], 'r-')
|
||||
for ii, cam in enumerate(cameras):
|
||||
pts = np.stack((cam.t.flatten(), cam.t.flatten()+cam.R[2]))
|
||||
ax.plot(pts[:,0], pts[:,1], pts[:,2], 'g-')
|
||||
print(ii, np.arccos(board_normal.dot(cam.R[2]) / np.linalg.norm(board_normal))*180/np.pi, np.arccos((-board_normal).dot(cam.R[2]) / np.linalg.norm(board_normal))*180/np.pi)
|
||||
plotAllProjectionsFig(np.stack(cam_points_2d), cameras, pts_color=board.colors_board)
|
||||
plt.show()
|
||||
"""
|
||||
|
||||
for cam_idx in range(len(cameras)):
|
||||
if not board.isProjectionValid(cam_points_2d[cam_idx]):
|
||||
cam_points_2d[cam_idx] = -np.ones_like(cam_points_2d[cam_idx])
|
||||
elif cameras[cam_idx].noise_scale_img_diag is not None:
|
||||
cam_points_2d[cam_idx] += np.random.normal(0, cameras[cam_idx].img_diag * cameras[cam_idx].noise_scale_img_diag, cam_points_2d[cam_idx].shape)
|
||||
|
||||
### test
|
||||
pts_inside_camera = np.zeros(len(cameras), dtype=bool)
|
||||
for ii, pts_2d in enumerate(cam_points_2d):
|
||||
mask = insideImageMask(pts_2d, cameras[ii].img_width, cameras[ii].img_height)
|
||||
# cam_points_2d[ii] = cam_points_2d[ii][:,mask]
|
||||
pts_inside_camera[ii] = mask.all()
|
||||
# print(pts_inside, end=' ')
|
||||
# print('from max inside', pts_board.shape[1])
|
||||
###
|
||||
|
||||
if pts_inside_camera.sum() >= 2:
|
||||
valid_frames_per_camera += np.array(pts_inside_camera, int)
|
||||
print(valid_frames_per_camera)
|
||||
points_2d.append(np.stack(cam_points_2d))
|
||||
points_3d.append(pts_board)
|
||||
|
||||
if len(points_2d) >= num_frames and (valid_frames_per_camera >= MIN_FRAMES_PER_CAM).all():
|
||||
print('tried samples', frame)
|
||||
break
|
||||
|
||||
VIDEOS_FPS = 5
|
||||
VIDEOS_DPI = 250
|
||||
MAX_FRAMES = 100
|
||||
if save_proj_animation is not None: animation2D(board, cameras, points_2d, save_proj_animation, VIDEOS_FPS, VIDEOS_DPI, MAX_FRAMES)
|
||||
if save_3d_animation is not None: animation3D(board, cameras, points_3d, save_3d_animation, VIDEOS_FPS, VIDEOS_DPI, MAX_FRAMES)
|
||||
|
||||
print('number of found frames', len(points_2d))
|
||||
return np.stack(points_2d), np.stack(points_3d)
|
||||
|
||||
def createConfigFile(fname, params):
|
||||
file = open(fname, 'w')
|
||||
|
||||
def writeDict(dict_write, tab):
|
||||
for key, value in dict_write.items():
|
||||
if isinstance(value, dict):
|
||||
file.write(tab+key+' :\n')
|
||||
writeDict(value, tab+' ')
|
||||
else:
|
||||
file.write(tab+key+' : '+str(value)+'\n')
|
||||
file.write('\n')
|
||||
writeDict(params, '')
|
||||
file.close()
|
||||
|
||||
def generateRoomConfiguration():
|
||||
params = {'NAME' : '"room_corners"', 'NUM_SAMPLES': 1, 'SEED': 0, 'MAX_FRAMES' : 50, 'MAX_RANDOM_ITERS' : 100000, 'NUM_CAMERAS': 4,
|
||||
'BOARD': {'WIDTH':9, 'HEIGHT':7, 'SQUARE_LEN':0.08, 'T_LIMIT': [[-0.2,0.2], [-0.2,0.2], [-0.1,0.1]], 'EULER_LIMIT': [[-45, 45], [-180, 180], [-45, 45]], 'T_ORIGIN': [-0.3,0,1.5]}}
|
||||
params['CAMERA1'] = {'FX': [1200, 1200], 'FY_DEVIATION': 'null', 'IMG_WIDTH': 1500, 'IMG_HEIGHT': 1080, 'EULER_LIMIT': 'null', 'T_LIMIT': 'null', 'NOISE_SCALE': 3.0e-4, 'FISHEYE': False, 'DIST': [[5.2e-1,5.2e-1], [0,0], [0,0], [0,0], [0,0]]}
|
||||
params['CAMERA2'] = {'FX': [1000, 1000], 'FY_DEVIATION': 'null', 'IMG_WIDTH': 1300, 'IMG_HEIGHT': 1000, 'EULER_LIMIT': [[0,0], [90,90], [0,0]], 'T_LIMIT': [[-2.0,-2.0], [0.0, 0.0], [1.5, 1.5]], 'NOISE_SCALE': 3.5e-4, 'FISHEYE': False, 'DIST': [[3.2e-1,3.2e-1], [0,0], [0,0], [0,0], [0,0]]}
|
||||
params['CAMERA3'] = {'FX': [1000, 1000], 'FY_DEVIATION': 'null', 'IMG_WIDTH': 1300, 'IMG_HEIGHT': 1000, 'EULER_LIMIT': [[0,0], [-90,-90], [0,0]], 'T_LIMIT': [[2.0,2.0], [0.0, 0.0], [1.5, 1.5]], 'NOISE_SCALE': 4.0e-4, 'FISHEYE': False, 'DIST': [[6.2e-1,6.2e-1], [0,0], [0,0], [0,0], [0,0]]}
|
||||
params['CAMERA4'] = {'FX': [1000, 1000], 'FY_DEVIATION': 'null', 'IMG_WIDTH': 1300, 'IMG_HEIGHT': 1000, 'EULER_LIMIT': [[0,0], [180,180], [0,0]], 'T_LIMIT': [[0.0,0.0], [0.0, 0.0], [3.0, 3.0]], 'NOISE_SCALE': 3.2e-4, 'FISHEYE': False, 'DIST': [[4.2e-1,4.2e-1], [0,0], [0,0], [0,0], [0,0]]}
|
||||
createConfigFile('python/configs/config_room_corners.yaml', params)
|
||||
|
||||
def generateCircularCameras():
|
||||
rand_gen = RandGen(0)
|
||||
params = {'NAME' : '"circular"', 'NUM_SAMPLES': 1, 'SEED': 0, 'MAX_FRAMES' : 70, 'MAX_RANDOM_ITERS' : 100000, 'NUM_CAMERAS': 9,
|
||||
'BOARD': {'WIDTH': 9, 'HEIGHT': 7, 'SQUARE_LEN':0.08, 'T_LIMIT': [[-0.2,0.2], [-0.2,0.2], [-0.1,0.1]], 'EULER_LIMIT': [[-45, 45], [-180, 180], [-45, 45]], 'T_ORIGIN': [-0.3,0,2.2]}}
|
||||
|
||||
dist = 1.1
|
||||
xs = np.arange(dist, dist*(params['NUM_CAMERAS']//4)+1e-3, dist)
|
||||
xs = np.concatenate((xs, xs[::-1]))
|
||||
xs = np.concatenate((xs, -xs))
|
||||
dist_z = 0.90
|
||||
zs = np.arange(dist_z, dist_z*(params['NUM_CAMERAS']//2)+1e-3, dist_z)
|
||||
zs = np.concatenate((zs, zs[::-1]))
|
||||
yaw = np.linspace(0, -360, params['NUM_CAMERAS']+1)[1:-1]
|
||||
for i in range(9):
|
||||
fx = rand_gen.randRange(900, 1300)
|
||||
d0 = rand_gen.randRange(4e-1, 7e-1)
|
||||
euler_limit = 'null'
|
||||
t_limit = 'null'
|
||||
if i > 0:
|
||||
euler_limit = [[0,0], [yaw[i-1], yaw[i-1]], [0,0]]
|
||||
t_limit = [[xs[i-1], xs[i-1]], [0,0], [zs[i-1], zs[i-1]]]
|
||||
params['CAMERA'+str((i+1))] = {'FX': [fx, fx], 'FY_DEVIATION': 'null', 'IMG_WIDTH': int(rand_gen.randRange(1200, 1600)), 'IMG_HEIGHT': int(rand_gen.randRange(800, 1200)),
|
||||
'EULER_LIMIT': euler_limit, 'T_LIMIT': t_limit, 'NOISE_SCALE': rand_gen.randRange(2e-4, 5e-4), 'FISHEYE': False, 'DIST': [[d0,d0], [0,0], [0,0], [0,0], [0,0]]}
|
||||
|
||||
createConfigFile('python/configs/config_circular.yaml', params)
|
||||
|
||||
def getCamerasFromCfg(cfg):
|
||||
cameras = []
|
||||
for i in range(cfg['NUM_CAMERAS']):
|
||||
cameras.append(Camera(i, cfg['CAMERA' + str(i+1)]['IMG_WIDTH'], cfg['CAMERA' + str(i+1)]['IMG_HEIGHT'],
|
||||
cfg['CAMERA' + str(i+1)]['FX'], cfg['CAMERA' + str(i+1)]['EULER_LIMIT'], cfg['CAMERA' + str(i+1)]['T_LIMIT'],
|
||||
cfg['CAMERA' + str(i+1)]['FISHEYE'], cfg['CAMERA' + str(i+1)]['FY_DEVIATION'],
|
||||
noise_scale_img_diag=cfg['CAMERA' + str(i+1)]['NOISE_SCALE'], distortion_limit=cfg['CAMERA' + str(i+1)]['DIST']))
|
||||
return cameras
|
||||
|
||||
def main(cfg_name, save_folder):
|
||||
cfg = yaml.safe_load(open(cfg_name, 'r'))
|
||||
print(cfg)
|
||||
np.random.seed(cfg['SEED'])
|
||||
for trial in range(cfg['NUM_SAMPLES']):
|
||||
Path(save_folder).mkdir(exist_ok=True, parents=True)
|
||||
|
||||
checkerboard = CheckerBoard(cfg['BOARD']['WIDTH'], cfg['BOARD']['HEIGHT'], cfg['BOARD']['SQUARE_LEN'], cfg['BOARD']['EULER_LIMIT'], cfg['BOARD']['T_LIMIT'], cfg['BOARD']['T_ORIGIN'])
|
||||
cameras = getCamerasFromCfg(cfg)
|
||||
points_2d, points_3d = generateAll(cameras, checkerboard, cfg['MAX_FRAMES'], RandGen(cfg['SEED']), cfg['MAX_RANDOM_ITERS'], save_folder+'plots_projections.mp4', save_folder+'board_cameras.mp4')
|
||||
|
||||
for i in range(len(cameras)):
|
||||
print('Camera', i)
|
||||
print('K', cameras[i].K)
|
||||
print('R', cameras[i].R)
|
||||
print('t', cameras[i].t.flatten())
|
||||
print('distortion', cameras[i].distortion.flatten())
|
||||
print('-----------------------------')
|
||||
|
||||
imgs_width_height = [[cam.img_width, cam.img_height] for cam in cameras]
|
||||
is_fisheye = [cam.is_fisheye for cam in cameras]
|
||||
export2JSON(checkerboard.pattern, points_2d, imgs_width_height, is_fisheye, save_folder+'opencv_sample_'+cfg['NAME']+'.json')
|
||||
saveKDRT(cameras, save_folder+'gt.txt')
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--cfg', type=str, required=True, help='path to config file, e.g., config_cv_test.yaml')
|
||||
parser.add_argument('--output_folder', type=str, default='', help='output folder')
|
||||
params, _ = parser.parse_known_args()
|
||||
main(params.cfg, params.output_folder)
|
||||
57
apps/python-calibration-generator/config_cv_test.yaml
Normal file
57
apps/python-calibration-generator/config_cv_test.yaml
Normal file
@ -0,0 +1,57 @@
|
||||
NAME : "cv_test"
|
||||
NUM_SAMPLES : 1
|
||||
SEED : 0
|
||||
MAX_FRAMES : 50
|
||||
MAX_RANDOM_ITERS : 100000
|
||||
NUM_CAMERAS : 4
|
||||
BOARD :
|
||||
WIDTH : 9
|
||||
HEIGHT : 7
|
||||
SQUARE_LEN : 0.08
|
||||
T_LIMIT : [[-0.2, 0.2], [-0.2, 0.2], [-0.1, 0.1]]
|
||||
EULER_LIMIT : [[-45, 45], [-180, 180], [-45, 45]]
|
||||
T_ORIGIN : [-0.3, 0, 1.5]
|
||||
|
||||
CAMERA1 :
|
||||
FX : [1200, 1200]
|
||||
FY_DEVIATION : null
|
||||
IMG_WIDTH : 1500
|
||||
IMG_HEIGHT : 1080
|
||||
EULER_LIMIT : null
|
||||
T_LIMIT : null
|
||||
NOISE_SCALE : 0.0003
|
||||
FISHEYE : False
|
||||
DIST : [[0.52, 0.52], [0, 0], [0, 0], [0, 0], [0, 0]]
|
||||
|
||||
CAMERA2 :
|
||||
FX : [1000, 1000]
|
||||
FY_DEVIATION : null
|
||||
IMG_WIDTH : 1300
|
||||
IMG_HEIGHT : 1000
|
||||
EULER_LIMIT : [[0, 0], [90, 90], [0, 0]]
|
||||
T_LIMIT : [[-2.0, -2.0], [0.0, 0.0], [1.5, 1.5]]
|
||||
NOISE_SCALE : 0.00035
|
||||
FISHEYE : False
|
||||
DIST : [[0.32, 0.32], [0, 0], [0, 0], [0, 0], [0, 0]]
|
||||
|
||||
CAMERA3 :
|
||||
FX : [1000, 1000]
|
||||
FY_DEVIATION : null
|
||||
IMG_WIDTH : 1300
|
||||
IMG_HEIGHT : 1000
|
||||
EULER_LIMIT : [[0, 0], [-90, -90], [0, 0]]
|
||||
T_LIMIT : [[2.0, 2.0], [0.0, 0.0], [1.5, 1.5]]
|
||||
NOISE_SCALE : 0.0004
|
||||
FISHEYE : False
|
||||
DIST : [[0.62, 0.62], [0, 0], [0, 0], [0, 0], [0, 0]]
|
||||
|
||||
CAMERA4 :
|
||||
FX : [1000, 1000]
|
||||
FY_DEVIATION : null
|
||||
IMG_WIDTH : 1300
|
||||
IMG_HEIGHT : 1000
|
||||
EULER_LIMIT : [[0, 0], [180, 180], [0, 0]]
|
||||
T_LIMIT : [[0.0, 0.0], [0.0, 0.0], [3.0, 3.0]]
|
||||
NOISE_SCALE : 0.00032
|
||||
FISHEYE : False
|
||||
DIST : [[0.42, 0.42], [0, 0], [0, 0], [0, 0], [0, 0]]
|
||||
138
apps/python-calibration-generator/drawer.py
Normal file
138
apps/python-calibration-generator/drawer.py
Normal file
@ -0,0 +1,138 @@
|
||||
# This file is part of OpenCV project.
|
||||
# It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
# of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.animation as manimation
|
||||
import matplotlib.pyplot as plt
|
||||
import cv2 as cv
|
||||
|
||||
def plotImg(img):
|
||||
new_img_size = 1200. * 800.
|
||||
if img.shape[0] * img.shape[1] > new_img_size:
|
||||
new_img = cv.resize(img, (int(np.sqrt(img.shape[1] * new_img_size / img.shape[0])), int(np.sqrt(img.shape[0] * new_img_size / img.shape[1]))))
|
||||
else: new_img = img
|
||||
fig = plt.figure(figsize=(14,8))
|
||||
fig.tight_layout()
|
||||
fig.subplots_adjust(left=0, bottom=0, right=1, top=1, wspace=None, hspace=None)
|
||||
plt.imshow(new_img)
|
||||
plt.show()
|
||||
|
||||
def getDimBox(points_, num_dim=3):
|
||||
points = np.array(points_) if type(points_) is list else points_
|
||||
assert points.ndim == 3
|
||||
return np.array([[np.median([pts[k].min() for pts in points]), np.median([pts[k].max() for pts in points])] for k in
|
||||
range(num_dim)])
|
||||
|
||||
def plotPoints(ax, points_list, num_dim, dim_box=None, title='', marker='o', s=7, legend=None, save_fname='', azim=0,
|
||||
elev=0, fontsize=15, are_pts_colorful=False):
|
||||
colors = ['red', 'green', 'blue', 'black', 'magenta', 'brown']
|
||||
if dim_box is None: dim_box = getDimBox(points_list, num_dim=num_dim)
|
||||
if isinstance(are_pts_colorful, bool):
|
||||
colors_pts = np.random.rand(len(points_list[0][0]), 3) if are_pts_colorful else None
|
||||
else:
|
||||
colors_pts = are_pts_colorful
|
||||
for ii, points in enumerate(points_list):
|
||||
color_ = colors_pts if colors_pts is not None else colors[ii]
|
||||
if num_dim == 2:
|
||||
ax.scatter(points[0], points[1], color=color_, marker=marker, s=s)
|
||||
else:
|
||||
ax.scatter(points[0], points[1], points[2], color=color_, marker=marker, s=s)
|
||||
|
||||
ax.set_xlim(dim_box[0])
|
||||
ax.set_ylim(dim_box[1])
|
||||
ax.set_xlabel('x', fontsize=fontsize)
|
||||
ax.set_ylabel('y', fontsize=fontsize)
|
||||
if num_dim == 3:
|
||||
ax.set_zlabel('z', fontsize=fontsize)
|
||||
ax.set_zlim(dim_box[2])
|
||||
ax.view_init(azim=azim, elev=elev)
|
||||
ax.set_box_aspect((dim_box[0, 1] - dim_box[0, 0], dim_box[1, 1] - dim_box[1, 0], dim_box[2, 1] - dim_box[2, 0]))
|
||||
else:
|
||||
ax.set_aspect('equal', 'box')
|
||||
|
||||
if legend is not None: ax.legend(legend)
|
||||
if title != '': ax.set_title(title, fontsize=30)
|
||||
if save_fname != '': plt.savefig(save_fname, bbox_inches='tight', pad_inches=0)
|
||||
|
||||
def plotAllProjections(axs, cam_points_2d, cameras, sqr, pts_color=False):
|
||||
for i in range(len(cameras)):
|
||||
axs[i // sqr, i % sqr].clear()
|
||||
plotPoints(axs[i // sqr, i % sqr], [cam_points_2d[i]], 2, dim_box=[[0, cameras[i].img_width], [0, cameras[i].img_height]], title='camera '+str(i), are_pts_colorful=pts_color)
|
||||
# plotPoints(axs[i // sqr, i % sqr], [cam_points_2d[i]], 2, title='projected points, camera '+str(i), are_pts_colorful=pts_color)
|
||||
axs[i // sqr, i % sqr].invert_yaxis()
|
||||
|
||||
def plotCamerasAndBoard(ax, pts_board, cam_box, cameras, colors, dim_box, pts_color=False):
|
||||
ax_lines = [None for ii in range(len(cameras))]
|
||||
ax.clear()
|
||||
ax.set_title('Cameras and board position', fontsize=40)
|
||||
plotPoints(ax, [pts_board], 3, s=10, are_pts_colorful=pts_color)
|
||||
all_pts = [pts_board]
|
||||
for ii, cam in enumerate(cameras):
|
||||
cam_box_i = cam_box.copy()
|
||||
cam_box_i[:,0] *= cam.img_width / max(cam.img_height, cam.img_width)
|
||||
cam_box_i[:,1] *= cam.img_height / max(cam.img_height, cam.img_width)
|
||||
cam_box_Rt = (cam.R @ cam_box_i.T + cam.t).T
|
||||
all_pts.append(np.concatenate((cam_box_Rt, cam.t.T)).T)
|
||||
|
||||
ax_lines[ii] = ax.plot([cam.t[0,0], cam_box_Rt[0,0]], [cam.t[1,0], cam_box_Rt[0,1]], [cam.t[2,0], cam_box_Rt[0,2]], '-', color=colors[ii])[0]
|
||||
ax.plot([cam.t[0,0], cam_box_Rt[1,0]], [cam.t[1,0], cam_box_Rt[1,1]], [cam.t[2,0], cam_box_Rt[1,2]], '-', color=colors[ii])
|
||||
ax.plot([cam.t[0,0], cam_box_Rt[2,0]], [cam.t[1,0], cam_box_Rt[2,1]], [cam.t[2,0], cam_box_Rt[2,2]], '-', color=colors[ii])
|
||||
ax.plot([cam.t[0,0], cam_box_Rt[3,0]], [cam.t[1,0], cam_box_Rt[3,1]], [cam.t[2,0], cam_box_Rt[3,2]], '-', color=colors[ii])
|
||||
|
||||
ax.plot([cam_box_Rt[0,0], cam_box_Rt[1,0]], [cam_box_Rt[0,1], cam_box_Rt[1,1]], [cam_box_Rt[0,2], cam_box_Rt[1,2]], '-', color=colors[ii])
|
||||
ax.plot([cam_box_Rt[1,0], cam_box_Rt[2,0]], [cam_box_Rt[1,1], cam_box_Rt[2,1]], [cam_box_Rt[1,2], cam_box_Rt[2,2]], '-', color=colors[ii])
|
||||
ax.plot([cam_box_Rt[2,0], cam_box_Rt[3,0]], [cam_box_Rt[2,1], cam_box_Rt[3,1]], [cam_box_Rt[2,2], cam_box_Rt[3,2]], '-', color=colors[ii])
|
||||
ax.plot([cam_box_Rt[3,0], cam_box_Rt[0,0]], [cam_box_Rt[3,1], cam_box_Rt[0,1]], [cam_box_Rt[3,2], cam_box_Rt[0,2]], '-', color=colors[ii])
|
||||
ax.legend(ax_lines, [str(ii) for ii in range(len(cameras))], fontsize=20)
|
||||
if dim_box is None: dim_box = getDimBox([np.concatenate((all_pts),1)])
|
||||
ax.set_xlim(dim_box[0])
|
||||
ax.set_ylim(dim_box[1])
|
||||
ax.set_zlim(dim_box[2])
|
||||
ax.set_box_aspect((dim_box[0, 1] - dim_box[0, 0], dim_box[1, 1] - dim_box[1, 0], dim_box[2, 1] - dim_box[2, 0]))
|
||||
ax.view_init(azim=-89, elev=-15)
|
||||
return ax
|
||||
|
||||
def plotAllProjectionsFig(cam_points_2d, cameras, pts_color=False):
|
||||
sqr = int(np.ceil(np.sqrt(len(cameras))))
|
||||
fig, axs = plt.subplots(sqr, sqr, figsize=(15,10))
|
||||
plotAllProjections(axs, cam_points_2d, cameras, sqr, pts_color)
|
||||
|
||||
def getCameraBox():
|
||||
cam_box = np.array([[1, 1, 0], [1, -1, 0], [-1, -1, 0], [-1, 1, 0]],dtype=np.float32)
|
||||
cam_box[:,2] = 3.0
|
||||
cam_box *= 0.15
|
||||
return cam_box
|
||||
|
||||
def plotCamerasAndBoardFig(pts_board, cameras, pts_color=False):
|
||||
fig = plt.figure(figsize=(13.0, 15.0))
|
||||
ax = fig.add_subplot(111, projection='3d')
|
||||
return plotCamerasAndBoard(ax, pts_board, getCameraBox(), cameras, np.random.rand(len(cameras),3), None, pts_color)
|
||||
|
||||
def animation2D(board, cameras, points_2d, save_proj_animation, VIDEOS_FPS, VIDEOS_DPI, MAX_FRAMES):
|
||||
writer = manimation.writers['ffmpeg'](fps=VIDEOS_FPS)
|
||||
sqr = int(np.ceil(np.sqrt(len(cameras))))
|
||||
fig, axs = plt.subplots(sqr, sqr, figsize=(15,10))
|
||||
with writer.saving(fig, save_proj_animation, dpi=VIDEOS_DPI):
|
||||
for k, cam_points_2d in enumerate(points_2d):
|
||||
if k >= MAX_FRAMES: break
|
||||
plotAllProjections(axs, cam_points_2d, cameras, sqr, pts_color=board.colors_board)
|
||||
writer.grab_frame()
|
||||
|
||||
def animation3D(board, cameras, points_3d, save_3d_animation, VIDEOS_FPS, VIDEOS_DPI, MAX_FRAMES):
|
||||
writer = manimation.writers['ffmpeg'](fps=VIDEOS_FPS)
|
||||
fig = plt.figure(figsize=(13.0, 15.0))
|
||||
ax = fig.add_subplot(111, projection='3d')
|
||||
dim_box = None
|
||||
cam_box = getCameraBox()
|
||||
colors = np.random.rand(10,3)
|
||||
all_pts = []
|
||||
cam_pts = np.concatenate([cam.R @ cam_box.T + cam.t for cam in cameras], 1)
|
||||
for k in range(min(50, len(points_3d))):
|
||||
all_pts.append(np.concatenate((cam_pts, points_3d[k]),1))
|
||||
dim_box = getDimBox(all_pts)
|
||||
with writer.saving(fig, save_3d_animation, dpi=VIDEOS_DPI):
|
||||
for i, pts_board in enumerate(points_3d):
|
||||
if i >= MAX_FRAMES: break
|
||||
plotCamerasAndBoard(ax, pts_board, cam_box, cameras, colors, dim_box, pts_color=board.colors_board)
|
||||
writer.grab_frame()
|
||||
71
apps/python-calibration-generator/utils.py
Normal file
71
apps/python-calibration-generator/utils.py
Normal file
@ -0,0 +1,71 @@
|
||||
# This file is part of OpenCV project.
|
||||
# It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
# of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
import cv2 as cv
|
||||
import json
|
||||
|
||||
class RandGen:
|
||||
def __init__(self, seed = 0):
|
||||
self.rand_gen = np.random.RandomState(seed)
|
||||
|
||||
def randRange(self, min_v, max_v):
|
||||
return self.rand_gen.rand(1).item() * (max_v - min_v) + min_v
|
||||
|
||||
def project(K, R, t, dist, pts_3d, is_fisheye):
|
||||
if is_fisheye:
|
||||
pts_2d = cv.fisheye.projectPoints(pts_3d.T[None,:], cv.Rodrigues(R)[0], t, K, dist.flatten())[0].reshape(-1,2).T
|
||||
else:
|
||||
pts_2d = cv.projectPoints(pts_3d, R, t, K, dist)[0].reshape(-1,2).T
|
||||
return pts_2d
|
||||
|
||||
def projectCamera(camera, pts_3d):
|
||||
return project(camera.K, camera.R, camera.t, camera.distortion, pts_3d, camera.is_fisheye)
|
||||
|
||||
def eul2rot(theta): # [x y z]
|
||||
# https://learnopencv.com/rotation-matrix-to-euler-angles/
|
||||
R_x = np.array([[1, 0, 0 ],
|
||||
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
|
||||
[0, math.sin(theta[0]), math.cos(theta[0]) ]])
|
||||
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
|
||||
[0, 1, 0 ],
|
||||
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]])
|
||||
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
|
||||
[math.sin(theta[2]), math.cos(theta[2]), 0],
|
||||
[0, 0, 1]])
|
||||
return np.dot(R_z, np.dot(R_y, R_x))
|
||||
|
||||
def insideImageMask(pts, w, h):
|
||||
return np.logical_and(np.logical_and(pts[0] < w, pts[1] < h), np.logical_and(pts[0] > 0, pts[1] > 0))
|
||||
|
||||
def insideImage(pts, w, h):
|
||||
return insideImageMask(pts, w, h).sum()
|
||||
|
||||
def areAllInsideImage(pts, w, h):
|
||||
return insideImageMask(pts, w, h).all()
|
||||
|
||||
def writeMatrix(file, M):
|
||||
for i in range(M.shape[0]):
|
||||
for j in range(M.shape[1]):
|
||||
file.write(str(M[i,j]) + ('\n' if j == M.shape[1]-1 else ' '))
|
||||
|
||||
def saveKDRT(cameras, fname):
|
||||
file = open(fname, 'w')
|
||||
for cam in cameras:
|
||||
writeMatrix(file, cam.K)
|
||||
writeMatrix(file, cam.distortion)
|
||||
writeMatrix(file, cam.R)
|
||||
writeMatrix(file, cam.t)
|
||||
|
||||
def export2JSON(pattern_points, image_points, image_sizes, is_fisheye, json_file):
|
||||
image_points = image_points.transpose(1,0,3,2)
|
||||
image_points_list = [[] for i in range(len(image_sizes))]
|
||||
for c in range(len(image_points)):
|
||||
for f in range(len(image_points[c])):
|
||||
if areAllInsideImage(image_points[c][f], image_sizes[c][0], image_sizes[c][1]):
|
||||
image_points_list[c].append(image_points[c][f].tolist())
|
||||
else:
|
||||
image_points_list[c].append([])
|
||||
json.dump({'object_points': pattern_points.tolist(), 'image_points': image_points_list, 'image_sizes': image_sizes, 'is_fisheye': is_fisheye}, open(json_file, 'w'))
|
||||
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/1.jpg
Normal file
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 113 KiB |
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/2.jpg
Normal file
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 140 KiB |
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/3.jpg
Normal file
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/3.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 115 KiB |
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/4.jpg
Normal file
BIN
doc/tutorials/calib3d/camera_multiview_calibration/images/4.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 126 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 130 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 24 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 336 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 199 KiB |
@ -0,0 +1,252 @@
|
||||
Multi-view Camera Calibration Tutorial {#tutorial_multiview_camera_calibration}
|
||||
==========================
|
||||
|
||||
@tableofcontents
|
||||
|
||||
@prev_tutorial{tutorial_interactive_calibration}
|
||||
|
||||
| | |
|
||||
| -: | :- |
|
||||
| Original author | Maksym Ivashechkin |
|
||||
| Compatibility | OpenCV >= 5.0 |
|
||||
|
||||
Structure:
|
||||
----
|
||||
This tutorial consists of the following sections:
|
||||
* Introduction
|
||||
* Briefly
|
||||
* How to run
|
||||
* Python example
|
||||
* Python visualization
|
||||
* Details Of The Algorithm
|
||||
* Method Input
|
||||
* Method Output
|
||||
* Method Input
|
||||
* Pseudocode
|
||||
* Python sample API
|
||||
* C++ sample API
|
||||
|
||||
Introduction
|
||||
----
|
||||
Multiview calibration is a very important task in computer vision. It is widely used in 3D reconstruction, structure from motion, autonomous driving etc. The calibration procedure is often the first step for any vision task that must be done to obtain intrinsics and extrinsics parameters of the cameras. The accuracy of camera calibration parameters directly influence all further computations and results, hence, estimating precise intrinsincs and extrinsics is crucial.
|
||||
|
||||
The calibration algorithms require a set of images for each camera, where on the images a calibration pattern (e.g., checkerboard, aruco etc) is visible and detected. Additionally, to get results with a real scale, the 3D distance between two neighbor points of the calibration pattern grid should be measured. For extrinsics calibration, images must share the calibration pattern obtained from different views, i.e., overlap of cameras' field of view. Moreover, images that share the pattern grid have to be taken at the same moment of time, or in other words, cameras must be synchronized. Otherwise, the extrinsics calibration will fail.
|
||||
|
||||
The intrinsics calibration incorporates estimation of focal lengths, skew, and principal point of the camera; these parameters are combined in the intrinsic upper triangular matrix of size 3x3. Additionally, intrinsic calibration includes finding distortion parameters of the camera. The extrinsics parameters represent a relative rotation and translation between two cameras. Therefore, for \f$N\f$ cameras, a sufficient amount of correctly selected pairs of estimated relative rotations and translations is \f$N-1\f$, while extrinsics parameters for all possible pairs \f$N^2 = N * (N-1) / 2\f$ could be derived from those that are estimated. More details about intrinsics calibration could be found in this tutorial @ref tutorial_camera_calibration_pattern, and its implementation @ref cv::calibrateCamera.
|
||||
|
||||
After intrinsics and extrinsics calibration, the projection matrices of cameras are found by combing intrinsic, rotation matrices and translation. The projection matrices enable doing triangulation (3D reconstruction), rectification, finding epipolar geometry etc.
|
||||
|
||||
The following sections describes the individual algorithmic steps of the overall multi-camera calibration pipeline:
|
||||
|
||||
Briefly
|
||||
----
|
||||
The algorithm consists of three major steps that could be enumerated as follows:
|
||||
|
||||
1. Calibrate intrinsics parameters (intrinsic matrix and distortion coefficients) for each camera independently.
|
||||
2. Calibrate pairwise cameras (using stereo calibration) using intrinsics parameters from the step 1.
|
||||
3. Do global optimization using all cameras simultaneously to refine extrinsic parameters.
|
||||
|
||||
|
||||
How to run:
|
||||
====
|
||||
|
||||
Assume we have `N` camera views, for each `i`-th view there are `M` images containing pattern points (e.g., checkerboard).
|
||||
|
||||
Python example
|
||||
--
|
||||
There are two options to run the sample code in Python (`opencv/samples/python/multiview_calibration.py`) either with raw images or provided points.
|
||||
The first option is to prepare `N` files where each file has path to image per line (images of a specific camera of the corresponding file). For example, a file for camera `i` should look like (`file_i.txt`):
|
||||
```
|
||||
/path/to/image_1_of_camera_i
|
||||
...
|
||||
/path/to/image_M_of_camera_i
|
||||
```
|
||||
|
||||
Then sample program could be run via command line as follows:
|
||||
```console
|
||||
$ python3 multiview_calibration.py --pattern_size W,H --pattern_type TYPE --fisheye IS_FISHEYE_1,...,IS_FISHEYE_N \
|
||||
--pattern_distance DIST --filenames /path/to/file_1.txt,...,/path/to/file_N.txt
|
||||
```
|
||||
|
||||
Replace `W` and `H` with size of the pattern points, `TYPE` with name of a type of the calibration grid (supported patterns: `checkerboard`, `circles`, `acircles`), `IS_FISHEYE` corresponds to the camera type (1 - is fisheye, 0 - pinhole), `DIST` is pattern distance (i.e., distance between two cells of checkerboard).
|
||||
The sample script automatically detects image points accordingly to the specified pattern type. By default detection is done in parallel, but this option could be turned off.
|
||||
|
||||
Additional (optional) flags to Python sample that could be used are as follows:
|
||||
* `--winsize` - pass values `H,W` to define window size for corners detection (default is 5,5).
|
||||
* `--debug_corners` - pass `True` or `False`. If `True` program shows several random images with detected corners for user to manually verify the detection (default is `False`).
|
||||
* `--points_json_file` - pass name of JSON file where image and pattern points could be saved after detection. Later this file could be used to run sample code. Default value is '' (nothing is saved).
|
||||
* `--find_intrinsics_in_python` - pass `0` or `1`. If `1` then the Python sample automatically calibrates intrinsics parameters and reports reprojection errors. The multiview calibration is done only for extrinsics parameters. This flag aims to separate calibration process and make it easier to debug what goes wrong.
|
||||
* `--path_to_save` - path to save results in pickle file
|
||||
* `--path_to_visualize` - path to results pickle file needed to run visualization
|
||||
* `--visualize` - visualization flag (True or False), if True only runs visualization but path_to_visualize must be provided
|
||||
* `--resize_image_detection` - True / False, if True an image will be resized to speed-up corners detection
|
||||
|
||||
Alternatively, the Python sample could be run from JSON file that should contain image points, pattern points, and boolean indicator whether a camera is fisheye.
|
||||
The example of JSON file is in `opencv_extra/testdata/python/multiview_calibration_data.json` (currently under pull request 1001 in `opencv_extra`). Its format should be dictionary with the following items:
|
||||
* `object_points` - list of lists of pattern (object) points (size NUM_POINTS x 3).
|
||||
* `image_points` - list of lists of lists of lists of image points (size NUM_CAMERAS x NUM_FRAMES x NUM_POINTS x 2).
|
||||
* `image_sizes` - list of tuples (width x height) of image size.
|
||||
* `is_fisheye` - list of boolean values (true - fisheye camera, false - otherwise).
|
||||
Optionally:
|
||||
* `Ks` and `distortions` - intrinsics parameters. If they are provided in JSON file then the proposed method does not estimate intrinsics parameters. `Ks` (intrinsic matrices) is list of lists of lists (NUM_CAMERAS x 3 x 3), `distortions` is list of lists (NUM_CAMERAS x NUM_VALUES) of distortion parameters.
|
||||
* `images_names` - list of lists (NUM_CAMERAS x NUM_FRAMES x string) of image filenames for visualization of points after calibration.
|
||||
|
||||
```console
|
||||
$ python3 multiview_calibration.py --json_file /path/to/json
|
||||
```
|
||||
|
||||
The description of flags could be found directly by running the sample script with `help` option:
|
||||
```console
|
||||
python3 multiview_calibration.py --help
|
||||
```
|
||||
|
||||
The expected output in Linux terminal for `multiview_calibration_images` data (from `opencv_extra/testdata/python/` generated in Blender) should be the following:
|
||||
|
||||
|
||||
The expected output for real-life calibration images in `opencv_extra/testdata/python/real_multiview_calibration_images` is the following:
|
||||
|
||||
|
||||
|
||||
Python visualization
|
||||
----
|
||||
|
||||
Apart from estimated extrinsics / intrinsics, the python sample provides a comprehensive visualization.
|
||||
Firstly, the sample shows positions of cameras, checkerboard (of a random frame), and pairs of cameras connected by black lines explicitly demonstrating tuples that were used in the initial stage of stereo calibration.
|
||||
If images are not known, then a simple plot with arrows (from given point to the back-projected one) visualizing errors is shown. The color of arrows highlights the error values. Additionally, the title reports mean error on this frame, and its accuracy among other frames used in calibration.
|
||||
The following test instances were synthetically generated (see `opencv/apps/python-calibration-generator/calibration_generator.py`):
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
This instance has large Gaussian points noise.
|
||||
|
||||
|
||||
|
||||
Another example, with more complex tree structure is here, it shows a weak connection between two groups of cameras.
|
||||
|
||||
|
||||
|
||||
If files to images are provided, then the output is an image with plotted arrows:
|
||||
|
||||
|
||||
|
||||
|
||||
Details Of The Algorithm
|
||||
----
|
||||
1. If the intrinsics are not provided, the calibration procedure starts intrinsics calibration independently for each camera using OpenCV function @ref cv::calibrateCamera.
|
||||
* a. If input is a combination of fisheye and pinhole cameras, then fisheye images are calibrated with the default OpenCV calibrate function. The reason is that stereo calibration in OpenCV does not support a mix of fisheye and pinhole cameras. The following flags are used in this scenario;
|
||||
* * i. @ref cv::CALIB_RATIONAL_MODEL - it extends default (5 coefficients) distortion model and returns more parameters.
|
||||
* * ii. @ref cv::CALIB_ZERO_TANGENT_DIST - it zeroes out tangential distortion coefficients, since the fisheye model does not have them.
|
||||
* * iii. @ref cv::CALIB_FIX_K5, @ref cv::CALIB_FIX_K6 - it zeroes out the fifth and sixth parameter, so in total 4 parameters are returned.
|
||||
* b. Output of intrinsic calibration is also rotation, translation vectors (transform of pattern points to camera frame), and errors per frame.
|
||||
* * i. For each frame, the index of the camera with the lowest error among all cameras is saved.
|
||||
2. Otherwise, if intrinsics are known, then the proposed algorithm runs perspective-n-point estimation (@ref cv::solvePnP) to estimate rotation and translation vectors, and reprojection error for each frame.
|
||||
3. Assume that cameras can be represented as nodes of a connected graph. An edge between two cameras is created if there is any image overlap over all frames. If the graph does not connect all cameras (i.e., exists a camera that has no overlap with other cameras) then calibration is not possible. Otherwise, the next step consists of finding the [maximum spanning tree](https://en.wikipedia.org/wiki/Minimum_spanning_tree) (MST) of this graph. The MST captures all best pairwise camera connections. The weight of edges across all frames is a weighted combination of multiple factors:
|
||||
* a. The main contribution is a number of pattern points detected in both images (cameras).
|
||||
* b. Ratio of area of convex hull of projected points in the image to the image resolution.
|
||||
* c. Angle between cameras' optical axes (found from rotation vectors).
|
||||
* d. Angle between the camera's optical axis and the pattern's normal vector (found from 3 non-collinear pattern's points).
|
||||
4. The initial estimate of cameras' extrinsics is found by pairwise stereo calibration (see @ref cv::stereoCalibrate). Without loss of generality, the 0-th camera’s rotation is fixed to identity and translation to zero vector, and the 0-th node becomes the root of the MST. The order of stereo calibration is selected by traversing MST in breadth first search, starting from the root. The total number of pairs (also number of edges of tree) is NUM_CAMERAS - 1, which is property of a tree graph.
|
||||
5. Given the initial estimate of extrinsics the aim is to polish results using global optimization (via Levenberq-Marquardt method, see @ref cv::LevMarq class).
|
||||
* a. To reduce the total number of parameters, all rotation and translation vectors estimated in the first step from intrinsics calibration with the lowest error are transformed to be relative with respect to the root camera.
|
||||
* b. The total number of parameters is (NUM_CAMERAS - 1) x (3 + 3) + NUM_FRAMES x (3 + 3), where 3 stands for a rotation vector and 3 for a translation vector. The first part of parameters are extrinsics, and the second part is for rotation and translation vectors per frame.
|
||||
* c. Robust function is additionally applied to mitigate impact of outlier points during the optimization. The function has the shape of derivative of Gaussian, or it is $x * exp(-x/s)$ (efficiently implemented by approximation of the `exp`), where `x` is a square pixel error, and `s` is manually pre-defined scale. The choice of this function is that it is increasing on the interval of `0` to `y` pixel error, and it’s decreasing thereafter. The idea is that the function slightly decreases errors until it reaches `y`, and if error is too high (more than `y`) then its robust value limits to `0`. The value of scale factor was found by exhaustive evaluation that forces robust function to almost linearly increase until the robust value of an error is 10 px and decrease afterwards (see graph of the function below). The value itself is equal to 30, but could be modified in OpenCV source code.
|
||||
|
||||
|
||||
Method Input
|
||||
----
|
||||
The high-level input of the proposed method is as follows:
|
||||
|
||||
* Pattern (object) points. (NUM_FRAMES x) NUM_PATTERN_POINTS x 3. Points may contain a copy of pattern points along frames.
|
||||
* Image points: NUM_CAMERAS x NUM_FRAMES x NUM_PATTERN_POINTS x 2.
|
||||
* Image sizes: NUM_CAMERAS x 2 (width and height).
|
||||
* Detection mask matrix of size NUM_CAMERAS x NUM_FRAMES that indicates whether pattern points are detected for specific camera and frame index.
|
||||
* Ks (optional) - intrinsic matrices per camera.
|
||||
* Distortions (optional).
|
||||
* use_intrinsics_guess - indicates whether intrinsics are provided.
|
||||
* Flags_intrinsics - flag for intrinsics estimation.
|
||||
|
||||
Method Output
|
||||
----
|
||||
The high-level output of the proposed method is the following:
|
||||
|
||||
* Boolean indicator of success
|
||||
* Rotation and translation vectors of extrinsics parameters with respect to camera (relative) 0. Number of vectors is `NUM_CAMERAS-1`, for the first camera rotation and translation vectors are zero.
|
||||
* Intrinsic matrix for each camera.
|
||||
* Distortion coefficients for each camera.
|
||||
* Rotation and translation vectors of each frame pattern with respect to camera 0. The combination of rotation and translation is able to tranform the pattern points to the camera coordinate space, and hence with intrinsics parameters project 3D points to image.
|
||||
* Matrix of reprojection errors of size NUM_CAMERAS x NUM_FRAMES
|
||||
* Output pairs used for initial estimation of extrinsics, number of pairs is `NUM_CAMERAS-1`.
|
||||
|
||||
Pseudocode
|
||||
----
|
||||
The idea of the method could be demonstrated in a high-level pseudocode whereas the whole C++ implementation of the proposed approach is implemented in `opencv/modules/calib/src/multiview_calibration.cpp` file.
|
||||
|
||||
```python
|
||||
def mutiviewCalibration (pattern_points, image_points, detection_mask):
|
||||
for cam_i = 1,…,NUMBER_CAMERAS:
|
||||
if CALIBRATE_INTRINSICS:
|
||||
K_i, distortion_i, rvecs_i, tvecs_i = calibrateCamera(pattern_points, image_points[cam_i])
|
||||
else:
|
||||
rvecs_i, tvecs_i = solvePnP(pattern_points, image_points[cam_i], K_i, distortion_i)
|
||||
# Select best rvecs, tvecs based on reprojection errors. Process data:
|
||||
pattern_img_area[cam_i][frame] = area(convexHull(image_points[cam_i][frame]
|
||||
angle_to_board[cam_i][frame] = arccos(pattern_normal_frame * optical_axis_cam_i)
|
||||
angle_cam_to_cam[cam_i][cam_j] = arccos(optical_axis_cam_i * optical_axis_cam_j)
|
||||
graph = maximumSpanningTree(detection_mask, pattern_img_area, angle_to_board, angle_cam_to_cam)
|
||||
camera_pairs = bread_first_search(graph, root_camera=0)
|
||||
for pair in camera_pairs:
|
||||
# find relative rotation, translation from camera i to j
|
||||
R_ij, t_ij = stereoCalibrate(pattern_points, image_points[i], image_points[j])
|
||||
R*, t* = optimizeLevenbergMarquardt(R, t, pattern_points, image_points, K, distortion)
|
||||
```
|
||||
|
||||
Python sample API
|
||||
----
|
||||
|
||||
To run the calibration procedure in Python follow the following steps (see sample code in `samples/python/multiview_calibration.py`):
|
||||
|
||||
1. Prepare data:
|
||||
|
||||
@snippet samples/python/multiview_calibration.py calib_init
|
||||
|
||||
The detection mask matrix is later built by checking the size of image points after detection:
|
||||
|
||||
3. Detect pattern points on images:
|
||||
|
||||
@snippet samples/python/multiview_calibration.py detect_pattern
|
||||
|
||||
4. Build detection mask matrix:
|
||||
|
||||
@snippet samples/python/multiview_calibration.py detection_matrix
|
||||
|
||||
5. Finally, the calibration function is run as follows:
|
||||
|
||||
@snippet samples/python/multiview_calibration.py multiview_calib
|
||||
|
||||
|
||||
C++ sample API
|
||||
----
|
||||
|
||||
To run the calibration procedure in C++ follow the following steps (see sample code in `opencv/samples/cpp/multiview_calibration_sample.cpp`):
|
||||
|
||||
1. Prepare data similarly to Python sample, ie., pattern size and scale, fisheye camera mask, files containing image filenames, and pass them to function:
|
||||
|
||||
@snippet samples/cpp/multiview_calibration_sample.cpp detectPointsAndCalibrate_signature
|
||||
|
||||
2. Initialize data:
|
||||
|
||||
@snippet samples/cpp/multiview_calibration_sample.cpp calib_init
|
||||
|
||||
3. Detect pattern points on images:
|
||||
|
||||
@snippet samples/cpp/multiview_calibration_sample.cpp detect_pattern
|
||||
|
||||
4. Build detection mask matrix:
|
||||
|
||||
@snippet samples/cpp/multiview_calibration_sample.cpp detection_matrix
|
||||
|
||||
5. Run calibration:
|
||||
|
||||
@snippet samples/cpp/multiview_calibration_sample.cpp multiview_calib
|
||||
@ -4,6 +4,7 @@ Interactive camera calibration application {#tutorial_interactive_calibration}
|
||||
@tableofcontents
|
||||
|
||||
@prev_tutorial{tutorial_real_time_pose}
|
||||
@next_tutorial{tutorial_multiview_camera_calibration}
|
||||
|
||||
| | |
|
||||
| -: | :- |
|
||||
|
||||
@ -6,3 +6,4 @@ Camera calibration and 3D reconstruction (calib3d module) {#tutorial_table_of_co
|
||||
- @subpage tutorial_camera_calibration
|
||||
- @subpage tutorial_real_time_pose
|
||||
- @subpage tutorial_interactive_calibration
|
||||
- @subpage tutorial_multiview_camera_calibration
|
||||
@ -1122,6 +1122,47 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
|
||||
OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
|
||||
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
|
||||
|
||||
/** @brief Estimates intrinsics and extrinsics (camera pose) for multi-camera system a.k.a multiview calibraton.
|
||||
|
||||
@param[in] objPoints Calibration pattern object points. Expected shape: NUM_FRAMES x NUM_POINTS x 3. Supported data type: CV_32F.
|
||||
@param[in] imagePoints Detected pattern points on camera images. Expected shape: NUM_CAMERAS x NUM_FRAMES x NUM_POINTS x 2.
|
||||
@param[in] imageSize Images resolution.
|
||||
@param[in] detectionMask Pattern detection mask. Each value defines if i-camera observes calibration pattern in j moment of time.
|
||||
Expected size: NUM_CAMERAS x NUM_FRAMES. Expected type: CV_8U.
|
||||
@param[in] isFisheye indicates whether i-th camera is fisheye. In case if the input data contains
|
||||
mix of pinhole and fisheye cameras Rational distortion model is used. See @ref CALIB_RATIONAL_MODEL
|
||||
for details. Expected type: CV_8U.
|
||||
@param[in] useIntrinsicsGuess Use user specified intrinsic parameters (internal camera matrix and distortion).
|
||||
If true intrinsics are not estimated during calibration.
|
||||
@param[in] flagsForIntrinsics Flags used for each camera intrinsics calibration.
|
||||
Use per-camera call and `useIntrinsicsGuess` flag to get custom intrinsics calibration for each camera.
|
||||
See @ref CALIB_USE_INTRINSIC_GUESS and other `CALIB_` constants. Expected shape: NUM_CAMERAS x 1. Supported data type: CV_32S.
|
||||
@param[out] Rs Rotation vectors relative to camera 0, where Rs[0] = 0. Output size: NUM_CAMERAS x 3 x 1. See @ref Rodrigues.
|
||||
@param[out] Ts Estimated translation vectors relative to camera 0, where Ts[0] = 0. Output size: NUM_CAMERAS x 3 x 1.
|
||||
@param[out] rvecs0 Estimated rotation vectors for camera 0. Output size: NUM_FRAMES x 3 x 1 (may contain null Mat, if frame is not valid). See @ref Rodrigues.
|
||||
@param[out] tvecs0 Translation vectors for camera 0. Output size: NUM_FRAMES x 3 x 1. (may contain null Mat, if frame is not valid).
|
||||
@param[out] Ks Estimated floating-point camera intrinsic matrix. Output size: NUM_CAMERAS x 3 x 3.
|
||||
@param[out] distortions Distortion coefficients. Output size: NUM_CAMERAS x NUM_PARAMS.
|
||||
@param[out] perFrameErrors RMSE value for each visible frame, (-1 for non-visible). Output size: NUM_CAMERAS x NUM_FRAMES.
|
||||
@param[out] initializationPairs Pairs with camera indices that were used for initial pairwise stereo calibration.
|
||||
Output size: (NUM_CAMERAS-1) x 2.
|
||||
|
||||
Similarly to #calibrateCamera, the function minimizes the total re-projection error for all the
|
||||
points in all the available views from all cameras.
|
||||
|
||||
@return Overall RMS re-projection error over detectionMask.
|
||||
|
||||
@sa findChessboardCorners, findCirclesGrid, calibrateCamera, fisheye::calibrate
|
||||
*/
|
||||
|
||||
CV_EXPORTS_W double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::vector<Mat>> &imagePoints,
|
||||
const std::vector<Size> &imageSize, InputArray detectionMask,
|
||||
OutputArrayOfArrays Rs, OutputArrayOfArrays Ts, CV_IN_OUT std::vector<Mat> &Ks, CV_IN_OUT std::vector<Mat> &distortions,
|
||||
OutputArrayOfArrays rvecs0, OutputArrayOfArrays tvecs0, InputArray isFisheye,
|
||||
OutputArray perFrameErrors, OutputArray initializationPairs,
|
||||
bool useIntrinsicsGuess=false, InputArray flagsForIntrinsics=noArray());
|
||||
|
||||
|
||||
/** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$
|
||||
|
||||
@param[in] R_gripper2base Rotation part extracted from the homogeneous matrix that transforms a point
|
||||
|
||||
@ -561,8 +561,6 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
||||
// geodesic is not supported for normal callbacks
|
||||
solver.optimize();
|
||||
|
||||
//std::cout << "single camera calib. param after LM: " << param0.t() << "\n";
|
||||
|
||||
// If solver failed or last LM iteration was not successful,
|
||||
// then the last calculated perViewErr can be wrong & should be recalculated
|
||||
Mat JtErr, JtJ, JtJinv, JtJN;
|
||||
|
||||
848
modules/calib/src/multiview_calibration.cpp
Normal file
848
modules/calib/src/multiview_calibration.cpp
Normal file
@ -0,0 +1,848 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
#include "precomp.hpp"
|
||||
#include "opencv2/core/utils/logger.hpp"
|
||||
#include "opencv2/core/softfloat.hpp"
|
||||
|
||||
namespace cv {
|
||||
namespace multiview {
|
||||
class RobustFunction : public Algorithm {
|
||||
public:
|
||||
virtual float getError(float err) const = 0;
|
||||
};
|
||||
|
||||
#define USE_FAST_EXP 0
|
||||
|
||||
#if USE_FAST_EXP
|
||||
class RobustExpFunction : public RobustFunction {
|
||||
private:
|
||||
const float over_scale, pow_23 = 1 << 23;
|
||||
public:
|
||||
explicit RobustExpFunction (float scale_=30.0f) : over_scale(-1.442695040f /scale_) {}
|
||||
// err > 0
|
||||
float getError(float err) const override {
|
||||
const float under_exp = err * over_scale;
|
||||
if (under_exp < -20) return 0; // prevent overflow further
|
||||
// http://www.machinedlearnings.com/2011/06/fast-approximate-logarithm-exponential.html
|
||||
softfloat vexp = softfloat::fromRaw(static_cast<uint32_t>(pow_23 * (under_exp + 126.94269504f)));
|
||||
return float(vexp);
|
||||
}
|
||||
};
|
||||
#else
|
||||
class RobustExpFunction : public RobustFunction {
|
||||
private:
|
||||
const float minvScale;
|
||||
public:
|
||||
explicit RobustExpFunction (float scale_=30.0f) : minvScale(-1.f / scale_) {}
|
||||
// err > 0
|
||||
float getError(float err) const override
|
||||
{
|
||||
return exp(minvScale * err);
|
||||
}
|
||||
};
|
||||
#endif
|
||||
|
||||
static double robustWrapper (const Mat& ptsErrors, Mat& weights, const RobustFunction &fnc) {
|
||||
Mat errs;
|
||||
ptsErrors.convertTo(errs, CV_32F);
|
||||
weights.create((int)ptsErrors.total()*ptsErrors.channels(), 1, CV_32FC1);
|
||||
const Point2f * errs_ptr = errs.ptr<Point2f>();
|
||||
float * weights_ptr = weights.ptr<float>();
|
||||
double robust_sum_sqr_errs = 0.0;
|
||||
for (int pt = 0; pt < (int)errs.total(); pt++) {
|
||||
Point2f p = errs_ptr[pt];
|
||||
float sqr_err = p.dot(p);
|
||||
float w = fnc.getError(sqr_err);
|
||||
weights_ptr[pt*2 + 0] = w;
|
||||
weights_ptr[pt*2 + 1] = w;
|
||||
robust_sum_sqr_errs += w * sqr_err;
|
||||
}
|
||||
return robust_sum_sqr_errs;
|
||||
}
|
||||
|
||||
static double computeReprojectionMSE(const Mat &obj_points_, const Mat &img_points_, const Matx33d &K, const Mat &distortion,
|
||||
const Mat &rvec, const Mat &tvec, InputArray rvec2, InputArray tvec2, bool is_fisheye) {
|
||||
Mat r, t;
|
||||
if (!rvec2.empty() && !tvec2.empty()) {
|
||||
composeRT(rvec, tvec, rvec2, tvec2, r, t);
|
||||
} else {
|
||||
r = rvec; t = tvec;
|
||||
}
|
||||
Mat tmpImagePoints, obj_points = obj_points_, img_points = img_points_;
|
||||
if (is_fisheye) {
|
||||
obj_points = obj_points.reshape(3); // must be 3 channels
|
||||
fisheye::projectPoints(obj_points, tmpImagePoints, r, t, K, distortion);
|
||||
} else
|
||||
projectPoints(obj_points, r, t, K, distortion, tmpImagePoints);
|
||||
if (img_points.channels() != tmpImagePoints.channels())
|
||||
img_points = img_points.reshape(tmpImagePoints.channels());
|
||||
if (img_points.rows != tmpImagePoints.rows)
|
||||
img_points = img_points.t();
|
||||
subtract (tmpImagePoints, img_points, tmpImagePoints);
|
||||
return norm(tmpImagePoints, NORM_L2SQR) / tmpImagePoints.rows;
|
||||
}
|
||||
|
||||
static bool maximumSpanningTree (int NUM_CAMERAS, int NUM_FRAMES, const std::vector<std::vector<bool>> &detection_mask,
|
||||
std::vector<int> &parent, std::vector<std::vector<int>> &overlap,
|
||||
std::vector<std::vector<Vec3d>> &opt_axes,
|
||||
const std::vector<std::vector<bool>> &is_valid_angle2pattern,
|
||||
const std::vector<std::vector<float>> &points_area_ratio,
|
||||
double WEIGHT_ANGLE_PATTERN, double WEIGHT_CAMERAS_ANGLES) {
|
||||
const double THR_CAMERAS_ANGLES = 160*M_PI/180;
|
||||
// build weights matrix
|
||||
overlap = std::vector<std::vector<int>>(NUM_CAMERAS, std::vector<int>(NUM_CAMERAS, 0));
|
||||
std::vector<std::vector<double>> weights(NUM_CAMERAS, std::vector<double>(NUM_CAMERAS, DBL_MIN));
|
||||
for (int c1 = 0; c1 < NUM_CAMERAS; c1++) {
|
||||
for (int c2 = c1+1; c2 < NUM_CAMERAS; c2++) {
|
||||
double weight = 0;
|
||||
int overlaps = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask[c1][f] && detection_mask[c2][f]) {
|
||||
overlaps += 1;
|
||||
weight += points_area_ratio[c1][f] + points_area_ratio[c2][f];
|
||||
weight += WEIGHT_ANGLE_PATTERN * ((int)is_valid_angle2pattern[c1][f] + (int)is_valid_angle2pattern[c2][f]);
|
||||
if (WEIGHT_CAMERAS_ANGLES > 0) {
|
||||
// angle between cameras optical axes
|
||||
weight += WEIGHT_CAMERAS_ANGLES * int(acos(opt_axes[c1][f].dot(opt_axes[c2][f])) < THR_CAMERAS_ANGLES);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (overlaps > 0) {
|
||||
overlap[c1][c2] = overlap[c2][c1] = overlaps;
|
||||
weights[c1][c2] = weights[c2][c1] = overlaps + weight;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// find maximum spanning tree using Prim's algorithm
|
||||
std::vector<bool> visited(NUM_CAMERAS, false);
|
||||
std::vector<double> weight(NUM_CAMERAS, DBL_MIN);
|
||||
parent = std::vector<int>(NUM_CAMERAS, -1);
|
||||
weight[0] = DBL_MAX;
|
||||
for (int cam = 0; cam < NUM_CAMERAS-1; cam++) {
|
||||
int max_weight_idx = -1;
|
||||
auto max_weight = DBL_MIN;
|
||||
for (int cam2 = 0; cam2 < NUM_CAMERAS; cam2++) {
|
||||
if (!visited[cam2] && max_weight < weight[cam2]) {
|
||||
max_weight = weight[cam2];
|
||||
max_weight_idx = cam2;
|
||||
}
|
||||
}
|
||||
if (max_weight_idx == -1)
|
||||
return false;
|
||||
visited[max_weight_idx] = true;
|
||||
for (int cam2 = 0; cam2 < NUM_CAMERAS; cam2++) {
|
||||
if (!visited[cam2] && overlap[max_weight_idx][cam2] > 0) {
|
||||
if (weight[cam2] < weights[max_weight_idx][cam2]) {
|
||||
weight[cam2] = weights[max_weight_idx][cam2];
|
||||
parent[cam2] = max_weight_idx;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void imagePointsArea (const std::vector<Size> &imageSize, const std::vector<std::vector<bool>> &detection_mask_mat,
|
||||
const std::vector<std::vector<Mat>> &imagePoints, std::vector<std::vector<float>> &points_ratio_area) {
|
||||
const int NUM_CAMERAS = (int) imageSize.size(), NUM_FRAMES = (int)detection_mask_mat[0].size();
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
const auto img_area = (float)(imageSize[c].width * imageSize[c].height);
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!detection_mask_mat[c][f])
|
||||
continue;
|
||||
CV_Assert((imagePoints[c][f].type() == CV_32F && imagePoints[c][f].cols == 2) || imagePoints[c][f].type() == CV_32FC2);
|
||||
std::vector<int> hull;
|
||||
const auto img_pts = imagePoints[c][f];
|
||||
const auto * const image_pts_ptr = (float *) img_pts.data;
|
||||
convexHull(img_pts, hull, true/*has to be clockwise*/, false/*indices*/);
|
||||
float area = 0;
|
||||
int j = hull.back();
|
||||
// http://alienryderflex.com/polygon_area/
|
||||
for (int i : hull) {
|
||||
area += (image_pts_ptr[j*2] + image_pts_ptr[i*2])*(image_pts_ptr[j*2+1] - image_pts_ptr[i*2+1]);
|
||||
j = i;
|
||||
}
|
||||
points_ratio_area[c][f] = area*.5f / img_area;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void selectPairsBFS (std::vector<std::pair<int,int>> &pairs, int NUM_CAMERAS, const std::vector<int> &parent) {
|
||||
// find pairs using Breadth First Search graph traversing
|
||||
// it is important to keep this order of pairs, since it is easier
|
||||
// to find relative views wrt to 0-th camera.
|
||||
std::vector<int> nodes = {0};
|
||||
pairs.reserve(NUM_CAMERAS-1);
|
||||
while (!nodes.empty()) {
|
||||
std::vector<int> new_nodes;
|
||||
for (int n : nodes) {
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
if (parent[c] == n) {
|
||||
pairs.emplace_back(std::make_pair(n, c));
|
||||
new_nodes.emplace_back(c);
|
||||
}
|
||||
}
|
||||
}
|
||||
nodes = new_nodes;
|
||||
}
|
||||
}
|
||||
|
||||
static double getScaleOfObjPoints (int NUM_PATTERN_PTS, const Mat &obj_pts, bool obj_points_in_rows) {
|
||||
double scale_3d_pts = 0.0;
|
||||
// compute scale of 3D points as the maximum pairwise distance
|
||||
for (int i = 0; i < NUM_PATTERN_PTS; i++) {
|
||||
for (int j = i+1; j < NUM_PATTERN_PTS; j++) {
|
||||
double dist;
|
||||
if (obj_points_in_rows) {
|
||||
dist = norm(obj_pts.row(i)-obj_pts.row(j), NORM_L2SQR);
|
||||
} else {
|
||||
dist = norm(obj_pts.col(i)-obj_pts.col(j), NORM_L2SQR);
|
||||
}
|
||||
if (scale_3d_pts < dist) {
|
||||
scale_3d_pts = dist;
|
||||
}
|
||||
}
|
||||
}
|
||||
return scale_3d_pts;
|
||||
}
|
||||
|
||||
static void thresholdPatternCameraAngles (int NUM_PATTERN_PTS, double THR_PATTERN_CAMERA_ANGLES,
|
||||
const std::vector<Mat> &objPoints_norm, const std::vector<std::vector<Vec3d>> &rvecs_all,
|
||||
std::vector<std::vector<Vec3d>> &opt_axes, std::vector<std::vector<bool>> &is_valid_angle2pattern) {
|
||||
const int NUM_FRAMES = (int)objPoints_norm.size(), NUM_CAMERAS = (int)rvecs_all.size();
|
||||
is_valid_angle2pattern = std::vector<std::vector<bool>>(NUM_CAMERAS, std::vector<bool>(NUM_FRAMES, true));
|
||||
int pattern1 = -1, pattern2 = -1, pattern3 = -1;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
double norm_normal = 0;
|
||||
if (pattern1 == -1) {
|
||||
// take non colinear 3 points and save them
|
||||
for (int p1 = 0; p1 < NUM_PATTERN_PTS; p1++) {
|
||||
for (int p2 = p1+1; p2 < NUM_PATTERN_PTS; p2++) {
|
||||
for (int p3 = NUM_PATTERN_PTS-1; p3 > p2; p3--) { // start from the last point
|
||||
Mat pattern_normal = (objPoints_norm[f].row(p2)-objPoints_norm[f].row(p1))
|
||||
.cross(objPoints_norm[f].row(p3)-objPoints_norm[f].row(p1));
|
||||
norm_normal = norm(pattern_normal, NORM_L2SQR);
|
||||
if (norm_normal > 1e-6) {
|
||||
pattern1 = p1;
|
||||
pattern2 = p2;
|
||||
pattern3 = p3;
|
||||
norm_normal = sqrt(norm_normal);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (pattern1 != -1) break;
|
||||
}
|
||||
if (pattern1 != -1) break;
|
||||
}
|
||||
if (pattern1 == -1) {
|
||||
CV_Error(Error::StsBadArg, "Pattern points are collinear!");
|
||||
}
|
||||
}
|
||||
Vec3d pattern_normal = (objPoints_norm[f].row(pattern2)-objPoints_norm[f].row(pattern1)).
|
||||
cross(objPoints_norm[f].row(pattern3)-objPoints_norm[f].row(pattern1));
|
||||
norm_normal = norm(pattern_normal);
|
||||
pattern_normal /= norm_normal;
|
||||
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
Matx33d R;
|
||||
Rodrigues(rvecs_all[c][f], R);
|
||||
opt_axes[c][f] = Vec3d(Mat(R.row(2)));
|
||||
const double angle = acos(opt_axes[c][f].dot(pattern_normal));
|
||||
is_valid_angle2pattern[c][f] = min(M_PI-angle, angle) < THR_PATTERN_CAMERA_ANGLES;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void pairwiseStereoCalibration (const std::vector<std::pair<int,int>> &pairs,
|
||||
const std::vector<bool> &is_fisheye_vec, const std::vector<Mat> &objPoints_norm,
|
||||
const std::vector<std::vector<Mat>> &imagePoints, const std::vector<std::vector<int>> &overlaps,
|
||||
const std::vector<std::vector<bool>> &detection_mask_mat, const std::vector<Mat> &Ks,
|
||||
const std::vector<Mat> &distortions, std::vector<Matx33d> &Rs_vec, std::vector<Vec3d> &Ts_vec,
|
||||
Mat &flags) {
|
||||
const int NUM_FRAMES = (int) objPoints_norm.size();
|
||||
for (const auto &pair : pairs) {
|
||||
const int c1 = pair.first, c2 = pair.second, overlap = overlaps[c1][c2];
|
||||
// prepare image points of two cameras and grid points
|
||||
std::vector<Mat> image_points1, image_points2, grid_points;
|
||||
grid_points.reserve(overlap);
|
||||
image_points1.reserve(overlap);
|
||||
image_points2.reserve(overlap);
|
||||
const bool are_fisheye_cams = is_fisheye_vec[c1] && is_fisheye_vec[c2];
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[c1][f] && detection_mask_mat[c2][f]) {
|
||||
grid_points.emplace_back((are_fisheye_cams && objPoints_norm[f].channels() != 3) ?
|
||||
objPoints_norm[f].reshape(3): objPoints_norm[f]);
|
||||
image_points1.emplace_back((are_fisheye_cams && imagePoints[c1][f].channels() != 2) ?
|
||||
imagePoints[c1][f].reshape(2) : imagePoints[c1][f]);
|
||||
image_points2.emplace_back((are_fisheye_cams && imagePoints[c2][f].channels() != 2) ?
|
||||
imagePoints[c2][f].reshape(2) : imagePoints[c2][f]);
|
||||
}
|
||||
}
|
||||
Matx33d R;
|
||||
Vec3d T;
|
||||
// image size does not matter since intrinsics are used
|
||||
if (are_fisheye_cams) {
|
||||
fisheye::stereoCalibrate(grid_points, image_points1, image_points2,
|
||||
Ks[c1], distortions[c1],
|
||||
Ks[c2], distortions[c2],
|
||||
Size(), R, T, CALIB_FIX_INTRINSIC);
|
||||
} else {
|
||||
int flags_extrinsics = CALIB_FIX_INTRINSIC;
|
||||
if ((flags.at<int>(c1) & CALIB_RATIONAL_MODEL) || (flags.at<int>(c2) & CALIB_RATIONAL_MODEL))
|
||||
flags_extrinsics += CALIB_RATIONAL_MODEL;
|
||||
if ((flags.at<int>(c1) & CALIB_THIN_PRISM_MODEL) || (flags.at<int>(c2) & CALIB_THIN_PRISM_MODEL))
|
||||
flags_extrinsics += CALIB_THIN_PRISM_MODEL;
|
||||
|
||||
stereoCalibrate(grid_points, image_points1, image_points2,
|
||||
Ks[c1], distortions[c1],
|
||||
Ks[c2], distortions[c2],
|
||||
Size(), R, T, noArray(), noArray(), noArray(), flags_extrinsics);
|
||||
}
|
||||
|
||||
// R_0 = I
|
||||
// R_ij = R_i R_j^T => R_i = R_ij R_j
|
||||
// t_ij = ti - R_ij tj => t_i = t_ij + R_ij t_j
|
||||
if (c1 == 0) {
|
||||
Rs_vec[c2] = R;
|
||||
Ts_vec[c2] = T;
|
||||
} else {
|
||||
Rs_vec[c2] = Matx33d(Mat(R * Rs_vec[c1]));
|
||||
Ts_vec[c2] = Vec3d(Mat(T + R * Ts_vec[c1]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void optimizeLM (std::vector<double> ¶m, const RobustFunction &robust_fnc, const TermCriteria &termCrit,
|
||||
const std::vector<bool> &valid_frames, const std::vector<std::vector<bool>> &detection_mask_mat,
|
||||
const std::vector<Mat> &objPoints_norm, const std::vector<std::vector<Mat>> &imagePoints,
|
||||
const std::vector<Mat> &Ks, const std::vector<Mat> &distortions,
|
||||
const std::vector<bool> &is_fisheye_vec, int NUM_PATTERN_PTS) {
|
||||
const int NUM_FRAMES = (int) objPoints_norm.size(), NUM_CAMERAS = (int)detection_mask_mat.size();
|
||||
int iters_lm = 0, cnt_valid_frame = 0;
|
||||
auto lmcallback = [&](InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm) {
|
||||
auto * param_p = _param.getMat().ptr<double>();
|
||||
errnorm = 0;
|
||||
cnt_valid_frame = 0;
|
||||
for (int i = 0; i < NUM_FRAMES; i++ ) {
|
||||
if (!valid_frames[i]) continue;
|
||||
for (int k = 1; k < NUM_CAMERAS; k++ ) { // skip first camera as there is nothing to optimize
|
||||
if (!detection_mask_mat[k][i]) continue;
|
||||
const int cam_idx = (k-1)*6; // camera extrinsics
|
||||
const auto * const pose_k = param_p + cam_idx;
|
||||
Vec3d om_0ToK(pose_k[0], pose_k[1], pose_k[2]), om[2];
|
||||
Vec3d T_0ToK(pose_k[3], pose_k[4], pose_k[5]), T[2];
|
||||
Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2;
|
||||
|
||||
auto * pi = param_p + (cnt_valid_frame+NUM_CAMERAS-1)*6; // get rvecs / tvecs for frame pose
|
||||
om[0] = Vec3d(pi[0], pi[1], pi[2]);
|
||||
T[0] = Vec3d(pi[3], pi[4], pi[5]);
|
||||
|
||||
if( JtJ_.needed() || JtErr_.needed() )
|
||||
composeRT( om[0], T[0], om_0ToK, T_0ToK, om[1], T[1], dr3dr1, noArray(),
|
||||
dr3dr2, noArray(), noArray(), dt3dt1, dt3dr2, dt3dt2 );
|
||||
else
|
||||
composeRT( om[0], T[0], om_0ToK, T_0ToK, om[1], T[1] );
|
||||
|
||||
// get object points
|
||||
Mat objpt_i = objPoints_norm[i].reshape(3, 1);
|
||||
objpt_i.convertTo(objpt_i, CV_64FC3);
|
||||
|
||||
Mat err( NUM_PATTERN_PTS*2, 1, CV_64F ), tmpImagePoints = err.reshape(2, 1);
|
||||
Mat Je( NUM_PATTERN_PTS*2, 6, CV_64F ), J_0ToK( NUM_PATTERN_PTS*2, 6, CV_64F );
|
||||
Mat dpdrot = Je.colRange(0, 3), dpdt = Je.colRange(3, 6); // num_points*2 x 3 each
|
||||
// get image points
|
||||
Mat imgpt_ik = imagePoints[k][i].reshape(2, 1);
|
||||
imgpt_ik.convertTo(imgpt_ik, CV_64FC2);
|
||||
|
||||
if (is_fisheye_vec[k]) {
|
||||
if( JtJ_.needed() || JtErr_.needed() ) {
|
||||
Mat jacobian; // of size num_points*2 x 15 (2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T)
|
||||
fisheye::projectPoints(objpt_i, tmpImagePoints, om[1], T[1], Ks[k], distortions[k], 0, jacobian);
|
||||
jacobian.colRange(8,11).copyTo(dpdrot);
|
||||
jacobian.colRange(11,14).copyTo(dpdt);
|
||||
} else
|
||||
fisheye::projectPoints(objpt_i, tmpImagePoints, om[1], T[1], Ks[k], distortions[k]);
|
||||
} else {
|
||||
if( JtJ_.needed() || JtErr_.needed() )
|
||||
projectPoints(objpt_i, om[1], T[1], Ks[k], distortions[k],
|
||||
tmpImagePoints, dpdrot, dpdt, noArray(), noArray(), noArray(), noArray());
|
||||
else
|
||||
projectPoints(objpt_i, om[1], T[1], Ks[k], distortions[k], tmpImagePoints);
|
||||
}
|
||||
subtract( tmpImagePoints, imgpt_ik, tmpImagePoints);
|
||||
Mat weights;
|
||||
const double robust_l2_norm = multiview::robustWrapper(tmpImagePoints, weights, robust_fnc);
|
||||
errnorm += robust_l2_norm;
|
||||
|
||||
if (JtJ_.needed()) {
|
||||
Mat JtErr = JtErr_.getMat(), JtJ = JtJ_.getMat();
|
||||
const int eofs = (cnt_valid_frame+NUM_CAMERAS-1)*6;
|
||||
assert( JtJ_.needed() && JtErr_.needed() );
|
||||
// JtJ : NUM_PARAMS x NUM_PARAMS, JtErr : NUM_PARAMS x 1
|
||||
|
||||
// k is always greater than zero
|
||||
// d(err_{x|y}R) ~ de3
|
||||
// convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
|
||||
for (int p = 0; p < NUM_PATTERN_PTS * 2; p++)
|
||||
{
|
||||
Matx13d de3dr3, de3dt3, de3dr2, de3dt2, de3dr1, de3dt1;
|
||||
for (int j = 0; j < 3; j++)
|
||||
de3dr3(j) = Je.at<double>(p, j);
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
de3dt3(j) = Je.at<double>(p, 3 + j);
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
de3dr2(j) = J_0ToK.at<double>(p, j);
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
de3dt2(j) = J_0ToK.at<double>(p, 3 + j);
|
||||
|
||||
de3dr1 = de3dr3 * dr3dr1;
|
||||
de3dt1 = de3dt3 * dt3dt1;
|
||||
de3dr2 = de3dr3 * dr3dr2 + de3dt3 * dt3dr2;
|
||||
de3dt2 = de3dt3 * dt3dt2;
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
Je.at<double>(p, j) = de3dr1(j);
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
Je.at<double>(p, 3 + j) = de3dt1(j);
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
J_0ToK.at<double>(p, j) = de3dr2(j);
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
J_0ToK.at<double>(p, 3 + j) = de3dt2(j);
|
||||
}
|
||||
|
||||
Mat wd;
|
||||
Mat::diag(weights).convertTo(wd, CV_64F);
|
||||
// 6 x (ni*2) * (ni*2 x ni*2) * (ni*2) x 6
|
||||
JtJ(Rect((k - 1) * 6, (k - 1) * 6, 6, 6)) += (J_0ToK.t() * wd * J_0ToK);
|
||||
JtJ(Rect(eofs, (k - 1) * 6, 6, 6)) = (J_0ToK.t() * wd * Je);
|
||||
JtErr.rowRange((k - 1) * 6, (k - 1) * 6 + 6) += (J_0ToK.t() * wd * err);
|
||||
|
||||
JtJ(Rect(eofs, eofs, 6, 6)) += Je.t() * wd * Je;
|
||||
JtErr.rowRange(eofs, eofs + 6) += Je.t() * wd * err;
|
||||
}
|
||||
}
|
||||
cnt_valid_frame++;
|
||||
}
|
||||
iters_lm += 1;
|
||||
return true;
|
||||
};
|
||||
LevMarq solver(param, lmcallback,
|
||||
LevMarq::Settings()
|
||||
.setMaxIterations(termCrit.maxCount)
|
||||
.setStepNormTolerance(termCrit.epsilon)
|
||||
.setSmallEnergyTolerance(termCrit.epsilon * termCrit.epsilon),
|
||||
noArray()/*mask, all variables to optimize*/);
|
||||
solver.optimize();
|
||||
}
|
||||
|
||||
static void checkConnected (const std::vector<std::vector<bool>> &detection_mask_mat) {
|
||||
const int NUM_CAMERAS = (int)detection_mask_mat.size(), NUM_FRAMES = (int)detection_mask_mat[0].size();
|
||||
std::vector<bool> visited(NUM_CAMERAS, false);
|
||||
std::function<void(int)> dfs_search;
|
||||
dfs_search = [&] (int cam) {
|
||||
visited[cam] = true;
|
||||
for (int cam2 = 0; cam2 < NUM_CAMERAS; cam2++) {
|
||||
if (!visited[cam2]) {
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[cam][f] && detection_mask_mat[cam2][f]) {
|
||||
dfs_search(cam2);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
dfs_search(0);
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
if (! visited[c]) {
|
||||
std::string isolated_cameras = "", visited_str = "";
|
||||
for (int i = 0; i < NUM_CAMERAS; i++) {
|
||||
if (!visited_str[i]) {
|
||||
if (isolated_cameras != "")
|
||||
isolated_cameras += ", ";
|
||||
isolated_cameras += std::to_string(i);
|
||||
} else {
|
||||
if (visited_str != "")
|
||||
visited_str += ", ";
|
||||
visited_str += std::to_string(i);
|
||||
}
|
||||
}
|
||||
CV_Error(Error::StsBadArg, "Isolated cameras (or components) "+isolated_cameras+" from the connected component "+visited_str+"!");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//TODO: use Input/OutputArrays for imagePoints, imageSize(?), Ks, distortions
|
||||
double calibrateMultiview (InputArrayOfArrays objPoints, const std::vector<std::vector<Mat>> &imagePoints,
|
||||
const std::vector<Size> &imageSize, InputArray detectionMask,
|
||||
OutputArrayOfArrays Rs, OutputArrayOfArrays Ts, std::vector<Mat> &Ks, std::vector<Mat> &distortions,
|
||||
OutputArrayOfArrays rvecs0, OutputArrayOfArrays tvecs0, InputArray isFisheye,
|
||||
OutputArray perFrameErrors, OutputArray initializationPairs, bool useIntrinsicsGuess, InputArray flagsForIntrinsics) {
|
||||
|
||||
CV_CheckEQ((int)objPoints.empty(), 0, "Objects points must not be empty!");
|
||||
CV_CheckEQ((int)imagePoints.empty(), 0, "Image points must not be empty!");
|
||||
CV_CheckEQ((int)imageSize.empty(), 0, "Image size per camera must not be empty!");
|
||||
CV_CheckEQ((int)detectionMask.empty(), 0, "detectionMask matrix must not be empty!");
|
||||
CV_CheckEQ((int)isFisheye.empty(), 0, "Fisheye mask must not be empty!");
|
||||
|
||||
Mat detection_mask_ = detectionMask.getMat(), is_fisheye_mat = isFisheye.getMat();
|
||||
CV_CheckEQ(detection_mask_.type(), CV_8U, "detectionMask must be of type CV_8U");
|
||||
CV_CheckEQ(is_fisheye_mat.type(), CV_8U, "isFisheye must be of type CV_8U");
|
||||
|
||||
bool is_fisheye = false;
|
||||
bool is_pinhole = false;
|
||||
|
||||
for (int i = 0; i < (int)is_fisheye_mat.total(); i++) {
|
||||
if (is_fisheye_mat.at<uchar>(i)) {
|
||||
is_fisheye = true;
|
||||
} else {
|
||||
is_pinhole = true;
|
||||
}
|
||||
}
|
||||
CV_CheckEQ(is_fisheye && is_pinhole, false, "Mix of pinhole and fisheye cameras is not supported for now");
|
||||
|
||||
// equal number of cameras
|
||||
CV_Assert(imageSize.size() == imagePoints.size());
|
||||
CV_Assert(detection_mask_.rows == std::max(isFisheye.rows(), isFisheye.cols()));
|
||||
CV_Assert(detection_mask_.rows == (int)imageSize.size());
|
||||
CV_Assert(detection_mask_.cols == std::max(objPoints.rows(), objPoints.cols())); // equal number of frames
|
||||
CV_Assert(Rs.isMatVector() == Ts.isMatVector());
|
||||
if (useIntrinsicsGuess) {
|
||||
CV_Assert(Ks.size() == distortions.size() && Ks.size() == imageSize.size());
|
||||
}
|
||||
// normalize object points
|
||||
const Mat obj_pts_0 = objPoints.getMat(0);
|
||||
CV_Assert((obj_pts_0.type() == CV_32F && (obj_pts_0.rows == 3 || obj_pts_0.cols == 3)) ||
|
||||
(obj_pts_0.type() == CV_32FC3 && (obj_pts_0.rows == 1 || obj_pts_0.cols == 1)));
|
||||
const bool obj_points_in_rows = obj_pts_0.cols == 3;
|
||||
const int NUM_CAMERAS = (int)detection_mask_.rows, NUM_FRAMES = (int)detection_mask_.cols;
|
||||
CV_Assert((NUM_CAMERAS > 1) && (NUM_FRAMES > 0));
|
||||
const int NUM_PATTERN_PTS = obj_points_in_rows ? obj_pts_0.rows : obj_pts_0.cols;
|
||||
const double scale_3d_pts = multiview::getScaleOfObjPoints(NUM_PATTERN_PTS, obj_pts_0, obj_points_in_rows);
|
||||
|
||||
Mat flagsForIntrinsics_mat = flagsForIntrinsics.getMat();
|
||||
if (flagsForIntrinsics_mat.empty())
|
||||
{
|
||||
flagsForIntrinsics_mat = Mat(Size(1, NUM_CAMERAS), CV_32SC1, cv::Scalar(0));
|
||||
}
|
||||
|
||||
CV_Assert(flagsForIntrinsics_mat.total() == size_t(NUM_CAMERAS));
|
||||
CV_CheckEQ(flagsForIntrinsics_mat.type(), CV_32S, "flagsForIntrinsics should be of type 32SC1");
|
||||
CV_CheckEQ(flagsForIntrinsics_mat.channels(), 1, "flagsForIntrinsics should be of type 32SC1");
|
||||
|
||||
std::vector<Mat> objPoints_norm;
|
||||
objPoints_norm.reserve(NUM_FRAMES);
|
||||
for (int i = 0; i < NUM_FRAMES; i++) {
|
||||
if (obj_points_in_rows)
|
||||
objPoints_norm.emplace_back(objPoints.getMat(i)*(1/scale_3d_pts));
|
||||
else
|
||||
objPoints_norm.emplace_back(objPoints.getMat(i).t()*(1/scale_3d_pts));
|
||||
objPoints_norm[i] = objPoints_norm[i].reshape(1);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////
|
||||
std::vector<int> num_visible_frames_per_camera(NUM_CAMERAS);
|
||||
std::vector<bool> valid_frames(NUM_FRAMES, false);
|
||||
// process input and count all visible frames and points
|
||||
|
||||
std::vector<bool> is_fisheye_vec(NUM_CAMERAS);
|
||||
std::vector<std::vector<bool>> detection_mask_mat(NUM_CAMERAS, std::vector<bool>(NUM_FRAMES));
|
||||
const auto * const detection_mask_ptr = detection_mask_.data, * const is_fisheye_ptr = is_fisheye_mat.data;
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
is_fisheye_vec[c] = is_fisheye_ptr[c] != 0;
|
||||
int num_visible_frames = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
detection_mask_mat[c][f] = detection_mask_ptr[c*NUM_FRAMES + f] != 0;
|
||||
if (detection_mask_mat[c][f]) {
|
||||
num_visible_frames++;
|
||||
valid_frames[f] = true; // if frame is visible by at least one camera then count is as a valid one
|
||||
}
|
||||
}
|
||||
if (num_visible_frames == 0) {
|
||||
CV_Error(Error::StsBadArg, "camera "+std::to_string(c)+" has no visible frames!");
|
||||
}
|
||||
num_visible_frames_per_camera[c] = num_visible_frames;
|
||||
}
|
||||
|
||||
multiview::checkConnected(detection_mask_mat);
|
||||
|
||||
std::vector<std::vector<float>> points_ratio_area(NUM_CAMERAS, std::vector<float>(NUM_FRAMES));
|
||||
multiview::imagePointsArea(imageSize, detection_mask_mat, imagePoints, points_ratio_area);
|
||||
|
||||
// constant threshold for angle between two camera axes in radians (=160*M_PI/180).
|
||||
// if angle exceeds this threshold then a weight of a camera pair is lowered.
|
||||
const double THR_PATTERN_CAMERA_ANGLES = 160*M_PI/180;
|
||||
std::vector<std::vector<Vec3d>> rvecs_all(NUM_CAMERAS, std::vector<Vec3d>(NUM_FRAMES)),
|
||||
tvecs_all(NUM_CAMERAS, std::vector<Vec3d>(NUM_FRAMES)),
|
||||
opt_axes(NUM_CAMERAS, std::vector<Vec3d>(NUM_FRAMES));
|
||||
|
||||
std::vector<int> camera_rt_best(NUM_FRAMES, -1);
|
||||
std::vector<double> camera_rt_errors(NUM_FRAMES, std::numeric_limits<double>::max());
|
||||
const double WARNING_RMSE = 15.;
|
||||
if (!useIntrinsicsGuess) {
|
||||
// calibrate each camera independently to find intrinsic parameters - K and distortion coefficients
|
||||
distortions = std::vector<Mat>(NUM_CAMERAS);
|
||||
Ks = std::vector<Mat>(NUM_CAMERAS);
|
||||
for (int camera = 0; camera < NUM_CAMERAS; camera++) {
|
||||
Mat rvecs, tvecs;
|
||||
std::vector<Mat> obj_points_, img_points_;
|
||||
std::vector<double> errors_per_view;
|
||||
obj_points_.reserve(num_visible_frames_per_camera[camera]);
|
||||
img_points_.reserve(num_visible_frames_per_camera[camera]);
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[camera][f]) {
|
||||
obj_points_.emplace_back((is_fisheye_vec[camera] && objPoints_norm[f].channels() != 3) ?
|
||||
objPoints_norm[f].reshape(3): objPoints_norm[f]);
|
||||
img_points_.emplace_back((is_fisheye_vec[camera] && imagePoints[camera][f].channels() != 2) ?
|
||||
imagePoints[camera][f].reshape(2) : imagePoints[camera][f]);
|
||||
}
|
||||
}
|
||||
double repr_err;
|
||||
if (is_fisheye_vec[camera]) {
|
||||
repr_err = fisheye::calibrate(obj_points_, img_points_, imageSize[camera],
|
||||
Ks[camera], distortions[camera], rvecs, tvecs, flagsForIntrinsics_mat.at<int>(camera));
|
||||
// calibrate does not compute error per view, so compute it manually
|
||||
errors_per_view = std::vector<double>(obj_points_.size());
|
||||
for (int f = 0; f < (int) obj_points_.size(); f++) {
|
||||
double err2 = multiview::computeReprojectionMSE(obj_points_[f],
|
||||
img_points_[f], Ks[camera], distortions[camera], rvecs.row(f), tvecs.row(f), noArray(), noArray(), true);
|
||||
errors_per_view[f] = sqrt(err2);
|
||||
}
|
||||
} else {
|
||||
repr_err = calibrateCamera(obj_points_, img_points_, imageSize[camera], Ks[camera], distortions[camera],
|
||||
rvecs, tvecs, noArray(), noArray(), errors_per_view, flagsForIntrinsics_mat.at<int>(camera));
|
||||
}
|
||||
CV_LOG_IF_WARNING(NULL, repr_err > WARNING_RMSE, "Warning! Mean RMSE of intrinsics calibration is higher than "+std::to_string(WARNING_RMSE)+" pixels!");
|
||||
int cnt_visible_frame = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[camera][f]) {
|
||||
rvecs_all[camera][f] = Vec3d(Mat(3, 1, CV_64F, rvecs.row(cnt_visible_frame).data));
|
||||
tvecs_all[camera][f] = Vec3d(Mat(3, 1, CV_64F, tvecs.row(cnt_visible_frame).data));
|
||||
double err = errors_per_view[cnt_visible_frame];
|
||||
double err2 = err * err;
|
||||
if (camera_rt_errors[f] > err2) {
|
||||
camera_rt_errors[f] = err2;
|
||||
camera_rt_best[f] = camera;
|
||||
}
|
||||
cnt_visible_frame++;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// use PnP to compute rvecs and tvecs
|
||||
for (int i = 0; i < NUM_FRAMES; i++) {
|
||||
for (int k = 0; k < NUM_CAMERAS; k++) {
|
||||
if (!detection_mask_mat[k][i]) continue;
|
||||
Vec3d rvec, tvec;
|
||||
solvePnP(objPoints_norm[i], imagePoints[k][i], Ks[k], distortions[k], rvec, tvec, false, SOLVEPNP_ITERATIVE);
|
||||
rvecs_all[k][i] = rvec;
|
||||
tvecs_all[k][i] = tvec;
|
||||
const double err2 = multiview::computeReprojectionMSE(objPoints_norm[i], imagePoints[k][i], Ks[k], distortions[k], Mat(rvec), Mat(tvec), noArray(), noArray(), is_fisheye_vec[k]);
|
||||
if (camera_rt_errors[i] > err2) {
|
||||
camera_rt_errors[i] = err2;
|
||||
camera_rt_best[i] = k;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::vector<bool>> is_valid_angle2pattern;
|
||||
multiview::thresholdPatternCameraAngles(NUM_PATTERN_PTS, THR_PATTERN_CAMERA_ANGLES, objPoints_norm, rvecs_all, opt_axes, is_valid_angle2pattern);
|
||||
|
||||
std::vector<Matx33d> Rs_vec(NUM_CAMERAS);
|
||||
std::vector<Vec3d> Ts_vec(NUM_CAMERAS);
|
||||
Rs_vec[0] = Matx33d ::eye();
|
||||
Ts_vec[0] = Vec3d::zeros();
|
||||
|
||||
std::vector<int> parent;
|
||||
std::vector<std::vector<int>> overlaps;
|
||||
if (! multiview::maximumSpanningTree(NUM_CAMERAS, NUM_FRAMES, detection_mask_mat, parent, overlaps, opt_axes,
|
||||
is_valid_angle2pattern, points_ratio_area, .5, 1.0)) {
|
||||
// failed to find suitable pairs with constraints!
|
||||
CV_Error(Error::StsInternal, "Failed to build tree for stereo calibration.");
|
||||
}
|
||||
|
||||
std::vector<std::pair<int,int>> pairs;
|
||||
multiview::selectPairsBFS (pairs, NUM_CAMERAS, parent);
|
||||
|
||||
if ((int)pairs.size() != NUM_CAMERAS-1) {
|
||||
CV_Error(Error::StsInternal, "Failed to build tree for stereo calibration. Incorrect number of pairs.");
|
||||
}
|
||||
if (initializationPairs.needed()) {
|
||||
Mat pairs_mat = Mat_<int>(NUM_CAMERAS-1, 2);
|
||||
auto * pairs_ptr = (int *) pairs_mat.data;
|
||||
for (const auto &p : pairs) {
|
||||
(*pairs_ptr++) = p.first;
|
||||
(*pairs_ptr++) = p.second;
|
||||
}
|
||||
pairs_mat.copyTo(initializationPairs);
|
||||
}
|
||||
multiview::pairwiseStereoCalibration(pairs, is_fisheye_vec, objPoints_norm, imagePoints,
|
||||
overlaps, detection_mask_mat, Ks, distortions, Rs_vec, Ts_vec, flagsForIntrinsics_mat);
|
||||
|
||||
const int NUM_VALID_FRAMES = countNonZero(valid_frames);
|
||||
const int nparams = (NUM_VALID_FRAMES + NUM_CAMERAS - 1) * 6; // rvecs + tvecs (6)
|
||||
std::vector<double> param(nparams, 0.);
|
||||
|
||||
// use found relative extrinsics to initialize parameters
|
||||
for (int c = 1; c < NUM_CAMERAS; c++) {
|
||||
Vec3d rvec;
|
||||
Rodrigues(Rs_vec[c], rvec);
|
||||
memcpy(¶m[0]+(c-1)*6 , rvec.val, 3*sizeof(double));
|
||||
memcpy(¶m[0]+(c-1)*6+3, Ts_vec[c].val, 3*sizeof(double));
|
||||
}
|
||||
|
||||
// use found rvecs / tvecs or estimate them to initialize rest of parameters
|
||||
int cnt_valid_frame = 0;
|
||||
for (int i = 0; i < NUM_FRAMES; i++ ) {
|
||||
if (!valid_frames[i]) continue;
|
||||
Vec3d rvec_0, tvec_0;
|
||||
if (camera_rt_best[i] != 0) {
|
||||
// convert rvecs / tvecs from k-th camera to the first one
|
||||
|
||||
// formulas for relative rotation / translation
|
||||
// R = R_k R0^T => R_k = R R_0
|
||||
// t = t_k - R t_0 => t_k = t + R t_0
|
||||
|
||||
// initial camera R_0 = I, t_0 = 0 is fixed to R(rvec_0) and tvec_0
|
||||
// R_0 = R(rvec_0)
|
||||
// t_0 = tvec_0
|
||||
|
||||
// R'_k = R(rvec_k) = R_k R_0 => R_0 = R_k^T R(rvec_k)
|
||||
// t'_k = tvec_k = t_k + R_k t_0 => t_0 = R_k^T (tvec_k - t_k)
|
||||
const int rt_best_idx = camera_rt_best[i];
|
||||
Matx33d R_k;
|
||||
Rodrigues(rvecs_all[rt_best_idx][i], R_k);
|
||||
tvec_0 = Rs_vec[rt_best_idx].t() * (tvecs_all[rt_best_idx][i] - Ts_vec[rt_best_idx]);
|
||||
Rodrigues(Rs_vec[rt_best_idx].t() * R_k, rvec_0);
|
||||
} else {
|
||||
rvec_0 = rvecs_all[0][i];
|
||||
tvec_0 = tvecs_all[0][i];
|
||||
}
|
||||
|
||||
// save rvecs0 / tvecs0 parameters
|
||||
memcpy(¶m[0]+(cnt_valid_frame+NUM_CAMERAS-1)*6 , rvec_0.val, 3*sizeof(double));
|
||||
memcpy(¶m[0]+(cnt_valid_frame+NUM_CAMERAS-1)*6+3, tvec_0.val, 3*sizeof(double));
|
||||
cnt_valid_frame++;
|
||||
}
|
||||
|
||||
TermCriteria termCrit (TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6);
|
||||
const float RBS_FNC_SCALE = 30;
|
||||
multiview::RobustExpFunction robust_fnc(RBS_FNC_SCALE);
|
||||
multiview::optimizeLM(param, robust_fnc, termCrit, valid_frames, detection_mask_mat, objPoints_norm, imagePoints, Ks, distortions, is_fisheye_vec, NUM_PATTERN_PTS);
|
||||
const auto * const params = ¶m[0];
|
||||
|
||||
// extract extrinsics (R_i, t_i) for i = 1 ... NUM_CAMERAS:
|
||||
const bool rt_mat_vec = Rs.isMatVector();
|
||||
Mat rs, ts;
|
||||
if (rt_mat_vec) {
|
||||
Rs.create(NUM_CAMERAS, 1, CV_64F);
|
||||
Ts.create(NUM_CAMERAS, 1, CV_64F);
|
||||
} else {
|
||||
rs = Mat_<double>(NUM_CAMERAS, 3);
|
||||
ts = Mat_<double>(NUM_CAMERAS, 3);
|
||||
}
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
Mat r_store, t_store;
|
||||
if (rt_mat_vec) {
|
||||
Rs.create(3, 1, CV_64F, c, true);
|
||||
Ts.create(3, 1, CV_64F, c, true);
|
||||
r_store = Rs.getMat(c);
|
||||
t_store = Ts.getMat(c);
|
||||
} else {
|
||||
r_store = rs.row(c);
|
||||
t_store = ts.row(c);
|
||||
}
|
||||
if (c == 0) {
|
||||
memcpy(r_store.ptr(), Vec3d(0,0,0).val, 3*sizeof(double));
|
||||
memcpy(t_store.ptr(), Vec3d(0,0,0).val, 3*sizeof(double));
|
||||
} else {
|
||||
memcpy(r_store.ptr(), params + (c-1)*6, 3*sizeof(double));
|
||||
memcpy(t_store.ptr(), params + (c-1)*6+3, 3*sizeof(double)); // and de-normalize translation
|
||||
t_store *= scale_3d_pts;
|
||||
}
|
||||
Mat R;
|
||||
Rodrigues(r_store, R);
|
||||
}
|
||||
if (! rt_mat_vec) {
|
||||
rs.copyTo(Rs);
|
||||
ts.copyTo(Ts);
|
||||
}
|
||||
Mat rvecs0_, tvecs0_;
|
||||
if (rvecs0.needed() || perFrameErrors.needed()) {
|
||||
const bool is_mat_vec = rvecs0.needed() && rvecs0.isMatVector();
|
||||
if (is_mat_vec) {
|
||||
rvecs0.create(NUM_FRAMES, 1, CV_64F);
|
||||
} else {
|
||||
rvecs0_ = Mat_<double>(NUM_FRAMES, 3);
|
||||
}
|
||||
cnt_valid_frame = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!valid_frames[f]) continue;
|
||||
if (is_mat_vec)
|
||||
rvecs0.create(3, 1, CV_64F, f, true);
|
||||
Mat store = is_mat_vec ? rvecs0.getMat(f) : rvecs0_.row(f);
|
||||
memcpy(store.ptr(), params + (cnt_valid_frame + NUM_CAMERAS - 1)*6, 3*sizeof(double));
|
||||
cnt_valid_frame += 1;
|
||||
}
|
||||
if (!is_mat_vec && rvecs0.needed())
|
||||
rvecs0_.copyTo(rvecs0);
|
||||
}
|
||||
|
||||
if (tvecs0.needed() || perFrameErrors.needed()) {
|
||||
const bool is_mat_vec = tvecs0.needed() && tvecs0.isMatVector();
|
||||
if (is_mat_vec) {
|
||||
tvecs0.create(NUM_FRAMES, 1, CV_64F);
|
||||
} else {
|
||||
tvecs0_ = Mat_<double>(NUM_FRAMES, 3);
|
||||
}
|
||||
cnt_valid_frame = 0;
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (!valid_frames[f]) continue;
|
||||
if (is_mat_vec)
|
||||
tvecs0.create(3, 1, CV_64F, f, true);
|
||||
Mat store = is_mat_vec ? tvecs0.getMat(f) : tvecs0_.row(f);
|
||||
memcpy(store.ptr(), params + (cnt_valid_frame + NUM_CAMERAS - 1)*6+3, 3*sizeof(double));
|
||||
store *= scale_3d_pts;
|
||||
cnt_valid_frame += 1;
|
||||
}
|
||||
if (!is_mat_vec && tvecs0.needed())
|
||||
tvecs0_.copyTo(tvecs0);
|
||||
}
|
||||
double sum_errors = 0, cnt_errors = 0;
|
||||
if (perFrameErrors.needed()) {
|
||||
const bool rvecs_mat_vec = rvecs0.needed() && rvecs0.isMatVector(), tvecs_mat_vec = tvecs0.needed() && tvecs0.isMatVector();
|
||||
const bool r_mat_vec = Rs.isMatVector(), t_mat_vec = Ts.isMatVector();
|
||||
Mat errs = Mat_<double>(NUM_CAMERAS, NUM_FRAMES);
|
||||
auto * errs_ptr = (double *) errs.data;
|
||||
for (int c = 0; c < NUM_CAMERAS; c++) {
|
||||
const Mat rvec = r_mat_vec ? Rs.getMat(c) : Rs.getMat().row(c).t();
|
||||
const Mat tvec = t_mat_vec ? Ts.getMat(c) : Ts.getMat().row(c).t();
|
||||
for (int f = 0; f < NUM_FRAMES; f++) {
|
||||
if (detection_mask_mat[c][f]) {
|
||||
const Mat rvec0 = rvecs_mat_vec ? rvecs0.getMat(f) : rvecs0_.row(f).t();
|
||||
const Mat tvec0 = tvecs_mat_vec ? tvecs0.getMat(f) : tvecs0_.row(f).t();
|
||||
const double err2 = multiview::computeReprojectionMSE(objPoints.getMat(f), imagePoints[c][f], Ks[c],
|
||||
distortions[c], rvec0, tvec0, rvec, tvec, is_fisheye_vec[c]);
|
||||
(*errs_ptr++) = sqrt(err2);
|
||||
sum_errors += err2;
|
||||
cnt_errors += 1;
|
||||
} else (*errs_ptr++) = -1.0;
|
||||
}
|
||||
}
|
||||
errs.copyTo(perFrameErrors);
|
||||
}
|
||||
|
||||
return sqrt(sum_errors / cnt_errors);
|
||||
}
|
||||
}
|
||||
@ -2034,6 +2034,152 @@ TEST(Calib3d_ProjectPoints_CPP, outputShape)
|
||||
TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); }
|
||||
|
||||
|
||||
//-------------------------------- CV_MultiviewCalibrationTest_CPP ------------------------------
|
||||
|
||||
class CV_MultiviewCalibrationTest_CPP : public CV_StereoCalibrationTest
|
||||
{
|
||||
public:
|
||||
CV_MultiviewCalibrationTest_CPP() {}
|
||||
protected:
|
||||
virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
|
||||
const vector<vector<Point2f> >& imagePoints1,
|
||||
const vector<vector<Point2f> >& imagePoints2,
|
||||
Mat& cameraMatrix1, Mat& distCoeffs1,
|
||||
Mat& cameraMatrix2, Mat& distCoeffs2,
|
||||
Size imageSize, Mat& R, Mat& T,
|
||||
Mat& E, Mat& F,
|
||||
std::vector<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
|
||||
vector<double>& perViewErrors1, vector<double>& perViewErrors2,
|
||||
TermCriteria criteria, int flags );
|
||||
virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
|
||||
const Mat& cameraMatrix2, const Mat& distCoeffs2,
|
||||
Size imageSize, const Mat& R, const Mat& T,
|
||||
Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
|
||||
double alpha, Size newImageSize,
|
||||
Rect* validPixROI1, Rect* validPixROI2, int flags );
|
||||
virtual bool rectifyUncalibrated( const Mat& points1,
|
||||
const Mat& points2, const Mat& F, Size imgSize,
|
||||
Mat& H1, Mat& H2, double threshold=5 );
|
||||
virtual void triangulate( const Mat& P1, const Mat& P2,
|
||||
const Mat &points1, const Mat &points2,
|
||||
Mat &points4D );
|
||||
virtual void correct( const Mat& F,
|
||||
const Mat &points1, const Mat &points2,
|
||||
Mat &newPoints1, Mat &newPoints2 );
|
||||
};
|
||||
|
||||
double CV_MultiviewCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
|
||||
const vector<vector<Point2f> >& imagePoints1,
|
||||
const vector<vector<Point2f> >& imagePoints2,
|
||||
Mat& cameraMatrix1, Mat& distCoeffs1,
|
||||
Mat& cameraMatrix2, Mat& distCoeffs2,
|
||||
Size imageSize, Mat& R, Mat& T,
|
||||
Mat& E, Mat& F,
|
||||
std::vector<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
|
||||
vector<double>& perViewErrors1, vector<double>& perViewErrors2,
|
||||
TermCriteria /*criteria*/, int flags )
|
||||
{
|
||||
std::vector<cv::Mat> rvecs, tvecs;
|
||||
std::vector<cv::Mat> Ks, distortions, Rs, Ts;
|
||||
cv::Mat errors_mat, output_pairs, rvecs0, tvecs0;
|
||||
int numImgs = (int)objectPoints.size();
|
||||
std::vector<std::vector<cv::Mat>> image_points_all(2, std::vector<Mat>(numImgs));
|
||||
for (int i = 0; i < numImgs; i++)
|
||||
{
|
||||
Mat img_pts1 = Mat(imagePoints1[i], false);
|
||||
Mat img_pts2 = Mat(imagePoints2[i], false);
|
||||
img_pts1.copyTo(image_points_all[0][i]);
|
||||
img_pts2.copyTo(image_points_all[1][i]);
|
||||
}
|
||||
std::vector<Size> image_sizes (2, imageSize);
|
||||
Mat visibility_mat = Mat_<bool>::ones(2, numImgs);
|
||||
std::vector<bool> is_fisheye(2, false);
|
||||
std::vector<int> all_flags(2, flags);
|
||||
double rms = calibrateMultiview(objectPoints, image_points_all, image_sizes, visibility_mat,
|
||||
Rs, Ts, Ks, distortions, rvecs, tvecs, is_fisheye, errors_mat, noArray(), false, all_flags);
|
||||
|
||||
if (perViewErrors1.size() != (size_t)numImgs)
|
||||
{
|
||||
perViewErrors1.resize(numImgs);
|
||||
}
|
||||
if (perViewErrors2.size() != (size_t)numImgs)
|
||||
{
|
||||
perViewErrors2.resize(numImgs);
|
||||
}
|
||||
|
||||
for (int i = 0; i < numImgs; i++)
|
||||
{
|
||||
perViewErrors1[i] = errors_mat.at<double>(0, i);
|
||||
perViewErrors2[i] = errors_mat.at<double>(1, i);
|
||||
}
|
||||
|
||||
if (rotationMatrices.size() != (size_t)numImgs)
|
||||
{
|
||||
rotationMatrices.resize(numImgs);
|
||||
}
|
||||
if (translationVectors.size() != (size_t)numImgs)
|
||||
{
|
||||
translationVectors.resize(numImgs);
|
||||
}
|
||||
|
||||
for( int i = 0; i < numImgs; i++ )
|
||||
{
|
||||
Mat r9;
|
||||
cv::Rodrigues( rvecs[i], r9 );
|
||||
r9.convertTo(rotationMatrices[i], CV_64F);
|
||||
tvecs[i].convertTo(translationVectors[i], CV_64F);
|
||||
}
|
||||
|
||||
cv::Rodrigues(Rs[1], R);
|
||||
Ts[1].copyTo(T);
|
||||
distortions[0].copyTo(distCoeffs1);
|
||||
distortions[1].copyTo(distCoeffs2);
|
||||
Ks[0].copyTo(cameraMatrix1);
|
||||
Ks[1].copyTo(cameraMatrix2);
|
||||
Matx33d skewT( 0, -T.at<double>(2), T.at<double>(1),
|
||||
T.at<double>(2), 0, -T.at<double>(0),
|
||||
-T.at<double>(1), T.at<double>(0), 0);
|
||||
E = Mat(skewT * R);
|
||||
F = Ks[1].inv().t() * E * Ks[0].inv();
|
||||
return rms;
|
||||
}
|
||||
|
||||
void CV_MultiviewCalibrationTest_CPP::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
|
||||
const Mat& cameraMatrix2, const Mat& distCoeffs2,
|
||||
Size imageSize, const Mat& R, const Mat& T,
|
||||
Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
|
||||
double alpha, Size newImageSize,
|
||||
Rect* validPixROI1, Rect* validPixROI2, int flags )
|
||||
{
|
||||
stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
|
||||
imageSize, R, T, R1, R2, P1, P2, Q, flags, alpha, newImageSize,validPixROI1, validPixROI2 );
|
||||
}
|
||||
|
||||
bool CV_MultiviewCalibrationTest_CPP::rectifyUncalibrated( const Mat& points1,
|
||||
const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold )
|
||||
{
|
||||
return stereoRectifyUncalibrated( points1, points2, F, imgSize, H1, H2, threshold );
|
||||
}
|
||||
|
||||
void CV_MultiviewCalibrationTest_CPP::triangulate( const Mat& P1, const Mat& P2,
|
||||
const Mat &points1, const Mat &points2,
|
||||
Mat &points4D )
|
||||
{
|
||||
triangulatePoints(P1, P2, points1, points2, points4D);
|
||||
}
|
||||
|
||||
void CV_MultiviewCalibrationTest_CPP::correct( const Mat& F,
|
||||
const Mat &points1, const Mat &points2,
|
||||
Mat &newPoints1, Mat &newPoints2 )
|
||||
{
|
||||
correctMatches(F, points1, points2, newPoints1, newPoints2);
|
||||
}
|
||||
|
||||
TEST(Calib3d_MultiviewCalibrate_CPP, regression) { CV_MultiviewCalibrationTest_CPP test; test.safe_run(); }
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
TEST(Calib3d_StereoCalibrate_CPP, extended)
|
||||
{
|
||||
cvtest::TS* ts = cvtest::TS::ptr();
|
||||
|
||||
@ -1038,6 +1038,83 @@ TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify)
|
||||
EXPECT_MAT_NEAR(K_new, K_new_truth, 1e-6);
|
||||
}
|
||||
|
||||
|
||||
TEST_F(fisheyeTest, multiview_calibration)
|
||||
{
|
||||
const int n_images = 34;
|
||||
|
||||
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
|
||||
|
||||
std::vector<std::vector<cv::Point2f> > leftPoints(n_images);
|
||||
std::vector<std::vector<cv::Point2f> > rightPoints(n_images);
|
||||
std::vector<std::vector<cv::Point3f> > objectPoints(n_images);
|
||||
|
||||
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_left.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
|
||||
fs_left.release();
|
||||
|
||||
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_right.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
|
||||
fs_right.release();
|
||||
|
||||
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_object.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
|
||||
fs_object.release();
|
||||
|
||||
std::vector<std::vector<cv::Mat>> image_points_all(2, std::vector<cv::Mat>(leftPoints.size()));
|
||||
for (int i = 0; i < (int)leftPoints.size(); i++) {
|
||||
cv::Mat left_pts(leftPoints[i], false) , right_pts(rightPoints[i], false);
|
||||
left_pts.copyTo(image_points_all[0][i]);
|
||||
right_pts.copyTo(image_points_all[1][i]);
|
||||
}
|
||||
std::vector<cv::Size> image_sizes(2, imageSize);
|
||||
cv::Mat visibility_mat = cv::Mat_<bool>::ones(2, (int)leftPoints.size()), errors_mat, output_pairs;
|
||||
std::vector<cv::Mat> Rs, Ts, Ks, distortions, rvecs0, tvecs0;
|
||||
std::vector<bool> is_fisheye(2, true);
|
||||
int flag = 0;
|
||||
flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
|
||||
flag |= cv::fisheye::CALIB_CHECK_COND;
|
||||
flag |= cv::fisheye::CALIB_FIX_SKEW;
|
||||
|
||||
std::vector<int> all_flags(2, flag);
|
||||
|
||||
calibrateMultiview (objectPoints, image_points_all, image_sizes, visibility_mat,
|
||||
Rs, Ts, Ks, distortions, rvecs0, tvecs0, is_fisheye, errors_mat, output_pairs, false, all_flags);
|
||||
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
|
||||
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
|
||||
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
|
||||
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
|
||||
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
|
||||
0, 562.849402029712, 380.555455380889,
|
||||
0, 0, 1);
|
||||
|
||||
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
|
||||
0, 561.90171021422, 380.401340535339,
|
||||
0, 0, 1);
|
||||
|
||||
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
|
||||
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
|
||||
|
||||
cv::Mat theR;
|
||||
cv::Rodrigues(Rs[1], theR);
|
||||
|
||||
EXPECT_MAT_NEAR(theR, R_correct, 1e-2);
|
||||
EXPECT_MAT_NEAR(Ts[1], T_correct, 5e-3);
|
||||
|
||||
EXPECT_MAT_NEAR(Ks[0], K1_correct, 4);
|
||||
EXPECT_MAT_NEAR(Ks[1], K2_correct, 5);
|
||||
|
||||
EXPECT_MAT_NEAR(distortions[0], D1_correct, 1e-2);
|
||||
EXPECT_MAT_NEAR(distortions[1], D2_correct, 5e-2);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// fisheyeTest::
|
||||
|
||||
|
||||
151
modules/calib/test/test_multiview_calib.cpp
Normal file
151
modules/calib/test/test_multiview_calib.cpp
Normal file
@ -0,0 +1,151 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
#include "test_precomp.hpp"
|
||||
#include <opencv2/core/utils/logger.hpp>
|
||||
#include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
TEST(multiview_calibration, accuracy) {
|
||||
// convert euler angles to rotation matrix
|
||||
const auto euler2rot = [] (double x, double y, double z) {
|
||||
cv::Matx33d R_x(1, 0, 0, 0, cos(x), -sin(x), 0, sin(x), cos(x));
|
||||
cv::Matx33d R_y(cos(y), 0, sin(y), 0, 1, 0, -sin(y), 0, cos(y));
|
||||
cv::Matx33d R_z(cos(z), -sin(z), 0, sin(z), cos(z), 0, 0, 0, 1);
|
||||
return cv::Mat(R_z * R_y * R_x);
|
||||
};
|
||||
const cv::Size board_size (5,4);
|
||||
cv::RNG rng(0);
|
||||
const double board_len = 0.08, noise_std = 0.04;
|
||||
const int num_cameras = 4, num_pts = board_size.area();
|
||||
std::vector<cv::Vec3f> board_pattern (num_pts);
|
||||
// fill pattern points
|
||||
for (int j = 0; j < board_size.height; j++) {
|
||||
for (int i = 0; i < board_size.width; i++) {
|
||||
board_pattern[j*board_size.width+i] = cv::Vec3f ((float)i, (float)j, 0)*board_len;
|
||||
}
|
||||
}
|
||||
std::vector<bool> is_fisheye(num_cameras, false);
|
||||
std::vector<cv::Size> image_sizes(num_cameras);
|
||||
std::vector<cv::Mat> Ks_gt, distortions_gt, Rs_gt, Ts_gt;
|
||||
for (int c = 0; c < num_cameras; c++) {
|
||||
// generate intrinsics and extrinsics
|
||||
image_sizes[c] = cv::Size(rng.uniform(1300, 1500), rng.uniform(900, 1300));
|
||||
const double focal = rng.uniform(900.0, 1300.0);
|
||||
cv::Matx33d K(focal, 0, (double)image_sizes[c].width/2.,
|
||||
0, focal, (double)image_sizes[c].height/2.,
|
||||
0, 0, 1);
|
||||
cv::Matx<double, 1, 5> dist (rng.uniform(1e-1, 3e-1), rng.uniform(1e-2, 5e-2), rng.uniform(1e-2, 5e-2), rng.uniform(1e-2, 5e-2), rng.uniform(1e-2, 5e-2));
|
||||
Ks_gt.emplace_back(cv::Mat(K));
|
||||
distortions_gt.emplace_back(cv::Mat(dist));
|
||||
if (c == 0) {
|
||||
// I | 0
|
||||
Rs_gt.emplace_back(cv::Mat(cv::Matx33d::eye()));
|
||||
Ts_gt.emplace_back(cv::Mat(cv::Vec3d::zeros()));
|
||||
} else {
|
||||
const double ty_min = -.3, ty_max = .3, tx_min = -.3, tx_max = .3, tz_min = -.1, tz_max = .1;
|
||||
const double yaw_min = -20, yaw_max = 20, pitch_min = -20, pitch_max = 20, roll_min = -20, roll_max = 20;
|
||||
Rs_gt.emplace_back(euler2rot(rng.uniform(yaw_min, yaw_max)*M_PI/180,
|
||||
rng.uniform(pitch_min, pitch_max)*M_PI/180,
|
||||
rng.uniform(roll_min, roll_max)*M_PI/180));
|
||||
Ts_gt.emplace_back(cv::Mat(cv::Vec3d(rng.uniform(tx_min, tx_max),
|
||||
rng.uniform(ty_min, ty_max),
|
||||
rng.uniform(tz_min, tz_max))));
|
||||
}
|
||||
}
|
||||
|
||||
const int MAX_SAMPLES = 2000, MAX_FRAMES = 50;
|
||||
cv::Mat pattern (board_pattern, true/*copy*/);
|
||||
pattern = pattern.reshape(1).t();
|
||||
pattern.row(2) = 2.0; // set approximate depth of object points
|
||||
const double ty_min = -2, ty_max = 2, tx_min = -2, tx_max = 2, tz_min = -1, tz_max = 1;
|
||||
const double yaw_min = -45, yaw_max = 45, pitch_min = -45, pitch_max = 45, roll_min = -45, roll_max = 45;
|
||||
std::vector<std::vector<cv::Vec3f>> objPoints;
|
||||
std::vector<std::vector<cv::Mat>> image_points_all(num_cameras);
|
||||
cv::Mat ones = cv::Mat_<float>::ones(1, num_pts);
|
||||
std::vector<std::vector<bool>> visibility;
|
||||
cv::Mat centroid = cv::Mat(cv::Matx31f(
|
||||
(float)cv::mean(pattern.row(0)).val[0],
|
||||
(float)cv::mean(pattern.row(1)).val[0],
|
||||
(float)cv::mean(pattern.row(2)).val[0]));
|
||||
for (int f = 0; f < MAX_SAMPLES; f++) {
|
||||
cv::Mat R = euler2rot(rng.uniform(yaw_min, yaw_max)*M_PI/180,
|
||||
rng.uniform(pitch_min, pitch_max)*M_PI/180,
|
||||
rng.uniform(roll_min, roll_max)*M_PI/180);
|
||||
cv::Mat t = cv::Mat(cv::Matx31f(
|
||||
(float)rng.uniform(tx_min, tx_max),
|
||||
(float)rng.uniform(ty_min, ty_max),
|
||||
(float)rng.uniform(tz_min, tz_max)));
|
||||
|
||||
R.convertTo(R, CV_32F);
|
||||
cv::Mat pattern_new = (R * (pattern - centroid * ones) + centroid * ones + t * ones).t();
|
||||
|
||||
std::vector<cv::Mat> img_pts_cams(num_cameras);
|
||||
std::vector<bool> visible(num_cameras, false);
|
||||
int num_visible_patterns = 0;
|
||||
for (int c = 0; c < num_cameras; c++) {
|
||||
cv::Mat img_pts;
|
||||
if (is_fisheye[c]) {
|
||||
cv::fisheye::projectPoints(pattern_new, img_pts, Rs_gt[c], Ts_gt[c], Ks_gt[c], distortions_gt[c]);
|
||||
} else {
|
||||
cv::projectPoints(pattern_new, Rs_gt[c], Ts_gt[c], Ks_gt[c], distortions_gt[c], img_pts);
|
||||
}
|
||||
|
||||
// add normal / Gaussian noise to image points
|
||||
cv::Mat noise (img_pts.rows, img_pts.cols, img_pts.type());
|
||||
rng.fill(noise, cv::RNG::NORMAL, 0, noise_std);
|
||||
img_pts += noise;
|
||||
|
||||
bool are_all_pts_in_image = true;
|
||||
const auto * const pts = (float *) img_pts.data;
|
||||
for (int i = 0; i < num_pts; i++) {
|
||||
if (pts[i*2 ] < 0 || pts[i*2 ] > (float)image_sizes[c].width ||
|
||||
pts[i*2+1] < 0 || pts[i*2+1] > (float)image_sizes[c].height) {
|
||||
are_all_pts_in_image = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (are_all_pts_in_image) {
|
||||
visible[c] = true;
|
||||
num_visible_patterns += 1;
|
||||
img_pts.copyTo(img_pts_cams[c]);
|
||||
}
|
||||
}
|
||||
|
||||
if (num_visible_patterns >= 2) {
|
||||
objPoints.emplace_back(board_pattern);
|
||||
visibility.emplace_back(visible);
|
||||
for (int c = 0; c < num_cameras; c++) {
|
||||
image_points_all[c].emplace_back(img_pts_cams[c].clone());
|
||||
}
|
||||
if (objPoints.size() >= MAX_FRAMES)
|
||||
break;
|
||||
}
|
||||
}
|
||||
cv::Mat visibility_mat = cv::Mat_<bool>(num_cameras, (int)objPoints.size());
|
||||
for (int c = 0; c < num_cameras; c++) {
|
||||
for (int f = 0; f < (int)objPoints.size(); f++) {
|
||||
visibility_mat.at<bool>(c, f) = visibility[f][c];
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> Ks, distortions, Rs, Ts;
|
||||
cv::Mat errors_mat, output_pairs, rvecs0, tvecs0;
|
||||
calibrateMultiview (objPoints, image_points_all, image_sizes, visibility_mat,
|
||||
Rs, Ts, Ks, distortions, rvecs0, tvecs0, is_fisheye, errors_mat, output_pairs, false);
|
||||
|
||||
const double K_err_tol = 1e1, dist_tol = 5e-2, R_tol = 1e-2, T_tol = 1e-2;
|
||||
for (int c = 0; c < num_cameras; c++) {
|
||||
cv::Mat R;
|
||||
cv::Rodrigues(Rs[c], R);
|
||||
EXPECT_MAT_NEAR(Ks_gt[c], Ks[c], K_err_tol);
|
||||
CV_LOG_INFO(NULL, "true distortions: " << distortions_gt[c]);
|
||||
CV_LOG_INFO(NULL, "found distortions: " << distortions[c]);
|
||||
EXPECT_MAT_NEAR(distortions_gt[c], distortions[c], dist_tol);
|
||||
EXPECT_MAT_NEAR(Rs_gt[c], R, R_tol);
|
||||
EXPECT_MAT_NEAR(Ts_gt[c], Ts[c], T_tol);
|
||||
}
|
||||
}
|
||||
}}
|
||||
173
samples/cpp/multiview_calibration_sample.cpp
Normal file
173
samples/cpp/multiview_calibration_sample.cpp
Normal file
@ -0,0 +1,173 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
// ! [detectPointsAndCalibrate_signature]
|
||||
static void detectPointsAndCalibrate (cv::Size pattern_size, float pattern_scale, const std::string &pattern_type,
|
||||
const std::vector<bool> &is_fisheye, const std::vector<std::string> &filenames)
|
||||
// ! [detectPointsAndCalibrate_signature]
|
||||
{
|
||||
// ! [calib_init]
|
||||
std::vector<cv::Point3f> board (pattern_size.area());
|
||||
const int num_cameras = (int)is_fisheye.size();
|
||||
std::vector<std::vector<cv::Mat>> image_points_all;
|
||||
std::vector<cv::Size> image_sizes;
|
||||
std::vector<cv::Mat> Ks, distortions, Ts, Rs;
|
||||
cv::Mat rvecs0, tvecs0, errors_mat, output_pairs;
|
||||
if (pattern_type == "checkerboard") {
|
||||
for (int i = 0; i < pattern_size.height; i++) {
|
||||
for (int j = 0; j < pattern_size.width; j++) {
|
||||
board[i*pattern_size.width+j] = cv::Point3f((float)j, (float)i, 0.f) * pattern_scale;
|
||||
}
|
||||
}
|
||||
} else if (pattern_type == "circles") {
|
||||
for (int i = 0; i < pattern_size.height; i++) {
|
||||
for (int j = 0; j < pattern_size.width; j++) {
|
||||
board[i*pattern_size.width+j] = cv::Point3f((float)j, (float)i, 0.f) * pattern_scale;
|
||||
}
|
||||
}
|
||||
} else if (pattern_type == "acircles") {
|
||||
for (int i = 0; i < pattern_size.height; i++) {
|
||||
for (int j = 0; j < pattern_size.width; j++) {
|
||||
if (i % 2 == 1) {
|
||||
board[i*pattern_size.width+j] = cv::Point3f((j + .5f)*pattern_scale, (i/2 + .5f) * pattern_scale, 0.f);
|
||||
} else{
|
||||
board[i*pattern_size.width+j] = cv::Point3f(j*pattern_scale, (i/2)*pattern_scale, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
CV_Error(cv::Error::StsNotImplemented, "pattern_type is not implemented!");
|
||||
}
|
||||
// ! [calib_init]
|
||||
// ! [detect_pattern]
|
||||
int num_frames = -1;
|
||||
for (const auto &filename : filenames) {
|
||||
std::fstream file(filename);
|
||||
CV_Assert(file.is_open());
|
||||
std::string img_file;
|
||||
std::vector<cv::Mat> image_points_cameras;
|
||||
bool save_img_size = true;
|
||||
while (std::getline(file, img_file)) {
|
||||
if (img_file.empty())
|
||||
break;
|
||||
std::cout << img_file << "\n";
|
||||
cv::Mat img = cv::imread(img_file), corners;
|
||||
if (save_img_size) {
|
||||
image_sizes.emplace_back(cv::Size(img.cols, img.rows));
|
||||
save_img_size = false;
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
if (pattern_type == "checkerboard") {
|
||||
cv::cvtColor(img, img, cv::COLOR_BGR2GRAY);
|
||||
success = cv::findChessboardCorners(img, pattern_size, corners);
|
||||
}
|
||||
else if (pattern_type == "circles")
|
||||
{
|
||||
success = cv::findCirclesGrid(img, pattern_size, corners, cv::CALIB_CB_SYMMETRIC_GRID);
|
||||
}
|
||||
else if (pattern_type == "acircles")
|
||||
{
|
||||
success = cv::findCirclesGrid(img, pattern_size, corners, cv::CALIB_CB_ASYMMETRIC_GRID);
|
||||
}
|
||||
|
||||
cv::Mat corners2;
|
||||
corners.convertTo(corners2, CV_32FC2);
|
||||
|
||||
if (success && corners.rows == pattern_size.area())
|
||||
image_points_cameras.emplace_back(corners2);
|
||||
else
|
||||
image_points_cameras.emplace_back(cv::Mat());
|
||||
}
|
||||
if (num_frames == -1)
|
||||
num_frames = (int)image_points_cameras.size();
|
||||
else
|
||||
CV_Assert(num_frames == (int)image_points_cameras.size());
|
||||
image_points_all.emplace_back(image_points_cameras);
|
||||
}
|
||||
// ! [detect_pattern]
|
||||
// ! [detection_matrix]
|
||||
cv::Mat visibility(num_cameras, num_frames, CV_8UC1);
|
||||
for (int i = 0; i < num_cameras; i++) {
|
||||
for (int j = 0; j < num_frames; j++) {
|
||||
visibility.at<unsigned char>(i,j) = image_points_all[i][j].empty() ? 0 : 1;
|
||||
}
|
||||
}
|
||||
// ! [detection_matrix]
|
||||
CV_Assert(num_frames != -1);
|
||||
|
||||
std::vector<std::vector<cv::Point3f>> objPoints(num_frames, board);
|
||||
|
||||
// ! [multiview_calib]
|
||||
const double rmse = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility,
|
||||
Rs, Ts, Ks, distortions, cv::noArray(), cv::noArray(),
|
||||
is_fisheye, errors_mat, output_pairs);
|
||||
// ! [multiview_calib]
|
||||
std::cout << "average RMSE over detection mask " << rmse << "\n";
|
||||
for (int c = 0; c < (int)Rs.size(); c++) {
|
||||
std::cout << "camera " << c << '\n';
|
||||
std::cout << Rs[c] << " rotation\n";
|
||||
std::cout << Ts[c] << " translation\n";
|
||||
std::cout << Ks[c] << " intrinsic matrix\n";
|
||||
std::cout << distortions[c] << " distortion\n";
|
||||
}
|
||||
}
|
||||
|
||||
int main (int argc, char **argv) {
|
||||
cv::String keys =
|
||||
"{help h usage ? || print help }"
|
||||
"{pattern_width || pattern grid width}"
|
||||
"{pattern_height || pattern grid height}"
|
||||
"{pattern_scale || pattern scale}"
|
||||
"{pattern_type | checkerboard | pattern type, e.g., checkerboard or acircles}"
|
||||
"{is_fisheye || cameras type fisheye (1), pinhole(0), separated by comma (no space)}"
|
||||
"{files_with_images || files containing path to image names separated by comma (no space)}";
|
||||
|
||||
cv::CommandLineParser parser(argc, argv, keys);
|
||||
if (parser.has("help")) {
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
|
||||
CV_Assert(parser.has("pattern_width") && parser.has("pattern_height") && parser.has("pattern_type") &&
|
||||
parser.has("is_fisheye") && parser.has("files_with_images"));
|
||||
CV_Assert(parser.get<cv::String>("pattern_type") == "checkerboard" ||
|
||||
parser.get<cv::String>("pattern_type") == "circles" ||
|
||||
parser.get<cv::String>("pattern_type") == "acircles");
|
||||
|
||||
const cv::Size pattern_size (parser.get<int>("pattern_width"), parser.get<int>("pattern_height"));
|
||||
std::vector<bool> is_fisheye;
|
||||
const cv::String is_fisheye_str = parser.get<cv::String>("is_fisheye");
|
||||
for (char i : is_fisheye_str) {
|
||||
if (i == '0') {
|
||||
is_fisheye.push_back(false);
|
||||
} else if (i == '1') {
|
||||
is_fisheye.push_back(true);
|
||||
}
|
||||
}
|
||||
const cv::String files_with_images_str = parser.get<cv::String>("files_with_images");
|
||||
std::vector<std::string> filenames;
|
||||
std::string temp_str;
|
||||
for (char i : files_with_images_str) {
|
||||
if (i == ',') {
|
||||
filenames.emplace_back(temp_str);
|
||||
temp_str = "";
|
||||
} else {
|
||||
temp_str += i;
|
||||
}
|
||||
}
|
||||
filenames.emplace_back(temp_str);
|
||||
CV_CheckEQ(filenames.size(), is_fisheye.size(), "filenames size must be equal to number of cameras!");
|
||||
detectPointsAndCalibrate (pattern_size, parser.get<float>("pattern_scale"), parser.get<cv::String>("pattern_type"), is_fisheye, filenames);
|
||||
return 0;
|
||||
}
|
||||
736
samples/python/multiview_calibration.py
Normal file
736
samples/python/multiview_calibration.py
Normal file
@ -0,0 +1,736 @@
|
||||
# This file is part of OpenCV project.
|
||||
# It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
# of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import json
|
||||
import multiprocessing
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
from datetime import datetime
|
||||
|
||||
import cv2 as cv
|
||||
import joblib
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
|
||||
def getDimBox(pts):
|
||||
return np.array([[pts[...,k].min(), pts[...,k].max()] for k in range(pts.shape[-1])])
|
||||
|
||||
|
||||
def plotCamerasPosition(R, t, image_sizes, pairs, pattern, frame_idx, cam_ids):
|
||||
cam_box = np.array([
|
||||
[ 1, 1, 3],
|
||||
[ 1, -1, 3],
|
||||
[-1, -1, 3],
|
||||
[-1, 1, 3]
|
||||
], dtype=np.float32)
|
||||
dist_to_pattern = np.linalg.norm(pattern.mean(0))
|
||||
cam_box *= 0.1 * dist_to_pattern
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection='3d')
|
||||
|
||||
ax_lines = [None] * len(R)
|
||||
ax.set_title(f'Cameras position and pattern of frame {frame_idx}',
|
||||
loc='center', wrap=True, fontsize=20)
|
||||
all_pts = [pattern]
|
||||
colors = np.random.RandomState(0).rand(len(R), 3)
|
||||
|
||||
for i in range(len(R)):
|
||||
cam_box_i = cam_box.copy()
|
||||
cam_box_i[:,0] *= image_sizes[i][0] / max(image_sizes[i][1], image_sizes[i][0])
|
||||
cam_box_i[:,1] *= image_sizes[i][1] / max(image_sizes[i][1], image_sizes[i][0])
|
||||
cam_box_Rt = (R[i] @ cam_box_i.T + t[i]).T
|
||||
all_pts.append(np.concatenate((cam_box_Rt, t[i].T)))
|
||||
|
||||
ax_lines[i] = ax.plot([t[i][0,0], cam_box_Rt[0,0]],
|
||||
[t[i][1,0], cam_box_Rt[0,1]],
|
||||
[t[i][2,0], cam_box_Rt[0,2]],
|
||||
'-', color=colors[i])[0]
|
||||
|
||||
ax.plot([t[i][0,0], cam_box_Rt[1,0]],
|
||||
[t[i][1,0], cam_box_Rt[1,1]],
|
||||
[t[i][2,0], cam_box_Rt[1,2]],
|
||||
'-', color=colors[i])
|
||||
ax.plot([t[i][0,0], cam_box_Rt[2,0]],
|
||||
[t[i][1,0], cam_box_Rt[2,1]],
|
||||
[t[i][2,0], cam_box_Rt[2,2]],
|
||||
'-', color=colors[i])
|
||||
ax.plot([t[i][0,0], cam_box_Rt[3,0]],
|
||||
[t[i][1,0], cam_box_Rt[3,1]],
|
||||
[t[i][2,0], cam_box_Rt[3,2]],
|
||||
'-', color=colors[i])
|
||||
|
||||
ax.plot([cam_box_Rt[0,0], cam_box_Rt[1,0]],
|
||||
[cam_box_Rt[0,1], cam_box_Rt[1,1]],
|
||||
[cam_box_Rt[0,2], cam_box_Rt[1,2]],
|
||||
'-', color=colors[i])
|
||||
ax.plot([cam_box_Rt[1,0], cam_box_Rt[2,0]],
|
||||
[cam_box_Rt[1,1], cam_box_Rt[2,1]],
|
||||
[cam_box_Rt[1,2], cam_box_Rt[2,2]],
|
||||
'-', color=colors[i])
|
||||
ax.plot([cam_box_Rt[2,0], cam_box_Rt[3,0]],
|
||||
[cam_box_Rt[2,1], cam_box_Rt[3,1]],
|
||||
[cam_box_Rt[2,2], cam_box_Rt[3,2]],
|
||||
'-', color=colors[i])
|
||||
ax.plot([cam_box_Rt[3,0], cam_box_Rt[0,0]],
|
||||
[cam_box_Rt[3,1], cam_box_Rt[0,1]],
|
||||
[cam_box_Rt[3,2], cam_box_Rt[0,2]],
|
||||
'-', color=colors[i])
|
||||
|
||||
# Plot lines between cameras
|
||||
for (i, j) in pairs:
|
||||
xs = [t[i][0,0], t[j][0,0]]
|
||||
ys = [t[i][1,0], t[j][1,0]]
|
||||
zs = [t[i][2,0], t[j][2,0]]
|
||||
edge_line = ax.plot(xs, ys, zs, '-', color='black')[0]
|
||||
|
||||
ax.scatter(pattern[:, 0], pattern[:, 1], pattern[:, 2], color='red', marker='o')
|
||||
ax.legend(ax_lines + [edge_line], cam_ids + ['stereo pair'], fontsize=6)
|
||||
|
||||
dim_box = getDimBox(np.concatenate((all_pts)))
|
||||
|
||||
ax.set_xlim(dim_box[0])
|
||||
ax.set_ylim(dim_box[1])
|
||||
ax.set_zlim(dim_box[2])
|
||||
|
||||
aspect = (
|
||||
dim_box[0, 1] - dim_box[0, 0],
|
||||
dim_box[1, 1] - dim_box[1, 0],
|
||||
dim_box[2, 1] - dim_box[2, 0],
|
||||
)
|
||||
ax.set_box_aspect(aspect)
|
||||
|
||||
ax.set_xlabel('x', fontsize=16)
|
||||
ax.set_ylabel('y', fontsize=16)
|
||||
ax.set_zlabel('z', fontsize=16)
|
||||
|
||||
ax.view_init(azim=90, elev=-40)
|
||||
|
||||
|
||||
def showUndistorted(image_points, Ks, distortions, image_names):
|
||||
detection_mask = getDetectionMask(image_points)
|
||||
for cam in range(len(image_points)):
|
||||
detected_imgs = np.where(detection_mask[cam])[0]
|
||||
random_frame = np.random.RandomState(0).choice(detected_imgs, 1, replace=False)[0]
|
||||
undistorted_pts = cv.undistortPoints(
|
||||
image_points[cam][random_frame],
|
||||
Ks[cam],
|
||||
distortions[cam],
|
||||
P=Ks[cam]
|
||||
)[:,0]
|
||||
|
||||
fig = plt.figure()
|
||||
if image_names is not None:
|
||||
plt.imshow(cv.cvtColor(cv.undistort(
|
||||
cv.imread(image_names[cam][random_frame]),
|
||||
Ks[cam],
|
||||
distortions[cam]
|
||||
), cv.COLOR_BGR2RGB))
|
||||
else:
|
||||
ax = fig.add_subplot(111)
|
||||
ax.set_aspect('equal', 'box')
|
||||
ax.set_xlabel('x', fontsize=20)
|
||||
ax.set_ylabel('y', fontsize=20)
|
||||
|
||||
plt.scatter(undistorted_pts[:,0], undistorted_pts[:,1], s=10)
|
||||
plt.title(
|
||||
f'Undistorted. Camera {cam_ids[cam]} frame {random_frame}',
|
||||
loc='center',
|
||||
wrap=True,
|
||||
fontsize=16
|
||||
)
|
||||
|
||||
save_file = f'undistorted_{cam_ids[cam]}.png'
|
||||
print('Saving:', save_file)
|
||||
plt.savefig(save_file)
|
||||
|
||||
|
||||
def plotProjection(points_2d, pattern_points, rvec0, tvec0, rvec1, tvec1,
|
||||
K, dist_coeff, is_fisheye, cam_idx, frame_idx, per_acc,
|
||||
image=None):
|
||||
rvec2, tvec2 = cv.composeRT(rvec0, tvec0, rvec1, tvec1)[:2]
|
||||
|
||||
if is_fisheye:
|
||||
points_2d_est = cv.fisheye.projectPoints(
|
||||
pattern_points[:, None], rvec2, tvec2, K, dist_coeff.flatten()
|
||||
)[0].reshape(-1, 2)
|
||||
else:
|
||||
points_2d_est = cv.projectPoints(
|
||||
pattern_points, rvec2, tvec2, K, dist_coeff
|
||||
)[0].reshape(-1, 2)
|
||||
|
||||
fig = plt.figure()
|
||||
errs = np.linalg.norm(points_2d - points_2d_est, axis=-1)
|
||||
mean_err = errs.mean()
|
||||
|
||||
title = f"Comparison of given point (start) and back-projected (end). " \
|
||||
f"Cam. {cam_idx} frame {frame_idx} mean err. (px) {mean_err:.1f}. " \
|
||||
f"In top {per_acc:.0f}% accurate frames"
|
||||
|
||||
dist_pattern = np.linalg.norm(points_2d_est.min(0) - points_2d_est.max(0))
|
||||
width = 2e-3 * dist_pattern
|
||||
head_width = 5 * width
|
||||
|
||||
if image is None:
|
||||
ax = fig.add_subplot(111)
|
||||
ax.set_aspect('equal', 'box')
|
||||
ax.set_xlabel('x', fontsize=20)
|
||||
ax.set_ylabel('y', fontsize=20)
|
||||
else:
|
||||
plt.imshow(image)
|
||||
ax = plt.gca()
|
||||
|
||||
num_colors = 8
|
||||
cmap_fnc = lambda x : np.concatenate((x, 1-x, np.zeros_like(x)))
|
||||
cmap = cmap_fnc(np.linspace(0, 1, num_colors)[None, :])
|
||||
thrs = np.linspace(0, 10, num_colors)
|
||||
arrows = [None] * num_colors
|
||||
|
||||
for k, (pt1, pt2) in enumerate(zip(points_2d, points_2d_est)):
|
||||
color = cmap[:, -1]
|
||||
for i, thr in enumerate(thrs):
|
||||
if errs[k] < thr:
|
||||
color = cmap[:, i]
|
||||
break
|
||||
arrow = ax.arrow(
|
||||
pt1[0], pt1[1], pt2[0]-pt1[0], pt2[1]-pt1[1],
|
||||
color=color, width=width, head_width=head_width,
|
||||
)
|
||||
for i, thr in enumerate(thrs):
|
||||
if errs[k] < thr:
|
||||
arrows[i] = arrow # type: ignore
|
||||
break
|
||||
|
||||
legend, legend_str = [], []
|
||||
for i in range(num_colors):
|
||||
if arrows[i] is not None:
|
||||
legend.append(arrows[i])
|
||||
if i == 0:
|
||||
legend_str.append(f'lower than {thrs[i]:.1f}')
|
||||
elif i == num_colors-1:
|
||||
legend_str.append(f'higher than {thrs[i]:.1f}')
|
||||
else:
|
||||
legend_str.append(f'between {thrs[i-1]:.1f} and {thrs[i]:.1f}')
|
||||
|
||||
ax.legend(legend, legend_str, fontsize=15)
|
||||
ax.set_title(title, loc='center', wrap=True, fontsize=16)
|
||||
|
||||
|
||||
def getDetectionMask(image_points):
|
||||
detection_mask = np.zeros((len(image_points), len(image_points[0])), dtype=np.uint8)
|
||||
# [detection_matrix]
|
||||
for i in range(len(image_points)):
|
||||
for j in range(len(image_points[0])):
|
||||
detection_mask[i,j] = int(len(image_points[i][j]) != 0)
|
||||
# [detection_matrix]
|
||||
return detection_mask
|
||||
|
||||
|
||||
def calibrateFromPoints(
|
||||
pattern_points,
|
||||
image_points,
|
||||
image_sizes,
|
||||
is_fisheye,
|
||||
image_names=None,
|
||||
find_intrinsics_in_python=False,
|
||||
Ks=None,
|
||||
distortions=None
|
||||
):
|
||||
"""
|
||||
pattern_points: NUM_POINTS x 3 (numpy array)
|
||||
image_points: NUM_CAMERAS x NUM_FRAMES x NUM_POINTS x 2
|
||||
is_fisheye: NUM_CAMERAS (bool)
|
||||
image_sizes: NUM_CAMERAS x [width, height]
|
||||
"""
|
||||
num_cameras = len(image_points)
|
||||
num_frames = len(image_points[0])
|
||||
detection_mask = getDetectionMask(image_points)
|
||||
pattern_points_all = [pattern_points] * num_frames
|
||||
with np.printoptions(threshold=np.inf): # type: ignore
|
||||
print("detection mask Matrix:\n", str(detection_mask).replace('0\n ', '0').replace('1\n ', '1'))
|
||||
|
||||
#HACK: OpenCV API does not well support mix of fisheye and pinhole models.
|
||||
# Pinhole models with rational distortion model is used instead
|
||||
fisheyes = np.count_nonzero(is_fisheye)
|
||||
intrinsics_flag = 0
|
||||
if (fisheyes > 0) and (fisheyes != num_cameras):
|
||||
intrinsics_flag = cv.CALIB_RATIONAL_MODEL + cv.CALIB_ZERO_TANGENT_DIST + cv.CALIB_FIX_K5 + cv.CALIB_FIX_K6
|
||||
|
||||
if Ks is not None and distortions is not None:
|
||||
USE_INTRINSICS_GUESS = True
|
||||
else:
|
||||
USE_INTRINSICS_GUESS = find_intrinsics_in_python
|
||||
if find_intrinsics_in_python:
|
||||
Ks, distortions = [], []
|
||||
for c in range(num_cameras):
|
||||
if is_fisheye[c]:
|
||||
image_points_c = [
|
||||
image_points[c][f][:, None] for f in range(num_frames) if len(image_points[c][f]) > 0
|
||||
]
|
||||
repr_err_c, K, dist_coeff, _, _ = cv.fisheye.calibrate(
|
||||
[pattern_points[:, None]] * len(image_points_c),
|
||||
image_points_c,
|
||||
image_sizes[c],
|
||||
None,
|
||||
None
|
||||
)
|
||||
else:
|
||||
image_points_c = [
|
||||
image_points[c][f] for f in range(num_frames) if len(image_points[c][f]) > 0
|
||||
]
|
||||
repr_err_c, K, dist_coeff, _, _ = cv.calibrateCamera(
|
||||
[pattern_points] * len(image_points_c),
|
||||
image_points_c,
|
||||
image_sizes[c],
|
||||
None,
|
||||
None,
|
||||
flags=intrinsics_flag
|
||||
)
|
||||
print(f'Intrinsics calibration for camera {c}, reproj error {repr_err_c:.2f} (px)')
|
||||
Ks.append(K)
|
||||
distortions.append(dist_coeff)
|
||||
|
||||
start_time = time.time()
|
||||
# try:
|
||||
# [multiview_calib]
|
||||
rmse, rvecs, Ts, Ks, distortions, rvecs0, tvecs0, errors_per_frame, output_pairs = \
|
||||
cv.calibrateMultiview(
|
||||
objPoints=pattern_points_all,
|
||||
imagePoints=image_points,
|
||||
imageSize=image_sizes,
|
||||
detectionMask=detection_mask,
|
||||
Ks=Ks,
|
||||
distortions=distortions,
|
||||
isFisheye=np.array(is_fisheye, dtype=np.uint8),
|
||||
useIntrinsicsGuess=USE_INTRINSICS_GUESS,
|
||||
flagsForIntrinsics=np.full((num_cameras), intrinsics_flag, dtype=int)
|
||||
)
|
||||
# [multiview_calib]
|
||||
# except Exception as e:
|
||||
# print("Multi-view calibration failed with the following exception:", e.__class__)
|
||||
# sys.exit(0)
|
||||
|
||||
print('calibration time', time.time() - start_time, 'seconds')
|
||||
print('rvecs', rvecs)
|
||||
print('tvecs', Ts)
|
||||
print('K', Ks)
|
||||
print('distortion', distortions)
|
||||
print('mean RMS error over all visible frames %.3E' % rmse)
|
||||
|
||||
with np.printoptions(precision=2):
|
||||
print('mean RMS errors per camera', np.array([np.mean(errs[errs > 0]) for errs in errors_per_frame]))
|
||||
|
||||
return {
|
||||
'rvecs': rvecs,
|
||||
'distortions': distortions,
|
||||
'Ks': Ks,
|
||||
'Ts': Ts,
|
||||
'rvecs0': rvecs0,
|
||||
'tvecs0': tvecs0,
|
||||
'errors_per_frame': errors_per_frame,
|
||||
'output_pairs': output_pairs,
|
||||
'image_points': image_points,
|
||||
'is_fisheye': is_fisheye,
|
||||
'image_sizes': image_sizes,
|
||||
'pattern_points': pattern_points,
|
||||
'detection_mask': detection_mask,
|
||||
'image_names': image_names,
|
||||
}
|
||||
|
||||
|
||||
def visualizeResults(detection_mask, rvecs, Ts, Ks, distortions, is_fisheye,
|
||||
image_points, errors_per_frame, rvecs0, tvecs0,
|
||||
pattern_points, image_sizes, output_pairs, image_names, cam_ids):
|
||||
Rs = [cv.Rodrigues(rvec)[0] for rvec in rvecs]
|
||||
errors = errors_per_frame[errors_per_frame > 0]
|
||||
detection_mask_idxs = np.stack(np.where(detection_mask)) # 2 x M, first row is camera idx, second is frame idx
|
||||
|
||||
# Get very first frame from first camera
|
||||
frame_idx = detection_mask_idxs[1, 0]
|
||||
R_frame = cv.Rodrigues(rvecs0[frame_idx])[0]
|
||||
pattern_frame = (R_frame @ pattern_points.T + tvecs0[frame_idx]).T
|
||||
plotCamerasPosition(Rs, Ts, image_sizes, output_pairs, pattern_frame, frame_idx, cam_ids)
|
||||
|
||||
save_file = 'cam_poses.png'
|
||||
print('Saving:', save_file)
|
||||
plt.savefig(save_file, dpi=300, bbox_inches='tight')
|
||||
|
||||
# Generate and save undistorted images
|
||||
def plot(cam_idx, frame_idx):
|
||||
image = None
|
||||
if image_names is not None:
|
||||
image = cv.cvtColor(cv.imread(image_names[cam_idx][frame_idx]), cv.COLOR_BGR2RGB)
|
||||
plotProjection(
|
||||
image_points[cam_idx][frame_idx],
|
||||
pattern_points,
|
||||
rvecs0[frame_idx],
|
||||
tvecs0[frame_idx],
|
||||
rvecs[cam_idx],
|
||||
Ts[cam_idx],
|
||||
Ks[cam_idx],
|
||||
distortions[cam_idx],
|
||||
is_fisheye[cam_idx],
|
||||
cam_idx,
|
||||
frame_idx,
|
||||
(errors_per_frame[cam_idx, frame_idx] < errors).sum() * 100 / len(errors),
|
||||
image,
|
||||
)
|
||||
|
||||
plot(detection_mask_idxs[0, 0], detection_mask_idxs[1, 0])
|
||||
showUndistorted(image_points, Ks, distortions, image_names)
|
||||
# plt.show()
|
||||
|
||||
|
||||
def visualizeFromFile(file):
|
||||
file_read = cv.FileStorage(file, cv.FileStorage_READ)
|
||||
assert file_read.isOpened(), file
|
||||
read_keys = [
|
||||
'rvecs', 'distortions', 'Ks', 'Ts', 'rvecs0', 'tvecs0',
|
||||
'errors_per_frame', 'output_pairs', 'image_points', 'is_fisheye',
|
||||
'image_sizes', 'pattern_points', 'detection_mask', 'cam_ids',
|
||||
]
|
||||
input = {}
|
||||
for key in read_keys:
|
||||
input[key] = file_read.getNode(key).mat()
|
||||
|
||||
im_names_len = file_read.getNode('image_names').size()
|
||||
input['image_names'] = np.array(
|
||||
[file_read.getNode('image_names').at(i).string() for i in range(im_names_len)]
|
||||
).reshape(input['image_points'].shape[:2])
|
||||
|
||||
input['tvecs0'] = input['tvecs0'][..., None]
|
||||
input['Ts'] = input['Ts'][..., None]
|
||||
visualizeResults(**input)
|
||||
|
||||
|
||||
def saveToFile(path_to_save, **kwargs):
|
||||
if path_to_save == '':
|
||||
path_to_save = datetime.now().strftime("%d-%b-%Y (%H:%M:%S.%f)")+'.yaml'
|
||||
save_file = cv.FileStorage(path_to_save, cv.FileStorage_WRITE)
|
||||
|
||||
kwargs['is_fisheye'] = np.array(kwargs['is_fisheye'], dtype=int)
|
||||
image_points = kwargs['image_points']
|
||||
|
||||
for i in range(len(image_points)):
|
||||
for j in range(len(image_points[0])):
|
||||
if len(image_points[i][j]) == 0:
|
||||
image_points[i][j] = np.zeros((kwargs['pattern_points'].shape[0], 2))
|
||||
|
||||
for key in kwargs.keys():
|
||||
if key == 'image_names':
|
||||
save_file.write('image_names', list(np.array(kwargs['image_names']).reshape(-1)))
|
||||
elif key == 'cam_ids':
|
||||
save_file.write('cam_ids', ','.join(cam_ids))
|
||||
else:
|
||||
value = kwargs[key]
|
||||
if key in ('rvecs0', 'tvecs0'):
|
||||
# Replace None by [0, 0, 0]
|
||||
value = [arr if arr is not None else np.zeros((3, 1)) for arr in value]
|
||||
save_file.write(key, np.array(value))
|
||||
|
||||
save_file.release()
|
||||
|
||||
|
||||
def chessboard_points(grid_size, dist_m):
|
||||
pattern = np.zeros((grid_size[0] * grid_size[1], 3), np.float32)
|
||||
pattern[:, :2] = np.mgrid[0:grid_size[0], 0:grid_size[1]].T.reshape(-1, 2) * dist_m # only for (x,y,z=0)
|
||||
return pattern
|
||||
|
||||
|
||||
def circles_grid_points(grid_size, dist_m):
|
||||
pattern = []
|
||||
for i in range(grid_size[0]):
|
||||
for j in range(grid_size[1]):
|
||||
pattern.append([j * dist_m, i * dist_m, 0])
|
||||
return np.array(pattern, dtype=np.float32)
|
||||
|
||||
|
||||
def asym_circles_grid_points(grid_size, dist_m):
|
||||
pattern = []
|
||||
for i in range(grid_size[1]):
|
||||
for j in range(grid_size[0]):
|
||||
if i % 2 == 1:
|
||||
pattern.append([(j + .5)*dist_m, dist_m*(i//2 + .5), 0])
|
||||
else:
|
||||
pattern.append([j*dist_m, (i//2)*dist_m, 0])
|
||||
return np.array(pattern, dtype=np.float32)
|
||||
|
||||
|
||||
def detect(cam_idx, frame_idx, img_name, pattern_type,
|
||||
grid_size, criteria, winsize, RESIZE_IMAGE):
|
||||
# print(img_name)
|
||||
assert os.path.exists(img_name), img_name
|
||||
img = cv.imread(img_name)
|
||||
img_size = img.shape[:2][::-1]
|
||||
|
||||
scale = 1.0
|
||||
img_detection = img
|
||||
if RESIZE_IMAGE:
|
||||
scale = 1000.0 / max(img.shape[0], img.shape[1])
|
||||
if scale < 1.0:
|
||||
img_detection = cv.resize(
|
||||
img,
|
||||
(int(scale * img.shape[1]), int(scale * img.shape[0])),
|
||||
interpolation=cv.INTER_AREA
|
||||
)
|
||||
# [detect_pattern]
|
||||
if pattern_type.lower() == 'checkerboard':
|
||||
ret, corners = cv.findChessboardCorners(
|
||||
cv.cvtColor(img_detection, cv.COLOR_BGR2GRAY), grid_size, None
|
||||
)
|
||||
if ret:
|
||||
if scale < 1.0:
|
||||
corners /= scale
|
||||
corners2 = cv.cornerSubPix(cv.cvtColor(img, cv.COLOR_BGR2GRAY),
|
||||
corners, winsize, (-1,-1), criteria)
|
||||
|
||||
elif pattern_type.lower() == 'circles':
|
||||
ret, corners = cv.findCirclesGrid(
|
||||
img_detection, patternSize=grid_size, flags=cv.CALIB_CB_SYMMETRIC_GRID
|
||||
)
|
||||
if ret:
|
||||
corners2 = corners / scale
|
||||
|
||||
elif pattern_type.lower() == 'acircles':
|
||||
ret, corners = cv.findCirclesGrid(
|
||||
img_detection, patternSize=grid_size, flags=cv.CALIB_CB_ASYMMETRIC_GRID
|
||||
)
|
||||
if ret:
|
||||
corners2 = corners / scale
|
||||
else:
|
||||
raise ValueError("Calibration pattern is not supported!")
|
||||
# [detect_pattern]
|
||||
if ret:
|
||||
# cv.drawChessboardCorners(img, grid_size, corners2, ret)
|
||||
# plt.imshow(img)
|
||||
# plt.show()
|
||||
return cam_idx, frame_idx, img_size, np.array(corners2, dtype=np.float32).reshape(-1, 2)
|
||||
else:
|
||||
# plt.imshow(img_detection)
|
||||
# plt.show()
|
||||
return cam_idx, frame_idx, img_size, np.array([], dtype=np.float32)
|
||||
|
||||
|
||||
def calibrateFromImages(files_with_images, grid_size, pattern_type, is_fisheye,
|
||||
dist_m, winsize, points_json_file, debug_corners,
|
||||
RESIZE_IMAGE, find_intrinsics_in_python,
|
||||
is_parallel_detection=True, cam_ids=None, intrinsics_dir=''):
|
||||
"""
|
||||
files_with_images: NUM_CAMERAS - path to file containing image names (NUM_FRAMES)
|
||||
grid_size: [width, height] -- size of grid pattern
|
||||
dist_m: length of a grid cell
|
||||
is_fisheye: NUM_CAMERAS (bool)
|
||||
"""
|
||||
# [calib_init]
|
||||
if pattern_type.lower() == 'checkerboard':
|
||||
pattern = chessboard_points(grid_size, dist_m)
|
||||
elif pattern_type.lower() == 'circles':
|
||||
pattern = circles_grid_points(grid_size, dist_m)
|
||||
elif pattern_type.lower() == 'acircles':
|
||||
pattern = asym_circles_grid_points(grid_size, dist_m)
|
||||
else:
|
||||
raise NotImplementedError("Pattern type is not implemented!")
|
||||
# [calib_init]
|
||||
|
||||
assert len(files_with_images) == len(is_fisheye) and len(grid_size) == 2
|
||||
if cam_ids is None:
|
||||
cam_ids = list(range(len(files_with_images)))
|
||||
|
||||
all_images_names, input_data = [], []
|
||||
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 50, 0.001)
|
||||
for cam_idx, filename in enumerate(files_with_images):
|
||||
assert os.path.exists(filename), filename
|
||||
print('cam_id:', cam_ids[cam_idx])
|
||||
|
||||
images_names = open(filename, 'r').readlines()
|
||||
for i in range(len(images_names)):
|
||||
images_names[i] = images_names[i].replace('\n', '')
|
||||
all_images_names.append(images_names)
|
||||
if cam_idx > 0:
|
||||
# same number of images per file
|
||||
assert len(images_names) == len(all_images_names[-1])
|
||||
for frame_idx, img_name in enumerate(images_names):
|
||||
input_data.append([cam_idx, frame_idx, img_name])
|
||||
|
||||
image_sizes = [None] * len(files_with_images)
|
||||
image_points_cameras = [[None] * len(images_names) for _ in files_with_images]
|
||||
|
||||
if is_parallel_detection:
|
||||
parallel_job = joblib.Parallel(n_jobs=multiprocessing.cpu_count())
|
||||
output = parallel_job(
|
||||
joblib.delayed(detect)(
|
||||
cam_idx, frame_idx, img_name, pattern_type,
|
||||
grid_size, criteria, winsize, RESIZE_IMAGE
|
||||
) for cam_idx, frame_idx, img_name in input_data
|
||||
)
|
||||
assert output is not None
|
||||
for cam_idx, frame_idx, img_size, corners in output:
|
||||
image_points_cameras[cam_idx][frame_idx] = corners
|
||||
if image_sizes[cam_idx] is None:
|
||||
image_sizes[cam_idx] = img_size
|
||||
else:
|
||||
for cam_idx, frame_idx, img_name in input_data:
|
||||
_, _, img_size, corners = detect(
|
||||
cam_idx, frame_idx, img_name, pattern_type,
|
||||
grid_size, criteria, winsize, RESIZE_IMAGE
|
||||
)
|
||||
image_points_cameras[cam_idx][frame_idx] = corners
|
||||
if image_sizes[cam_idx] is None:
|
||||
image_sizes[cam_idx] = img_size
|
||||
|
||||
if debug_corners:
|
||||
# plots random image frames with detected points
|
||||
num_random_plots = 5
|
||||
visible_frames = []
|
||||
for c, pts_cam in enumerate(image_points_cameras):
|
||||
for f, pts_frame in enumerate(pts_cam):
|
||||
if pts_frame is not None:
|
||||
visible_frames.append((c,f))
|
||||
random_images = np.random.RandomState(0).choice(
|
||||
range(len(visible_frames)), min(num_random_plots, len(visible_frames))
|
||||
)
|
||||
for idx in random_images:
|
||||
c, f = visible_frames[idx]
|
||||
img = cv.cvtColor(cv.imread(all_images_names[c][f]), cv.COLOR_BGR2RGB)
|
||||
cv.drawChessboardCorners(img, grid_size, image_points_cameras[c][f], True)
|
||||
plt.figure()
|
||||
plt.imshow(img)
|
||||
plt.show()
|
||||
|
||||
if points_json_file:
|
||||
image_points_cameras_list = []
|
||||
for pts_cam in image_points_cameras:
|
||||
cam_pts = []
|
||||
for pts_frame in pts_cam:
|
||||
if pts_frame is not None:
|
||||
cam_pts.append(pts_frame.tolist())
|
||||
else:
|
||||
cam_pts.append([])
|
||||
image_points_cameras_list.append(cam_pts)
|
||||
|
||||
with open(points_json_file, 'w') as wf:
|
||||
json.dump({
|
||||
'object_points': pattern.tolist(),
|
||||
'image_points': image_points_cameras_list,
|
||||
'image_sizes': image_sizes,
|
||||
'is_fisheye': is_fisheye,
|
||||
}, wf)
|
||||
|
||||
Ks = None
|
||||
distortions = None
|
||||
if intrinsics_dir:
|
||||
# Read camera instrinsic matrices (Ks) and dictortions
|
||||
Ks, distortions = [], []
|
||||
for cam_id in cam_ids:
|
||||
input_file = os.path.join(intrinsics_dir, f"cameraParameters_{cam_id}.xml")
|
||||
print("Reading intrinsics from", input_file)
|
||||
storage = cv.FileStorage(input_file, cv.FileStorage_READ)
|
||||
camera_matrix = storage.getNode('cameraMatrix').mat()
|
||||
dist_coeffs = storage.getNode('dist_coeffs').mat()
|
||||
Ks.append(camera_matrix)
|
||||
distortions.append(dist_coeffs)
|
||||
find_intrinsics_in_python = True
|
||||
|
||||
return calibrateFromPoints(
|
||||
pattern,
|
||||
image_points_cameras,
|
||||
image_sizes,
|
||||
is_fisheye,
|
||||
all_images_names,
|
||||
find_intrinsics_in_python,
|
||||
Ks=Ks,
|
||||
distortions=distortions,
|
||||
)
|
||||
|
||||
|
||||
def calibrateFromJSON(json_file, find_intrinsics_in_python):
|
||||
assert os.path.exists(json_file)
|
||||
data = json.load(open(json_file, 'r'))
|
||||
|
||||
for i in range(len(data['image_points'])):
|
||||
for j in range(len(data['image_points'][i])):
|
||||
data['image_points'][i][j] = np.array(data['image_points'][i][j], dtype=np.float32)
|
||||
|
||||
Ks = data['Ks'] if 'Ks' in data else None
|
||||
distortions = data['distortions'] if 'distortions' in data else None
|
||||
images_names = data['images_names'] if 'images_names' in data else None
|
||||
|
||||
return calibrateFromPoints(
|
||||
np.array(data['object_points'], dtype=np.float32).T,
|
||||
data['image_points'],
|
||||
data['image_sizes'],
|
||||
data['is_fisheye'],
|
||||
images_names,
|
||||
find_intrinsics_in_python,
|
||||
Ks,
|
||||
distortions,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--json_file', type=str, default=None, help="json file with all data. Must have keys: 'object_points', 'image_points', 'image_sizes', 'is_fisheye'")
|
||||
parser.add_argument('--filenames', type=str, default=None, help='Txt files containg image lists, e.g., cam_1.txt,cam_2.txt,...,cam_N.txt for N cameras')
|
||||
parser.add_argument('--pattern_size', type=str, default=None, help='pattern size: width,height')
|
||||
parser.add_argument('--pattern_type', type=str, default=None, help='supported: checkeboard, circles, acircles')
|
||||
parser.add_argument('--fisheye', type=str, default=None, help='fisheye mask, e.g., 0,1,...')
|
||||
parser.add_argument('--pattern_distance', type=float, default=None, help='distance between object / pattern points')
|
||||
parser.add_argument('--find_intrinsics_in_python', required=False, action='store_true', help='calibrate intrinsics in Python sample instead of C++')
|
||||
parser.add_argument('--winsize', type=str, default='5,5', help='window size for corners detection: w,h')
|
||||
parser.add_argument('--debug_corners', required=False, action='store_true', help='debug flag for corners detection visualization of images')
|
||||
parser.add_argument('--points_json_file', type=str, default='', help='if path is provided then image and object points will be saved to JSON file.')
|
||||
parser.add_argument('--path_to_save', type=str, default='', help='path and filename to save results in yaml file')
|
||||
parser.add_argument('--path_to_visualize', type=str, default='', help='path to results pickle file needed to run visualization')
|
||||
parser.add_argument('--visualize', required=False, action='store_true', help='visualization flag. If set, only runs visualization but path_to_visualize must be provided')
|
||||
parser.add_argument('--resize_image_detection', required=False, action='store_true', help='If set, an image will be resized to speed-up corners detection')
|
||||
parser.add_argument('--intrinsics_dir', type=str, default='', help='Path to measured intrinsics')
|
||||
|
||||
params, _ = parser.parse_known_args()
|
||||
|
||||
if params.visualize:
|
||||
assert os.path.exists(params.path_to_visualize), f'Path to result file does not exist: {params.path_to_visualize}'
|
||||
visualizeFromFile(params.path_to_visualize)
|
||||
sys.exit(0)
|
||||
|
||||
if params.filenames is None:
|
||||
cam_files = sorted(glob.glob('cam_*.txt'))
|
||||
params.filenames = ','.join(cam_files)
|
||||
print('Found camera filenames:', params.filenames)
|
||||
params.fisheye = ','.join('0' * len(cam_files))
|
||||
print('Fisheye parameters:', params.fisheye) # TODO: Calculate it automatically
|
||||
|
||||
if params.json_file is not None:
|
||||
output = calibrateFromJSON(params.json_file, params.find_intrinsics_in_python)
|
||||
else:
|
||||
if (params.pattern_type is None and params.pattern_size is None and params.pattern_distance is None):
|
||||
assert False and 'Either json file or all other parameters must be set'
|
||||
|
||||
# cam_N.txt --> cam_N --> N
|
||||
cam_ids = [os.path.splitext(f)[0].split('_')[-1] for f in params.filenames.split(',')]
|
||||
|
||||
output = calibrateFromImages(
|
||||
files_with_images=params.filenames.split(','),
|
||||
grid_size=[int(v) for v in params.pattern_size.split(',')],
|
||||
pattern_type=params.pattern_type,
|
||||
is_fisheye=[bool(int(v)) for v in params.fisheye.split(',')],
|
||||
dist_m=params.pattern_distance,
|
||||
winsize=tuple([int(v) for v in params.winsize.split(',')]),
|
||||
points_json_file=params.points_json_file,
|
||||
debug_corners=params.debug_corners,
|
||||
RESIZE_IMAGE=params.resize_image_detection,
|
||||
find_intrinsics_in_python=params.find_intrinsics_in_python,
|
||||
cam_ids=cam_ids,
|
||||
intrinsics_dir=params.intrinsics_dir,
|
||||
)
|
||||
output['cam_ids'] = cam_ids
|
||||
|
||||
visualizeResults(**output)
|
||||
|
||||
print('Saving:', params.path_to_save)
|
||||
saveToFile(params.path_to_save, **output)
|
||||
Loading…
Reference in New Issue
Block a user