def main():
description = 'This script is for converting Numpy Mat txt file to OpenCV mat YAML file.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('input_file', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Input file path of Mat text data saved by Python Numpy.')
parser.add_argument('output_file', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Ouput file path of YAML Mat data for C++ OpenCV.')
parser.add_argument('output_name', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Mat name stored in output YAML file.')
args = parser.parse_args()
input_file = args.input_file
output_file = args.output_file
output_name = args.output_name
FileUtils.convertNumpyMatTxt2OpenCvMatYml(input_file, output_file, output_name)
def main():
description = "This script is for converting Numpy Mat txt file to OpenCV mat YAML file."
parser = argparse.ArgumentParser(description=description)
parser.add_argument(
"input_file",
action="store",
nargs=None,
const=None,
default=None,
type=str,
choices=None,
metavar=None,
help="Input file path of Mat text data saved by Python Numpy.",
)
parser.add_argument(
"output_file",
action="store",
nargs=None,
const=None,
default=None,
type=str,
choices=None,
metavar=None,
help="Ouput file path of YAML Mat data for C++ OpenCV.",
)
parser.add_argument(
"output_name",
action="store",
nargs=None,
const=None,
default=None,
type=str,
choices=None,
metavar=None,
help="Mat name stored in output YAML file.",
)
args = parser.parse_args()
input_file = args.input_file
output_file = args.output_file
output_name = args.output_name
FileUtils.convertNumpyMatTxt2OpenCvMatYml(input_file, output_file, output_name)
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 merging multiple SfM output models to one SfM model.' + \
'Please prepare multiple OpenMVG projects which have output SfM models, and matrix to convert to global coordinate.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('input_csv', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Input CSV file which lists OpenMVG projects which will be merged.')
parser.add_argument('output_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Output directory path where merged model will be saved.')
args = parser.parse_args()
input_csv = args.input_csv
output_dir = args.output_dir
# load reconstruct parameters
reconstructParam = ReconstructParam.ReconstructParam
# read projects list
projectList = []
with open(input_csv, "r") as f:
reader = csv.reader(f)
for row in reader:
project = {}
project["dir"] = row[0]
project["sfm_data"] = row[1]
project["A"] = row[2]
projectList.append(project)
# copy source files to output directory
for project in projectList:
copyOriginalFiles(project["dir"], output_dir)
# prepare output directory
if not os.path.isdir(os.path.join(output_dir, "Ref")):
FileUtils.makedir(os.path.join(output_dir, "Ref"))
if not os.path.isdir(os.path.join(output_dir, "Ref", "loc")):
FileUtils.makedir(os.path.join(output_dir, "Ref", "loc"))
if not os.path.isdir(os.path.join(output_dir, "Output", "SfM")):
FileUtils.makedir(os.path.join(output_dir, "Output", "SfM"))
if not os.path.isdir(
os.path.join(output_dir, "Output", "SfM", "reconstruction")):
FileUtils.makedir(
os.path.join(output_dir, "Output", "SfM", "reconstruction"))
if not os.path.isdir(
os.path.join(output_dir, "Output", "SfM", "reconstruction",
"global")):
FileUtils.makedir(
os.path.join(output_dir, "Output", "SfM", "reconstruction",
"global"))
sfmDataList = []
sfmViewBeaconDataList = []
sfmBeaconMap = None
for project in projectList:
if not os.path.exists(project["sfm_data"]):
print "cannot find sfm data : " + project["sfm_data"]
sys.exit()
with open(project["sfm_data"]) as jsonFile:
sfmDataList.append(json.load(jsonFile))
sfmBeaconFile = os.path.join(os.path.dirname(project["sfm_data"]),
"beacon.txt")
if os.path.exists(sfmBeaconFile):
print "find beacon.txt for sfm data : " + project["sfm_data"]
imgBeaconList, beaconMap = iBeaconUtils.readBeaconData(
sfmBeaconFile)
sfmViewBeaconDataList.append(imgBeaconList)
if sfmBeaconMap is None:
sfmBeaconMap = beaconMap
else:
if sfmBeaconMap != beaconMap:
print "invalid find beacon.txt for sfm data : " + project[
"sfm_data"]
print "beacon.txt should be same for all merged sfm_data"
sys.exit()
else:
print "valid beacon.txt for sfm data : " + project[
"sfm_data"]
AList = []
for project in projectList:
AList.append(np.loadtxt(project["A"]))
print "load mat : " + project["A"]
print(np.loadtxt(project["A"]))
print "Load 3D points"
pointIdList = []
pointList = []
for sfmData in sfmDataList:
pointId, point = mergeSfM.getAll3DPointloc(sfmData)
pointn = np.asarray(point, dtype=np.float).T
pointIdList.append(pointId)
pointList.append(pointn)
# merge models
mergeSfmData = None
mergePointId = None
mergePointn = None
mergeSfmViewBeaconData = None
for idx in range(0, len(sfmDataList)):
if idx == 0:
mergeSfmData = sfmDataList[0]
mergeSfM.transform_sfm_data(mergeSfmData, AList[0])
if len(sfmViewBeaconDataList) > 0:
mergeSfmViewBeaconData = sfmViewBeaconDataList[0]
else:
ransacThres = mergeSfM.findMedianStructurePointsThres(
mergeSfmData, reconstructParam.ransacStructureThresMul)
print "thres to merge 3D points : " + str(ransacThres)
inlierMap = findInliersByKnownTransform(mergePointId,
pointIdList[idx],
mergePointn,
pointList[idx], AList[idx],
ransacThres)
print "number of points in base model : " + str(len(
mergePointn[0]))
print "number of points in model " + str(idx) + " : " + str(
len(pointList[idx]))
print "number of inliers : " + str(len(inlierMap))
if len(sfmViewBeaconDataList) > 0:
mergeSfM.merge_sfm_data(mergeSfmData, sfmDataList[idx],
AList[idx],
{x[0]: x[1]
for x in inlierMap},
mergeSfmViewBeaconData,
sfmViewBeaconDataList[idx])
else:
mergeSfM.merge_sfm_data(mergeSfmData, sfmDataList[idx],
AList[idx],
{x[0]: x[1]
for x in inlierMap})
mergePointId, mergePoint = mergeSfM.getAll3DPointloc(mergeSfmData)
mergePointn = np.asarray(mergePoint, dtype=np.float).T
# go back to coordinate of the first model
_invA = np.linalg.inv(AList[0][0:3, 0:3])
invA = np.c_[_invA, -np.dot(_invA, AList[0][:, 3])]
mergeSfM.transform_sfm_data(mergeSfmData, invA)
mergeSfmData["root_path"] = os.path.join(output_dir, "Input", "inputImg")
resultSfMDataFile = os.path.join(output_dir, "Output", "SfM",
"reconstruction", "global",
"sfm_data.json")
with open(os.path.join(resultSfMDataFile), "w") as jsonfile:
json.dump(mergeSfmData, jsonfile)
if mergeSfmViewBeaconData is not None:
mergeSfmViewBeaconDataMapList = []
for key in mergeSfmViewBeaconData:
mergeSfmViewBeaconDataMap = {}
mergeSfmViewBeaconDataMap[key] = mergeSfmViewBeaconData[key]
mergeSfmViewBeaconDataMapList.append(mergeSfmViewBeaconDataMap)
iBeaconUtils.exportBeaconData(
len(mergeSfmData["views"]), sfmBeaconMap,
mergeSfmViewBeaconDataMapList,
os.path.join(os.path.dirname(resultSfMDataFile), "beacon.txt"))
'''
os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + resultSfMDataFile + " " + resultSfMDataFile)
'''
Amat = AList[0]
with open(os.path.join(output_dir, "Ref", "Amat.txt"), "w") as AmatFile:
np.savetxt(AmatFile, Amat)
FileUtils.convertNumpyMatTxt2OpenCvMatYml(
os.path.join(output_dir, "Ref", "Amat.txt"),
os.path.join(output_dir, "Ref", "Amat.yml"), "A")
# To create same directory structure before merging, create sfm_data.json without structure information in matches directory
with open(resultSfMDataFile) as fpr:
sfmData = json.load(fpr)
sfmData["extrinsics"] = []
sfmData["control_points"] = []
sfmData["structure"] = []
with open(
os.path.join(output_dir, "Output", "matches", "sfm_data.json"),
"w") as fpw:
json.dump(sfmData, fpw)
print "Execute : " + reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + os.path.join(output_dir,"Output") + " " + \
os.path.join(output_dir,"Output", "matches", "BOWfile.yml") + " -p=" + os.path.join(output_dir,"Output", "matches", "PCAfile.yml")
os.system(reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + os.path.join(output_dir,"Output") + " " + \
os.path.join(output_dir,"Output", "matches", "BOWfile.yml") + " -p=" + os.path.join(output_dir,"Output", "matches", "PCAfile.yml"))
os.system("openMVG_main_ComputeSfM_DataColor -i " + resultSfMDataFile + \
" -o " + os.path.join(output_dir,"Output","SfM","reconstruction","global","colorized.ply"))
def main():
description = 'This script is for merging multiple SfM output models to one SfM model.' + \
'Please prepare multiple OpenMVG projects which have output SfM models, and matrix to convert to global coordinate.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('input_csv', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Input CSV file which lists OpenMVG projects which will be merged.')
parser.add_argument('output_dir', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Output directory path where merged model will be saved.')
args = parser.parse_args()
input_csv = args.input_csv
output_dir = args.output_dir
# load reconstruct parameters
reconstructParam = ReconstructParam.ReconstructParam
# read projects list
projectList = []
with open(input_csv, "r") as f:
reader = csv.reader(f)
for row in reader:
project = {}
project["dir"] = row[0]
project["sfm_data"] = row[1]
project["A"] = row[2]
projectList.append(project)
# copy source files to output directory
for project in projectList:
copyOriginalFiles(project["dir"], output_dir)
# prepare output directory
if not os.path.isdir(os.path.join(output_dir,"Ref")):
FileUtils.makedir(os.path.join(output_dir,"Ref"))
if not os.path.isdir(os.path.join(output_dir,"Ref","loc")):
FileUtils.makedir(os.path.join(output_dir,"Ref","loc"))
if not os.path.isdir(os.path.join(output_dir,"Output","SfM")):
FileUtils.makedir(os.path.join(output_dir,"Output","SfM"))
if not os.path.isdir(os.path.join(output_dir,"Output","SfM","reconstruction")):
FileUtils.makedir(os.path.join(output_dir,"Output","SfM","reconstruction"))
if not os.path.isdir(os.path.join(output_dir,"Output","SfM","reconstruction","global")):
FileUtils.makedir(os.path.join(output_dir,"Output","SfM","reconstruction","global"))
sfmDataList = []
sfmViewBeaconDataList = []
sfmBeaconMap = None
for project in projectList:
if not os.path.exists(project["sfm_data"]):
print "cannot find sfm data : " + project["sfm_data"]
sys.exit()
with open(project["sfm_data"]) as jsonFile:
sfmDataList.append(json.load(jsonFile))
sfmBeaconFile = os.path.join(os.path.dirname(project["sfm_data"]), "beacon.txt")
if os.path.exists(sfmBeaconFile):
print "find beacon.txt for sfm data : " + project["sfm_data"]
imgBeaconList, beaconMap = iBeaconUtils.readBeaconData(sfmBeaconFile)
sfmViewBeaconDataList.append(imgBeaconList)
if sfmBeaconMap is None:
sfmBeaconMap = beaconMap
else:
if sfmBeaconMap!=beaconMap:
print "invalid find beacon.txt for sfm data : " + project["sfm_data"]
print "beacon.txt should be same for all merged sfm_data"
sys.exit()
else:
print "valid beacon.txt for sfm data : " + project["sfm_data"]
AList = []
for project in projectList:
AList.append(np.loadtxt(project["A"]))
print "load mat : " + project["A"]
print (np.loadtxt(project["A"]))
print "Load 3D points"
pointIdList = []
pointList = []
for sfmData in sfmDataList:
pointId, point = mergeSfM.getAll3DPointloc(sfmData)
pointn = np.asarray(point, dtype=np.float).T
pointIdList.append(pointId)
pointList.append(pointn)
# merge models
mergeSfmData = None
mergePointId = None
mergePointn = None
mergeSfmViewBeaconData = None
for idx in range(0, len(sfmDataList)):
if idx==0:
mergeSfmData = sfmDataList[0]
mergeSfM.transform_sfm_data(mergeSfmData, AList[0])
if len(sfmViewBeaconDataList)>0:
mergeSfmViewBeaconData = sfmViewBeaconDataList[0]
else:
mergePointThres = mergeSfM.findMedianStructurePointsThres(mergeSfmData, reconstructParam.mergePointThresMul)
print "thres to merge 3D points : " + str(mergePointThres)
inlierMap = findInliersByKnownTransform(mergePointId, pointIdList[idx], mergePointn, pointList[idx], AList[idx], mergePointThres)
print "number of points in base model : " + str(len(mergePointn[0]))
print "number of points in model " + str(idx) + " : " + str(len(pointList[idx]))
print "number of inliers : " + str(len(inlierMap))
if len(sfmViewBeaconDataList)>0:
mergeSfM.merge_sfm_data(mergeSfmData, sfmDataList[idx], AList[idx], {x[0]: x[1] for x in inlierMap}, mergeSfmViewBeaconData, sfmViewBeaconDataList[idx])
else:
mergeSfM.merge_sfm_data(mergeSfmData, sfmDataList[idx], AList[idx], {x[0]: x[1] for x in inlierMap})
mergePointId, mergePoint = mergeSfM.getAll3DPointloc(mergeSfmData)
mergePointn = np.asarray(mergePoint, dtype=np.float).T
# go back to coordinate of the first model
_invA = np.linalg.inv(AList[0][0:3,0:3])
invA = np.c_[_invA, -np.dot(_invA,AList[0][:,3])]
mergeSfM.transform_sfm_data(mergeSfmData, invA)
mergeSfmData["root_path"] = os.path.join(output_dir,"Input","inputImg")
resultSfMDataFile = os.path.join(output_dir,"Output","SfM","reconstruction","global","sfm_data.json")
with open(os.path.join(resultSfMDataFile),"w") as jsonfile:
json.dump(mergeSfmData, jsonfile)
if mergeSfmViewBeaconData is not None:
mergeSfmViewBeaconDataMapList = []
for key in mergeSfmViewBeaconData:
mergeSfmViewBeaconDataMap = {}
mergeSfmViewBeaconDataMap[key] = mergeSfmViewBeaconData[key]
mergeSfmViewBeaconDataMapList.append(mergeSfmViewBeaconDataMap)
iBeaconUtils.exportBeaconData(len(mergeSfmData["views"]), sfmBeaconMap, mergeSfmViewBeaconDataMapList,
os.path.join(os.path.dirname(resultSfMDataFile), "beacon.txt"))
'''
os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + resultSfMDataFile + " " + resultSfMDataFile)
'''
os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + resultSfMDataFile + " " + resultSfMDataFile + \
" -c=" + "rst,rsti" + " -r=" + "1")
Amat = AList[0]
with open(os.path.join(output_dir,"Ref","Amat.txt"),"w") as AmatFile:
np.savetxt(AmatFile,Amat)
FileUtils.convertNumpyMatTxt2OpenCvMatYml(os.path.join(output_dir,"Ref","Amat.txt"), os.path.join(output_dir,"Ref","Amat.yml"), "A")
# To create same directory structure before merging, create sfm_data.json without structure information in matches directory
with open(resultSfMDataFile) as fpr:
sfmData = json.load(fpr)
sfmData["extrinsics"] = []
sfmData["control_points"] = []
sfmData["structure"] = []
with open(os.path.join(output_dir,"Output","matches","sfm_data.json"),"w") as fpw:
json.dump(sfmData, fpw)
print "Execute : " + reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + os.path.join(output_dir,"Output") + " " + \
os.path.join(output_dir,"Output", "matches", "BOWfile.yml") + " -p=" + os.path.join(output_dir,"Output", "matches", "PCAfile.yml")
os.system(reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + os.path.join(output_dir,"Output") + " " + \
os.path.join(output_dir,"Output", "matches", "BOWfile.yml") + " -p=" + os.path.join(output_dir,"Output", "matches", "PCAfile.yml"))
os.system("openMVG_main_ComputeSfM_DataColor -i " + resultSfMDataFile + \
" -o " + os.path.join(output_dir,"Output","SfM","reconstruction","global","colorized.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)
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.ransacAffineTransform(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:
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(Amat[:, 0:3],jsonLoc["R"]).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")
