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))