def compareTruthToEpipolar(configFile, seq, cam, idxList=None, type='both'):
config = ThesisConfig(configFile)
# get epipolar data
epiFilesDict = config.kittiPrepared['epipolar']
epiFile = config.getInputFiles(epiFilesDict, seq, cam)
with h5py.File(epiFile, 'r') as f:
epi_rot_xyz = np.array(f['epi_rot_xyz'])
epi_trans_xyz = np.array(f['epi_trans_xyz'])
# get truth data
truthFilesDict = config.kittiPrepared['truth']
truthFile = config.getInputFiles(truthFilesDict, seq)
with h5py.File(truthFile, 'r') as f:
true_rot_xyz = np.array(f['rot_xyz'])
true_trans_xyz = np.array(f['trans_xyz'])
if idxList is None:
idxList = range(len(true_rot_xyz))
for img_idx in idxList:
if type == 'rot' or type == 'both':
epi_rot = epi_rot_xyz[img_idx] * 180 / np.pi
true_rot = true_rot_xyz[img_idx] * 180 / np.pi
print('Epi Rot [%d]: [%12f, %12f, %12f]' %
(img_idx, epi_rot[0], epi_rot[1], epi_rot[2]))
print('True Rot [%d]: [%12f, %12f, %12f]' %
(img_idx, true_rot[0], true_rot[1], true_rot[2]))
if type == 'trans' or type == 'both':
epi_trans = epi_trans_xyz[img_idx]
true_trans = true_trans_xyz[img_idx]
true_trans = true_trans / np.sqrt(np.sum(
true_trans**2)) # Convert to unit vector
print('Epi Trans [%d]: [%12f, %12f, %12f]' %
(img_idx, epi_trans[0], epi_trans[1], epi_trans[2]))
print('True Trans [%d]: [%12f, %12f, %12f]' %
(img_idx, true_trans[0], true_trans[1], true_trans[2]))
def calcRotScaleParm(rotData, transData, trainIdxs):
trans = transData[trainIdxs, :]
rot = rotData[trainIdxs, :]
trans_means = np.mean(trans, axis=0)
rot_means = np.mean(rot, axis=0)
scaleParm = abs(np.mean(trans_means) / np.mean(rot_means))
return scaleParm
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
config = ThesisConfig(configFile)
# Parameters
usedCams = config.usedCams
usedSeqs = config.usedSeqs
reapplyDataNorm = config.expKittiParms['prepared']['runPrep'][
'reapplyDataNorm']
# Files
splitFilesDict = config.kittiPrepared['split']
truthFilesDict = config.kittiPrepared['truth']
imuFilesDict = config.kittiPrepared['imu']
epiFilesDict = config.kittiPrepared['epipolar']
normParmsFilesDict = config.kittiNormalized['normParms']
normDataFilesDict = config.kittiNormalized['normData']
normEpiFilesDict = config.kittiNormalized['normEpi']
# import matplotlib.pyplot as plt
from src.helpers.cfg import ThesisConfig
from src.helpers.kitti_helpers import getCalMat
from src.helpers.coframes import cvtToRpy_sd, cart2sph
from src.helpers.helper_functions import getKpsAndDescs, \
matchAndRatioTest, compareTruthToEpipolar, outlierRejectionRANSAC
# compareTruthToEpipolar('exp_configs/scale_test_3.yaml',0,0,range(100),'rot')
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
configFile = 'exp_configs/scale_test_3.yaml'
config = ThesisConfig(configFile)
# Parameters
kittiSeqs = config.kittiSeqs
usedCams = config.usedCams
normRange = config.thesisKittiParms['normPixelRange']
destImageShape = config.thesisKittiParms['downsampledImageShape']
recalcEpiData = config.expKittiParms['prepared']['runPrep']['recalcEpi']
showProgress = config.expKittiParms['prepared']['showProg']
numFeat = config.thesisKittiParms['epiNumFeatures']
epiRatio = config.thesisKittiParms['epiRatio']
# Files
origImageFilesDict = config.kittiOriginal['cams']
calFilesDict = config.kittiOriginal['cal']
import numpy as np
import h5py
import os
import sys
from src.helpers.cfg import ThesisConfig
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
config = ThesisConfig(configFile)
# Parameters
kittiSeqs = config.kittiSeqs
usedCams = config.usedCams
recalcIMUData = config.expKittiParms['prepared']['runPrep']['recalcIMU']
# Files
truthFilesDict = config.kittiPrepared['truth']
imuFilesDict = config.kittiPrepared['imu']
# Calculations
# deg^2/hr * 1/3600 * time_interval
sampleRate = config.thesisKittiParms['sampleRate'] # Hz
arwError = config.expKittiParms['imu'][
'arwError'] # deg/sqrt(hr) -- Based on Novatel UIMU-HG1700 # TODO: Cite novatel datasheet
angular_std = np.sqrt(
arwError**2 * 1 / 3600 * 1 / sampleRate) * np.pi / 180 # radians
# sqrt( deg^2/hr * hr/sec * sec ) * radians/deg
# VALUE IS APPROXIMATELY 0.002635 deg
# cfg = tf.compat.v1.ConfigProto()
# cfg.gpu_options.allow_growth = True
# K.set_session(tf.compat.v1.Session(config=cfg))
# Load Configuration
configFile = None
if len(sys.argv)>1:
configFile = sys.argv[1]
configFile = 'exp_configs/CNN_test_16.yaml'
config = ThesisConfig(configFile)
# Parameters
expName = config.experiment['experiment']['name']
# Dataset
targetImageSize = config.thesisKittiParms['downsampledImageShape']
# Training
OVERWRITE_EPOCHS = config.trainingParms['overwriteTrainEpochs']
nextEpoch = config.trainingParms['nextEpoch']
totalEpochs = config.trainingParms['totalEpochs']
initLR = config.trainingParms['initLearningRate']
trainVerbosity = config.trainingParms['verbosity']
tests = ['trans_test_2', 'trans_test_4', 'scale_test_2', 'scale_test_4']
# evalType = None
# if len(sys.argv)>1:
# evalType = sys.argv[1]
# if evalType is None:
evalType = 'val'
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '1'
logging.getLogger('tensorflow').setLevel(logging.FATAL)
# if True:
# test = tests[0]
for test in tests:
configFile = os.path.join('exp_configs', test + '.yaml')
config = ThesisConfig(configFile)
checkpointFilesDict = config.trainPaths['checkpoint']
checkpointFiles = config.getInputFiles(checkpointFilesDict)
print('Getting all epoch predictions for %s' % (test))
totalEpochs = len(checkpointFiles)
for epoch in range(1,totalEpochs+1):
t = time.time()
K.clear_session()
print(' epoch %03d of %03d' % (epoch, totalEpochs))
getPredictions(config, evalType, epochNum=epoch, batchSize=1, saveData=True);
elapsed = time.time() - t
print(' time: %s' % (elapsed))
# evalType = 'val'
evalType = 'test'
# tests = ['CNN_test_17', 'CNN_test_12']
# tests = ['trans_test_4', 'trans_test_2']
# tests = ['scale_test_4', 'scale_test_2']
tests = [
'CNN_test_17', 'CNN_test_12', 'trans_test_4', 'trans_test_2',
'scale_test_4', 'scale_test_2'
]
for test in tests:
# test = tests[0]
configFile = os.path.join('exp_configs', test + '.yaml')
config = ThesisConfig(configFile)
# LOAD PARAMETERS
numOutputs = config.modelParms['numOutputs']
useIMUData = config.constraintParms['useIMU']
targetImageSize = config.thesisKittiParms['downsampledImageShape']
numChannels = config.modelParms['numImageChannels']
defaultLossFunc = config.modelParms['defaultLossFunction']
lossRotScale = config.modelParms['lossRotScale']
# Read in sequence 10 input
seq = 10
cam = 0
# FILE DICTIONARIES
normImageFilesDict = config.kittiNormalized['normImages']
def loadPredictions(tests, eType):
errors_list = []
preds_list = []
truth_list = []
filetruthrots_list = []
for i, test in enumerate(tests):
# if True:
# test = tests[0]
configFile = 'exp_configs/%s.yaml' % test
config = ThesisConfig(configFile)
# Parameters
name = config.expName
numOutputs = config.modelParms['numOutputs']
# Get Files
evalFilesDict = config.resultPaths['evaluations']
figFilesDict = config.resultPaths['figures']
truthFilesDict = config.kittiPrepared['truth']
figFolder = config.getFolderRef(
config.getOutputFiles(figFilesDict, True))
# Get Predictions Save File
evalFolder = ''
for pathSection in evalFilesDict['dir']:
evalFolder = os.path.join(evalFolder, pathSection)
predictionsFile = os.path.join(evalFolder, eType + '_predictions.hdf5')
if os.path.exists(predictionsFile):
# Load Predictions
y_pred_real, y_true_real, evalType, min_val_epoch, min_val_loss = loadPredFile(
predictionsFile)
print('Min Validation Loss: %s, Epoch %s' %
(min_val_loss, min_val_epoch))
# get test idxs
# numTested = y_true_real.shape[0]
splitFilesDict = config.kittiPrepared['split']
splitFile = config.getInputFiles(splitFilesDict)
with h5py.File(splitFile, 'r') as f:
if evalType == 'test':
turnIdxs = np.array(f['testTurnIdxs'])
nonTurnIdxs = np.array(f['testNonTurnIdxs'])
elif evalType == 'val':
turnIdxs = np.array(f['valTurnIdxs'])
nonTurnIdxs = np.array(f['valNonTurnIdxs'])
idxs = np.sort(np.concatenate(
(turnIdxs, nonTurnIdxs))) #[:numTested]
truthData = np.empty((0, 3))
for seq in config.usedSeqs:
truthFile = config.getInputFiles(truthFilesDict, seq)
with h5py.File(truthFile, 'r') as f:
rot_xyz = np.array(f['rot_xyz'])
truthData = np.append(truthData, rot_xyz, axis=0)
file_truth_rots = truthData[idxs, :]
# Calculate average loss in each direction
errors = y_true_real - y_pred_real
errors_list.append(errors)
preds_list.append(y_pred_real)
truth_list.append(y_true_real)
filetruthrots_list.append(file_truth_rots)
else:
print('predictions file %s does not exist' % predictionsFile)
return (errors_list, preds_list, truth_list, filetruthrots_list)
import cv2 as cv
import numpy as np
import os
import sys
import h5py
from src.helpers.cfg import ThesisConfig
# Load Configuration
configFile = None
if len(sys.argv)>1:
configFile = sys.argv[1]
config = ThesisConfig(configFile)
# Parameters
kittiSeqs = config.kittiSeqs
usedCams = config.usedCams
normRange = config.thesisKittiParms['normPixelRange']
destImageShape = config.thesisKittiParms['downsampledImageShape']
recalcNormImages = config.expKittiParms['prepared']['runPrep']['recalcNormImages']
showProgress = config.expKittiParms['prepared']['showProg']
# Files
standardImageFilesDict = config.kittiPrepared['standardImages']
normImageFilesDict = config.kittiNormalized['normImages']
print()
from keras.models import load_model, model_from_yaml
from keras.utils import plot_model
from src.arch.VO_Models import buildModel
from src.helpers.cfg import ThesisConfig
from src.helpers.helper_functions import getOptimizer
from src.helpers.training_helpers import getCallbacksList, getGenerator, getTrainAndValGenerators
from src.helpers.custom_loss import scaledMSE_RT
import tensorflow as tf
from keras import backend as K
import keras.losses
# Load Configuration
configFile = 'exp_configs/scale_test_4.yaml'
config = ThesisConfig(configFile)
checkpoint_filename = config.checkpointParms['filename']
checkpointFilesDict = config.trainPaths['checkpoint']
checkpointFiles = config.getOutputFiles(checkpointFilesDict, True)
saveCheckpointPath = config.getFolderRef(checkpointFiles)
checkpoint_filepath = os.path.join(saveCheckpointPath, checkpoint_filename)
numOutputs = config.modelParms['numOutputs']
lossRotScale = config.modelParms['lossRotScale']
defaultLossFunc = config.modelParms['defaultLossFunction']
if numOutputs > 3:
lossFunc = scaledMSE_RT(lossRotScale)
keras.losses.lossFunction = lossFunc
else:
lossFunc = defaultLossFunc
# # calculate errors
# errors = y_true_real - y_pred_real
# rmseErrors = np.sqrt(np.mean(errors**2,axis=0)) # dx,dy,dz,X,Y,Z
# meanErrors = np.mean(errors,axis=0)
# stdErrors = np.std(errors,axis=0)
# rot_format = 'dx: %8f, dy: %8f, dz: %8f'
# rotErrRMSE_deg, transErrRMSE_meters = rmseErrors[:3] * 180 / np.pi, rmseErrors[3:]
# rotErrMean_deg, transErrMean_meters = meanErrors[:3] * 180 / np.pi, meanErrors[3:]
# rotErrStd_deg, transErrStd_meters = stdErrors[:3] * 180 / np.pi, stdErrors[3:]
# print('Rotation Error RMSE (deg): %s' % (rot_format % tuple(rotErrRMSE_deg)))
# print('Rotation Error Mean (deg): %s' % (rot_format % tuple(rotErrMean_deg)))
# print('Rotation Error Std (deg): %s' % (rot_format % tuple(rotErrStd_deg)))
for configFile in configFiles:
config = ThesisConfig(configFile)
# Parameters
name = config.expName
numOutputs = config.modelParms['numOutputs']
# Get Files
evalFilesDict = config.resultPaths['evaluations']
figFilesDict = config.resultPaths['figures']
figFolder = config.getFolderRef(config.getOutputFiles(figFilesDict, True))
import numpy as np
import glob
import h5py
import os
import sys
from src.helpers.cfg import ThesisConfig
from src.helpers.kitti_helpers import IndexConverter
# TODO: Handle multiple cameras
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
config = ThesisConfig(configFile)
# Parameters
kittiSeqs = config.kittiSeqs
usedCams = config.usedCams
usedSeqs = config.usedSeqs
splitFracs = config.splitFracs
turnThresh_rad = config.thesisKittiParms['turnThreshold'] * np.pi / 180
# Files
truthFilesDict = config.kittiPrepared['truth']
splitFilesDict = config.kittiPrepared['split']
print()
print('v Splitting Dataset')
splitFile = config.getOutputFiles(splitFilesDict)
#
# with h5py.File(lrFile, 'w') as f:
# f.create_dataset('learning_rate', data=np.array(learning_rates))
# f.create_dataset('epochs', data=np.array(epochs))
#
# print('saved data to %s' % lrFile)
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '1'
logging.getLogger('tensorflow').setLevel(logging.FATAL)
# test = tests[0]
# if True:
#
for test in tests:
configFile = os.path.join('exp_configs', test + '.yaml')
config = ThesisConfig(configFile)
numOutputs = config.modelParms['numOutputs']
lossRotScale = config.modelParms['lossRotScale']
defaultLossFunc = config.modelParms['defaultLossFunction']
checkpointFilesDict = config.trainPaths['checkpoint']
checkpointFiles = config.getInputFiles(checkpointFilesDict)
if isinstance(checkpointFiles, str):
checkpointFiles = np.array([checkpointFiles])
checkpointFolder = config.getFolderRef(checkpointFiles)
# Loss Function
if numOutputs > 3:
lossFunc = scaledMSE_RT(lossRotScale)
keras.losses.lossFunction = lossFunc
# configFileList = ['exp_configs/trans_test_1.yaml',
# 'exp_configs/trans_test_2.yaml',
# 'exp_configs/trans_test_3.yaml']
#
# configFileList = ['exp_configs/CNN_test_11.yaml',
# 'exp_configs/CNN_test_12.yaml',
# 'exp_configs/CNN_test_13.yaml',
# 'exp_configs/CNN_test_14.yaml',
# 'exp_configs/CNN_test_15.yaml',
# 'exp_configs/CNN_test_16.yaml',
# 'exp_configs/CNN_test_17.yaml']
configFileList = ['exp_configs/scale_test_3.yaml']
for configFile in configFileList:
config = ThesisConfig(configFile)
history_filename = config.trainingParms['histFilename']
historyFilesDict = config.trainPaths['history']
historyFiles = config.getInputFiles(historyFilesDict)
saveHistoryPath = config.getFolderRef(historyFiles)
history_filepath = os.path.join(saveHistoryPath, history_filename)
saveFigFilesDict = config.resultPaths['figures']
figureFile = config.getOutputFiles(saveFigFilesDict, True)
saveFigFolder = config.getFolderRef(figureFile)
saveFigFilename = config.experiment['experiment'][
'name'] + config.resultPaths['figures']['type']
saveFigFile = os.path.join(saveFigFolder, saveFigFilename)
if os.path.exists(history_filepath):
print()
if splitFile is None:
print('Invalid Split File')
return None
with h5py.File(splitFile, 'r') as f:
trainNonturn = np.array(f['trainNonTurnIdxs'])
trainTurn = np.array(f['trainTurnIdxs'])
trainIdxs = np.concatenate((trainNonturn, trainTurn))
return trainIdxs
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
# configFile = 'exp_configs/CNN_test_12.yaml'
config = ThesisConfig(configFile)
# Parameters
usedCams = config.usedCams
usedSeqs = config.usedSeqs
recalcDataNorm = config.expKittiParms['prepared']['runPrep']['recalcNormParms']
# Files
splitFilesDict = config.kittiPrepared['split']
truthFilesDict = config.kittiPrepared['truth']
imuFilesDict = config.kittiPrepared['imu']
epiFilesDict = config.kittiPrepared['epipolar']
normParmsFilesDict = config.kittiNormalized['normParms']
print()
print('v Calculating Normalization Parameters')
# Determine model to use for evaluation
test = 'scale_test_2'
evalType = 'val'
savePlots = False
blocking = True
# compareData='predictions'
compareData = 'epipolar'
# GET DATA
y_pred_real = None
y_true_real = None
if compareData == 'predictions':
configFile = os.path.join('exp_configs', test + '.yaml')
config = ThesisConfig(configFile)
# Parameters
numOutputs = config.modelParms['numOutputs']
# GET TRUTH AND PREDICTIONS
evalFilesDict = config.resultPaths['evaluations']
evalFolder = getFolder(evalFilesDict['dir'])
predictionsFile = os.path.join(evalFolder, evalType + '_predictions.hdf5')
if os.path.exists(predictionsFile):
# Load Predictions
with h5py.File(predictionsFile, 'r') as f:
y_pred_real = np.array(f['predictions'])
y_true_real = np.array(f['truth'])
evalType = np.array(f['evalType'])
min_val_epoch = np.array(f['epoch'])
np.ones_like(gridX).ravel()])
# get map for sequence
mapPts = np.dot(ccm, lin_homg_norm) # convert to pixel coordinates
mapX, mapY = mapPts[:-1] / mapPts[-1] # ensure homogeneity
mapX = mapX.reshape(height, width).astype(np.float32)
mapY = mapY.reshape(height, width).astype(np.float32)
return (mapX, mapY)
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
config = ThesisConfig(configFile)
# Parameters
kittiSeqs = config.kittiSeqs
usedCams = config.usedCams
destImageShape = config.thesisKittiParms['standardImageShape']
recalcStandardImages = config.expKittiParms['prepared']['runPrep'][
'recalcStandardImages']
showFigures = config.expKittiParms['prepared']['showPlots']
showProgress = config.expKittiParms['prepared']['showProg']
# Files
origImageFilesDict = config.kittiOriginal['cams']
calFilesDict = config.kittiOriginal['cal']
standardImageFilesDict = config.kittiPrepared['standardImages']
import numpy as np
import h5py
import os
import sys
from src.helpers.cfg import ThesisConfig
from src.helpers.kitti_helpers import getPoses
from src.helpers.coframes import cvtToRel, cvtToAbs, getRT_sd_ss2d, \
getXYZ_ss2d, cvtToRpy_sd, cart2sph, sph2cart
# Load Configuration
configFile = None
if len(sys.argv) > 1:
configFile = sys.argv[1]
config = ThesisConfig(configFile)
# Parameters
kittiSeqs = config.kittiSeqs
usedCams = config.usedCams
recalcTruthData = config.expKittiParms['prepared']['runPrep']['recalcTruth']
# Files
poseFilesDict = config.kittiOriginal['truth']
truthFilesDict = config.kittiPrepared['truth']
print()
print('v Creating Truth Data')
for cam in usedCams:
for seqStr in kittiSeqs:
seq = int(seqStr)
print('%sReading Camera %s, Sequence %02d' % (' ' * 2, cam, seq))
import subprocess
from src.helpers.cfg import ThesisConfig
import sys
# Parameters
configFileList = None
if len(sys.argv)>1:
configFileList = sys.argv[1:]
for configFile in configFileList:
config = ThesisConfig(configFile)
nextEpoch = config.trainingParms['nextEpoch']
runPrepDict = config.expKittiParms['prepared']['runPrep']
runPrep = True in list(runPrepDict.values())
scripts = []
# if runPrep and nextEpoch<2:
# scripts.append('src/prep/RunPrep')
scripts.append('src/train_cdn/TrainThesis_cdn')
for script in scripts:
subprocess.call(f'python ./{script}.py {configFile}', shell=True)
trans_preds = []
preds = []
tests = ['CNN_test_12', 'CNN_test_17']
# tests = ['trans_test_2', 'trans_test_4']
# tests = ['scale_test_2', 'scale_test_4']
# tests = ['CNN_test_12', 'CNN_test_17','trans_test_2', 'trans_test_4','scale_test_2', 'scale_test_4']
evalType = 'test'
seq = 10
labels = ['INS-Aided', 'Only Images']
# test = tests[0]
# if True:
for test in tests:
configFile = os.path.join('exp_configs', test + '.yaml')
config = ThesisConfig(configFile)
# name = config.expName
numOutputs = config.modelParms['numOutputs']
evalFilesDict = config.resultPaths['evaluations']
truthFilesDict = config.kittiPrepared['truth']
figFilesDict = config.resultPaths['figures']
figFolder = config.getFolderRef(config.getOutputFiles(figFilesDict, True))
# Get Predictions Save File
evalFolder = ''
for pathSection in evalFilesDict['dir']:
evalFolder = os.path.join(evalFolder, pathSection)
predictionsFile = os.path.join(evalFolder,
'seq10_' + evalType + '_predictions.hdf5')