def remove_points_with_big_reproj_err(self, points):
with self._lock:
with self.update_lock:
#print('map points: ', sorted([p.id for p in self.points]))
#print('points: ', sorted([p.id for p in points]))
culled_pt_count = 0
for p in points:
# compute reprojection error
chi2s = []
for f, idx in p.observations():
uv = f.kpsu[idx]
proj, _ = f.project_map_point(p)
invSigma2 = Frame.feature_manager.inv_level_sigmas2[
f.octaves[idx]]
err = (proj - uv)
chi2s.append(np.inner(err, err) * invSigma2)
# cull
mean_chi2 = np.mean(chi2s)
if np.mean(
chi2s
) > Parameters.kChi2Mono: # chi-square 2 DOFs (Hartley Zisserman pg 119)
culled_pt_count += 1
#print('removing point: ',p.id, 'from frames: ', [f.id for f in p.keyframes])
self.remove_point(p)
Printer.blue("# culled map points: ", culled_pt_count)
def large_window_BA(self):
Printer.blue('@large BA')
# large window optimization of the map
self.kid_last_BA = self.kf_cur.kid
self.time_large_opt.start()
err = self.map.optimize(local_window=Parameters.kLargeBAWindow, abort_flag=self.opt_abort_flag) # verbose=True)
self.time_large_opt.refresh()
Printer.blue('large window optimization error^2: %f, KF id: %d' % (err,self.kf_cur.kid))
