Skip to content

Commit 29759d3

Browse files
shrddrshrddr
committed
batch projection
1 parent 7d321a5 commit 29759d3

File tree

4 files changed

+48
-49
lines changed

4 files changed

+48
-49
lines changed

‎Cloud.py

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -134,8 +134,7 @@ def optimize(self, K, frames, fix_points=False):
134134
vp.set_marginalized(True)
135135
vp.set_fixed(fix_points)
136136
opt.add_vertex(vp)
137-
138-
137+
139138
# edge connects every camera with every point
140139
for f in p.frames:
141140
edge = g2o.EdgeProjectP2MC()
@@ -179,7 +178,7 @@ def optimize(self, K, frames, fix_points=False):
179178
vp = opt.vertex(PT_OFFSET + p.id)
180179
p_est = vp.estimate()
181180

182-
if (len(p.frames) <= 2) and (p.frames[-1] not in local_frames):
181+
if (len(p.frames) <= 5) and (p.frames[-1] not in local_frames):
183182
p.delete()
184183
continue
185184

@@ -189,8 +188,7 @@ def optimize(self, K, frames, fix_points=False):
189188
measured = f.kpus[f.pts.index(p)]
190189
projected = f_est.w2i.dot(np.hstack([p_est, [1]])) # only works because is VertexCam
191190
projected = projected[:2] / projected[2] # don't forget to homo
192-
errors.append(np.linalg.norm(measured-projected))
193-
191+
errors.append(np.linalg.norm(measured-projected))
194192
error = np.mean(errors) # mean of squares - over all frames
195193
if error > 1.0: # px
196194
# print(f"error {error}, dropping")
@@ -199,6 +197,6 @@ def optimize(self, K, frames, fix_points=False):
199197
p.pt3d = np.array(p_est)
200198
new_points.append(p)
201199

202-
print("Dropping: ", len(self.points)-len(new_points), "Remaining", len(new_points))
200+
print("Dropping:", len(self.points)-len(new_points), ", Remaining:", len(new_points))
203201
self.points = new_points
204202

‎Frame.py

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,6 @@ def matchFrames(old, new, reverse=False):
7676

7777
class Frame():
7878
def __init__(self, img, K):
79-
# self.diag = np.linalg.norm(img.shape[:2])
8079
self.pose = np.eye(4)
8180
# self.pose[2,3] = 100
8281
self.orb = cv2.ORB_create()

‎Point.py

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -11,12 +11,6 @@ def __init__(self, cloud, pt4d, color):
1111
cloud.points_newid += 1
1212

1313
cloud.points.append(self)
14-
15-
def project(self, pose, K):
16-
local = np.dot(np.linalg.inv(pose), self.pt4d)
17-
projected = np.dot(K, local[:3])
18-
projected = projected[:2] / projected[2]
19-
return projected
2014

2115
def orb(self):
2216
f = self.frames[-1]

‎main.py

Lines changed: 44 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -50,21 +50,22 @@ def hamming_distance(a, b):
5050
return np.count_nonzero((np.bitwise_xor(a,b) & r) != 0)
5151

5252
if __name__ == '__main__':
53-
# cap = cv2.VideoCapture('./videos/1.MP4')
54-
# cap = cv2.VideoCapture('./videos/5.MP4')
55-
# cap = cv2.VideoCapture('./videos/test_ohio.mp4')
56-
# cap = cv2.VideoCapture('./videos/test_drone.mp4')
57-
# cap = cv2.VideoCapture('./videos/test_countryroad.mp4')
58-
# cap = cv2.VideoCapture('./videos/test_countryroad_reverse.mp4')
59-
# cap = cv2.VideoCapture('./videos/test_kitti984.mp4')
60-
# cap = cv2.VideoCapture('./videos/test_kitti984_reverse.mp4')
61-
cap = cv2.VideoCapture('./videos/test_freiburgxyz525.mp4')
53+
# cap = cv2.VideoCapture('../videos/1.MP4')
54+
# cap = cv2.VideoCapture('../videos/5.MP4')
55+
cap = cv2.VideoCapture('../videos/test_ohio.mp4')
56+
# cap = cv2.VideoCapture('../videos/test_drone.mp4')
57+
# cap = cv2.VideoCapture('../videos/test_countryroad.mp4')
58+
# cap = cv2.VideoCapture('../videos/test_countryroad_reverse.mp4')
59+
# cap = cv2.VideoCapture('../videos/test_kitti984.mp4')
60+
# cap = cv2.VideoCapture('../videos/test_kitti984_reverse.mp4')
61+
# cap = cv2.VideoCapture('../videos/test_freiburgxyz525.mp4')
6262

6363
W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
6464
H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
65-
F = 500
66-
MIRROR = not True
67-
REVERSE = False
65+
F = 500 # focal distance of camera
66+
MIRROR = not True # try to recover points mis-triangulated behind the camera
67+
REVERSE = False # hack for camera moving backwards
68+
SBP_PX = 5 # max euclidian distance to search by projection
6869

6970
K = np.array([[F, 0, W/2],
7071
[0, F, H/2],
@@ -96,32 +97,33 @@ def hamming_distance(a, b):
9697
pts_old = f_old.kps[idx_old]
9798
pts_new = f_new.kps[idx_new]
9899

99-
# optimize pose only
100+
count_sbp = 0
101+
projs = None
100102
if f_new.id >= 2:
103+
# optimize pose only
101104
c.optimize(K, frames=1, fix_points=True)
102105
Rt = np.dot(f_new.pose, np.linalg.inv(f_old.pose))
103106

104-
count_sbp = 0
105-
for p in c.points:
106-
proj = p.project(f_new.pose, K)
107-
q = f_new.kd.query_ball_point(proj, 5)
108-
for m_id in q:
109-
if f_new.pts[m_id] is None:
110-
dist = cv2.norm(p.orb(), f_new.des[m_id], cv2.NORM_HAMMING)
111-
# print(proj[0], proj[1], dist)
112-
if dist < 32:
113-
# p.addObservation(f_new, m_id)
114-
# print("hdist", dist, "edist", np.linalg.norm(proj-f_new.kd.data[m_id]))
115-
count_sbp += 1
116-
117-
# for p in c.points:
118-
#
119-
# projected = f_est.w2i.dot(np.hstack([p_est, [1]])) # only works because is VertexCam
120-
# projected = projected[:2] / projected[2] # don't forget to homo
121-
# errors.append(np.linalg.norm(measured-projected))
107+
cloud_points = np.array([p.pt4d for p in c.points])
108+
KP = np.dot(K, np.linalg.inv(f_new.pose)[:3])
109+
projs = np.dot(KP, cloud_points.T).T
110+
projs = projs[:,:2] / projs[:,2:]
111+
112+
# search by projection
113+
for i,p in enumerate(c.points):
114+
proj = projs[i]
115+
q = f_new.kd.query_ball_point(proj, 5)
116+
for m_id in q:
117+
if f_new.pts[m_id] is None:
118+
dist = cv2.norm(p.orb(), f_new.des[m_id], cv2.NORM_HAMMING)
119+
if dist < 32:
120+
p.addObservation(f_new, m_id)
121+
count_sbp += 1
122122

123123
# local
124124
pts4d = triangulate(np.eye(4), Rt, pts_old, pts_new, MIRROR)
125+
if len(pts4d) == 0:
126+
print(Rt, pts_old, pts_new)
125127
# to world
126128
pts4d = np.dot(f_old.pose, pts4d.T).T
127129
# only create new Point if seen for the first time
@@ -149,11 +151,17 @@ def hamming_distance(a, b):
149151

150152
c.show()
151153

152-
kpus_old = f_old.kpus[idx_old]
153-
kpus_new = f_new.kpus[idx_new]
154-
for kpu_old, kpu_new in zip(kpus_old, kpus_new):
155-
cv2.circle(img, ipair(kpu_new), color=(0,0,255), radius=5)
156-
cv2.line(img, ipair(kpu_new), ipair(kpu_old), color=(0,0,255))
154+
for i_old, i_new in zip(idx_old, idx_new):
155+
kpu_old = f_old.kpus[i_old]
156+
kpu_new = f_new.kpus[i_new]
157+
if f_new.pts[i_new] is not None:
158+
cv2.circle(img, ipair(kpu_new), color=(0,255,0), radius=5)
159+
cv2.line(img, ipair(kpu_new), ipair(kpu_old), color=(0,255,0))
160+
else:
161+
cv2.circle(img, ipair(kpu_new), color=(0,0,255), radius=5)
162+
# if projs is not None:
163+
# for proj in projs:
164+
# cv2.circle(img, ipair(proj), color=(255,0,0), radius=3)
157165

158166
cv2.imshow('cv2', img)
159167
# time.sleep(3.5)

0 commit comments

Comments
 (0)