近似多項式の係数推定

前提・実現したいこと

図のような位置校正用の格子板の交点を既知点として抽出し、
そして補正式を３次の近似多項式として係数を最小二乗法を用いて求め、補正式の逆変換を利用して画像の歪曲収差を補正したいです。

該当のソースコード

kosi.py
1"""coding: utf-8"""
2
3from PIL import Image
4import numpy as np
5import cv2
6from matplotlib import pyplot as plt
7from scipy import optimize
8
9"""
10位置校正用の格子板の交点を既知点として抽出
11そして補正式を３次の近似多項式として係数を最小二乗法を用いて求め、濃度値は３次畳み込み内挿法で内挿
12"""
13
14img = cv2.imread('図１.jpg')
15gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
16
17"""Harris corners"""
18gray = np.float32(gray)
19dst = cv2.cornerHarris(gray,2,3,0.04)
20dst = cv2.dilate(dst,None)
21ret, dst = cv2.threshold(dst,0.01*dst.max(),255,0)
22dst = np.uint8(dst)
23
24"""中心見つける"""
25ret, labels, stats, centroids = cv2.connectedComponentsWithStats(dst)
26
27"""コーナーをサブピクセル精度で検出、また基準を設ける"""
28criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
29corners = cv2.cornerSubPix(gray,np.float32(centroids),(5,5),(-1,-1),criteria)
30
31
32dst = cv2.dilate(dst,None)
33
34"""描画"""
35res = np.hstack((centroids,corners))
36res = np.int0(res)
37img[res[:,3],res[:,2]] = [0,255,0]
38
39
40"""格子点の座標を抽出"""
41newcorners = np.delete(corners, 0, 0)
42print(newcorners) 
43a = int(len(newcorners))
44cv2.namedWindow('dst', cv2.WINDOW_NORMAL)
45cv2.imshow('dst',img)
46if cv2.waitKey(0) & 0xff == 27:
47    cv2.destroyAllWindows()
48
49
50
51"""サブピクセル精度で検出した座標を与えて、多項式の係数を最小二乗法で求める。"""
52x = newcorners[:,0]
53y = newcorners[:,1]
54f = 1 + x + y + x**2 + y**2 + x*y + x**3 + y**3 + y*x**2 + x*y**2
55
56Xtil = np.c_[np.ones(newcorners.shape[0]), newcorners] # Xの行列の左端に[1,1,1,...,1]^Tを加える。
57A = np.dot(Xtil.T, Xtil) # 標準形A,bに当てはめる。
58b = np.dot(Xtil.T, f)
59w = linalg.solve(A, b) # wについて解く。
60
61xmesh, ymesh = np.meshgrid(np.linspace(0, 1000, 20),
62                            np.linspace(0,1000, 20))
63zmesh = (w[0] + w[1] * xmesh.ravel() +
64        w[2] * ymesh.ravel() ).reshape(xmesh.shape)
65
66fig = plt.figure()
67ax = fig.add_subplot(111, projection='3d')
68ax.scatter(x, y, f, color='k')
69ax.plot_wireframe(xmesh, ymesh, zmesh, color='r')
70plt.show()
71
72