-
Notifications
You must be signed in to change notification settings - Fork 0
/
svo.py
77 lines (52 loc) · 2.59 KB
/
svo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
import numpy as np
import cv2
import params
import openImg
import triangulation
import PnP
import features
def idxMerge(matchKeptL_St,matchKept_3d,x3dprev,corLnext_T,matchKeptPrev_T,matchKeptNext_T):
#Find the overlapping indices
# import pdb; pdb.set_trace()
#find the keypoints from LEFT CAM at TIME=K-1 that were preserved all the way through 3D coordinate generation
comparePrev_T=[]
for i in xrange(len(matchKept_3d)):
comparePrev_T.append(matchKeptL_St[0,int(matchKept_3d[i])])
comparePrev_T=np.array(comparePrev_T)
finPrev_T=[]
finNext_T=[]
#find where the PREVIOUS LEFT CAMS intersect when finding the 3d coordinates at TIME=K-1 and finding correspondences at TIME=K
for j in xrange(matchKeptPrev_T.shape[1]):
#print np.where(comparePrev_T==matchKeptPrev_T[0,j])[0]
#print j
if not np.any(np.where(comparePrev_T==matchKeptPrev_T[0,j])[0]) == False:
finPrev_T.append(matchKeptPrev_T[0,j])
finNext_T.append(matchKeptNext_T[0,j])
comparePrev_T=np.array(comparePrev_T)
x3dprev_final = np.matrix(np.zeros((x3dprev.shape[0],len(finPrev_T))))
x2dnext_final = np.matrix(np.zeros((corLnext_T.shape[0],len(finNext_T))))
for k in xrange(len(finPrev_T)):
x3dIdx = (((np.where(comparePrev_T==finPrev_T[k]))[0])[0])
x2dIdx = (((np.where(matchKeptNext_T==finNext_T[k]))[0])[0])
x3dprev_final[:,k] = x3dprev[:,x3dIdx]
x2dnext_final[:,k] = np.matrix(corLnext_T)[:,x2dIdx]
return (x3dprev_final,x2dnext_final)
def svoExecute(fpathL1,fpathR1,fpathL2,fpathR2):
#initialize
(imLprev,imRprev) = openImg.getImgs(fpathL1,fpathR1)
(imLnext,imRnext) = openImg.getImgs(fpathL2,fpathR2)
size=[2,2]
#get key points
kpLprev,descLprev = features.getFeatures(imLprev,size)
kpRprev,descRprev = features.getFeatures(imRprev,size)
#correspondences
corLprev_St, corRprev_St,matchKeptL_St,matchKeptR_St = features.getCorres(descLprev,descRprev,kpLprev,kpRprev)
#triangulate must be of the form 2 x N
[x3dprev,pErrPrev,matchKept_3d] = triangulation.triangulate(corLprev_St,corRprev_St)
#Get temporal correspondences
kpLnext,descLnext = features.getFeatures(imLnext,size)
corLprev_T,corLnext_T,matchKeptPrev_T,matchKeptNext_T = features.getCorres(descLprev,descLnext,kpLprev,kpLnext)
(x3dprev_final,x2dnext_final)=idxMerge(matchKeptL_St,matchKept_3d,x3dprev,corLnext_T,matchKeptPrev_T,matchKeptNext_T)
#Get updated camera pose
[rot,trans]=PnP.camPose(x3dprev_final,x2dnext_final,pErrPrev)
return (rot,trans,x3dprev_final)