def SfM_gcp2xyz(GPS, RTK): from opensfm.dataset import DataSet RRF = DataSet(RTK).load_reference() a = 3 if os.path.isdir(GPS + '/matches') else 2 if not os.path.isfile(GPS + '/reconstruction.json'): SfM_cmd(GPS, range(a, 5)) GTM = DataSet(GPS).load_tracks_manager() res = [] GRC = GPS + '/reconstruction.topocentric.json' if not os.path.isfile(GRC): GRC = GRC[:-16] + 'json' with open(GRC) as f: GRC = json.load(f)[0]['points'] GIM = GPS if os.path.isdir(GPS + '/images') else GPS + '/..' with open(GPS + '/gcp_list.txt') as f: gcp = f.readlines() #dtp = [(k,float) for k in ('lon','lat','alt','x','y')] for v in gcp[1:]: # skip 1st-row *v, im = v.split() v = np.float64(v) x = [] h, w, c = cv2.imread(f'{GIM}/images/{im}').shape denm = lambda x: (x * max(w, h) * 2 + (w, h) - 1) / 2 for tid, ob in GTM.get_shot_observations(im).items(): d = np.linalg.norm(denm(ob.point) - v[3:5]) if d < 1: x.append([d, tid]) # pixel d, tid = min(x) if len(x) else [np.inf, ''] if tid not in GRC: INFO(f'skip {tid}: {[*v,im]}') else: res += [(*GRC[tid]['coordinates'], *v[[1, 0, 2]])] v = np.array(res).T v[3:] = RRF.to_topocentric(*v[3:]) return v.T
def check_gcp(gcp, cam, org=0, n=0): res = {} K = Camera(cam).K() if os.path.isdir(org): from opensfm.dataset import DataSet ref = DataSet(org).load_reference() with open(gcp) as f: data = f.readlines()[n:] for v in data: # skip first n-rows v = v.split() im = v[-1] v = v[:5] + [np.inf] * 2 if os.path.isdir(org): # lat,lon.alt->xyz lon, lat, alt = [float(i) for i in v[:3]] v[:3] = ref.to_topocentric(lat, lon, alt) if im not in res: res[im] = [v] else: res[im].append(v) for k, v in res.items(): v = res[k] = np.float64(v) if len(v) < 5: continue # skip pt, uv = v[:, :3].copy(), v[:, 3:5] # copy()->new mem-block _, Rvec, Tvec, Ins = cv2.solvePnPRansac(pt, uv, K, None) xy, Jacob = cv2.projectPoints(pt, Rvec, Tvec, K, None) err = v[:, 5] = np.linalg.norm(xy.squeeze() - uv, axis=1) his = np.histogram(err, bins=[*range(11), np.inf])[0] for c in range(len(his) - 1, 0, -1): # len(v)=sum(his) if sum(his[c:]) >= len(v) * 0.2: break idx = np.where(err <= c)[0] #print(c, his) if len(idx) < 7: continue # skip _, Rvec, Tvec = cv2.solvePnP(pt[idx], uv[idx], K, None) xy, Jacob = cv2.projectPoints(pt, Rvec, Tvec, K, None) v[:, -1] = np.linalg.norm(xy.squeeze() - uv, axis=1) # err2 out = os.path.abspath(gcp + '.err') print(out) with open(out, 'w') as f: for k, v in zip(data, np.vstack([*res.values()])): f.write(k[:-1] + '%11.3f%11.3f\n' % (*v[-2:], ))
def filter_gcp(GPS, RTK, thd=1): # reproject from odm_filter import Camera from opensfm.dataset import DataSet K = Camera(GPS + '/camera_models.json').K() ref = DataSet(RTK).load_reference() res = {} PM = 8 if hasattr(cv2, 'SOLVEPNP_SQPNP') else 1 # cv2.SOLVEPNP_ITERATIVE=0: need n>=6 non-planar # cv2.SOLVEPNP_EPNP=1, cv2.SOLVEPNP_SQPNP=8: n>=4 out, err = GPS + '/gcp_list.txt', GPS + '/gcp_err.txt' if os.path.isfile(out): with open(out) as f: gcp = f.readlines() elif os.path.isfile(err): with open(err) as f: gcp = f.readlines() for i, v in enumerate(gcp): v = v.split() x = np.float64(v[1:4]) gcp[i] = GCF([*x, *v[4:6], v[0]]) gcp.insert(0, LLA + '\n') for v in gcp[1:]: # skip 1st-row *v, im = v.split() v = np.float64(v + 2 * [np.inf]) res.setdefault(im, []) res[im].append(v) for im, v in res.items(): P = 0 if len(v) > 5 else PM if len(v) < (4 if P > 0 else 6): continue v = res[im] = np.float64(v) uv = v[:, 3:5] # lon,lat,alt pt = np.array(ref.to_topocentric(*v[:, :3].T[[1, 0, 2]])).T # for coplanar points; cv2.Rodrigues(Rv): RotVector->RotMatrix try: _, Rv, Tv, _ = cv2.solvePnPRansac(pt, uv, K, None, flags=P) except: _, Rv, Tv, _ = cv2.solvePnPRansac(pt, uv, K, None, flags=PM) # cv2.projectPoints: np.array/np.ascontiguousarray->mem-block xy, Jacob = cv2.projectPoints(pt, Rv, Tv, K, None) dis = v[:, 5] = np.linalg.norm(xy.squeeze() - uv, axis=1) his = np.histogram(dis, bins=[*range(11), np.inf])[0] for c in range(len(his) - 1, -1, -1): # len(v)=sum(his) if sum(his[c:]) >= len(v) * 0.2: break idx = np.where(dis <= c)[0] P = 0 if len(idx) > 5 else PM if len(idx) < (4 if P > 0 else 6): continue # for cv2.solvePnP try: _, Rv, Tv = cv2.solvePnP(pt[idx], uv[idx], K, None, flags=P) except: _, Rv, Tv = cv2.solvePnP(pt[idx], uv[idx], K, None, flags=PM) xy, Jacob = cv2.projectPoints(pt, Rv, Tv, K, None) v[:, 6] = np.linalg.norm(xy.squeeze() - uv, axis=1) # dis2 with open(GPS + '/gcp_err.txt', 'w') as f: # save F = lambda x: ('%s' + 3 * ' %.15f' + 2 * ' %5d' + 2 * ' %9.3f' + '\n' ) % x for k, v in res.items(): f.writelines([F((k, *e)) for e in v]) F = lambda x: np.where( x.max(axis=1) < np.inf, x.mean(axis=1), x.min(axis=1) ) # np.mean(v) if max(v)<np.inf else min(v) F = lambda x: np.where(x[:, 1] < np.inf, x[:, 1], x[:, 0]) dis = F(np.vstack([*res.values()])[:, 5:]) < thd new = [gcp[0]] + [gcp[i] for i in np.where(dis)[0] + 1] with open(GPS + '/gcp_list.txt', 'w') as f: f.writelines(new) INFO(f'Filter_GCPs: {len(gcp)} -> {len(new)}') return new