def main():
description = 'This script is for calcularing the matrix for converting 3D model to world coordinate and evaluating localization accuracy.' + \
'Before running this script, please prepare the text file which has image names and 3D coordinate where photos are taken in Ref and Test folder.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('project_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG project is located.')
parser.add_argument('matches_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG created matches files.')
parser.add_argument('sfm_data_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG sfm_data.json is located.')
parser.add_argument('--bow', action='store_true', default=False, \
help='Use BOW to accelerate localization if this flag is set (default: False)')
parser.add_argument('--beacon', action='store_true', default=False, \
help='Use iBeacon to accelerate localization if this flag is set (default: False)')
args = parser.parse_args()
project_dir = args.project_dir
matches_dir = args.matches_dir
sfm_data_dir = args.sfm_data_dir
USE_BOW = args.bow
USE_BEACON = args.beacon
BOW_FILE = os.path.join(matches_dir, "BOWfile.yml")
PCA_FILE = os.path.join(matches_dir, "PCAfile.yml")
SFM_BEACON_FILE = sfm_data_dir + "/beacon.txt"
REF_FOLDER = project_dir + "/Ref"
TEST_FOLDER = project_dir + "/Test"
if USE_BOW and not os.path.isfile(BOW_FILE):
print "Use BOW flag is set, but cannot find BOW model file"
sys.exit()
if USE_BEACON and not os.path.isfile(SFM_BEACON_FILE):
print "Use iBeacon flag is set, but cannot find beacon signal file for SfM data"
sys.exit()
if not os.path.isfile(os.path.join(REF_FOLDER, "Amat.txt")):
# 1. find transformation between reconstructed coordinate and world coordinate
# 1.1 localize reference images
REF_FOLDER_LOC = os.path.join(REF_FOLDER, "loc")
if os.path.isdir(REF_FOLDER_LOC):
shutil.rmtree(REF_FOLDER_LOC)
os.mkdir(REF_FOLDER_LOC)
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(REF_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach))
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(REF_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound))
# extract centers from all json file and write to a file
fileLoc = open(os.path.join(REF_FOLDER_LOC, "center.txt"), "w")
countLocFrame = 0
for filename in sorted(os.listdir(REF_FOLDER_LOC)):
if filename[-4:] != "json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(REF_FOLDER_LOC, filename)) as locJson:
#print os.path.join(sfm_locOut,filename)
locJsonDict = json.load(locJson)
loc = locJsonDict["t"]
fileLoc.write(
str(loc[0]) + " " + str(loc[1]) + " " + str(loc[2]) +
" 255 0 0\n")
fileLoc.close()
# read reference data
mapNameLocRef = FileUtils.loadImageLocationListTxt(
os.path.join(REF_FOLDER, "refcoor.txt"))
# read localized json file and find its matching world coordinate
worldCoor = []
locCoor = []
countLoc = 0
for filename in os.listdir(REF_FOLDER_LOC):
if filename[-4:] != "json":
continue
# read json localization file
with open(os.path.join(REF_FOLDER_LOC, filename)) as jsonlocfile:
jsonLoc = json.load(jsonlocfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map, add to matrix
if imgLocName in mapNameLocRef:
locCoor.append(jsonLoc["t"])
worldCoor.append(mapNameLocRef[imgLocName])
countLoc = countLoc + 1
print "From " + str(len(mapNameLocRef)) + " reference images, " + str(
countLoc) + " images has been localized."
if countLoc < 4:
print "Cannot fix to world coordinate because of less than 4 reference points"
return
# find tranformation
Amat, inliers = mergeSfM.ransacTransform(
np.array(worldCoor).T,
np.array(locCoor).T,
reconstructParam.ransacThresTransformWorldCoordinateRefImage,
ransacRound=1000)
if len(inliers) < 4:
print "Cannot estimate transformation matrix to world coordinate"
print Amat
return
print "Transformation matrix has " + str(len(inliers)) + "inliers"
print Amat
with open(os.path.join(REF_FOLDER, "Amat.txt"), "w") as AmatFile:
np.savetxt(AmatFile, Amat)
FileUtils.convertNumpyMatTxt2OpenCvMatYml(
os.path.join(REF_FOLDER, "Amat.txt"),
os.path.join(REF_FOLDER, "Amat.yml"), "A")
else:
with open(os.path.join(REF_FOLDER, "Amat.txt"), "r") as AmatFile:
Amat = np.loadtxt(AmatFile)
# convert ply file to world coordinate
SfmDataUtils.saveGlobalSfM(
os.path.join(sfm_data_dir, "sfm_data.json"),
os.path.join(REF_FOLDER, "Amat.txt"),
os.path.join(sfm_data_dir, "sfm_data_global.json"))
os.system("openMVG_main_ComputeSfM_DataColor -i " +
os.path.join(sfm_data_dir, "sfm_data_global.json") + " -o " +
os.path.join(sfm_data_dir, "colorized_global.ply"))
PlyUtis.saveCameraPly(
os.path.join(sfm_data_dir, "sfm_data_global.json"),
os.path.join(sfm_data_dir, "colorized_global_camera.ply"))
PlyUtis.saveStructurePly(
os.path.join(sfm_data_dir, "sfm_data_global.json"),
os.path.join(sfm_data_dir, "colorized_global_structure.ply"))
# start localize test
TEST_FOLDER_LOC = os.path.join(TEST_FOLDER, "loc")
if not os.path.isfile(os.path.join(TEST_FOLDER_LOC, "center.txt")):
# localize test images
if os.path.isdir(TEST_FOLDER_LOC):
shutil.rmtree(TEST_FOLDER_LOC)
os.mkdir(TEST_FOLDER_LOC)
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach))
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound))
# extract centers from all json file and write to a file
fileLoc = open(os.path.join(TEST_FOLDER_LOC, "center.txt"), "w")
countLocFrame = 0
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:] != "json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(TEST_FOLDER_LOC, filename)) as locJson:
#print os.path.join(sfm_locOut,filename)
locJsonDict = json.load(locJson)
loc = locJsonDict["t"]
fileLoc.write(
str(loc[0]) + " " + str(loc[1]) + " " + str(loc[2]) +
" 255 0 0\n")
fileLoc.close()
# read test data
mapNameLocTest = FileUtils.loadImageLocationListTxt(
os.path.join(TEST_FOLDER, "testcoor.txt"))
# read localized json file and find its matching world coordinate
worldCoorTest = []
locCoorTest = []
countLocTest = 0
for filename in os.listdir(TEST_FOLDER_LOC):
if filename[-4:] != "json":
continue
# read json localization file
with open(os.path.join(TEST_FOLDER_LOC, filename)) as jsonlocfile:
jsonLoc = json.load(jsonlocfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map, add to matrix
if imgLocName in mapNameLocTest:
locCoorTest.append(jsonLoc["t"])
worldCoorTest.append(mapNameLocTest[imgLocName])
countLocTest = countLocTest + 1
# transform loc coordinate to world coordinate
print "From " + str(len(mapNameLocTest)) + " test images, " + str(
countLocTest) + " images has been localized."
if countLocTest == 0:
return
locCoorTest1 = np.hstack(
(locCoorTest, np.ones((len(locCoorTest), 1), dtype=np.float)))
locCoorTestWorld = np.dot(Amat, locCoorTest1.T).T
# calculate error
normDiff = np.linalg.norm(worldCoorTest - locCoorTestWorld, axis=1)
meanErr = np.mean(normDiff)
medianErr = np.median(normDiff)
print "Mean error = " + str(meanErr) + " meters."
print "Median error = " + str(medianErr) + " meters."
binEdge = [0.3 * float(x) for x in range(0, 11)]
hist = np.histogram(normDiff, bins=binEdge)[0]
print "Histogram of error: " + str(hist)
print "Cumulative ratio: " + str(
np.around(np.cumsum(hist, dtype=float) / countLocTest, 2))
print "Total loc err larger than " + str(
np.max(binEdge)) + " meters: " + str(countLocTest - np.sum(hist))
# convert all localization results to world coordinate and merge to one json file
locGlobalJsonObj = {}
locGlobalJsonObj["locGlobal"] = []
locGlobalPoints = []
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:] != "json":
continue
with open(os.path.join(TEST_FOLDER_LOC, filename)) as jsonfile:
jsonLoc = json.load(jsonfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map
if imgLocName in mapNameLocTest:
jsonLoc["t_relative"] = jsonLoc["t"]
jsonLoc["R_relative"] = jsonLoc["R"]
jsonLoc["t"] = np.dot(Amat, np.concatenate([jsonLoc["t"],
[1]])).tolist()
jsonLoc["R"] = np.dot(jsonLoc["R"], Amat[:, 0:3].T).tolist()
jsonLoc["groundtruth"] = mapNameLocTest[imgLocName]
locGlobalJsonObj["locGlobal"].append(jsonLoc)
locGlobalPoints.append(jsonLoc["t"])
with open(os.path.join(TEST_FOLDER_LOC, "loc_global.json"),
"w") as jsonfile:
json.dump(locGlobalJsonObj, jsonfile)
# save localization results to ply file
PlyUtis.addPointToPly(
os.path.join(sfm_data_dir, "colorized_global_structure.ply"),
locGlobalPoints,
os.path.join(TEST_FOLDER_LOC, "colorized_global_localize.ply"))
def main():
description = 'This script is for calcularing the matrix for converting 3D model to world coordinate and evaluating localization accuracy.' + \
'Before running this script, please prepare the text file which has image names and 3D coordinate where photos are taken in Ref and Test folder.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('project_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG project is located.')
parser.add_argument('matches_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG created matches files.')
parser.add_argument('sfm_data_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG sfm_data.json is located.')
parser.add_argument('--bow', action='store_true', default=False, \
help='Use BOW to accelerate localization if this flag is set (default: False)')
parser.add_argument('--beacon', action='store_true', default=False, \
help='Use iBeacon to accelerate localization if this flag is set (default: False)')
args = parser.parse_args()
project_dir = args.project_dir
matches_dir = args.matches_dir
sfm_data_dir = args.sfm_data_dir
USE_BOW = args.bow
USE_BEACON = args.beacon
BOW_FILE = os.path.join(matches_dir, "BOWfile.yml")
PCA_FILE = os.path.join(matches_dir, "PCAfile.yml")
SFM_BEACON_FILE = sfm_data_dir + "/beacon.txt"
REF_FOLDER = project_dir + "/Ref"
TEST_FOLDER = project_dir + "/Test"
if USE_BOW and not os.path.isfile(BOW_FILE):
print "Use BOW flag is set, but cannot find BOW model file"
sys.exit()
if USE_BEACON and not os.path.isfile(SFM_BEACON_FILE):
print "Use iBeacon flag is set, but cannot find beacon signal file for SfM data"
sys.exit()
if not os.path.isfile(os.path.join(REF_FOLDER,"Amat.txt")):
# 1. find transformation between reconstructed coordinate and world coordinate
# 1.1 localize reference images
REF_FOLDER_LOC = os.path.join(REF_FOLDER,"loc")
if os.path.isdir(REF_FOLDER_LOC):
shutil.rmtree(REF_FOLDER_LOC)
os.mkdir(REF_FOLDER_LOC)
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(REF_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach))
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(REF_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound))
# extract centers from all json file and write to a file
fileLoc = open(os.path.join(REF_FOLDER_LOC,"center.txt"),"w")
countLocFrame = 0
for filename in sorted(os.listdir(REF_FOLDER_LOC)):
if filename[-4:]!="json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(REF_FOLDER_LOC,filename)) as locJson:
#print os.path.join(sfm_locOut,filename)
locJsonDict = json.load(locJson)
loc = locJsonDict["t"]
fileLoc.write(str(loc[0]) + " " + str(loc[1]) + " " +str(loc[2]) + " 255 0 0\n" )
fileLoc.close()
# read reference data
mapNameLocRef = FileUtils.loadImageLocationListTxt(os.path.join(REF_FOLDER,"refcoor.txt"))
# read localized json file and find its matching world coordinate
worldCoor = []
locCoor = []
countLoc = 0
for filename in os.listdir(REF_FOLDER_LOC):
if filename[-4:] != "json":
continue
# read json localization file
with open(os.path.join(REF_FOLDER_LOC,filename)) as jsonlocfile:
jsonLoc = json.load(jsonlocfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map, add to matrix
if imgLocName in mapNameLocRef:
locCoor.append(jsonLoc["t"])
worldCoor.append(mapNameLocRef[imgLocName])
countLoc = countLoc + 1
print "From " + str(len(mapNameLocRef)) + " reference images, " + str(countLoc) + " images has been localized."
if countLoc < 4:
print "Cannot fix to world coordinate because of less than 4 reference points"
return
# find tranformation
Amat, inliers = mergeSfM.ransacTransform(np.array(worldCoor).T, np.array(locCoor).T,
reconstructParam.ransacThresTransformWorldCoordinateRefImage, ransacRound=1000)
if len(inliers) < 4:
print "Cannot estimate transformation matrix to world coordinate"
print Amat
return
print "Transformation matrix has " + str(len(inliers)) + "inliers"
print Amat
with open(os.path.join(REF_FOLDER,"Amat.txt"),"w") as AmatFile:
np.savetxt(AmatFile,Amat)
FileUtils.convertNumpyMatTxt2OpenCvMatYml(os.path.join(REF_FOLDER,"Amat.txt"), os.path.join(REF_FOLDER,"Amat.yml"), "A")
else:
with open(os.path.join(REF_FOLDER,"Amat.txt"),"r") as AmatFile:
Amat = np.loadtxt(AmatFile)
# convert ply file to world coordinate
SfmDataUtils.saveGlobalSfM(os.path.join(sfm_data_dir,"sfm_data.json"), os.path.join(REF_FOLDER,"Amat.txt"), os.path.join(sfm_data_dir,"sfm_data_global.json"))
os.system("openMVG_main_ComputeSfM_DataColor -i " + os.path.join(sfm_data_dir,"sfm_data_global.json") + " -o " + os.path.join(sfm_data_dir,"colorized_global.ply"))
PlyUtis.saveCameraPly(os.path.join(sfm_data_dir,"sfm_data_global.json"), os.path.join(sfm_data_dir,"colorized_global_camera.ply"))
PlyUtis.saveStructurePly(os.path.join(sfm_data_dir,"sfm_data_global.json"), os.path.join(sfm_data_dir,"colorized_global_structure.ply"))
# start localize test
TEST_FOLDER_LOC = os.path.join(TEST_FOLDER,"loc")
if not os.path.isfile(os.path.join(TEST_FOLDER_LOC,"center.txt")):
# localize test images
if os.path.isdir(TEST_FOLDER_LOC):
shutil.rmtree(TEST_FOLDER_LOC)
os.mkdir(TEST_FOLDER_LOC)
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach))
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound))
# extract centers from all json file and write to a file
fileLoc = open(os.path.join(TEST_FOLDER_LOC,"center.txt"),"w")
countLocFrame = 0
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:]!="json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(TEST_FOLDER_LOC,filename)) as locJson:
#print os.path.join(sfm_locOut,filename)
locJsonDict = json.load(locJson)
loc = locJsonDict["t"]
fileLoc.write(str(loc[0]) + " " + str(loc[1]) + " " +str(loc[2]) + " 255 0 0\n" )
fileLoc.close()
# read test data
mapNameLocTest = FileUtils.loadImageLocationListTxt(os.path.join(TEST_FOLDER,"testcoor.txt"))
# read localized json file and find its matching world coordinate
worldCoorTest = []
locCoorTest = []
countLocTest = 0
for filename in os.listdir(TEST_FOLDER_LOC):
if filename[-4:] != "json":
continue
# read json localization file
with open(os.path.join(TEST_FOLDER_LOC,filename)) as jsonlocfile:
jsonLoc = json.load(jsonlocfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map, add to matrix
if imgLocName in mapNameLocTest:
locCoorTest.append(jsonLoc["t"])
worldCoorTest.append(mapNameLocTest[imgLocName])
countLocTest = countLocTest + 1
# transform loc coordinate to world coordinate
print "From " + str(len(mapNameLocTest)) + " test images, " + str(countLocTest) + " images has been localized."
if countLocTest == 0:
return
locCoorTest1 = np.hstack((locCoorTest,np.ones((len(locCoorTest),1),dtype=np.float)))
locCoorTestWorld = np.dot(Amat,locCoorTest1.T).T
# calculate error
normDiff = np.linalg.norm(worldCoorTest - locCoorTestWorld,axis=1)
meanErr = np.mean(normDiff)
medianErr = np.median(normDiff)
print "Mean error = " + str(meanErr) + " meters."
print "Median error = " + str(medianErr) + " meters."
binEdge = [0.3*float(x) for x in range(0,11)]
hist = np.histogram(normDiff,bins=binEdge)[0]
print "Histogram of error: " + str(hist)
print "Cumulative ratio: " + str(np.around(np.cumsum(hist,dtype=float)/countLocTest,2))
print "Total loc err larger than " + str(np.max(binEdge)) + " meters: " + str(countLocTest-np.sum(hist))
# convert all localization results to world coordinate and merge to one json file
locGlobalJsonObj = {}
locGlobalJsonObj["locGlobal"] = []
locGlobalPoints = []
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:]!="json":
continue
with open(os.path.join(TEST_FOLDER_LOC,filename)) as jsonfile:
jsonLoc = json.load(jsonfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map
if imgLocName in mapNameLocTest:
jsonLoc["t_relative"] = jsonLoc["t"]
jsonLoc["R_relative"] = jsonLoc["R"]
jsonLoc["t"] = np.dot(Amat,np.concatenate([jsonLoc["t"],[1]])).tolist()
jsonLoc["R"] = np.dot(jsonLoc["R"],Amat[:, 0:3].T).tolist()
jsonLoc["groundtruth"] = mapNameLocTest[imgLocName]
locGlobalJsonObj["locGlobal"].append(jsonLoc)
locGlobalPoints.append(jsonLoc["t"])
with open(os.path.join(TEST_FOLDER_LOC,"loc_global.json"),"w") as jsonfile:
json.dump(locGlobalJsonObj, jsonfile)
# save localization results to ply file
PlyUtis.addPointToPly(os.path.join(sfm_data_dir,"colorized_global_structure.ply"), locGlobalPoints,
os.path.join(TEST_FOLDER_LOC,"colorized_global_localize.ply"))
def main():
description = 'This script is for calcularing the matrix for converting 3D model to world coordinate and testing localization.' + \
'Before running this script, please prepare the json file which has 3D coordinate for reference points in Ref folder.' + \
'You can create reference points json file by sfmCoordinateEditor'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('project_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG project is located.')
parser.add_argument('matches_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG created matches files.')
parser.add_argument('sfm_data_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG sfm_data.json is located.')
parser.add_argument('-t', '--test-project-dir', action='store', nargs='?', const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where localization test image files are located.')
parser.add_argument('-o', '--output-json-filename', action='store', nargs='?', const=None, \
default='loc_global.json', type=str, choices=None, metavar=None, \
help='Output localization result json filename.')
parser.add_argument('--bow', action='store_true', default=False, \
help='Use BOW to accelerate localization if this flag is set (default: False)')
parser.add_argument('--beacon', action='store_true', default=False, \
help='Use iBeacon to accelerate localization if this flag is set (default: False)')
parser.add_argument('--reduce-points', action='store_true', default=False, \
help='Reduce 3D points if points are close after transforming to global coordinate (default: False)')
args = parser.parse_args()
project_dir = args.project_dir
matches_dir = args.matches_dir
sfm_data_dir = args.sfm_data_dir
test_project_dir = args.test_project_dir
output_json_filename = args.output_json_filename
USE_BOW = args.bow
USE_BEACON = args.beacon
USE_REDUCE_POINTS = args.reduce_points
BOW_FILE = os.path.join(matches_dir, "BOWfile.yml")
PCA_FILE = os.path.join(matches_dir, "PCAfile.yml")
SFM_BEACON_FILE = sfm_data_dir + "/beacon.txt"
REF_FOLDER = project_dir + "/Ref"
if USE_BOW and not os.path.isfile(BOW_FILE):
print "Use BOW flag is set, but cannot find BOW model file"
sys.exit()
if USE_BEACON and not os.path.isfile(SFM_BEACON_FILE):
print "Use iBeacon flag is set, but cannot find beacon signal file for SfM data"
sys.exit()
if not os.path.isfile(os.path.join(REF_FOLDER,"Amat.txt")):
# 1. find transformation between reconstructed coordinate and world coordinate
refPoints = FileUtils.loadRefPointsJson(os.path.join(REF_FOLDER,"refpoints.json"))
# read localized json file and find its matching world coordinate
worldCoor = []
locCoor = []
with open(os.path.join(sfm_data_dir, "sfm_data.json")) as fp:
sfmData = json.load(fp)
for pointId in refPoints:
selectedPoint = [point for point in sfmData["structure"] if point["key"]==pointId][0]
print "relative coordinate : " + str(selectedPoint["value"]["X"])
print "world coordinate : " + str(refPoints[pointId])
locCoor.append(selectedPoint["value"]["X"])
worldCoor.append(refPoints[pointId])
print "Number of reference points : " + str(len(worldCoor))
if len(worldCoor) < 4:
print "Cannot fix to world coordinate because of less than 4 reference points"
return
# find tranformation
Amat, inliers = mergeSfM.ransacTransform(np.array(worldCoor).T, np.array(locCoor).T,
reconstructParam.ransacThresTransformWorldCoordinateRefPoint, ransacRound=1000)
if len(inliers) < 4:
print "Cannot estimate transformation matrix to world coordinate"
print Amat
return
print "Transformation matrix has " + str(len(inliers)) + "inliers"
print Amat
with open(os.path.join(REF_FOLDER,"Amat.txt"),"w") as AmatFile:
np.savetxt(AmatFile,Amat)
FileUtils.convertNumpyMatTxt2OpenCvMatYml(os.path.join(REF_FOLDER,"Amat.txt"), os.path.join(REF_FOLDER,"Amat.yml"), "A")
else:
with open(os.path.join(REF_FOLDER,"Amat.txt"),"r") as AmatFile:
Amat = np.loadtxt(AmatFile)
if USE_REDUCE_POINTS:
print "start reducing 3D points..."
with open(os.path.join(sfm_data_dir, "sfm_data.json")) as fp:
sfmData = json.load(fp)
with open(os.path.join(REF_FOLDER,"Amat.txt"),"r") as AmatFile:
Amat = np.loadtxt(AmatFile)
print "point size before reducing : " + str(len(sfmData['structure']))
# TODO : revisit the threshold for reducing points
#reduceClosePoints(sfmData, Amat, 0.01)
reduceClosePointsKDTree(sfmData, Amat, 0.01, 1000)
print "point size after reducing : " + str(len(sfmData['structure']))
print "start save reduced sfm data..."
os.system("cp " + os.path.join(sfm_data_dir, "sfm_data.json") + " " + os.path.join(sfm_data_dir, "sfm_data_b4rp.json"))
with open(os.path.join(sfm_data_dir, "sfm_data.json"), "w") as fp:
json.dump(sfmData, fp)
print "finish save reduced sfm data..."
print "finish reducing 3D points."
# convert ply file to world coordinate
SfmDataUtils.saveGlobalSfM(os.path.join(sfm_data_dir,"sfm_data.json"), os.path.join(REF_FOLDER,"Amat.txt"), os.path.join(sfm_data_dir,"sfm_data_global.json"))
os.system("openMVG_main_ComputeSfM_DataColor -i " + os.path.join(sfm_data_dir,"sfm_data_global.json") + " -o " + os.path.join(sfm_data_dir,"colorized_global.ply"))
PlyUtis.saveCameraPly(os.path.join(sfm_data_dir,"sfm_data_global.json"), os.path.join(sfm_data_dir,"colorized_global_camera.ply"))
PlyUtis.saveStructurePly(os.path.join(sfm_data_dir,"sfm_data_global.json"), os.path.join(sfm_data_dir,"colorized_global_structure.ply"))
# start localize test
if test_project_dir:
for testFolder in sorted(os.listdir(test_project_dir)):
TEST_DIR = os.path.join(test_project_dir,testFolder)
if not os.path.exists(os.path.join(TEST_DIR,"inputImg")):
continue
TEST_FOLDER_LOC = os.path.join(TEST_DIR,"loc")
if not os.path.isfile(os.path.join(TEST_FOLDER_LOC,"center.txt")):
# localize test images
if os.path.isdir(TEST_FOLDER_LOC):
shutil.rmtree(TEST_FOLDER_LOC)
os.mkdir(TEST_FOLDER_LOC)
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_DIR,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach))
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_DIR,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound))
# extract centers from all json file and write to a file
fileLoc = open(os.path.join(TEST_FOLDER_LOC,"center.txt"),"w")
countLocFrame = 0
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:]!="json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(TEST_FOLDER_LOC,filename)) as locJson:
#print os.path.join(sfm_locOut,filename)
locJsonDict = json.load(locJson)
loc = locJsonDict["t"]
fileLoc.write(str(loc[0]) + " " + str(loc[1]) + " " +str(loc[2]) + " 255 0 0\n" )
fileLoc.close()
# convert all localization results to world coordinate and merge to one json file
locGlobalJsonObj = {}
locGlobalJsonObj["locGlobal"] = []
locGlobalPoints = []
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:]!="json":
continue
with open(os.path.join(TEST_FOLDER_LOC,filename)) as jsonfile:
jsonLoc = json.load(jsonfile)
jsonLoc["t_relative"] = jsonLoc["t"]
jsonLoc["R_relative"] = jsonLoc["R"]
jsonLoc["t"] = np.dot(Amat,np.concatenate([jsonLoc["t"],[1]])).tolist()
jsonLoc["R"] = np.dot(jsonLoc["R"],Amat[:, 0:3].T).tolist()
locGlobalJsonObj["locGlobal"].append(jsonLoc)
locGlobalPoints.append(jsonLoc["t"])
with open(os.path.join(TEST_FOLDER_LOC, output_json_filename),"w") as jsonfile:
json.dump(locGlobalJsonObj, jsonfile)
# save localization results to ply file
PlyUtis.addPointToPly(os.path.join(sfm_data_dir,"colorized_global_structure.ply"), locGlobalPoints,
os.path.join(TEST_FOLDER_LOC,"colorized_global_localize.ply"))
def main():
description = 'This script is for calcularing the matrix for converting 3D model to world coordinate and testing localization.' + \
'Before running this script, please prepare the text file which has image names and 3D coordinate where photos are taken in Ref folder.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('project_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG project is located.')
parser.add_argument('matches_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG created matches files.')
parser.add_argument('sfm_data_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where OpenMVG sfm_data.json is located.')
parser.add_argument('-t', '--test-project-dir', action='store', nargs='?', const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where localization test image files are located.')
parser.add_argument('-o', '--output-json-filename', action='store', nargs='?', const=None, \
default='loc_global.json', type=str, choices=None, metavar=None, \
help='Output localization result json filename.')
parser.add_argument('--bow', action='store_true', default=False, \
help='Use BOW to accelerate localization if this flag is set (default: False)')
parser.add_argument('--beacon', action='store_true', default=False, \
help='Use iBeacon to accelerate localization if this flag is set (default: False)')
args = parser.parse_args()
project_dir = args.project_dir
matches_dir = args.matches_dir
sfm_data_dir = args.sfm_data_dir
test_project_dir = args.test_project_dir
output_json_filename = args.output_json_filename
USE_BOW = args.bow
USE_BEACON = args.beacon
BOW_FILE = os.path.join(matches_dir, "BOWfile.yml")
PCA_FILE = os.path.join(matches_dir, "PCAfile.yml")
SFM_BEACON_FILE = sfm_data_dir + "/beacon.txt"
REF_FOLDER = project_dir + "/Ref"
if USE_BOW and not os.path.isfile(BOW_FILE):
print "Use BOW flag is set, but cannot find BOW model file"
sys.exit()
if USE_BEACON and not os.path.isfile(SFM_BEACON_FILE):
print "Use iBeacon flag is set, but cannot find beacon signal file for SfM data"
sys.exit()
if not os.path.isfile(os.path.join(REF_FOLDER, "Amat.txt")):
# 1. find transformation between reconstructed coordinate and world coordinate
# 1.1 localize reference images
REF_FOLDER_LOC = os.path.join(REF_FOLDER, "loc")
if os.path.isdir(REF_FOLDER_LOC):
shutil.rmtree(REF_FOLDER_LOC)
os.mkdir(REF_FOLDER_LOC)
guideMatchOption = ""
if reconstructParam.bGuidedMatchingLocalize:
guideMatchOption = " -gm"
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE + \
guideMatchOption)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(REF_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
guideMatchOption)
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(REF_FOLDER,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE + \
guideMatchOption)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(REF_FOLDER,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + REF_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
guideMatchOption)
# extract centers from all json file and write to a file
fileLoc = open(os.path.join(REF_FOLDER_LOC, "center.txt"), "w")
countLocFrame = 0
for filename in sorted(os.listdir(REF_FOLDER_LOC)):
if filename[-4:] != "json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(REF_FOLDER_LOC, filename)) as locJson:
locJsonDict = json.load(locJson)
if "t" in locJsonDict:
loc = locJsonDict["t"]
fileLoc.write(
str(loc[0]) + " " + str(loc[1]) + " " + str(loc[2]) +
" 255 0 0\n")
fileLoc.close()
# read reference data
mapNameLocRef = FileUtils.loadImageLocationListTxt(
os.path.join(REF_FOLDER, "refcoor.txt"))
# read localized json file and find its matching world coordinate
worldCoor = []
locCoor = []
countLoc = 0
for filename in os.listdir(REF_FOLDER_LOC):
if filename[-4:] != "json":
continue
# read json localization file
with open(os.path.join(REF_FOLDER_LOC, filename)) as jsonlocfile:
jsonLoc = json.load(jsonlocfile)
imgLocName = os.path.basename(jsonLoc["filename"])
# if file exist in map, add to matrix
if imgLocName in mapNameLocRef:
if "t" in jsonLoc:
locCoor.append(jsonLoc["t"])
worldCoor.append(mapNameLocRef[imgLocName])
countLoc = countLoc + 1
print "From " + str(len(mapNameLocRef)) + " reference images, " + str(
countLoc) + " images has been localized."
if countLoc < 4:
print "Cannot fix to world coordinate because of less than 4 reference points"
return
# find tranformation
Amat, inliers = mergeSfM.ransacTransform(
np.array(worldCoor).T,
np.array(locCoor).T,
reconstructParam.ransacThresTransformWorldCoordinateRefImage,
ransacRound=1000)
if len(inliers) < 4:
print "Cannot estimate transformation matrix to world coordinate"
print Amat
return
print "Transformation matrix has " + str(len(inliers)) + "inliers"
print Amat
with open(os.path.join(REF_FOLDER, "Amat.txt"), "w") as AmatFile:
np.savetxt(AmatFile, Amat)
FileUtils.convertNumpyMatTxt2OpenCvMatYml(
os.path.join(REF_FOLDER, "Amat.txt"),
os.path.join(REF_FOLDER, "Amat.yml"), "A")
else:
with open(os.path.join(REF_FOLDER, "Amat.txt"), "r") as AmatFile:
Amat = np.loadtxt(AmatFile)
# convert ply file to world coordinate
SfmDataUtils.saveGlobalSfM(
os.path.join(sfm_data_dir, "sfm_data.json"),
os.path.join(REF_FOLDER, "Amat.txt"),
os.path.join(sfm_data_dir, "sfm_data_global.json"))
os.system("openMVG_main_ComputeSfM_DataColor -i " +
os.path.join(sfm_data_dir, "sfm_data_global.json") + " -o " +
os.path.join(sfm_data_dir, "colorized_global.ply"))
PlyUtis.saveCameraPly(
os.path.join(sfm_data_dir, "sfm_data_global.json"),
os.path.join(sfm_data_dir, "colorized_global_camera.ply"))
PlyUtis.saveStructurePly(
os.path.join(sfm_data_dir, "sfm_data_global.json"),
os.path.join(sfm_data_dir, "colorized_global_structure.ply"))
# start localize test
if test_project_dir:
countFrameTotal = 0
countLocFrameTotal = 0
log_txt_filename = os.path.join(test_project_dir, "log.txt")
if os.path.exists(log_txt_filename):
os.remove(log_txt_filename)
for testFolder in sorted(os.listdir(test_project_dir)):
TEST_DIR = os.path.join(test_project_dir, testFolder)
if not os.path.exists(os.path.join(TEST_DIR, "inputImg")):
continue
TEST_FOLDER_LOC = os.path.join(TEST_DIR, "loc")
if not os.path.isfile(os.path.join(TEST_FOLDER_LOC, "center.txt")):
# localize test images
if os.path.isdir(TEST_FOLDER_LOC):
shutil.rmtree(TEST_FOLDER_LOC)
os.mkdir(TEST_FOLDER_LOC)
if USE_BOW and not USE_BEACON:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -k=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
elif not USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_DIR,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach))
elif USE_BOW and USE_BEACON:
os.system(reconstructIBeaconParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound) + \
" -b=" + SFM_BEACON_FILE + \
" -e=" + os.path.join(TEST_DIR,"csv") + \
" -k=" + str(localizeIBeaconParam.locKNNnum) + \
" -c=" + str(localizeIBeaconParam.coocThres) + \
" -v=" + str(localizeIBeaconParam.locSkipSelKNN) + \
" -n=" + str(localizeIBeaconParam.normApproach) + \
" -kb=" + str(localizeBOWParam.locKNNnum) + \
" -a=" + BOW_FILE + \
" -p=" + PCA_FILE)
else:
os.system(reconstructParam.LOCALIZE_PROJECT_PATH + \
" " + os.path.join(TEST_DIR,"inputImg") + \
" " + sfm_data_dir + \
" " + matches_dir + \
" " + TEST_FOLDER_LOC + \
" -f=" + str(localizeParam.locFeatDistRatio) + \
" -r=" + str(localizeParam.locRansacRound))
# extract centers from all json file and write to a file
countLocFrame = 0
fileLoc = open(os.path.join(TEST_FOLDER_LOC, "center.txt"),
"w")
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:] != "json":
continue
countLocFrame = countLocFrame + 1
with open(os.path.join(TEST_FOLDER_LOC,
filename)) as locJson:
if "t" in locJsonDict:
locJsonDict = json.load(locJson)
loc = locJsonDict["t"]
fileLoc.write(
str(loc[0]) + " " + str(loc[1]) + " " +
str(loc[2]) + " 255 0 0\n")
fileLoc.close()
# count input images
imageTypes = ("*.jpg", "*.JPG", "*.jpeg", "*.JPEG", "*.png",
"*.PNG")
imageFiles = []
for imageType in imageTypes:
imageFiles.extend(
glob.glob(os.path.join(TEST_DIR, "inputImg",
imageType)))
countFrame = len(imageFiles)
# write log file
with open(log_txt_filename, "a") as logfile:
logfile.write("result for : " + TEST_DIR + "\n")
logfile.write("number of localized frame : " +
str(countLocFrame) + "/" + str(countFrame) +
"\n")
logfile.write("ratio of localized frame : " +
str(float(countLocFrame) / countFrame) +
"\n")
countFrameTotal += countFrame
countLocFrameTotal += countLocFrame
# convert all localization results to world coordinate and merge to one json file
locGlobalJsonObj = {}
locGlobalJsonObj["locGlobal"] = []
locGlobalPoints = []
for filename in sorted(os.listdir(TEST_FOLDER_LOC)):
if filename[-4:] != "json":
continue
with open(os.path.join(TEST_FOLDER_LOC, filename)) as jsonfile:
jsonLoc = json.load(jsonfile)
if "t" in jsonLoc:
jsonLoc["t_relative"] = jsonLoc["t"]
jsonLoc["R_relative"] = jsonLoc["R"]
jsonLoc["t"] = np.dot(
Amat, np.concatenate([jsonLoc["t"],
[1]])).tolist()
jsonLoc["R"] = np.dot(jsonLoc["R"],
Amat[:, 0:3].T).tolist()
locGlobalPoints.append(jsonLoc["t"])
locGlobalJsonObj["locGlobal"].append(jsonLoc)
with open(os.path.join(TEST_FOLDER_LOC, output_json_filename),
"w") as jsonfile:
json.dump(locGlobalJsonObj, jsonfile)
# save localization results to ply file
PlyUtis.addPointToPly(
os.path.join(sfm_data_dir, "colorized_global_structure.ply"),
locGlobalPoints,
os.path.join(TEST_FOLDER_LOC, "colorized_global_localize.ply"))
# write log file
with open(log_txt_filename, "a") as logfile:
logfile.write("total result" + "\n")
logfile.write("number of localized frame : " +
str(countLocFrameTotal) + "/" +
str(countFrameTotal) + "\n")
logfile.write("ratio of localized frame : " +
str(float(countLocFrameTotal) / countFrameTotal) +
"\n")