import navpy
import navfeatdb.projection.localprojection as llproj
import navfeatdb.utils.nav as nu
from scipy.interpolate import interp1d
if __name__ == '__main__':
out_path = '/data/aspn/s3/june2016'
athens_images = pd.read_hdf('/data/aspn/s3/june2016/athens_imgs.h5')
proj_imgs = athens_images.loc[225:275]
lon_lat_h = proj_imgs[['longitude', 'latitude', 'altitude']]
rpy = proj_imgs[['roll', 'pitch', 'yaw']] * 180.0 / np.pi
tf = llproj.LocalLevelProjector('/home/RYWN_Data/srtm/SRTM1/Region_06',
'/data/geoid/egm08_25.gtx')
tf.load_camera_cal(
'/home/venabldt/pysrc/navfeatdb/data/aspn_s3_cam_model.yaml')
tf.load_cam_and_vehicle_frames(
'/home/venabldt/pysrc/navfeatdb/data/aspn_s3_frames.yaml')
kml = simplekml.Kml()
img_folder = kml.newfolder(name='Athens Aspn Projection')
mf = kml.newfolder(name='Images')
# Dump image
for ii, row in proj_imgs.iterrows():
lon_lat_h = np.array(row[['longitude', 'latitude', 'altitude']])
c_n_v = nu.rpy_to_cnb(row['roll'],
row['pitch'],
good_pva = pva.read_where('(t_valid >= img0) & (t_valid <= imgN)')
lon_lat_h = np.array(
[good_pva[:]['lon'], good_pva[:]['lat'], good_pva[:]['height']]).T
rpy = np.array([
pnputils.DcmToRpy(dcm.reshape(3, 3)) for dcm in good_pva['c_nav_veh']
])
pva_interp = interp1d(good_pva['t_valid'], np.hstack((lon_lat_h, rpy)).T)
srtm_path = '/Users/venabled/data/srtm/SRTM1/Region_01'
cam_path = '/Users/venabled/pysrc/pnpnav/data/nom_autocamcal2.yaml'
uvan_frames = '/Users/venabled/pysrc/pnpnav/data/nom_autoframes2.yaml'
geoid_file = '/Users/venabled/data/geoid/egm96_15.tiff'
tf = llproj.LocalLevelProjector(srtm_path, geoid_file)
tf.load_cam_and_vehicle_frames(uvan_frames)
tf.load_camera_cal(cam_path)
# Boot up that PNP class
brisk_matcher = pmatch.BFMatcher(nn_ratio=0.8,
norm_type=cv2.HAMMING_NORM_TYPE)
pnp2 = ptude.NoAttitudePnP(matcher=brisk_matcher)
pnp2.load_pytables_db('/Users/venabled/data/neogeo/briskdb.hdf')
pnp2.load_camera_parameters(cam_path)
pnp2.use_homography_constraint(25.0)
pnp2.load_frames(uvan_frames)
flight_path = '/Users/venabled/data/uvan/fc2brisk'
feat_meta = pd.read_hdf(os.path.join(flight_path, 'feat_meta.hdf'))
def extract_kp_from_frame(flight_dict):
"""
This function extracts Keypoints
:param flight_dict:
:param img_num:
:param o_path:
:param kp_det_func:
:param kp_desc_func:
:param p_meta:
:return:
"""
import navfeatdb.db.features as fdb
import bcolz
import os
import numpy as np
import tables as tb
from scipy.interpolate import interp1d
import navfeatdb.utils.nav as nu
import navfeatdb.projection.localprojection as llproj
flight_hdf = flight_dict['flight_hdf']
img_num = flight_dict['img_num']
o_path = flight_dict['o_path']
kp_det_func = flight_dict['kp_det_func']
kp_desc_func = flight_dict['kp_desc_func']
p_meta = flight_dict['p_meta']
flight = tb.open_file(flight_hdf, 'r')
pva = flight.root.nov_span.pva
pva_times = flight.root.nov_span.pva.cols.t_valid
img_times = flight.root.camera.image_raw.compressed.metadata.cols.t_valid
images = flight.root.camera.image_raw.compressed.images
img0 = img_times[0] - 1.0
imgN = img_times[-1] + 1.0
good_pva = pva.read_where('(t_valid >= img0) & (t_valid <= imgN)')
lon_lat_h = np.array(
[good_pva[:]['lon'], good_pva[:]['lat'], good_pva[:]['height']]).T
rpy = np.array(
[nu.DcmToRpy(dcm.reshape(3, 3)) for dcm in good_pva['c_nav_veh']])
pva_interp = interp1d(good_pva['t_valid'], np.hstack((lon_lat_h, rpy)).T)
llh_rpy = pva_interp(img_times[img_num])
lon_lat_h = llh_rpy[0:3]
c_n_v = nu.rpy_to_cnb(*llh_rpy[3:])
proj = llproj.LocalLevelProjector(p_meta['srtm_path'],
p_meta['geoid_file'])
proj.load_cam_and_vehicle_frames(p_meta['uvan_frames'])
proj.load_camera_cal(p_meta['cam_path'])
aie = fdb.AirborneImageExtractor(kp_det_func(), kp_desc_func(), proj)
feat_df, desc = aie.extract_features(images[img_num], lon_lat_h, c_n_v)
center_wgs = proj.project_center(lon_lat_h, c_n_v).flatten()
df_path = os.path.join(o_path, 'feat/df/feat_%d.hdf' % img_num)
desc_path = os.path.join(o_path, 'feat/desc/desc_%d' % img_num)
if feat_df is None:
kp_meta = [
0, center_wgs[0], center_wgs[1], df_path, desc_path, flight_hdf,
img_num
]
else:
kp_meta = [
desc.shape[0], center_wgs[0], center_wgs[1], df_path, desc_path,
flight_hdf, img_num
]
feat_df.to_hdf(os.path.join(o_path, df_path),
'feat_df',
mode='w',
format='table',
complib='zlib',
complevel=7)
bcolz.carray(desc, rootdir=os.path.join(o_path, desc_path),
mode='w').flush()
flight.close()
return kp_meta