def EstimateFundamentalMatrixRANSAC(img1pts,
img2pts,
outlierThres,
prob=None,
iters=None):
if img1pts.shape[1] == 2:
#converting to homogenous coordinates if not already
img1pts = cv2.convertPointsToHomogeneous(img1pts)[:, 0, :]
img2pts = cv2.convertPointsToHomogeneous(img2pts)[:, 0, :]
bestInliers, bestF, bestmask = 0, None, None
for i in range(iters):
#Selecting 8 random points
mask = np.random.randint(low=0, high=img1pts.shape[0], size=(8, ))
img1ptsiter = img1pts[mask]
img2ptsiter = img2pts[mask]
#Fitting fundamental matrix and evaluating error
Fiter = EstimateFundamentalMatrix(img1ptsiter, img2ptsiter)
err = SampsonError(Fiter, img1pts, img2pts)
mask = err < outlierThres
numInliers = np.sum(mask)
#Updating best measurements if appropriate
if bestInliers < numInliers:
bestInliers = numInliers
bestF = Fiter
bestmask = mask
#Final least squares fit on all inliers found..
F = EstimateFundamentalMatrix(img1pts[bestmask], img2pts[bestmask])
return F, bestmask
def GetTriangulatedPts(img1pts,
img2pts,
K,
R,
t,
triangulateFunc,
Rbase=None,
tbase=None):
if Rbase is None:
Rbase = np.eye((3, 3))
if tbase is None:
tbase = np.zeros((3, 1))
img1ptsHom = cv2.convertPointsToHomogeneous(img1pts)[:, 0, :]
img2ptsHom = cv2.convertPointsToHomogeneous(img2pts)[:, 0, :]
img1ptsNorm = (np.linalg.inv(K).dot(img1ptsHom.T)).T
img2ptsNorm = (np.linalg.inv(K).dot(img2ptsHom.T)).T
img1ptsNorm = cv2.convertPointsFromHomogeneous(img1ptsNorm)[:, 0, :]
img2ptsNorm = cv2.convertPointsFromHomogeneous(img2ptsNorm)[:, 0, :]
print(Rbase.shape, tbase.shape, R.shape, t.shape)
pts4d = triangulateFunc(np.hstack((Rbase, tbase)), np.hstack((R, t)),
img1ptsNorm.T, img2ptsNorm.T)
pts3d = cv2.convertPointsFromHomogeneous(pts4d.T)[:, 0, :]
return pts3d
def getTriangulatedPoints(image_points1, image_points2, R2, t2, K, R1=[], t1=[]): """ Get the triangulated world points on the basis of corresponding image points and a rotation matrix, displacement vector and calibration matrix. The rotation and translation of the first camera can optionally also be given. """ # 3D-reconstruction: ip1 = cv2.convertPointsToHomogeneous(image_points1.astype(np.float32)); ip2 = cv2.convertPointsToHomogeneous(image_points2.astype(np.float32)); # P1 is at the origin P1 = np.zeros([3, 4]); # default assumption is that the first camera is at the origin: if(len(R1) == 0): P1[:3,:3] = np.eye(3); else: P1[:3,:3] = R1; if(len(t1) != 0): P1[:3,3] = np.array([t1[0][0], t1[1][0], t1[2][0]]); P1 = np.dot(K, P1); # P2 is the displaced camera: P2_est = getProjectionMatrix(R2, t2, K); # reconstruct the points: X_est = triangulate(ip1, ip2, P1, P2_est); return X_est;
def match(c1, c2, F):
'''
Find the corresponding point in image 2 of the centers in the firsrt view
by estimating the epipolar constraint (x'^T F x = 0) between the points in
the first view and all the possible correspondences in the second view.
Matches with lowest epipolar constraint scores (ideally 0) are the true
correspondences.
input:
c1: 3 x 1 x 2 matrix with image coordinates of concentric circle
centers in camera 1
c2: 3 x 1 x 2 matrix with image coordinates of concentric circle
centers in camera 2
F: 3 x 3 Fundamental matrix
output:
* c2 correspondences of points c1 (c2 rearranged).
'''
# Convert points to homogeneous
x1 = cv2.convertPointsToHomogeneous(c1)
x2 = cv2.convertPointsToHomogeneous(c2)
# Permutation of all possible mathces of the three points in the 2nd view
indexes = np.array(list(itertools.permutations([0, 1, 2])))
# Estimate the epipolar constraint with all the possible matches of c1
# epc^ijk = x'^ijr F_rs x^ks
epc = np.einsum('ijr,rs,ks->ijk', x2[indexes].reshape(6, 3, 3), F,
x1[:, 0, :])
# Scores are in the diagonal of each 2D array of the 3D tensor
# We look for the lowest norm of the values in the diagonal
ind = np.argmin(np.linalg.norm(np.einsum('ijj->ij', epc), axis=1))
return c2[indexes[ind]]
def EstimateFundamentalMatrixNormalized(x1, x2):
if x1.shape[1] == 2: #converting to homogenous coordinates if not already
x1 = cv2.convertPointsToHomogeneous(x1)[:, 0, :]
x2 = cv2.convertPointsToHomogeneous(x2)[:, 0, :]
x1Norm, T1 = NormalizePts(x1)
x2Norm, T2 = NormalizePts(x2)
F = EstimateFundamentalMatrix(x1Norm, x2Norm)
F = T1.T.dot(F.dot(T2))
return F
def image_to_camera(self, points, depth=1):
if self.distortion_coeffs is None:
normalized_points = ((
(points - self.intrinsic_matrix[:2, 2]) @ np.linalg.inv(
self.intrinsic_matrix[:2, :2])))
return cv2.convertPointsToHomogeneous(
normalized_points)[:, 0, :] * depth
points = np.expand_dims(np.asarray(points, np.float32), 0)
new_image_points = cv2.undistortPoints(points, self.intrinsic_matrix,
self.distortion_coeffs, None,
None, None)
return cv2.convertPointsToHomogeneous(new_image_points)[:,
0, :] * depth
def main(opts):
#Loading 5th and 6th image data only (hardcoded for now)..
with open('../data/fountain-P11/images/keypoints_descriptors/0005.pkl'
) as fileobj:
data1 = pkl.load(fileobj)
kp1, desc1 = data1
kp1 = DeserializeKeypoints(kp1)
with open('../data/fountain-P11/images/keypoints_descriptors/0006.pkl'
) as fileobj:
data2 = pkl.load(fileobj)
kp2, desc2 = data2
kp2 = DeserializeKeypoints(kp2)
with open('../data/fountain-P11/images/matches/matches.pkl') as fileobj:
matches = pkl.load(fileobj)
matches = matches[('0005.pkl', '0006.pkl')]
matches = DeserializeMatchesDict(matches)
#2/4. FUNDAMENTAL MATRIX ESTIMATION
img1pts, img2pts = GetAlignedMatches(kp1, desc1, kp2, desc2, matches)
F, mask = cv2.findFundamentalMat(img1pts,
img2pts,
method=cv2.FM_RANSAC,
param1=opts.outlierThres,
param2=opts.fundProb)
#3/4. CAMERA POSE ESTIMATION
K = np.array([[2759.48, 0, 1520.69], [0, 2764.16, 1006.81],
[0, 0, 1]]) #hardcoded for now, have to generalize..
E = K.T.dot(F.dot(K))
retval, R, t, mask2 = cv2.recoverPose(E, img1pts[mask], img2pts[mask], K)
#4/4. TRIANGULATION.
img1ptsHom = cv2.convertPointsToHomogeneous(img1pts[mask])[:, 0, :]
img2ptsHom = cv2.convertPointsToHomogeneous(img2pts[mask])[:, 0, :]
img1ptsNorm = (np.linalg.inv(K).dot(img1ptsHom.T)).T
img2ptsNorm = (np.linalg.inv(K).dot(img2ptsHom.T)).T
img1ptsNorm = cv2.convertPointsFromHomogeneous(img1ptsNorm)[:, 0, :]
img2ptsNorm = cv2.convertPointsFromHomogeneous(img2ptsNorm)[:, 0, :]
pts4d = cv2.triangulatePoints(np.eye(3, 4), np.hstack((R, t)),
img1ptsNorm.T, img2ptsNorm.T)
pts3d = cv2.convertPointsFromHomogeneous(pts4d.T)[:, 0, :]
#Finally, saving 3d points in .ply format to view in meshlab software
pts2ply(pts3d)
def GetTriangulatedPts(img1pts, img2pts, K, R, t):
img1ptsHom = cv2.convertPointsToHomogeneous(img1pts)[:, 0, :]
img2ptsHom = cv2.convertPointsToHomogeneous(img2pts)[:, 0, :]
img1ptsNorm = (np.linalg.inv(K).dot(img1ptsHom.T)).T
img2ptsNorm = (np.linalg.inv(K).dot(img2ptsHom.T)).T
img1ptsNorm = cv2.convertPointsFromHomogeneous(img1ptsNorm)[:, 0, :]
img2ptsNorm = cv2.convertPointsFromHomogeneous(img2ptsNorm)[:, 0, :]
pts4d = cv2.triangulatePoints(np.eye(3, 4), np.hstack((R, t)),
img1ptsNorm.T, img2ptsNorm.T)
pts3d = cv2.convertPointsFromHomogeneous(pts4d.T)[:, 0, :]
return pts3d
def selectCorrectRotationTranslation(image_points1, image_points2, K, R21, R22, t21, t22, R1 = [], l1 = []): """ The 8-point algorithm gives back four options for the rotation and translation matrix. The matrices that result in all points lying in front of the cameras should be selected. """ if(len(R1) == 0 or len(l1) == 0): # location camera 1: l1 = np.zeros([3,1]); R1 = np.eye(3); # 3D-reconstruction: ip1 = cv2.convertPointsToHomogeneous(image_points1.astype(np.float32)); ip2 = cv2.convertPointsToHomogeneous(image_points2.astype(np.float32)); # P1 is at the origin P1 = np.zeros([3, 4]); P1[:3,:3] = np.eye(3); P1 = np.dot(K, P1); # P2 is rotated and translated with respect to camera 1 # determine the right R, t: # iterate over all points, reproject them to the 3D-world # exclude one of the options as soon as # first determine all 4 projection matrices: R2s = []; R2s.append(R21); R2s.append(R22); t2s = []; t2s.append(t21); t2s.append(t22); # reproject the points into the 3D-world: index_r = 0; index_t = 0; for ir in range(2): # clean up the rotation matrix, i.e., don't allow mirroring of any axis: R2s[ir] = cleanUpR(R2s[ir]); for it in range(2): point_behind = infeasibleP2(ip1, ip2, R1, l1, R2s[ir], t2s[it], K); if(point_behind == 0): index_r = ir; index_t = it; print 'ir, it = %d, %d' % (ir, it) P2_est = getProjectionMatrix(R2s[index_r], t2s[index_t], K); R2_est = R2s[index_r]; t2_est = t2s[index_t]; return (P2_est, R2_est, t2_est);
def project(objectPoints, K, R, t):
print "--------- MANUAL PROJECTION -----------"
objectPoints = cv2.convertPointsToHomogeneous(objectPoints)
objectPoints = fixExtraneousParentheses(objectPoints)
imagePoints = []
print "K:\n", K
t = np.mat(t)
t = t.T
print "R:\n", R
print "t:\n", t
Rt = np.concatenate((R, t), 1)
print "R|t:\n", Rt
P = K * Rt
print "P = k(R|t):\n", P
for X in objectPoints:
x = np.mat(X).T
x_ = P * x
imagePoints.append(x_)
# image points are homogeneous (xz, yz, z) -> (x, y, 1)
normed = []
for p in imagePoints:
p = p / p[2]
normed.append(p)
normed = np.array(normed)
normed = np.delete(normed, 2, 1)
return normed
def dataIncreasing_camera(img):
#这里包含了相机的视角的变换
ax = np.random.uniform(0, np.pi / 4)
ay = np.random.uniform(0, np.pi / 4)
az = np.random.uniform(0, np.pi / 6)
gAlpha = np.random.uniform(15, 100)
gGamma = 2
s = np.random.uniform(0.1, 2)
height, width, channel = img.shape
pts = [[-height / 2, -width / 2, 0], [-height / 2, width / 2, 0],
[height / 2, -width / 2, 0], [height / 2, width / 2, 0]]
pts = np.array([pts])
ptsv = cv2.convertPointsToHomogeneous(pts)
Rs = np.mat([[s, 0, 0, 0], [0, s, 0, 0], [0, 0, s, 0], [0, 0, 0, 1]])
Rx = np.mat([[1, 0, 0, 0], [0, np.cos(ax), np.sin(ax), 0],
[0, -np.sin(ax), np.cos(ax), 0], [0, 0, 0, 1]])
Ry = np.mat([[np.cos(ay), 0, np.sin(ay), 0], [0, 1, 0, 0],
[-np.sin(ay), 0, np.cos(ay), 0], [0, 0, 0, 1]])
Rz = np.mat([[np.cos(az), np.sin(az), 0, 0],
[-np.sin(az), np.cos(az), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
dspts = (Rz * Ry * Rx * Rs * ptsv.T).T
dst = dspts[:, 0:3]
d = np.array([[
x[0, 0] * height / (height + x[0, 2]),
x[0, 1] * width / (x[0, 2] + width)
] for x in dst])
H, k = cv2.findHomography(pts, d)
out = cv2.warpPerspective(img, H, (height * 3, width * 3))
#l=np.float32(1)
#
# cv2.pow(out,l,out)
#out=img
cv2.addWeighted(out, (gAlpha / 50.0), out, 0, gGamma, out)
return out, H
def undistort_unproject_pts(pts_uv, camera_matrix, dist_coefs):
"""
This function converts a set of 2D image coordinates to vectors in pinhole camera space.
Hereby the intrinsics of the camera are taken into account.
UV is converted to normalized image space (think frustum with image plane at z=1) then undistored
adding a z_coordinate of 1 yield vectors pointing from 0,0,0 to the undistored image pixel.
@return: ndarray with shape=(n, 3)
"""
pts_uv = np.array(pts_uv)
num_pts = pts_uv.size / 2
if pts_uv.shape[1] == 2:
pass
elif pts_uv.shape[0] == 2:
pts_uv = pts_uv.T
else:
raise('change shape of pointcloud to (num_points, 2) oder (2,num_points)')
pts_uv.shape = (int(num_pts), 1, 2) # pts_uv = pts_uv.reshape(int(num_pts), 1, 2)
pts_uv = cv2.undistortPoints(pts_uv, camera_matrix, dist_coefs)
pts_3d = cv2.convertPointsToHomogeneous(np.float32(pts_uv))
pts_3d.shape = (int(num_pts), 3)
return pts_3d
def project(objectPoints, K, R, t):
print "--------- MANUAL PROJECTION -----------"
objectPoints = cv2.convertPointsToHomogeneous(objectPoints)
objectPoints = fixExtraneousParentheses(objectPoints)
imagePoints = []
print "K:\n", K
t = np.mat(t)
t = t.T
print "R:\n", R
print "t:\n", t
Rt = np.concatenate((R, t), 1)
print "R|t:\n", Rt
P = K * Rt
print "P = k(R|t):\n", P
for X in objectPoints:
x = np.mat(X).T
x_ = P * x
imagePoints.append(x_)
# image points are homogeneous (xz, yz, z) -> (x, y, 1)
normed = []
for p in imagePoints:
p = p / p[2]
normed.append(p)
normed = np.array(normed)
normed = np.delete(normed, 2, 1)
return normed
def distort_points(self, pts_in, intrinsics, distortion_model,
distortion_coeffs):
"""
Arguments:
pts_in: points to be distorted.
intrinsics: intrinsics of the camera.
distortion_model: distortion model of the camera.
distortion_coeffs: distortion coefficients.
Returns:
pts_out: distorted points. (N, 2)
"""
if len(pts_in) == 0:
return []
K = np.array([[intrinsics[0], 0.0, intrinsics[2]],
[0.0, intrinsics[1], intrinsics[3]], [0.0, 0.0, 1.0]])
if distortion_model == 'equidistant':
pts_out = cv2.fisheye.distortPoints(pts_in, K, distortion_coeffs)
else: # default: 'radtan'
homogenous_pts = cv2.convertPointsToHomogeneous(pts_in)
pts_out, _ = cv2.projectPoints(homogenous_pts, np.zeros(3),
np.zeros(3), K, distortion_coeffs)
return pts_out.reshape((-1, 2))
def unprojectPoints(self, pts_2d, use_distortion=True, normalize=False):
"""
Undistorts points according to the camera model.
:param pts_2d, shape: Nx2
:return: Array of unprojected 3d points, shape: Nx3
"""
pts_2d = np.array(pts_2d, dtype=np.float32)
# Delete any posibly wrong 3rd dimension
if pts_2d.ndim == 1 or pts_2d.ndim == 3:
pts_2d = pts_2d.reshape((-1, 2))
# Add third dimension the way cv2 wants it
if pts_2d.ndim == 2:
pts_2d = pts_2d.reshape((-1, 1, 2))
if use_distortion:
_D = self.D
else:
_D = np.asarray([[0.0, 0.0, 0.0, 0.0, 0.0]])
pts_2d_undist = cv2.undistortPoints(pts_2d, self.K, _D)
pts_3d = cv2.convertPointsToHomogeneous(pts_2d_undist)
pts_3d.shape = -1, 3
if normalize:
pts_3d /= np.linalg.norm(pts_3d, axis=1)[:, np.newaxis]
return pts_3d
def plot_distorted_image(_fig, _i, _cam, title, degrees_per_step=5):
plt.subplot(2, 3, _i + 1)
fov = _cam.fov
# fov_overshoot = .2
u_ = np.linspace(
0, _cam.resolution[0],
int(fov[0] / degrees_per_step) +
1) # Convert FOV into pixels (pixel coords are 0 at top left
v_ = np.linspace(0, _cam.resolution[1], int(fov[1] / degrees_per_step) + 1)
# u_ = u_*fov_overshoot - _cam.resolution[0]*fov_overshoot/2
# v_ = u_*fov_overshoot - _cam.resolution[0]*fov_overshoot/2
M = len(u_)
N = len(v_)
u, v = np.meshgrid(u_, v_) # create meshgrid
#
# plt.plot(u-np.max(u)/2, v - np.max(v)/2, color='r', linestyle='-', linewidth=.5)
# plt.plot(u.T - np.max(u)/2, v.T - np.max(v)/2, color='r', linestyle='-', linewidth=.5)
u = u.reshape(-1) # stack all grid points into column to pass to undistort
v = v.reshape(-1)
uv_ = np.expand_dims(np.vstack([u, v]).T, axis=1)
distCoeffs = np.array([_cam.k1, _cam.k2, _cam.p1, _cam.p2, _cam.k3])
K_new = np.array([[_cam.resolution[0] / 2, 0, -.5],
[0, _cam.resolution[0] / 2, -.5], [0, 0, 1]])
pts = cv.convertPointsToHomogeneous(src=uv_)
undistored_pts = cv.undistortPoints(src=uv_,
cameraMatrix=_cam.K,
distCoeffs=distCoeffs,
P=K_new).squeeze()
undistored_pts = undistored_pts.reshape(N, M, 2)
xs = undistored_pts[:, :, 0]
ys = undistored_pts[:, :, 1]
plt.plot(xs,
ys,
color='royalblue',
marker='o',
markersize=3,
linestyle='-')
plt.plot(xs.T,
ys.T,
color='royalblue',
marker='o',
markersize=3,
linestyle='-')
if (i) % 3 == 0:
plt.ylabel('Undistored Image Y Coordinate')
else:
plt.yticks([])
if (i >= 3):
plt.xlabel('Undistorted Image X Coordinate')
else:
plt.xticks([])
plt.title(title)
plt.xlim([-1400, 1400])
plt.ylim([-800, 800])
def undistort_unproject_pts(pts_uv, camera_matrix, dist_coefs):
"""
This function converts a set of 2D image coordinates to the spherical coordinate system.
Hereby the intrinsics of the camera are taken into account.
The 2d point set gets undistorted, converted to cartesian vertices and then converted to spherical coordinates.
@return: ndarray with shape=(n, 3)
"""
pts_uv = np.array(pts_uv)
camera_matrix_inv = np.linalg.inv(camera_matrix)
num_pts = pts_uv.size / 2
pts_uv.shape = (num_pts, 1, 2)
pts_uv = cv2.undistortPoints(pts_uv,
camera_matrix,
dist_coefs,
P=camera_matrix)
# return pts_uv
# P = camera_matrix enables denormalization as follows:
# ```
# pts_uv *= np.array([camera_matrix[0,0], camera_matrix[1,1]]) # [fx, fy]
# pts_uv += np.array([camera_matrix[0,2], camera_matrix[1,2]]) # [cx, cy]
# ```
pts_h = cv2.convertPointsToHomogeneous(np.float32(pts_uv))
pts_h.shape = (num_pts, 3)
xyz = np.zeros((num_pts, 3), dtype=np.float32)
for i in range(num_pts):
xyz[i] = camera_matrix_inv.dot(pts_h[i])
return xyz
def estimate_motion(self, g0, g1):
now_h = cv2.convertPointsToHomogeneous(g1)
prev_h = cv2.convertPointsToHomogeneous(g0)
# print("test", now_h, self.p1)
motion_avg = np.array((0, 0, 0), dtype="float")
if prev_h is not None:
n = len(prev_h)
if n > 0:
for i in range(n):
motion_avg += (now_h[i][0] - prev_h[i][0]) / n
t = time.time() - self.pr_t
self.pr_t = time.time()
sh = self.frame.shape
motion_avg[0] /= sh[1]
motion_avg[1] /= sh[0]
return motion_avg / t
def preprocess_3d_data(matched_data, g_pool):
ref_processed = []
pupil0_processed = []
pupil1_processed = []
is_binocular = len(matched_data[0]) == 3
for data_point in matched_data:
try:
# taking the pupil normal as line of sight vector
pupil0 = data_point['pupil']
gaze_vector0 = np.array(pupil0['circle_3d']['normal'])
pupil0_processed.append(gaze_vector0)
if is_binocular: # we have binocular data
pupil1 = data_point['pupil1']
gaze_vector1 = np.array(pupil1['circle_3d']['normal'])
pupil1_processed.append(gaze_vector1)
# projected point uv to normal ray vector of camera
ref = data_point['ref']['screen_pos']
ref_vector = np.array(ref).reshape(-1, 1, 2)
ref_vector = g_pool.capture.intrinsics.undistortPoints(ref_vector)
ref_vector = cv2.convertPointsToHomogeneous(np.float32(ref_vector))
ref_vector.shape = (-1, 3)
ref_vector = ref_vector.tolist()[0]
ref_vector = ref_vector / np.linalg.norm(ref_vector)
# assuming a fixed (assumed) distance we get a 3d point in world camera 3d coords.
ref_processed.append(ref_vector)
except KeyError as e:
# this pupil data point did not have 3d detected data.
pass
return ref_processed, pupil0_processed, pupil1_processed
def get_distor_point(pt, mtx, dist):
""" Gets the coordinate equivalent in original view space from surface
projection space
Args:
pt: `numpy.darray` point in undistorted image
mtx: `numpy.narray` camera's distortion matrix
dist: `numpy.narray` camera's distortion vector
Returns:
distor_point: `numpy.darray` coordinate in image with distortion
http://answers.opencv.org/question/148670/re-distorting-a-set-of-points-af
ter-camera-calibration/
"""
test = np.zeros((1, 1, 2), dtype=np.float32)
test[0, 0, 0] = pt[0]
test[0, 0, 1] = pt[1]
rtemp = ttemp = np.array([0, 0, 0], dtype='float32')
# Normalize the points to be independent of the camera matrix using undistorted points with no distortion matrix
xy_normalized = cv2.undistortPoints(test, mtx, None)
# Convert them to 3d points
ptsTemp = cv2.convertPointsToHomogeneous(xy_normalized)
# Project them back to image space using the distortion matrix
output = cv2.projectPoints(ptsTemp, rtemp, ttemp, mtx, dist, xy_normalized)
x_undistor = output[0][0, 0, 0]
y_undistor = output[0][0, 0, 1]
distor_point = int(round(x_undistor)), int(round(y_undistor))
return distor_point
def findFocusLengths(objpoints, imgpoints):
px = 750
py = 500
f = []
v = []
for i in range(len(objpoints)):
imgpoints[i] = cv2.convertPointsToHomogeneous(imgpoints[i])
objpoints[i][:, :, 2] = 1
h = cv2.findHomography(objpoints[i], imgpoints[i])
h = h[0]
tmp = px * h[0, 0] / h[2, 0] + px * h[0, 1] / h[2, 1] + py * h[
1, 0] / h[2, 0] + py * h[1, 1] / h[2, 1] - (
h[0, 0] * h[0, 1] +
h[1, 0] * h[1, 1]) / (h[2, 0] * h[2, 1]) - px**2 - py**2
f.append(np.sqrt(tmp))
right = findVanishingPoint([
imgpoints[i][0][0], imgpoints[i][8][0], imgpoints[i][27][0],
imgpoints[i][35][0], imgpoints[i][45][0], imgpoints[i][53][0]
])
up = findVanishingPoint([
imgpoints[i][0][0], imgpoints[i][45][0], imgpoints[i][8][0],
imgpoints[i][53][0], imgpoints[i][4][0], imgpoints[i][49][0]
])
tmp = px * up[0] / up[2] + px * right[0] / right[2] + py * up[1] / up[
2] + py * right[1] / right[2] - (
up[0] * right[0] +
up[1] * right[1]) / (up[2] * right[2]) - px**2 - py**2
v.append(np.sqrt(tmp))
print('pic ', i + 1, ' *method 1:', f[i], ' *method 2:', v[i])
def fundamentalMatErrorRMS(lines, pts):
'''Calculate the error of the Fundamental Matrix estimate by finding the
RMS of the orthogonal distances from points [PTS] to their
corresponding epipolar LINES. Epipolar lines can be generated using
cv2.computeCorrespondEpilines().
Orthogonal distance formula from point to line:
d = |ax + by + c| / sqrt(a^2 + b^2)
lines - [N_LINES x 3] matrix of coefficients for lines of the form
Ax + By + C = 0.
pts - [N_PTS x 3] or [N_PTS x 2] matrix of [x y] or homogeneous [x y 1]
values'''
# If PTS is not homogeneous, convert to homogenous coordinates.
if pts.shape[1] == 2:
pts = np.squeeze(cv2.convertPointsToHomogeneous(pts))
else:
pts = np.squeeze(pts)
# TODO: np.matmul?
dist = np.diagonal(abs(np.dot(
lines, np.transpose(pts)))) / np.sqrt(lines[:, 0]**2 + lines[:, 1]**2)
rms = np.sqrt((dist**2).sum())
return rms
def reproject_image(
image, old_camera, new_camera, output_imshape, border_mode=cv2.BORDER_CONSTANT,
border_value=0, interp=None):
"""Transforms an image captured with `old_camera` to look like it was captured by
`new_camera`. The optical center (3D world position) of the cameras must be the same, otherwise
we'd have parallax effects and no unambiguous way to construct the output."""
if old_camera.distortion_coeffs is None and new_camera.distortion_coeffs is None:
return reproject_image_fast(
image, old_camera, new_camera, output_imshape, border_mode, border_value)
if not np.allclose(old_camera.t, new_camera.t):
raise Exception(
'The optical center of the camera must not change, else warping is not enough!')
output_size = (output_imshape[1], output_imshape[0])
# 1. Simplest case: if only the intrinsics have changed we can use an affine warp
if (np.allclose(new_camera.R, old_camera.R) and
allclose_or_nones(new_camera.distortion_coeffs, old_camera.distortion_coeffs)):
relative_intrinsics_inv = np.linalg.solve(
new_camera.intrinsic_matrix.T, old_camera.intrinsic_matrix.T).T
scaling_factor = 1 / np.linalg.norm(relative_intrinsics_inv[:2, 0])
if interp is None:
interp = cv2.INTER_LINEAR if scaling_factor > 1 else cv2.INTER_AREA
return cv2.warpAffine(
image, relative_intrinsics_inv[:2], output_size, flags=cv2.WARP_INVERSE_MAP | interp,
borderMode=border_mode, borderValue=border_value)
# 2. The general case handled by transforming the coordinates of every pixel
# (i.e. computing the source pixel coordinates for each destination pixel)
# and remapping (i.e. resampling the image at the resulting coordinates)
y, x = np.mgrid[:output_imshape[0], :output_imshape[1]].astype(np.float32)
new_maps = np.stack([x, y], axis=-1)
newim_coords = new_maps.reshape([-1, 2])
if new_camera.distortion_coeffs is None:
partial_homography = (
old_camera.R @ np.linalg.inv(new_camera.R) @
np.linalg.inv(new_camera.intrinsic_matrix))
new_im_homogeneous = cv2.convertPointsToHomogeneous(newim_coords)[:, 0, :]
old_camera_coords = new_im_homogeneous @ partial_homography.T
oldim_coords = old_camera.camera_to_image(old_camera_coords)
else:
world_coords = new_camera.image_to_world(newim_coords)
oldim_coords = old_camera.world_to_image(world_coords)
old_maps = oldim_coords.reshape(new_maps.shape).astype(np.float32)
# For cv2.remap, we need to provide a grid of lookup pixel coordinates for
# each output pixel.
if interp is None:
interp = cv2.INTER_LINEAR
remapped = cv2.remap(
image, old_maps, None, interp, borderMode=border_mode, borderValue=border_value)
if remapped.ndim < image.ndim:
return np.expand_dims(remapped, -1)
return remapped
def unprojectPoints(self, pts_2d, use_distortion=True, normalize=False):
"""
Undistorts points according to the camera model.
:param pts_2d, shape: Nx2
:return: Array of unprojected 3d points, shape: Nx3
"""
pts_2d = np.array(pts_2d, dtype=np.float32)
# Delete any posibly wrong 3rd dimension
if pts_2d.ndim == 1 or pts_2d.ndim == 3:
pts_2d = pts_2d.reshape((-1, 2))
# Add third dimension the way cv2 wants it
if pts_2d.ndim == 2:
pts_2d = pts_2d.reshape((-1, 1, 2))
if use_distortion:
_D = self.D
else:
_D = np.asarray([[0.0, 0.0, 0.0, 0.0, 0.0]])
pts_2d_undist = cv2.undistortPoints(pts_2d, self.K, _D)
pts_3d = cv2.convertPointsToHomogeneous(pts_2d_undist)
pts_3d.shape = -1, 3
if normalize:
pts_3d /= np.linalg.norm(pts_3d, axis=1)[:, np.newaxis]
return pts_3d
def undistort_unproject_pts(pts_uv, camera_matrix, dist_coefs):
"""
This function converts a set of 2D image coordinates to the spherical coordinate system.
Hereby the intrinsics of the camera are taken into account.
The 2d point set gets undistorted, converted to cartesian vertices and then converted to spherical coordinates.
@return: ndarray with shape=(n, 3)
"""
pts_uv = np.array(pts_uv)
camera_matrix_inv = np.linalg.inv(camera_matrix)
num_pts = pts_uv.size / 2
pts_uv.shape = (num_pts, 1, 2)
pts_uv = cv2.undistortPoints(pts_uv, camera_matrix, dist_coefs, P=camera_matrix)
# return pts_uv
# P = camera_matrix enables denormalization as follows:
# ```
# pts_uv *= np.array([camera_matrix[0,0], camera_matrix[1,1]]) # [fx, fy]
# pts_uv += np.array([camera_matrix[0,2], camera_matrix[1,2]]) # [cx, cy]
# ```
pts_h = cv2.convertPointsToHomogeneous(np.float32(pts_uv))
pts_h.shape = (num_pts,3)
xyz = np.zeros((num_pts, 3), dtype=np.float32)
for i in range(num_pts):
xyz[i] = camera_matrix_inv.dot(pts_h[i])
return xyz
def __correct_distortion_contours(self, contours):
contours_no_distortion = []
for contour in contours:
camera_matrix = self.__calibration_results["new_camera_matrix"]
points_with_distortion = np.array(contour, dtype='float32')
points_without_distortion = cv.undistortPoints(
points_with_distortion, camera_matrix, None)
points_homogeneous = cv.convertPointsToHomogeneous(
points_without_distortion)
rotation_vector = np.array([0, 0, 0], dtype='float32')
translation_vector = np.array([0, 0, 0], dtype='float32')
distortion_coefficients = self.__calibration_results[
"distortion_coefficients"]
points_on_plane, _ = cv.projectPoints(points_homogeneous,
rotation_vector,
translation_vector,
camera_matrix,
distortion_coefficients,
points_without_distortion)
contours_no_distortion.append(points_on_plane)
return contours_no_distortion
def redistort_points(mtx, distCoeff, undist_points, imsize):
ptsOut = undist_points
ptsTemp = np.array([], dtype='float32')
rtemp = ttemp = np.array([0, 0, 0], dtype='float32')
ptsOut = cv2.undistortPoints(ptsOut, mtx, None)
ptsTemp = cv2.convertPointsToHomogeneous(ptsOut)
output = cv2.projectPoints(ptsTemp, rtemp, ttemp, mtx, distCoeff, ptsOut)
return output
def convertPointsToHomogeneous(image):
"""The function is part of the camera calibration library of OpenCV and
converts points from Euclidean space to homogeneous space.
The input tuple is thereby expanded by adding another dimension set to 1
for each point part of the tuple.
"""
return cv2.convertPointsToHomogeneous(image)
def homography(from_data, to_data, target_data):
"""Compute the homography transformation between the data sets via opencv"""
# find the homography transformation
h, mask = cv2.findHomography(from_data, to_data, method=cv2.RANSAC)
# make the transformed data homogeneous for multiplication with the affine
transformed_data = np.squeeze(cv2.convertPointsToHomogeneous(target_data))
# apply the homography matrix
return np.matmul(transformed_data, h.T)
def ConvertImagePointToCameraFrame(self, ImagePoint):
self.captureDepthFrame()
Zc = self.PixelToDepth(self.currentDepthFrame[ImagePoint[1],
ImagePoint[0]])
ImagePointHomogeneous = cv2.convertPointsToHomogeneous(
np.array([ImagePoint]))
PointInCameraFrame = Zc * np.matmul(
np.linalg.inv(self.IntrinsicMatrix), ImagePointHomogeneous[0][0])
return PointInCameraFrame
def sampson_distance(pts1, pts2, F, mask=None):
pts1_ = np.float64(pts1).reshape(-1,2)
pts2_ = np.float64(pts2).reshape(-1,2)
F_ = np.float64(F)
if mask is not None:
pts1_ = pts1_[np.bool_(mask.ravel())]
pts2_ = pts2_[np.bool_(mask.ravel())]
pts1_ = cv2.convertPointsToHomogeneous(pts1_).reshape(-1,3)
pts2_ = cv2.convertPointsToHomogeneous(pts2_).reshape(-1,3)
errors = []
for pt1, pt2 in zip(pts1_, pts2_):
errors.append(cv2.sampsonDistance(pt1[None], pt2[None], F_))
return np.mean(errors), errors
def __TriangulateTwoViews(img1pts, img2pts, R1, t1, R2, t2):
img1ptsHom = cv2.convertPointsToHomogeneous(img1pts)[:, 0, :]
img2ptsHom = cv2.convertPointsToHomogeneous(img2pts)[:, 0, :]
img1ptsNorm = (np.linalg.inv(self.K).dot(img1ptsHom.T)).T
img2ptsNorm = (np.linalg.inv(self.K).dot(img2ptsHom.T)).T
img1ptsNorm = cv2.convertPointsFromHomogeneous(img1ptsNorm)[:,
0, :]
img2ptsNorm = cv2.convertPointsFromHomogeneous(img2ptsNorm)[:,
0, :]
pts4d = cv2.triangulatePoints(np.hstack((R1, t1)),
np.hstack((R2, t2)), img1ptsNorm.T,
img2ptsNorm.T)
pts3d = cv2.convertPointsFromHomogeneous(pts4d.T)[:, 0, :]
return pts3d
def plot_charuco(R,
tvec,
charucoCorners,
charucoIds,
K=K_cam,
dist_coef=dist_coef,
ori_idx=0):
charucoCorners_normalized = cv2.convertPointsToHomogeneous(
cv2.undistortPoints(charucoCorners, K, dist_coef))
charucoCorners_normalized = np.squeeze(charucoCorners_normalized)
t = np.array(tvec)
plane_normal = R[
2, :] # last row of plane rotation matrix is normal to plane
plane_point = t.reshape(3, ) # t is a point on the plane
charucoCorners = []
epsilon = 1e-06
for p in charucoCorners_normalized:
p = p.reshape(3, )
ray_direction = p / np.linalg.norm(p)
ray_point = p
ndotu = plane_normal.dot(ray_direction.T)
if abs(ndotu) < epsilon:
print("no intersection or line is within plane")
w = ray_point - plane_point
si = -plane_normal.dot(w.T) / ndotu
v = w + si * ray_direction
Psi = w + si * ray_direction + plane_point
charucoCorners.append(Psi)
# dist = np.array([np.linalg.norm(tvec - np.array(pt)) for pt in charucoCorners])
ori = charucoCorners[ori_idx]
# print('Origin id: ', charucoIds[ori_idx], ' origin xyz: ', ori)
charuco_plot = []
charuco_plot.append(
mlab.points3d(ori[0],
ori[1],
ori[2],
scale_factor=0.01,
color=(1, 0, 0)))
mlab.text3d(ori[0],
ori[1],
ori[2],
str(charucoIds[ori_idx]),
scale=(0.01, 0.01, 0.01))
new_corners = np.delete(charucoCorners, (ori_idx), axis=0)
for pt in new_corners:
charuco_plot.append(
mlab.points3d(pt[0],
pt[1],
pt[2],
scale_factor=0.01,
color=(0, 0, 1)))
return charucoCorners, np.array(ori)
def undistort_pixel(dist_coeff, camera_matrix, pixel_coords, nr):
print("pixel_coord:\n", pixel_coords)
# after first camera calibration cam_mtx and distCoff, pixel_coords in form np.array([[[x, y]]], np.float32)
undist_pixel = cv2.undistortPoints(pixel_coords, camera_matrix, dist_coeff)
print("new pixel_coord:\n", undist_pixel[0][0])
pix1 = cv2.convertPointsToHomogeneous(undist_pixel)[0][0]
print("pix1:\n", pix1)
pix = np.dot(camera_matrix, np.transpose(pix1[np.newaxis]))
print("pix:\n", pix)
return
def affine(from_data, to_data, target_data):
"""Compute the 2D affine transformation between the data sets via opencv"""
# calculate an approximate affine
affine_matrix, inliers = cv2.estimateAffine2D(from_data, to_data, ransacReprojThreshold=3,
maxIters=20000, refineIters=0, method=cv2.LMEDS)
print('Percentage inliers used:' + str(np.sum(inliers)*100/from_data.shape[0]))
# make the transformed data homogeneous for multiplication with the affine
transformed_data = np.squeeze(cv2.convertPointsToHomogeneous(target_data))
# apply the affine matrix
return np.matmul(transformed_data, affine_matrix.T)
def unprojectPoints(pts_2d, use_distortion=True, normalize=False):
"""
Undistorts points according to the camera model.
cv2.fisheye.undistortPoints does *NOT* perform the same unprojection step the original cv2.unprojectPoints does.
Thus we implement this function ourselves.
https://github.com/pupil-labs/pupil/blob/6bef222339317092ac8e8392187d8485f4290979/pupil_src/shared_modules/camera_models.py#L342-L392
:param pts_2d:, shape: Nx2:
:param use_distortion:
:param normalize:
:return: Array of unprojected 3d points, shape: Nx3
"""
import cv2 # pip install opencv-python-headless
# intrinsics taken from dummy world camera with 1280 x 720 resolution.
intrinsics = {
'K': np.array([[1000., 0., 640.], [0., 1000., 360.], [0., 0., 1.]]),
'D': np.array([[0., 0., 0., 0., 0.]])
}
pts_2d = np.array(pts_2d.T, dtype=np.float32)
eps = np.finfo(np.float32).eps
f = np.array((intrinsics['K'][0, 0], intrinsics['K'][1, 1])).reshape(1, 2)
c = np.array((intrinsics['K'][0, 2], intrinsics['K'][1, 2])).reshape(1, 2)
if use_distortion:
k = intrinsics['D'].ravel().astype(np.float32)
else:
k = np.asarray([1.0 / 3.0, 2.0 / 15.0, 17.0 / 315.0, 62.0 / 2835.0],
dtype=np.float32)
pi = pts_2d.astype(np.float32)
pw = (pi - c) / f
theta_d = np.linalg.norm(pw, ord=2, axis=1)
theta = theta_d
for j in range(10):
theta2 = theta**2
theta4 = theta2**2
theta6 = theta4 * theta2
theta8 = theta6 * theta2
theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 +
k[3] * theta8)
scale = np.tan(theta) / (theta_d + eps)
pts_2d_undist = pw * scale.reshape(-1, 1)
pts_3d = cv2.convertPointsToHomogeneous(pts_2d_undist)
pts_3d.shape = -1, 3
if normalize:
pts_3d /= np.linalg.norm(pts_3d, axis=1)[:, np.newaxis]
return pts_3d.T
def eightPointNormalisation(pts):
print "> 8POINT NORMALISATION"
cx = 0
cy = 0
pts_ = []
for p in pts:
cx += p[0]
cy += p[1]
cx = cx / len(pts)
cy = cy / len(pts)
# translation to (cx,cy) = (0,0)
T = np.mat([[1, 0, -cx],
[0, 1, -cy],
[0, 0, 1]])
print "Translate by:", -cx, -cy
# now scale to rms_d = sqrt(2)
total_d = 0
for p in pts:
d = math.hypot(p[0] - cx, p[1] - cy)
total_d += (d * d)
# square root of the mean of the squares
rms_d = math.sqrt((total_d / len(pts)))
scale_factor = math.sqrt(2) / rms_d
print "Scale by:", scale_factor
T = scale_factor * T
T[2, 2] = 1
print "T:\n", T
# apply the transformation
hom = cv2.convertPointsToHomogeneous(pts)
for h in hom:
h_ = T * h.T
pts_.append(h_)
pts_ = cv2.convertPointsFromHomogeneous(np.array(pts_, dtype='float32'))
check8PointNormalisation(pts_)
# make sure the normalised points are in the same format as original
pts_r = []
for p in pts_:
pts_r.append(p[0])
pts_r = np.array(pts_r, dtype='float32')
return pts_r, T
def unprojectPoints(self, pts_2d, use_distortion=True, normalize=False):
"""
Undistorts points according to the camera model.
cv2.fisheye.undistortPoints does *NOT* perform the same unprojection step the original cv2.unprojectPoints does.
Thus we implement this function ourselves.
:param pts_2d, shape: Nx2
:return: Array of unprojected 3d points, shape: Nx3
"""
pts_2d = np.array(pts_2d, dtype=np.float32)
# Delete any posibly wrong 3rd dimension
if pts_2d.ndim == 1 or pts_2d.ndim == 3:
pts_2d = pts_2d.reshape((-1, 2))
eps = np.finfo(np.float32).eps
f = np.array((self.K[0, 0], self.K[1, 1])).reshape(1, 2)
c = np.array((self.K[0, 2], self.K[1, 2])).reshape(1, 2)
if use_distortion:
k = self.D.ravel().astype(np.float32)
else:
k = np.asarray(
[1.0 / 3.0, 2.0 / 15.0, 17.0 / 315.0, 62.0 / 2835.0], dtype=np.float32
)
pi = pts_2d.astype(np.float32)
pw = (pi - c) / f
theta_d = np.linalg.norm(pw, ord=2, axis=1)
theta = theta_d
for j in range(10):
theta2 = theta ** 2
theta4 = theta2 ** 2
theta6 = theta4 * theta2
theta8 = theta6 * theta2
theta = theta_d / (
1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8
)
scale = np.tan(theta) / (theta_d + eps)
pts_2d_undist = pw * scale.reshape(-1, 1)
pts_3d = cv2.convertPointsToHomogeneous(pts_2d_undist)
pts_3d.shape = -1, 3
if normalize:
pts_3d /= np.linalg.norm(pts_3d, axis=1)[:, np.newaxis]
return pts_3d
def undistort_unproject_pts(pts_uv, camera_matrix, dist_coefs):
"""
This function converts a set of 2D image coordinates to vectors in pinhole camera space.
Hereby the intrinsics of the camera are taken into account.
UV is converted to normalized image space (think frustum with image plane at z=1) then undistored
adding a z_coordinate of 1 yield vectors pointing from 0,0,0 to the undistored image pixel.
@return: ndarray with shape=(n, 3)
"""
pts_uv = np.array(pts_uv)
num_pts = pts_uv.size / 2
pts_uv.shape = (num_pts, 1, 2)
pts_uv = cv2.undistortPoints(pts_uv, camera_matrix, dist_coefs)
pts_3d = cv2.convertPointsToHomogeneous(np.float32(pts_uv))
pts_3d.shape = (num_pts,3)
return pts_3d
def synchroniseGeometric(pts_1, pts_2, F):
print "> GEOMETRIC SYNCHRONISATION:"
syncd1 = []
syncd2 = []
shorter = []
longer = []
short_flag = 0
# Work out which of two trajectories is shorter
if len(pts_1) < len(pts_2):
shorter = pts_1
longer = pts_2
short_flag = 1
else:
shorter = pts_2
longer = pts_1
short_flag = 2
diff = len(longer) - len(shorter)
if debug:
print "Longer:", len(longer)
print "Shorter:", len(shorter)
print "Diff:", diff
# Convert to homogeneous
shorter_hom = cv2.convertPointsToHomogeneous(shorter)
longer_hom = cv2.convertPointsToHomogeneous(longer)
averages = []
# Shift the shorter through the longer
for offset in xrange(0, diff + 1):
err = 0
avg = 0
for i in xrange(0, len(shorter)):
# current matching of pts a-b from trajectories A-B
a = shorter_hom[i]
b = longer_hom[i + offset]
# Test x'Fx = 0
this_err = abs(np.mat(a) * F * np.mat(b).T)
err += this_err
avg = err / len(shorter)
avg_off = (avg, offset)
averages.append(avg_off)
m = min(float(a[0]) for a in averages)
ret = [item for item in averages if item[0] == m]
# trim the beginning of the longer list
offset = ret[0][1]
longer = longer[offset:]
# trim its end
tail = len(longer) - len(shorter)
if tail != 0:
longer = longer[:-tail]
if short_flag == 1:
syncd1 = shorter
syncd2 = longer
else:
syncd1 = longer
syncd2 = shorter
if debug:
print "Synced Trajectory Length:", len(longer), len(shorter)
if view and debug:
plot.plot2D(syncd1, name='First Synced Trajectory')
plot.plot2D(syncd2, name='Second Synced Trajectory')
return syncd1, syncd2
def _get_location(self,visible_markers,camera_calibration,min_marker_perimeter,locate_3d=False):
marker_by_id = dict([(m['id'],m) for m in visible_markers if m['perimeter']>=min_marker_perimeter])
visible_ids = set(marker_by_id.keys())
requested_ids = set(self.markers.keys())
overlap = visible_ids & requested_ids
overlap_perimeter = sum(marker_by_id[i]['perimeter'] for i in overlap)
if overlap and overlap_perimeter>=min_marker_perimeter*min(2,len(requested_ids)):
detected = True
xy = np.array( [marker_by_id[i]['verts'] for i in overlap] )
uv = np.array( [self.markers[i].uv_coords for i in overlap] )
uv.shape=(-1,1,2)
# our camera lens creates distortions we want to get a good 2d estimate despite that so we:
# compute the homography transform from marker into the undistored normalized image space
# (the line below is the same as what you find in methods.undistort_unproject_pts, except that we ommit the z corrd as it is always one.)
xy_undistorted_normalized = cv2.undistortPoints(xy.reshape(-1,1,2), camera_calibration['camera_matrix'],camera_calibration['dist_coefs'])
m_to_undistored_norm_space,mask = cv2.findHomography(uv,xy_undistorted_normalized)
m_from_undistored_norm_space,mask = cv2.findHomography(xy_undistorted_normalized,uv)
# project the corners of the surface to undistored space
corners_undistored_space = cv2.perspectiveTransform(marker_corners_norm.reshape(-1,1,2),m_to_undistored_norm_space)
# project and distort these points and normalize them
corners_redistorted, corners_redistorted_jacobian = cv2.projectPoints(cv2.convertPointsToHomogeneous(corners_undistored_space), np.array([0,0,0], dtype=np.float32) , np.array([0,0,0], dtype=np.float32), camera_calibration['camera_matrix'], camera_calibration['dist_coefs'])
corners_nulldistorted, corners_nulldistorted_jacobian = cv2.projectPoints(cv2.convertPointsToHomogeneous(corners_undistored_space), np.array([0,0,0], dtype=np.float32) , np.array([0,0,0], dtype=np.float32), camera_calibration['camera_matrix'], camera_calibration['dist_coefs']*0)
#normalize to pupil norm space
corners_redistorted.shape = -1,2
corners_redistorted /= camera_calibration['resolution']
corners_redistorted[:,-1] = 1-corners_redistorted[:,-1]
#normalize to pupil norm space
corners_nulldistorted.shape = -1,2
corners_nulldistorted /= camera_calibration['resolution']
corners_nulldistorted[:,-1] = 1-corners_nulldistorted[:,-1]
# maps for extreme lens distortions will behave irratically beyond the image bounds
# since our surfaces often extend beyond the screen we need to interpolate
# between a distored projection and undistored one.
# def ratio(val):
# centered_val = abs(.5 - val)
# # signed distance to img cennter .5 is imag bound
# # we look to interpolate between .7 and .9
# inter = max()
corners_robust = []
for nulldist,redist in zip(corners_nulldistorted,corners_redistorted):
if -.4 < nulldist[0] <1.4 and -.4 < nulldist[1] <1.4:
corners_robust.append(redist)
else:
corners_robust.append(nulldist)
corners_robust = np.array(corners_robust)
#compute a perspective thransform from from the marker norm space to the apparent image.
# The surface corners will be at the right points
# However the space between the corners may be distored due to distortions of the lens,
m_to_screen = m_verts_to_screen(corners_robust)
m_from_screen = m_verts_from_screen(corners_robust)
if locate_3d:
dist_coef, = camera_calibration['dist_coefs']
img_size = camera_calibration['resolution']
K = camera_calibration['camera_matrix']
# 3d marker support pose estimation:
# scale normalized object points to world space units (think m,cm,mm)
uv.shape = -1,2
uv *= [self.real_world_size['x'], self.real_world_size['y']]
# convert object points to lie on z==0 plane in 3d space
uv3d = np.zeros((uv.shape[0], uv.shape[1]+1))
uv3d[:,:-1] = uv
xy.shape = -1,1,2
# compute pose of object relative to camera center
is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(uv3d, xy, K, dist_coef,flags=cv2.CV_EPNP)
# not verifed, potentially usefull info: http://stackoverflow.com/questions/17423302/opencv-solvepnp-tvec-units-and-axes-directions
###marker posed estimation from virtually projected points.
# object_pts = np.array([[[0,0],[0,1],[1,1],[1,0]]],dtype=np.float32)
# projected_pts = cv2.perspectiveTransform(object_pts,self.m_to_screen)
# projected_pts.shape = -1,2
# projected_pts *= img_size
# projected_pts.shape = -1, 1, 2
# # scale object points to world space units (think m,cm,mm)
# object_pts.shape = -1,2
# object_pts *= self.real_world_size
# # convert object points to lie on z==0 plane in 3d space
# object_pts_3d = np.zeros((4,3))
# object_pts_3d[:,:-1] = object_pts
# self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(object_pts_3d, projected_pts, K, dist_coef,flags=cv2.CV_EPNP)
# transformation from Camera Optical Center:
# first: translate from Camera center to object origin.
# second: rotate x,y,z
# coordinate system is x,y,z where z goes out from the camera into the viewed volume.
# print rot3d_cam_to_object[0],rot3d_cam_to_object[1],rot3d_cam_to_object[2], translate3d_cam_to_object[0],translate3d_cam_to_object[1],translate3d_cam_to_object[2]
#turn translation vectors into 3x3 rot mat.
rot3d_cam_to_object_mat, _ = cv2.Rodrigues(rot3d_cam_to_object)
#to get the transformation from object to camera we need to reverse rotation and translation
translate3d_object_to_cam = - translate3d_cam_to_object
# rotation matrix inverse == transpose
rot3d_object_to_cam_mat = rot3d_cam_to_object_mat.T
# we assume that the volume of the object grows out of the marker surface and not into it. We thus have to flip the z-Axis:
flip_z_axix_hm = np.eye(4, dtype=np.float32)
flip_z_axix_hm[2,2] = -1
# create a homogenous tranformation matrix from the rotation mat
rot3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
rot3d_object_to_cam_hm[:-1,:-1] = rot3d_object_to_cam_mat
# create a homogenous tranformation matrix from the translation vect
translate3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
translate3d_object_to_cam_hm[:-1, -1] = translate3d_object_to_cam.reshape(3)
# combine all tranformations into transformation matrix that decribes the move from object origin and orientation to camera origin and orientation
tranform3d_object_to_cam = np.matrix(flip_z_axix_hm) * np.matrix(rot3d_object_to_cam_hm) * np.matrix(translate3d_object_to_cam_hm)
camera_pose_3d = tranform3d_object_to_cam
else:
is3dPoseAvailable = False
camera_pose_3d = None
else:
detected = False
camera_pose_3d = None
is3dPoseAvailable = False
m_from_screen = None
m_to_screen = None
m_from_undistored_norm_space = None
m_to_undistored_norm_space = None
return {'detected':detected,'detected_markers':len(overlap),'m_from_undistored_norm_space':m_from_undistored_norm_space,'m_to_undistored_norm_space':m_to_undistored_norm_space,'m_from_screen':m_from_screen,'m_to_screen':m_to_screen,'is3dPoseAvailable':is3dPoseAvailable,'camera_pose_3d':camera_pose_3d}
def testFundamentalReln(F, pts_1, pts_2, view):
# check that xFx = 0 for homogenenous coords x x'
F = np.mat(F)
tools.is_singular(F)
pts1_hom = cv2.convertPointsToHomogeneous(pts_1)
pts2_hom = cv2.convertPointsToHomogeneous(pts_2)
errors = []
sum_err = 0
# forwards: pt1 * F * pt2 = ?
for i in range(0, len(pts1_hom)):
this_err = abs(np.mat(pts1_hom[i]) * F * np.mat(pts2_hom[i]).T)
sum_err += this_err[0, 0]
errors.append(this_err[0, 0])
# backwards 2 * F * 1 = ?
for i in range(0, len(pts2_hom)):
this_err = abs(np.mat(pts2_hom[i]) * F * np.mat(pts1_hom[i]).T)
sum_err += this_err[0, 0]
errors.append(this_err[0, 0])
# NB: although defining eqn is K'.T * F * K, this just means
# row x grid x col or (3x1)(3x3)(1x3). here our points are already rows
# so we have to transpose the last to get our column
err = sum_err / (2 * len(pts1_hom))
print "> x'Fx = 0:", err
# inspect the error distribution
if view:
plot.plotOrderedBar(errors,
name='x\'Fx = 0 Test Results ',
ylabel='Deflection from zero',
xlabel='Point Index')
# test the epilines
pts1_epi = pts_1.reshape((-1, 1, 2))
pts2_epi = pts_2.reshape((-1, 1, 2))
# lines computed from pts1
# arg 2/3 is a flag to transpose F (2) or not (1)
lines1 = cv2.computeCorrespondEpilines(pts1_epi, 1, F)
lines1 = lines1.reshape(-1, 3)
# lines computed from pts2
# NB: passing 2 at pos1 results in a transpose of F in the calculation
lines2 = cv2.computeCorrespondEpilines(pts2_epi, 2, F)
lines2 = lines2.reshape(-1, 3)
distances2 = []
for l, p in zip(lines1, pts_2):
distances2.append(distanceToEpiline(l, p))
distances1 = []
for l, p in zip(lines2, pts_1):
distances1.append(distanceToEpiline(l, p))
# Average p-line distances
avg1 = sum(distances1) / len(distances1)
avg2 = sum(distances2) / len(distances2)
std1 = np.std(distances1)
std2 = np.std(distances2)
# Append the two lists of p-line measures
distances = distances1 + distances2
avg = np.mean(distances)
std = np.std(distances)
print "> Average distance to epiline in image 1 and 2 (px):", avg1, avg2
print "> Overall Average:", avg
print "> Std Dev:", std
if view:
# Inspect the distributions
plot.plotOrderedBar(distances1,
'Image 1: Point-Epiline Distances', 'Index', 'px')
plot.plotOrderedBar(distances2,
'Image 2: Point-Epiline Distances', 'Index', 'px')
# overlay lines2 on pts1
plot.plotEpilines(lines2, pts_1, 1)
# overlay lines1 on pts2
plot.plotEpilines(lines1, pts_2, 2)
return avg, std
def performStructureFromMotion(image_points1, image_points2, K, W, H):
""" Performs structure from motion on the basis of image matches (image_points1, image_points2, both Nx2),
a calibration matrix K, and a given width and height of an image.
It returns (R, t, X), i.e., a rotation matrix R, translation t, and world points X.
"""
n_points = len(image_points1);
# location camera 1:
l1 = np.zeros([3,1]);
R1 = np.eye(3);
rvec1 = cv2.Rodrigues(R1);
rvec1 = rvec1[0];
# determine the rotation and translation between the two views:
(R21, R22, t21, t22) = determineTransformation(image_points1, image_points2, K, W, H);
# 3D-reconstruction:
ip1 = cv2.convertPointsToHomogeneous(image_points1.astype(np.float32));
ip2 = cv2.convertPointsToHomogeneous(image_points2.astype(np.float32));
# P1 is at the origin
P1 = np.zeros([3, 4]);
P1[:3,:3] = np.eye(3);
P1 = np.dot(K, P1);
# P2 is rotated and translated with respect to camera 1
# determine the right R, t:
# reproject a point to the 3D-world
# exclude the camera if the point falls behind it.
# first determine all 4 projection matrices:
R2s = [];
R2s.append(R21);
R2s.append(R22);
t2s = [];
t2s.append(t21);
t2s.append(t22);
# reproject the points into the 3D-world:
index_r = 0; index_t = 0;
for ir in range(2):
# clean up the rotation matrix, i.e., don't allow mirroring of any axis:
R2s[ir] = cleanUpR(R2s[ir]);
for it in range(2):
point_behind = infeasibleP2(ip1, ip2, R1, l1, R2s[ir], t2s[it], K);
if(point_behind == 0):
index_r = ir;
index_t = it;
print 'ir, it = %d, %d' % (ir, it)
P2_est = getProjectionMatrix(R2s[index_r], t2s[index_t], K);
R2_est = R2s[index_r]; t2_est = t2s[index_t];
# triangulate the image points to obtain world coordinates:
X_est = triangulate(ip1, ip2, P1, P2_est);
# BUNDLE ADJUSTMENT:
bundle_adjustment = True;
if(bundle_adjustment):
# evolve a solution:
IPs = [];
IPs.append(image_points1);
IPs.append(image_points2);
# seed the evolution with some pre-knowledge:
phis = [0.0];
thetas = [0.0];
psis = [0.0];
#Ts = [t];
t2e = np.zeros([3,1]);
for i in range(3):
t2e[i,0] = t2_est[i];
Ts = [t2e];
# points;
W = np.zeros([n_points, 3]);
for p in range(n_points):
for i in range(3):
W[p, i] = X_est[p][i];
# calculate reprojection error before further optimization:
Rs = [R1]; Rs.append(R2_est);
Ts = [l1]; Ts.append(t2e);
(err, errors_per_point) = calculateReprojectionError(Rs, Ts, W, IPs, 2, n_points, K);
# determine the genome on the above information:
genome = constructGenome(phis, thetas, psis, Ts, n_points, W);
# Get rotations, translations, X_est:
(Rs, Ts, X_est) = evolveReconstruction('test', 2, n_points, IPs, 3.0, 10.0, K, genome);
R2_est = Rs[1];
t2_est = Ts[1];
print 't_est = %f, %f, %f' % (t2_est[0], t2_est[1], t2_est[2]);
print 'R = '
printRotationMatrix(R2_est);
# now we have R2, t2, and X, which we return:
return (R2_est, t2_est, X_est, errors_per_point);
def testVisualOdometry(n_points=100):
# location camera 1:
l1 = np.zeros([3,1]);
R1 = np.eye(3);
rvec1 = cv2.Rodrigues(R1);
rvec1 = rvec1[0];
# translation vector
t = np.zeros([3,1]);#np.random.rand(3,1);
t[2] = 0;
t[1] = 1;
print 't = %f, %f, %f' % (t[0], t[1], t[2]);
scale = np.linalg.norm(t);
print 'scale = %f' % scale;
l2 = l1 + t;
# Rotation matrix:
phi = 0.2 * np.pi;#0.001*(np.random.random(1)*2-1) * np.pi;
theta = 0.1 * np.pi;#0.001*(np.random.random(1)*2-1) * np.pi;
psi = 0.0;#0.001*(np.random.random(1)*2-1) * np.pi;
R_phi = np.zeros([3,3]);
R_phi[0,0] = 1;
R_phi[1,1] = np.cos(phi);
R_phi[1,2] = np.sin(phi);
R_phi[2,1] = -np.sin(phi);
R_phi[2,2] = np.cos(phi);
R_theta = np.zeros([3,3]);
R_theta[1,1] = 1;
R_theta[0,0] = np.cos(theta);
R_theta[2,0] = -np.sin(theta);
R_theta[0,2] = np.sin(theta);
R_theta[2,2] = np.cos(theta);
R_psi = np.zeros([3,3]);
R_psi[0,0] = np.cos(psi);
R_psi[0,1] = np.sin(psi);
R_psi[1,0] = -np.sin(psi);
R_psi[1,1] = np.cos(psi);
R_psi[2,2] = 1;
R2 = np.dot(R_psi, np.dot(R_theta, R_phi));
print 'R = '
printRotationMatrix(R2);
rvec2 = cv2.Rodrigues(R2);
rvec2 = rvec2[0];
# create X, Y, Z points:
size = 3;
distance = 5;
transl = np.zeros([1,3]);
transl[0,2] = distance; # is Z in the direction of the principal axis?
points_world = np.zeros([n_points, 3]);
for p in range(n_points):
points_world[p, :] = size * (np.random.rand(1,3)*2-np.ones([1,3])) + transl;
# camera calibration matrix:
K = np.zeros([3,3]);
W = 320.0;
H = 240.0;
K[0,0] = W;
K[1,1] = H;
K[2,2] = 1.0;
distCoeffs = np.zeros([4]); # no clue what this means
# project the world points in the cameras:
result = cv2.projectPoints(points_world, rvec1, np.array([0.0]*3), K, distCoeffs);
image_points1 = result[0];
result = cv2.projectPoints(points_world, rvec2, t, K, distCoeffs);
image_points2 = result[0];
add_noise = True;
if(add_noise):
for p in range(n_points):
image_points1[p][0][0] += np.random.normal(0.0, 0.5);
image_points1[p][0][1] += np.random.normal(0.0, 0.5);
image_points2[p][0][0] += np.random.normal(0.0, 0.5);
image_points2[p][0][1] += np.random.normal(0.0, 0.5);
# determine the rotation and translation between the two views:
(R21, R22, t21, t22) = determineTransformation(image_points1, image_points2, K, W, H);
# 'wrong' solutions have a negative element on the diagonal, but calling determineTransformation repetitively does not help...
# ws = wrongSolution(R21, R22);
# while(ws == 1):
# (R21, R22, t21, t22) = determineTransformation(image_points1, image_points2, K, W, H);
# ws = wrongSolution(R21, R22);
print 'R21 = '
printRotationMatrix(R21);
print 'R22 = '
printRotationMatrix(R22);
#R21 = R2.T;
#R22 = R2;
#t21 = -t;
#t22 = t;
# 3D-reconstruction:
ip1 = cv2.convertPointsToHomogeneous(image_points1.astype(np.float32));
ip2 = cv2.convertPointsToHomogeneous(image_points2.astype(np.float32));
# P1 is at the origin
P1 = np.zeros([3, 4]);
P1[:3,:3] = np.eye(3);
P1 = np.dot(K, P1);
# P2 is rotated and translated with respect to camera 1
# determine the right R, t:
# iterate over all points, reproject them to the 3D-world
# exclude one of the options as soon as
# first determine all 4 projection matrices:
R2s = [];
R2s.append(R21);
R2s.append(R22);
t2s = [];
t2s.append(t21);
t2s.append(t22);
# reproject the points into the 3D-world:
index_r = 0; index_t = 0;
for ir in range(2):
# clean up the rotation matrix, i.e., don't allow mirroring of any axis:
R2s[ir] = cleanUpR(R2s[ir]);
for it in range(2):
point_behind = infeasibleP2(ip1, ip2, R1, l1, R2s[ir], t2s[it], K);
if(point_behind == 0):
index_r = ir;
index_t = it;
print 'ir, it = %d, %d' % (ir, it)
P2_est = getProjectionMatrix(R2s[index_r], t2s[index_t], K);
R2_est = R2s[index_r]; t2_est = t2s[index_t];
# triangulate the image points to obtain world coordinates:
X_est = triangulate(ip1, ip2, P1, P2_est);
# We could determine the reprojection error already here. It is close to 0 for a good estimate
# and in the 10,000s for a bad estimate.
# BUNDLE ADJUSTMENT:
bundle_adjustment = True;
if(bundle_adjustment):
# evolve a solution:
IPs = [];
IPs.append(image_points1);
IPs.append(image_points2);
# seed the evolution with some pre-knowledge:
phis = [phi];
thetas = [theta];
psis = [psi];
#Ts = [t];
t2e = np.zeros([3,1]);
for i in range(3):
t2e[i,0] = t2_est[i];
Ts = [t2e];
# points;
W = np.zeros([n_points, 3]);
for p in range(n_points):
for i in range(3):
W[p, i] = X_est[p][i];
genome = constructGenome(phis, thetas, psis, Ts, n_points, W);
# Get rotations, translations, X_est
(Rs, Ts, X_est) = evolveReconstruction('test', 2, n_points, IPs, 3.0, 10.0, K, genome);
R2_est = Rs[1];
t2_est = Ts[1];
scales = np.array([0.0] * n_points);
for i in range(n_points):
scales[i] = X_est[i][0] / points_world[i][0];
print 't_est = %f, %f, %f' % (t2_est[0], t2_est[1], t2_est[2]);
print 'R = '
printRotationMatrix(R2_est);
sc = np.mean(scales);
print 'Scale = %f, Mean scale = %f' % (scale, 1.0/sc);
# scale:
# X_est = X_est * (1.0/sc);
# show visually:
# calculate reprojection error:
Rs = [R1]; Rs.append(R2_est);
Ts = [l1]; Ts.append(t2_est);
(err, errors_per_point) = calculateReprojectionError(Rs, Ts, W, IPs, 2, n_points, K);
fig = pl.figure()
ax = fig.gca(projection='3d')
M_world = np.matrix(points_world);
M_est = np.matrix(X_est);
M_est = M_est[:,:3];
x = np.array(M_est[:,0]); y = np.array(M_est[:,1]); z = np.array(M_est[:,2]);
x = flatten(x); y = flatten(y); z = flatten(z);
#ax.scatter(x, y, z, '*', color=(1.0,0,0));
cm = pl.cm.get_cmap('hot')
ax.scatter(x, y, z, '*', c=errors_per_point, cmap=cm);
fig.hold = True;
#x = (1.0/sc)*np.array(M_est[:,0]); y = (1.0/sc)*np.array(M_est[:,1]); z = (1.0/sc)*np.array(M_est[:,2]);
#x = flatten(x); y = flatten(y); z = flatten(z);
#ax.scatter(x, y, z, 's', color=(0,0,1.0));
x = np.array(M_world[:,0]); y = np.array(M_world[:,1]); z = np.array(M_world[:,2]);
x = flatten(x); y = flatten(y); z = flatten(z);
ax.scatter(x, y, z, 'o', color=(0.0,1.0,0.0));
ax.axis('tight');
pl.show();
# if(1.0/sc > 0.99 * scale and 1.0/sc < 1.01 * scale):
# pdb.set_trace()
# now we have R2, t2, and X, which we return:
return (R2_est, t2_est, X_est);
