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
