def un_distort_point(point):
"""
un_distort a specific point
:param point: point to distort
:return: undistorted point
"""
points = np.array([[(point.x, point.y)]], np.float32)
temp = cv2.undistortPoints(points, _camera_matrix, _camera_distortion)
fx, fy = _camera_tuned_matrix[0][0], _camera_tuned_matrix[1][1]
cx, cy = _camera_tuned_matrix[0][2], _camera_tuned_matrix[1][2]
x = temp[0][0][0] * fx + cx
y = temp[0][0][1] * fy + cy
return ge.Point(x, y)
def distort_point(point):
"""
distort a specific point
:param point: point to distort
:return: distorted point
"""
fx, fy = _camera_tuned_matrix[0][0], _camera_tuned_matrix[1][1]
cx, cy = _camera_tuned_matrix[0][2], _camera_tuned_matrix[1][2]
x, y = (point.x - cx) / fx, (point.y - cy) / fy
k1, k2, p1, p2, k3 = _camera_distortion[0]
r2 = x**2 + y**2
r4 = r2 * r2
r6 = r2 * r4
x = x * (1 + k1 * r2 + k2 * r4 +
k3 * r6) + 2 * p1 * x * y + p2 * (r2 + 2 * x * x)
y = y * (1 + k1 * r2 + k2 * r4 +
k3 * r6) + p1 * (r2 + 2 * y * y) + 2 * p2 * x * y
fx2, fy2 = _camera_matrix[0][0], _camera_matrix[1][1]
cx2, cy2 = _camera_matrix[0][2], _camera_matrix[1][2]
x2 = x * fx2 + cx2
y2 = y * fy2 + cy2
return ge.Point(x2, y2)
def pixel2cam(point, _camera_matrix):
fx, fy = _camera_matrix[0][0], _camera_matrix[1][1]
cx, cy = _camera_matrix[0][2], _camera_matrix[1][2]
x = (point.x - cx) / fx
y = (point.y - cy) / fy
return ge.Point(x, y)
def _check_calibration():
"""
check if the calibration is correct
"""
image_list = glob.glob(
os.path.join(
"C:\\Users\\chuyangl\\Desktop\\liushuai\\calibrator\\board\\left",
"*.bmp"))
for single_img in image_list:
image = cv2.imread(single_img)
new_image = un_distort_image(image)
cv2.imshow(
'before',
cv2.resize(image,
(int(image.shape[1] * 0.7), int(image.shape[0] * 0.7))))
cv2.imshow(
'after',
cv2.resize(new_image, (int(
new_image.shape[1] * 0.7), int(new_image.shape[0] * 0.7))))
cv2.waitKey(0)
image = cv2.imread(image_list[0])
# distortion_points = [ge.Point(110, 437), ge.Point(932, 151), ge.Point(1034, 331)]
# calibration_points = [ge.Point(510, 437), ge.Point(832, 151), ge.Point(1134, 331)]
distortion_points = [
ge.Point(110, 437),
ge.Point(632, 151),
ge.Point(333, 331)
]
calibration_points = [
ge.Point(510, 437),
ge.Point(532, 151),
ge.Point(234, 331)
]
for p in distortion_points:
cv2.circle(image, p.tuple(), 23, (0, 0, 255), 2)
new_image = un_distort_image(image)
for p in calibration_points:
cv2.circle(new_image, p.tuple(), 23, (255, 0, 0), 4)
p2 = distort_point(p)
p3 = un_distort_point(p2)
cv2.circle(image, p2.int().tuple(), 23, (0, 255, 255), 4)
cv2.circle(new_image, p3.int().tuple(), 23, (0, 0, 255), 4)
print(p.int().tuple(), p2.int().tuple(), p3.int().tuple())
for p in distortion_points:
p2 = un_distort_point(p)
p3 = distort_point(p2)
cv2.circle(new_image, p2.int().tuple(), 23, (0, 255, 255), 2)
cv2.circle(image, p3.int().tuple(), 23, (0, 255, 255), 2)
print(p.int().tuple(), p2.int().tuple(), p3.int().tuple())
cv2.imshow(
'before',
cv2.resize(image,
(int(image.shape[1] * 0.7), int(image.shape[0] * 0.7))))
cv2.imshow(
'after',
cv2.resize(
new_image,
(int(new_image.shape[1] * 0.7), int(new_image.shape[0] * 0.7))))
cv2.waitKey(0)
X = cv2.triangulatePoints(projl, projr, _2dl[:2], _2dr[:2]) # coor
# Remember to divide out the 4th row. Make it homogeneous
X /= X[3]
# Recover the origin arrays from PX
x1 = np.dot(projl, X)
x2 = np.dot(projr, X)
# Again, put in homogeneous form before using them
x1 /= x1[2]
x2 /= x2[2]
print('X\n', X)
print('x1\n', x1)
print('x2\n', x2)
pl = ge.Point(498.0, 186.)
pr = ge.Point(130., 179.)
pl = pixel2cam(pl, matrixl)
pr = pixel2cam(pr, matrixr)
_2dl = np.array([[pl.x, 404., 447.], [pl.y, 181., 180.], [1., 1., 1.]])
_2dr = np.array([[pr.x, 70., 96], [pr.y, 168., 170.], [1., 1., 1.]])
X = cv2.triangulatePoints(projl, projr, _2dl[:2], _2dr[:2]) # coor
# Remember to divide out the 4th row. Make it homogeneous
X /= X[3]
# Recover the origin arrays from PX
x1 = np.dot(projl, X)
x2 = np.dot(projr, X)
# Again, put in homogeneous form before using them
x1 /= x1[2]