Skip to content

Commit 27dca0c

Browse files
sungloksunglok
committed
Add examples for 'Solving Problems'
1 parent e0dd7eb commit 27dca0c

13 files changed

Lines changed: 299 additions & 31 deletions

‎examples/affine_estimation.py‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
import cv2 as cv
2+
import numpy as np
3+
4+
def getAffineTransform(src, dst):
5+
if len(src) == len(dst):
6+
# Solve 'Ax = b'
7+
A, b = [], []
8+
for p, q in zip(src, dst):
9+
A.append([p[0], p[1], 0, 0, 1, 0])
10+
A.append([0, 0, p[0], p[1], 0, 1])
11+
b.append(q[0])
12+
b.append(q[1])
13+
x = np.linalg.pinv(A) @ b
14+
15+
# Reorganize 'H'
16+
H = np.array([[x[0], x[1], x[4]], [x[2], x[3], x[5]]])
17+
return H
18+
19+
if __name__ == '__main__':
20+
src = np.array([[115, 401], [776, 180], [330, 793]], dtype=np.float32)
21+
dst = np.array([[0, 0], [900, 0], [0, 500]], dtype=np.float32)
22+
23+
my_H = getAffineTransform(src, dst)
24+
cv_H = cv.getAffineTransform(src, dst) # Note) It accepts only 3 pairs of points.
25+
26+
print('\n### My Affine Transformation')
27+
print(my_H)
28+
print('\n### OpenCV Affine Transformation')
29+
print(cv_H)

‎examples/camera_calibration_implement.py‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
import numpy as np
2+
from scipy.optimize import least_squares
3+
from pose_estimation_implement import project_no_distort
4+
5+
def fcxcy_to_K(f, cx, cy):
6+
return np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
7+
8+
def reproject_error_calib(unknown, Xs, xs):
9+
K = fcxcy_to_K(*unknown[0:3])
10+
err = []
11+
for i in range(len(xs)):
12+
offset = 3 + 6 * i
13+
rvec, tvec = unknown[offset:offset+3], unknown[offset+3:offset+6]
14+
xp = project_no_distort(Xs[i], rvec, tvec, K)
15+
err.append(xs[i] - xp)
16+
return np.vstack(err).ravel()
17+
18+
def calibrateCamera(obj_pts, img_pts, img_size):
19+
img_n = len(img_pts)
20+
unknown_init = np.array([img_size[0], img_size[0]/2, img_size[1]/2] + img_n * [0, 0, 0, 0, 0, 1.]) # Sequence: f, cx, cy, img_n * (rvec, tvec)
21+
result = least_squares(reproject_error_calib, unknown_init, args=(obj_pts, img_pts))
22+
K = fcxcy_to_K(*result['x'][0:3])
23+
rvecs = [result['x'][(6*i+3):(6*i+6)] for i in range(img_n)]
24+
tvecs = [result['x'][(6*i+6):(6*i+9)] for i in range(img_n)]
25+
return result['cost'], K, np.zeros(5), rvecs, tvecs
26+
27+
if __name__ == '__main__':
28+
img_size = (640, 480)
29+
img_files = ['../bin/data/image_formation1.xyz', '../bin/data/image_formation2.xyz']
30+
img_pts = []
31+
for file in img_files:
32+
pts = np.loadtxt('../bin/data/image_formation1.xyz', dtype=np.float32)
33+
img_pts.append(pts[:,:2])
34+
35+
pts = np.loadtxt('../bin/data/box.xyz', dtype=np.float32)
36+
obj_pts = [pts] * len(img_pts) # Copy the object point as much as the number of image observation
37+
38+
# Calibrate the camera
39+
_, K, *_ = calibrateCamera(obj_pts, img_pts, img_size)
40+
41+
print('\n### Ground Truth')
42+
print('* f, cx, cy = 1000, 320, 240')
43+
print('\n### My Calibration')
44+
print(f'* f, cx, cy = {K[0,0]:.1f}, {K[0,2]:.1f}, {K[1,2]:.1f}')

‎examples/fundamental_mat_estimation.py‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
import cv2 as cv
2+
import numpy as np
3+
4+
def findFundamentalMat(pts1, pts2):
5+
if len(pts1) == len(pts2):
6+
# Make homogeneous coordiates if necessary
7+
if pts1.shape[1] == 2:
8+
pts1 = np.hstack((pts1, np.ones((len(pts1), 1), dtype=pts1.dtype)))
9+
if pts2.shape[1] == 2:
10+
pts2 = np.hstack((pts2, np.ones((len(pts2), 1), dtype=pts2.dtype)))
11+
12+
# Solve 'Ax = 0'
13+
A = []
14+
for p, q in zip(pts1, pts2):
15+
A.append([q[0]*p[0], q[0]*p[1], q[0]*p[2], q[1]*p[0], q[1]*p[1], q[1]*p[2], q[2]*p[0], q[2]*p[1], q[2]*p[2]])
16+
_, _, Vt = np.linalg.svd(A, full_matrices=True)
17+
x = Vt[-1]
18+
19+
# Reorganize 'F' and enforce 'rank(F) = 2'
20+
F = x.reshape(3, -1)
21+
U, S, Vt = np.linalg.svd(F)
22+
S[-1] = 0
23+
F = U @ np.diag(S) @ Vt
24+
return F / F[-1,-1] # Normalize the last element as 1
25+
26+
if __name__ == '__main__':
27+
pts0 = np.loadtxt('../bin/data/image_formation0.xyz')
28+
pts1 = np.loadtxt('../bin/data/image_formation1.xyz')
29+
30+
my_F = findFundamentalMat(pts0, pts1)
31+
cv_F, _ = cv.findFundamentalMat(pts0, pts1, cv.FM_8POINT)
32+
33+
print('\n### My Fundamental Matrix')
34+
print(my_F)
35+
print('\n### OpenCV Fundamental Matrix')
36+
print(cv_F) # Note) The result is slightly different because OpenCV considered normalization

‎examples/homography_estimation.py‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
import cv2 as cv
2+
import numpy as np
3+
4+
def getPerspectiveTransform(src, dst):
5+
if len(src) == len(dst):
6+
# Make homogeneous coordiates if necessary
7+
if src.shape[1] == 2:
8+
src = np.hstack((src, np.ones((len(src), 1), dtype=src.dtype)))
9+
if dst.shape[1] == 2:
10+
dst = np.hstack((dst, np.ones((len(dst), 1), dtype=dst.dtype)))
11+
12+
# Solve 'Ax = 0'
13+
A = []
14+
for p, q in zip(src, dst):
15+
A.append([0, 0, 0, q[2]*p[0], q[2]*p[1], q[2]*p[2], -q[1]*p[0], -q[1]*p[1], -q[1]*p[2]])
16+
A.append([q[2]*p[0], q[2]*p[1], q[2]*p[2], 0, 0, 0, -q[0]*p[0], -q[0]*p[1], -q[0]*p[2]])
17+
_, _, Vt = np.linalg.svd(A, full_matrices=True)
18+
x = Vt[-1]
19+
20+
# Reorganize 'H'
21+
H = x.reshape(3, -1) / x[-1] # Normalize the last element as 1
22+
return H
23+
24+
if __name__ == '__main__':
25+
src = np.array([[115, 401], [776, 180], [330, 793], [1080, 383]], dtype=np.float32)
26+
dst = np.array([[0, 0], [900, 0], [0, 500], [900, 500]], dtype=np.float32)
27+
28+
my_H = getPerspectiveTransform(src, dst)
29+
cv_H = cv.getPerspectiveTransform(src, dst) # Note) It accepts only 4 pairs of points.
30+
31+
print('\n### My Planar Homography')
32+
print(my_H)
33+
print('\n### OpenCV Planar Homography')
34+
print(cv_H)

‎examples/image_warping.py‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
import cv2 as cv
2+
import numpy as np
3+
from homography_estimation import getPerspectiveTransform
4+
5+
def warpPerspective1(src, H, dst_size):
6+
# Generate an empty image
7+
width, height = dst_size
8+
channel = src.shape[2] if src.ndim > 2 else 1
9+
dst = np.zeros((height, width, channel), dtype=src.dtype)
10+
11+
# Copy a pixel from 'src' to 'dst'
12+
for py in range(img.shape[0]):
13+
for px in range(img.shape[1]):
14+
q = H @ [px, py, 1]
15+
qx, qy = int(q[0]/q[-1] + 0.5), int(q[1]/q[-1] + 0.5)
16+
if qx >= 0 and qy >= 0 and qx < width and qy < height:
17+
dst[qy, qx] = src[py, px]
18+
return dst
19+
20+
def warpPerspective2(src, H, dst_size):
21+
# Generate an empty image
22+
width, height = dst_size
23+
channel = src.shape[2] if src.ndim > 2 else 1
24+
dst = np.zeros((height, width, channel), dtype=src.dtype)
25+
26+
# Copy a pixel from 'src' to 'dst'
27+
H_inv = np.linalg.inv(H)
28+
for qy in range(height):
29+
for qx in range(width):
30+
p = H_inv @ [qx, qy, 1]
31+
px, py = int(p[0]/p[-1] + 0.5), int(p[1]/p[-1] + 0.5)
32+
if px >= 0 and py >= 0 and px < img.shape[1] and py < img.shape[0]:
33+
dst[qy, qx] = src[py, px]
34+
return dst
35+
36+
if __name__ == '__main__':
37+
img = cv.imread('../bin/data/sunglok_desk.jpg')
38+
wnd_name = '3DV Tutorial: Image Warping'
39+
card_size = (900, 500)
40+
src = np.array([[115, 401], [776, 180], [330, 793], [1080, 383]], dtype=np.float32)
41+
dst = np.array([[0, 0], [card_size[0], 0], [0, card_size[1]], card_size], dtype=np.float32)
42+
43+
# Find planar homography and transform the original image
44+
H = getPerspectiveTransform(src, dst)
45+
warp1 = warpPerspective1(img, H, card_size)
46+
warp2 = warpPerspective2(img, H, card_size)
47+
48+
# Show images generated from two methods
49+
cv.imshow(wnd_name + ' (Method 1)', warp1)
50+
cv.imshow(wnd_name + ' (Method 2)', warp2)
51+
cv.waitKey(0)
52+
cv.destroyAllWindows()

examples/line_fitting_m_est.cpp renamed to examples/line_fitting_m_estimation.cpp

File renamed without changes.

examples/line_fitting_m_est.py renamed to examples/line_fitting_m_estimation.py

File renamed without changes.

‎examples/pose_estimation_implement.py‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
import cv2 as cv
2+
import numpy as np
3+
from scipy.optimize import least_squares
4+
from scipy.spatial.transform import Rotation
5+
6+
def project_no_distort(X, rvec, t, K):
7+
R = Rotation.from_rotvec(rvec.flatten()).as_matrix()
8+
XT = X @ R.T + t # Transpose of 'X = R @ X + t'
9+
xT = XT @ K.T # Transpose of 'x = KX'
10+
xT = xT / xT[:,-1].reshape((-1, 1)) # Normalize
11+
return xT[:,0:2]
12+
13+
def reproject_error_pnp(unknown, X, x, K):
14+
rvec, tvec = unknown[:3], unknown[3:]
15+
xp = project_no_distort(X, rvec, tvec, K)
16+
err = x - xp
17+
return err.ravel()
18+
19+
def solvePnP(obj_pts, img_pts, K):
20+
unknown_init = np.array([0, 0, 0, 0, 0, 1.]) # Sequence: rvec(3), tvec(3)
21+
result = least_squares(reproject_error_pnp, unknown_init, args=(obj_pts, img_pts, K))
22+
return result['success'], result['x'][:3], result['x'][3:]
23+
24+
if __name__ == '__main__':
25+
f, cx, cy = 1000., 320., 240.
26+
obj_pts = np.loadtxt('../bin/data/box.xyz')
27+
img_pts = np.loadtxt('../bin/data/image_formation1.xyz')[:,:2].copy()
28+
K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
29+
dist_coeff = np.zeros(4)
30+
31+
# Estimate camera pose
32+
_, rvec, tvec = solvePnP(obj_pts, img_pts, K) # Note) Ignore lens distortion
33+
R = Rotation.from_rotvec(rvec.flatten()).as_matrix()
34+
my_ori = Rotation.from_matrix(R.T).as_euler('xyz')
35+
my_pos = -R.T @ tvec
36+
37+
# Estimate camera pose using OpenCV
38+
_, rvec, tvec = cv.solvePnP(obj_pts, img_pts, K, dist_coeff)
39+
R = Rotation.from_rotvec(rvec.flatten()).as_matrix()
40+
cv_ori = Rotation.from_matrix(R.T).as_euler('xyz')
41+
cv_pos = -R.T @ tvec.flatten()
42+
43+
print('\n### Ground Truth')
44+
print('* Camera orientation: [-15, 15, 0] [deg]')
45+
print('* Camera position : [-2, -2, 0] [m]')
46+
print('\n### My Camera Pose')
47+
print(f'* Camera orientation: {np.rad2deg(my_ori)} [deg]')
48+
print(f'* Camera position : {my_pos} [m]')
49+
print('\n### OpenCV Camera Pose')
50+
print(f'* Camera orientation: {np.rad2deg(cv_ori)} [deg]')
51+
print(f'* Camera position : {cv_pos} [m]')

‎examples/triangulation.py‎

Lines changed: 14 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -1,40 +1,25 @@
1-
2-
import cv2
1+
import cv2 as cv
32
import numpy as np
43

5-
def main():
6-
input0, input1 = "../bin/data/image_formation0.xyz", "../bin/data/image_formation1.xyz"
7-
points0, points1 = None, None
8-
file0, file1 = open(input0, 'rt'), open(input1, 'rt')
9-
10-
with open(input0,'rt') as file0:
11-
points0 = [ list(xyz.split(' '))[:2] for xyz in file0.read().splitlines() if xyz != None ]
12-
points0 = np.array(points0, dtype=np.float32)
13-
with open(input1,'rt') as file0:
14-
points1 = [ list(xyz.split(' '))[:2] for xyz in file1.read().splitlines() if xyz != None ]
15-
points1 = np.array(points1, dtype=np.float32)
16-
17-
f, cx, cy = 1000, 320, 240
18-
# print(points0.shape)
19-
if len(points0) != len(points1): raise Exception("Not matching!")
4+
if __name__ == '__main__':
5+
f, cx, cy = 1000., 320., 240.
6+
pts0 = np.loadtxt('../bin/data/image_formation0.xyz')[:,:2]
7+
pts1 = np.loadtxt('../bin/data/image_formation1.xyz')[:,:2]
8+
output_file = '../bin/triangulation.xyz'
209

21-
# # Estimate relative pose of two view
22-
F,_ = cv2.findFundamentalMat(points0, points1, cv2.FM_8POINT)
23-
K = np.array([[f,0,cx],[0,f,cy],[0,0,1]])
10+
# Estimate relative pose of two view
11+
F, _ = cv.findFundamentalMat(pts0, pts1, cv.FM_8POINT)
12+
K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
2413
E = K.T @ F @ K
25-
_, R, t, _ = cv2.recoverPose(E, points0, points1)
14+
_, R, t, _ = cv.recoverPose(E, pts0, pts1)
2615

2716
# Reconstruct 3D points (triangulation)
28-
P0 = K @ np.eye(3,4, dtype=np.float32)
17+
P0 = K @ np.eye(3, 4, dtype=np.float32)
2918
Rt = np.hstack((R, t))
3019
P1 = K @ Rt
31-
X = cv2.triangulatePoints(P0, P1, points0.T, points1.T)
20+
X = cv.triangulatePoints(P0, P1, pts0.T, pts1.T)
3221
X /= X[3]
3322
X = X.T
34-
35-
triangular_file = "../bin/data/triangulation.xyz"
36-
with open(triangular_file, 'wt') as f:
37-
f.write(str(X[:,:3]))
3823

39-
if __name__=="__main__":
40-
main()
24+
# Write the reconstructed 3D points
25+
np.savetxt(output_file, X)

‎examples/triangulation_implement.py‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
import cv2 as cv
2+
import numpy as np
3+
4+
def triangulatePoints(P0, P1, pts0, pts1):
5+
Xs = []
6+
for (p, q) in zip(pts0.T, pts1.T):
7+
# Solve 'AX = 0'
8+
A = np.vstack((p[0] * P0[2] - P0[0],
9+
p[1] * P0[2] - P0[1],
10+
q[0] * P1[2] - P1[0],
11+
q[1] * P1[2] - P1[1]))
12+
_, _, Vt = np.linalg.svd(A, full_matrices=True)
13+
Xs.append(Vt[-1])
14+
return np.vstack(Xs).T
15+
16+
if __name__ == '__main__':
17+
f, cx, cy = 1000., 320., 240.
18+
pts0 = np.loadtxt('../bin/data/image_formation0.xyz')[:,:2]
19+
pts1 = np.loadtxt('../bin/data/image_formation1.xyz')[:,:2]
20+
output_file = '../bin/triangulation_implement.xyz'
21+
22+
# Estimate relative pose of two view
23+
F, _ = cv.findFundamentalMat(pts0, pts1, cv.FM_8POINT)
24+
K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
25+
E = K.T @ F @ K
26+
_, R, t, _ = cv.recoverPose(E, pts0, pts1)
27+
28+
# Reconstruct 3D points (triangulation)
29+
P0 = K @ np.eye(3, 4, dtype=np.float32)
30+
Rt = np.hstack((R, t))
31+
P1 = K @ Rt
32+
X = triangulatePoints(P0, P1, pts0.T, pts1.T)
33+
X /= X[3]
34+
X = X.T
35+
36+
# Write the reconstructed 3D points
37+
np.savetxt(output_file, X)

0 commit comments

Comments
 (0)