对极几何基础矩阵原理及实现
对极几何基础矩阵原理以及实现
原理参考:https://www.cnblogs.com/sunny-li/p/7500541.html
1、基本概念
立体成像的基本几何就是对极几何。下图是最经典的对极几何示意图。
O1和O2是两个相机的主点(可以是不同时刻)。P为空间中一个物点,两个相对的白色平面是像面,p1和p2是P点在像面上的对应点,e1 e2为像面和O1 O2的交点。O1O2为基线,也被称作相机的移动方向。
下面介绍极点的性质:
极点
=像平面和的基线O1O2交点
=O2在相机1上的像点为e1
=基线的平行线在各自像面上的消失点
对极约束
p点在像面2上的对应点一定在极线l’上。
2、基础矩阵
如果已知基础矩阵F,以及一个3D点在一个像面上的像素坐标p,则可以求得在另一个像面上的像素坐标p’。这个是基础矩阵的作用,可以表征两个相机的相对位置及相机内参数。
下面具体介绍基础矩阵与像素坐标p和p’的关系。
以O1为原点,光轴方向为z轴,另外两个方向为x, y轴可以得到一个坐标系,在这个坐标系下,可以对P, p1(即图中所标p), p2(即图中所标p’)得到三维坐标,同理,对O2也可以得到一个三维坐标,这两个坐标之间的转换矩阵为[R T],即通过旋转R和平移T可以将O1坐标系下的点P1(x1, y1, z1), 转换成O2坐标系下的P2(x2, y2, z2)。
则可知,P2 = R(P1-T) ———— (1)
采用简单的立体几何知识,可以知道
(2)
其中,p, p’分别为P点的像点在两个坐标系下分别得到的坐标(非二维像素坐标)。Rp’为极面上一矢量,T为极面上一矢量,则两矢量一叉乘为极面的法向量, 这个法向量与极面上一矢量p一定是垂直的,所以上式一定成立。(这里采用转置是因为p会表示为列向量的形式,此处需要为行向量)
采用一个常用的叉乘转矩阵的方法,
(3)
将我们的叉乘采用上面的转换,会变成
(4)
红框中所标即为本征矩阵E, 他描述了三维像点p和p’之间的关系
(5)
有了本征矩阵,我们的基础矩阵也就容易推导了
注意到将p和p’换成P1和P2式(4)也是成立的,且有
q1 = K1P1 (6)
q2 = K2P2 (7)
上式中, K1K2为相机的校准矩阵, 描述相机的内参数 q1q2为相机的像素坐标
代入式(4)中,得
(8)
上式中p->q1, p’->q2
这样我们就得到了两个相机上的像素坐标和基础矩阵F之间的关系了
(9)
2.1、基础矩阵性质
F有什么样的性质呢?简单说来, 3x3的矩阵,理论上9个*度,但是需要符合以下两个约束
a)如果F为基础矩阵,那么kF也为基础矩阵
b)秩为2
所以减去两个*度,F有7个*度。
至于秩=2。我们知道,矩阵的秩有这么一个性质,矩阵相乘的秩不大于各矩阵的秩
那么,可以知道F其实是以下这些矩阵相乘的结果
其中,Tx的具体形式在式(3)中有表述
可以知道,第三列A2可以用第一列A0和第二列A1线性表示
A2 = -TyA1/Tz-TxA0/Tz
所以Tx秩为2
那么F秩为2也得证了。
3、对极几何应用
#!/usr/bin/env python
# coding: utf-8
# In[1]:
from PIL import Image
from numpy import *
from pylab import *
import numpy as np
from imp import reload
# In[2]:
import importlib
from PCV.geometry import camera
from PCV.geometry import homography
from PCV.geometry import sfm
from PCV.localdescriptors import sift
camera = importlib.reload(camera)
homography = importlib.reload(homography)
sfm = importlib.reload(sfm)
sift =importlib.reload(sift)
# In[3]:
# Read features
im1 = array(Image.open('Epipolar_Geometry_pic\pic1.jpg'))
sift.process_image('Epipolar_Geometry_pic\pic1.jpg', 'im1.sift')
l1, d1 = sift.read_features_from_file('im1.sift')
im2 = array(Image.open('Epipolar_Geometry_pic\pic2.jpg'))
sift.process_image('Epipolar_Geometry_pic\pic2.jpg', 'im2.sift')
l2, d2 = sift.read_features_from_file('im2.sift')
# In[9]:
matches = sift.match_twosided(d1, d2)
# In[10]:
ndx = matches.nonzero()[0]
x1 = homography.make_homog(l1[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2, :2].T)
x1n = x1.copy()
x2n = x2.copy()
# In[11]:
print (len(ndx))
# In[12]:
figure(figsize=(16,16))
sift.plot_matches(im1, im2, l1, l2, matches, True)
show()
# In[13]:
# Chapter 5 Exercise 1
# Don't use K1, and K2
#def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
""" Robust estimation of a fundamental matrix F from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: x1, x2 (3*n arrays) points in hom. coordinates. """
from PCV.tools import ransac
data = np.vstack((x1, x2))
d = 20 # 20 is the original
# compute F and return with inlier index
F, ransac_data = ransac.ransac(data.T, model,8, maxiter, match_threshold, d, return_all=True)
return F, ransac_data['inliers']
# In[15]:
# find E through RANSAC
model = sfm.RansacModel()
F, inliers = F_from_ransac(x1n, x2n, model, maxiter=5000, match_threshold=1e-3)
# In[16]:
print (len(x1n[0]))
print (len(inliers))
# In[17]:
P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2 = sfm.compute_P_from_fundamental(F)
# In[18]:
# triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2)
# In[19]:
# plot the projection of X
cam1 = camera.Camera(P1)
cam2 = camera.Camera(P2)
x1p = cam1.project(X)
x2p = cam2.project(X)
# In[20]:
figure()
imshow(im1)
gray()
plot(x1p[0], x1p[1], 'o')
#plot(x1[0], x1[1], 'r.')
axis('off')
figure()
imshow(im2)
gray()
plot(x2p[0], x2p[1], 'o')
#plot(x2[0], x2[1], 'r.')
axis('off')
show()
# In[21]:
figure(figsize=(16, 16))
im3 = sift.appendimages(im1, im2)
im3 = vstack((im3, im3))
imshow(im3)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1p[0])):
if (0<= x1p[0][i]<cols1) and (0<= x2p[0][i]<cols1) and (0<=x1p[1][i]<rows1) and (0<=x2p[1][i]<rows1):
plot([x1p[0][i], x2p[0][i]+cols1],[x1p[1][i], x2p[1][i]],'c')
axis('off')
show()
# In[22]:
print (F)
# In[23]:
# Chapter 5 Exercise 2
x1e = []
x2e = []
ers = []
for i,m in enumerate(matches):
if m>0: #plot([locs1[i][0],locs2[m][0]+cols1],[locs1[i][1],locs2[m][1]],'c')
p1 = array([l1[i][0], l1[i][1], 1])
p2 = array([l2[m][0], l2[m][1], 1])
# Use Sampson distance as error
Fx1 = dot(F, p1)
Fx2 = dot(F, p2)
denom = Fx1[0]**2 + Fx1[1]**2 + Fx2[0]**2 + Fx2[1]**2
e = (dot(p1.T, dot(F, p2)))**2 / denom
x1e.append([p1[0], p1[1]])
x2e.append([p2[0], p2[1]])
ers.append(e)
x1e = array(x1e)
x2e = array(x2e)
ers = array(ers)
# In[24]:
indices = np.argsort(ers)
x1s = x1e[indices]
x2s = x2e[indices]
ers = ers[indices]
x1s = x1s[:20]
x2s = x2s[:20]
# In[25]:
figure(figsize=(16, 16))
im3 = sift.appendimages(im1, im2)
im3 = vstack((im3, im3))
imshow(im3)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1s)):
if (0<= x1s[i][0]<cols1) and (0<= x2s[i][0]<cols1) and (0<=x1s[i][1]<rows1) and (0<=x2s[i][1]<rows1):
plot([x1s[i][0], x2s[i][0]+cols1],[x1s[i][1], x2s[i][1]],'c')
axis('off')
show()
# In[ ]:
室内:
室内拍摄了三张图片,分别命名为pic1、pic2、pic3。
pic1与pic2的sift特征匹配:基础矩阵:
恢复点的三维位置
修改特征匹配结果后
# coding: utf-8
# In[1]:
from PIL import Image
from numpy import *
from pylab import *
import numpy as np
import importlib
# In[2]:
from PCV.geometry import camera
from PCV.geometry import homography
from PCV.geometry import sfm
from PCV.localdescriptors import sift
camera = importlib.reload(camera)
homography = importlib.reload(homography)
sfm = importlib.reload(sfm)
sift =importlib.reload(sift)
# In[3]:
# Read features
im1 = array(Image.open('Epipolar_Geometry_pic\pic1.jpg'))
sift.process_image('Epipolar_Geometry_pic\pic1.jpg', 'im1.sift')
im2 = array(Image.open('Epipolar_Geometry_pic\pic2.jpg'))
sift.process_image('Epipolar_Geometry_pic\pic2.jpg', 'im2.sift')
# In[4]:
l1, d1 = sift.read_features_from_file('im1.sift')
l2, d2 = sift.read_features_from_file('im2.sift')
# In[5]:
matches = sift.match_twosided(d1, d2)
# In[6]:
ndx = matches.nonzero()[0]
x1 = homography.make_homog(l1[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
x2 = homography.make_homog(l2[ndx2, :2].T)
d1n = d1[ndx]
d2n = d2[ndx2]
x1n = x1.copy()
x2n = x2.copy()
# In[7]:
figure(figsize=(16,16))
sift.plot_matches(im1, im2, l1, l2, matches, True)
show()
# In[26]:
#def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
def F_from_ransac(x1, x2, model, maxiter=5000, match_threshold=1e-6):
""" Robust estimation of a fundamental matrix F from point
correspondences using RANSAC (ransac.py from
http://www.scipy.org/Cookbook/RANSAC).
input: x1, x2 (3*n arrays) points in hom. coordinates. """
from PCV.tools import ransac
data = np.vstack((x1, x2))
d = 10 # 20 is the original
# compute F and return with inlier index
F, ransac_data = ransac.ransac(data.T, model,
8, maxiter, match_threshold, d, return_all=True)
return F, ransac_data['inliers']
# In[27]:
# find F through RANSAC
model = sfm.RansacModel()
F, inliers = F_from_ransac(x1n, x2n, model, maxiter=5000, match_threshold=1e-5)
print (F)
# In[28]:
P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2 = sfm.compute_P_from_fundamental(F)
# In[29]:
print (P2)
print (F)
# In[30]:
# P2, F (1e-4, d=20)
# [[ -1.48067422e+00 1.14802177e+01 5.62878044e+02 4.74418238e+03]
# [ 1.24802182e+01 -9.67640761e+01 -4.74418113e+03 5.62856097e+02]
# [ 2.16588305e-02 3.69220292e-03 -1.04831621e+02 1.00000000e+00]]
# [[ -1.14890281e-07 4.55171451e-06 -2.63063628e-03]
# [ -1.26569570e-06 6.28095242e-07 2.03963649e-02]
# [ 1.25746499e-03 -2.19476910e-02 1.00000000e+00]]
# In[31]:
# triangulate inliers and remove points not in front of both cameras
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2)
# In[32]:
# plot the projection of X
cam1 = camera.Camera(P1)
cam2 = camera.Camera(P2)
x1p = cam1.project(X)
x2p = cam2.project(X)
# In[33]:
figure(figsize=(16, 16))
imj = sift.appendimages(im1, im2)
imj = vstack((imj, imj))
imshow(imj)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1p[0])):
if (0<= x1p[0][i]<cols1) and (0<= x2p[0][i]<cols1) and (0<=x1p[1][i]<rows1) and (0<=x2p[1][i]<rows1):
plot([x1p[0][i], x2p[0][i]+cols1],[x1p[1][i], x2p[1][i]],'c')
axis('off')
show()
# In[34]:
d1p = d1n[inliers]
d2p = d2n[inliers]
# In[35]:
# Read features
im3 = array(Image.open('Epipolar_Geometry_pic\pic3.jpg'))
sift.process_image('Epipolar_Geometry_pic\pic3.jpg', 'im3.sift')
l3, d3 = sift.read_features_from_file('im3.sift')
# In[36]:
matches13 = sift.match_twosided(d1p, d3)
# In[37]:
ndx_13 = matches13.nonzero()[0]
x1_13 = homography.make_homog(x1p[:, ndx_13])
ndx2_13 = [int(matches13[i]) for i in ndx_13]
x3_13 = homography.make_homog(l3[ndx2_13, :2].T)
# In[38]:
figure(figsize=(16, 16))
imj = sift.appendimages(im1, im3)
imj = vstack((imj, imj))
imshow(imj)
cols1 = im1.shape[1]
rows1 = im1.shape[0]
for i in range(len(x1_13[0])):
if (0<= x1_13[0][i]<cols1) and (0<= x3_13[0][i]<cols1) and (0<=x1_13[1][i]<rows1) and (0<=x3_13[1][i]<rows1):
plot([x1_13[0][i], x3_13[0][i]+cols1],[x1_13[1][i], x3_13[1][i]],'c')
axis('off')
show()
# In[39]:
P3 = sfm.compute_P(x3_13, X[:, ndx_13])
# In[40]:
print (P3)
# In[41]:
print (P1)
print (P2)
print (P3)
# In[22]:
# Can't tell the camera position because there's no calibration matrix (K)
依然是使用室内三张图。
pic1与pic2匹配结果
得到了基础矩阵:
其中pic2的相机矩阵为
处理后的特征匹配为:
pic1和pic3的匹配结果为
三张图片的相机矩阵为
4实验总结
在拍摄图片之后,进行sift特征匹配的时候,会遇到下面的问题。
出现这个问题说明,代码读取的两张图片特征点匹配数量太少了,所以在拍摄的时候一定要注意,可以先多拍几张,然后多试一试。
另外室外的照片,使用的是一张近景、一张远景,在尝试过以后。出现了上述的错误。
得到sift特征匹配结果:
出现错误结果。原因是照片之间的特征匹配数太少。