Ejemplo n.º 1
0
    def draw3d_reconstracted(self):
        self.ax_rcn.cla()

        # カメラ行列(内部・外部)
        M1 = self.K.dot(self.P1)
        M2 = self.K.dot(self.P2)

        # 画像座標 ( sx, sy, s )
        x1p = M1.dot(self.Xpt)
        x2p = M2.dot(self.Xpt)

        # sで正規化 (x, y, 1)
        for i in range(3):
            x1p[i] /= x1p[2]
            x2p[i] /= x2p[2]

        # 三角測量と正規化 (X,Y,Z,1)
        X = sfm.triangulate(x1p, x2p, M1, M2)

        self.ax_rcn.set_xlabel("x_wld_rcn")
        self.ax_rcn.set_ylabel("y_wld_rcn")
        self.ax_rcn.set_zlabel("z_wld_rcn")
        self.ax_rcn.set_xlim([-5, 5])
        self.ax_rcn.set_ylim([-5, 5])
        self.ax_rcn.set_zlim([-5, 5])
        self.ax_rcn.set_title("obj wld rcn")
        for l in self.ll:
            x = np.array([X[0][l[0]], X[0][l[1]]])
            y = np.array([X[1][l[0]], X[1][l[1]]])
            z = np.array([X[2][l[0]], X[2][l[1]]])
            self.ax_rcn.plot(x, y, z, marker='.', markersize=5, color='red')
Ejemplo n.º 2
0
x2n = np.dot(np.linalg.inv(K), x2)

# estimate E with RANSAC
model = sfm.RansacModel()
E, inliers = sfm.F_from_ransac(x1n, x2n, model)

# compute camera matrices (P2 will be list of four solutions)
P1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2 = sfm.compute_P_from_essential(E)

# pick the solution with points in front of cameras
ind = 0
maxres = 0
for i in range(4):
    # triangulate inliers and compute depth for each camera
    X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2[i])
    d1 = np.dot(P1, X)[2]
    d2 = np.dot(P2[i], X)[2]
    if np.sum(d1 > 0) + np.sum(d2 > 0) > maxres:
        maxres = np.sum(d1 > 0) + np.sum(d2 > 0)
        ind = i
        infront = (d1 > 0) & (d2 > 0)

# triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2[ind])
X = X[:, infront]

# 3D plot
from mpl_toolkits.mplot3d import axes3d

fig = plt.figure()
Ejemplo n.º 3
0
from pylab import *
import sfm

execfile('load_vggdata.py')

# Points in first 2 views.
ndx = (corr[:, 0] >= 0) & (corr[:, 1] >= 0)

x1 = points2D[0][:, corr[ndx, 0]]
x1 = numpy.vstack( (x1, numpy.ones(x1.shape[1])) )
x2 = points2D[1][:, corr[ndx, 1]]
x2 = numpy.vstack( (x2, numpy.ones(x2.shape[1])) )

Xtrue = points3D[:, ndx]
Xtrue = numpy.vstack( (Xtrue, numpy.ones(Xtrue.shape[1])) )

Xest = sfm.triangulate(x1, x2, P[0].P, P[1].P)

# Check 3 points:
print Xest[:, :3]
print Xtrue[:, :3]

from mpl_toolkits.mplot3d import axes3d
fig = figure()
ax = fig.gca(projection='3d')
ax.plot(Xest[0], Xest[1], Xest[2], 'ko')
ax.plot(Xtrue[0], Xtrue[1], Xtrue[2], 'b.')
axis('equal')

show()
Ejemplo n.º 4
0
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2,:2].T)
x1n = dot(inv(K),x1)
x2n = dot(inv(K),x2)
print "estimate E with RANSAC"
model = sfm.RansacModel()
E,inliers = sfm.F_from_ransac(x1n,x2n,model)
print "compute camera matrices (P2 will be list of four solutions)"
P1 = array([[1,0,0,0],[0,1,0,0],[0,0,1,0]]) 
P2 = sfm.compute_P_from_essential(E)
print "pick the solution with points in front of cameras"
ind = 0
maxres = 0
for i in range(4):
    # triangulate inliers and compute depth for each camera
    X = sfm.triangulate(x1n[:,inliers],x2n[:,inliers],P1,P2[i]) 
    d1 = dot(P1,X)[2]
    d2 = dot(P2[i],X)[2]
    if sum(d1>0)+sum(d2>0) > maxres:
        maxres = sum(d1>0)+sum(d2>0) 
        ind = i
        infront = (d1>0) & (d2>0)
    # triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:,inliers],x2n[:,inliers],P1,P2[ind]) 
X = X[:,infront]
print "3D plot"
from mpl_toolkits.mplot3d import axes3d
fig = figure()
ax = fig.gca(projection='3d')
ax.plot(-X[0],X[1],X[2],'k.')
axis('off')
Ejemplo n.º 5
0
# %matplotlib notebook

# 最初の2枚の画像の点のインデックス番号
ndx = (corr[:, 0] >= 0) & (corr[:, 1] >= 0)

# 座標値を取得し、同座標にする
x1 = points2D[0][:, corr[ndx, 0]]
x1 = homography.make_homog(x1)
x2 = points2D[1][:, corr[ndx, 1]]
x2 = homography.make_homog(x2)

Xtrue = points3D[:, ndx]
Xtrue = homography.make_homog(Xtrue)

#最初の3点を調べる
Xest = sfm.triangulate(x1, x2, P[0].P, P[1].P)
print(Xest[:, :3])
print(Xtrue[:, :3])

#描写する
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(Xest[0], Xest[1], Xest[2], 'ko')
ax.plot(Xtrue[0], Xtrue[1], Xtrue[2], 'r.')
plt.show

# +
# %matplotlib inline
corr1 = corr[:, 0]  #第一視点のインデックスを取り出す。
ndx3D = np.where(corr1 >= 0)[0]
ndx2D = corr1[ndx3D]
Ejemplo n.º 6
0
import sfm

execfile('load_vggdata.py')

# Points in first 2 views.
ndx = (corr[:, 0] >= 0) & (corr[:, 1] >= 0)

x1 = points2D[0][:, corr[ndx, 0]]
x1 = numpy.vstack((x1, numpy.ones(x1.shape[1])))
x2 = points2D[1][:, corr[ndx, 1]]
x2 = numpy.vstack((x2, numpy.ones(x2.shape[1])))

Xtrue = points3D[:, ndx]
Xtrue = numpy.vstack((Xtrue, numpy.ones(Xtrue.shape[1])))

Xest = sfm.triangulate(x1, x2, P[0].P, P[1].P)

# Check 3 points:
print Xest[:, :3]
print Xtrue[:, :3]

from mpl_toolkits.mplot3d import axes3d

fig = figure()
ax = fig.gca(projection='3d')
ax.plot(Xest[0], Xest[1], Xest[2], 'ko')
ax.plot(Xtrue[0], Xtrue[1], Xtrue[2], 'r.')
axis('equal')

show()
Ejemplo n.º 7
0
def processInput():
    print ""
    if inputArgs.left == "" or inputArgs.right == "":
        print "Missing images!"
        quit()

    # here we go ...

    # load image pair - we might need them for later
    img_l = cv2.imread(inputArgs.left)
    img_r = cv2.imread(inputArgs.right)

    if img_l == None or img_r == None:
        print "Missing images!"
        quit()

    ### git them points - calibration
    mkp_l = []
    mkp_r = []

    file_kp = open(inputArgs.nameCalibration, 'r')

    if file_kp == None:
        print "Missing matching points file"
        quit()

    for line in file_kp:
        l_r = line.split(';')
        mkp_l.append(
            [float(l_r[0].split(',')[0]),
             float(l_r[0].split(',')[1])])
        mkp_r.append(
            [float(l_r[1].split(',')[0]),
             float(l_r[1].split(',')[1])])
    file_kp.close()

    mkp_l = np.float32(mkp_l)
    mkp_r = np.float32(mkp_r)

    ### git them points - reconstruction
    mkp_l_p = []
    mkp_r_p = []

    file_kp = open(inputArgs.namePoints, 'r')

    if file_kp == None:
        print "Missing matching points file"
        quit()

    for line in file_kp:
        l_r = line.split(';')
        mkp_l_p.append(
            [float(l_r[0].split(',')[0]),
             float(l_r[0].split(',')[1])])
        mkp_r_p.append(
            [float(l_r[1].split(',')[0]),
             float(l_r[1].split(',')[1])])
    file_kp.close()

    mkp_l_p = np.float32(mkp_l_p)
    mkp_r_p = np.float32(mkp_r_p)

    ### git them calibrations - left
    K_l = []
    file_c_l = open(inputArgs.leftCalibration, 'r')

    if file_c_l == None:
        print "Missing left calibration file"
        quit()

    for line in file_c_l:
        c_l = line.split(' ')
        K_l.append([float(c_l[0]), float(c_l[1]), float(c_l[2])])
    file_c_l.close()
    K_l = np.float32(K_l)

    ### git them calibrations - right
    K_r = []
    file_c_r = open(inputArgs.rightCalibration, 'r')

    if file_c_r == None:
        print "Missing right calibration file"
        quit()

    for line in file_c_r:
        c_l = line.split(' ')
        K_r.append([float(c_l[0]), float(c_l[1]), float(c_l[2])])
    file_c_r.close()
    K_r = np.float32(K_r)

    ### ok, now we start work

    # make homogeneous and normalize with inv(K)
    x1 = homography.make_homog(mkp_l_p.T)
    x2 = homography.make_homog(mkp_r_p.T)

    x1n = dot(inv(K_l), x1)
    x2n = dot(inv(K_r), x2)

    # compute E (E = (K_r)T * F * K_l)
    #F, mask = cv2.findFundamentalMat(mkp_l, mkp_r, cv2.FM_8POINT)
    F, mask = cv2.findFundamentalMat(mkp_l, mkp_r, cv2.FM_RANSAC, 1, 0.99)

    # we select only inlier points - most pf the time this makes it worse
    #mkp_l = mkp_l[mask.ravel()==1]
    #mkp_r = mkp_r[mask.ravel()==1]

    E = K_r.transpose()
    E = E.dot(F)
    E = E.dot(K_l)

    # compute camera matrices (P2 will be list of four solutions)
    P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
    P2 = sfm.compute_P_from_essential(E)

    # pick the solution with points in front of cameras
    ind = 0
    maxres = 0

    for i in range(4):
        # triangulate inliers and compute depth for each camera
        X = sfm.triangulate(x1n, x2n, P1, P2[i])
        d1 = dot(P1, X)[2]
        d2 = dot(P2[i], X)[2]
        if sum(d1 > 0) + sum(d2 > 0) > maxres:
            maxres = sum(d1 > 0) + sum(d2 > 0)
            ind = i
            infront = (d1 > 0) & (d2 > 0)

    # triangulate inliers and remove points not in front of both cameras
    X = sfm.triangulate(x1n, x2n, P1, P2[ind])
    X = X[:, infront]

    # visualization
    if inputArgs.debug == 1:
        # draw points
        img_tmp = img_l.copy()
        for kp in mkp_l_p:
            x = int(kp[0])
            y = int(kp[1])
            cv2.circle(img_tmp, (x, y), 3, (0, 0, 255))

        cv2.imwrite(inputArgs.namePoints + ".jpg", img_tmp)

    # 3D plot
    out_points = []
    out_colors = []

    for i in range(len(X[0])):
        out_points.append([X[0][i], X[1][i], X[2][i]])
        #out_colors.append(img_l[int(x1[1][i])][int(x1[0][i])])
        out_colors.append([
            img_l[int(x1[1][i])][int(x1[0][i])][2],
            img_l[int(x1[1][i])][int(x1[0][i])][1],
            img_l[int(x1[1][i])][int(x1[0][i])][0]
        ])

    out_points = np.float32(out_points)
    out_colors = np.float32(out_colors)
    write_ply(inputArgs.namePoints + ".ply", out_points, out_colors)
Ejemplo n.º 8
0
    def draw3d_reconstracted_estP(self):
        self.ax_rcn_est.cla()

        # カメラ行列(内部・外部)
        M1 = self.K.dot(self.P1)
        M2 = self.K.dot(self.P2)

        #  画像座標 ( sx, sy, s )
        x1p = M1.dot(self.Xpt)
        x2p = M2.dot(self.Xpt)

        # sで正規化 (x, y, 1)
        for i in range(3):
            x1p[i] /= x1p[2]
            x2p[i] /= x2p[2]

        # pts1 = []
        # pts2 = []
        # for i in range(len(x1p[0])):
        #     pts1.append([x1p[0][i], x1p[1][i]])
        #     pts2.append([x2p[0][i], x2p[1][i]])
        # pts1 = np.int32(pts1)
        # pts2 = np.int32(pts2)

        # 画像1, 画像2の特徴点を対応付ける行列Fを計算
        # F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)
        # mask = np.reshape(mask, (1, len(mask)))[0]
        # idx_mask = np.arange(len(mask))
        # idx_mask = idx_mask[mask == 1]

        x1n = np.dot(inv(self.K), x1p)
        x2n = np.dot(inv(self.K), x2p)

        # RANSACでEを推定
        # Method_EstE = "RAN"
        Method_EstE = ""
        E = ""
        inliers = ""
        if Method_EstE is "RAN":
            model = sfm.RansacModel()
            E, inliers = sfm.F_from_ransac(x1n, x2n, model)
        else:
            E = sfm.compute_fundamental(x1n, x2n)
            inliers = [True for i in range(len(x1n[0, :]))]

        print("E: ", E)

        # # カメラ行列を計算する(P2は4つの解のリスト)
        P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
        P2, S, R1, R2, U, V = sfm.compute_P_from_essential_mod(E)

        print("S: ", S)
        print("S fro: ", np.linalg.norm(S, "fro"))

        print("R1: ", R1)
        print("R2: ", R2)
        print("R1 deg: ", calcRmatDeg(R1))
        print("R2 deg: ", calcRmatDeg(R2))
        P2_R = self.P2[:, 0:3]
        print("P2_R: ", P2_R)
        # print("U: ", U)
        # print("V: ", V)
        # print("S*R1: ", S.dot(R1))
        # print("S*R2: ", S.dot(R2))

        # 2つのカメラの前に点のある解を選ぶ
        ind = 0
        maxres = 0
        for i in range(4):
            # triangulate inliers and compute depth for each camera
            # インライアを三角測量し各カメラからの奥行きを計算する
            X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2[i])
            d1 = np.dot(P1, X)[2]
            d2 = np.dot(P2[i], X)[2]
            if sum(d1 > 0) + sum(d2 > 0) > maxres:
                maxres = sum(d1 > 0) + sum(d2 > 0)
                ind = i
                infront = (d1 > 0) & (d2 > 0)

        #全表示用
        M1 = self.K.dot(P1)
        M2_est = self.K.dot(P2[ind])
        X = sfm.triangulate(x1p, x2p, M1, M2_est)

        # print("P2 est:", P2[ind])
        # print("P2: ", self.P2)

        # for i in range(4):
        #     print("P2 est[ ", i, " ]: ", P2[i])
        #
        # M2 = self.K.dot(self.P2)
        # print("M2: ", M2)

        self.ax_rcn_est.set_xlabel("x_wld_rcn")
        self.ax_rcn_est.set_ylabel("y_wld_rcn")
        self.ax_rcn_est.set_zlabel("z_wld_rcn")
        # self.ax_rcn_est.set_xlim([-5, 5])
        # self.ax_rcn_est.set_ylim([-5, 5])
        # self.ax_rcn_est.set_zlim([-5, 5])
        self.ax_rcn_est.set_title("obj wld rcn est")
        for l in self.ll:
            x = np.array([X[0][l[0]], X[0][l[1]]])
            y = np.array([X[1][l[0]], X[1][l[1]]])
            z = np.array([X[2][l[0]], X[2][l[1]]])
            self.ax_rcn_est.plot(x,
                                 y,
                                 z,
                                 marker='.',
                                 markersize=5,
                                 color='red')

        # # 対応が見つからなかった点
        # x = X[0,[ not val for val in mask]]
        # y = X[1,[ not val for val in mask]]
        # z = X[2,[ not val for val in mask]]
        # self.ax_rcn_est.plot(x, y, z, marker='.', markersize=5, color='black', linestyle = "")

        # カメラより奥にあると推定された点
        # X = sfm.triangulate(x1[:, idx_mask], x2[:, idx_mask], P1, P2[i])
        x = X[0, [not val for val in infront]]
        y = X[1, [not val for val in infront]]
        z = X[2, [not val for val in infront]]
        self.ax_rcn_est.plot(x,
                             y,
                             z,
                             marker='.',
                             markersize=5,
                             color='blue',
                             linestyle="")