def main():
SfmDataUtils.saveGlobalSfM(os.path.join("../data", "sfm_data.json"),
os.path.join("../data", "Amat.txt"),
os.path.join("../data", "sfm_data_global.json"))
os.system("openMVG_main_ComputeSfM_DataColor -i " +
os.path.join("../data", "sfm_data.json") + " -o " +
os.path.join("../data", "sfm_data.ply"))
os.system("openMVG_main_ComputeSfM_DataColor -i " +
os.path.join("../data", "sfm_data_global.json") + " -o " +
os.path.join("../data", "sfm_data_global.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():
# set default parameter
reconstructParam = ReconstructParam.ReconstructParam
reconstructBOWParam = ReconstructBOWParam.ReconstructBOWParam
# parse parameters
description = 'This script is for reconstruct 3D models from multiple videos and merge to one 3D model. ' + \
'BOW is used for accelerating 3D model merge. ' + \
'Please prepare multiple videos in Input folder.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('project_path', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where your 3D model project is stored.')
parser.add_argument('-k', '--path-camera-file', action='store', nargs='?', const=None, \
default=None, type=str, choices=None, metavar=None, \
help='File path where camera matrix is stored in Numpy text format. (default: focal length ' + \
str(reconstructParam.focalLength) + ' will be used)')
parser.add_argument('--bow', action='store_true', default=False, \
help='Use BOW to accelerate 3D model merge if this flag is set (default: False)')
args = parser.parse_args()
PROJECT_PATH = args.project_path
USE_BOW = args.bow
PATH_CAMERA_FILE = args.path_camera_file
if PATH_CAMERA_FILE:
if os.path.exists(PATH_CAMERA_FILE):
with open(PATH_CAMERA_FILE,"r") as camMatFile:
K = np.loadtxt(camMatFile)
if K.shape[0]!=3 or K.shape[1]!=3:
print "Error : invalid camera matrix size = " + str(K)
sys.exit()
print "Focal length " + str(K[0][0]) + " is set for reconstruction"
reconstructParam.focalLength = K[0][0]
else:
print "Error : invalid camera matrix file = " + PATH_CAMERA_FILE
sys.exit()
# get paths
inputPath = os.path.join(PROJECT_PATH, "Input")
outputPath = os.path.join(PROJECT_PATH, "Output")
FileUtils.makedir(outputPath)
# reconstruct all videos
listVideo = sorted(os.listdir(inputPath))
for video in listVideo:
if not os.path.isdir(os.path.join(inputPath, video)):
continue
print "Begin reconstructing video : " + video
sfm_mainDir = os.path.join(outputPath, video)
sfm_inputDir = os.path.join(inputPath, video)
sfm_inputImgDir = os.path.join(sfm_inputDir, "inputImg")
sfm_matchesDir = os.path.join(sfm_mainDir, "matches")
sfm_sfmDir = os.path.join(sfm_mainDir, "SfM")
sfm_reconstructDir = os.path.join(sfm_sfmDir, "reconstruction")
sfm_globalDir = os.path.join(sfm_reconstructDir, "global")
FileUtils.makedir(sfm_mainDir)
FileUtils.makedir(sfm_inputImgDir)
FileUtils.makedir(sfm_matchesDir)
FileUtils.makedir(sfm_sfmDir)
FileUtils.makedir(sfm_reconstructDir)
FileUtils.makedir(sfm_globalDir)
if not os.path.isfile(os.path.join(sfm_globalDir, "sfm_data.json")):
# list images
os.system("openMVG_main_SfMInit_ImageListing -i " + sfm_inputImgDir + " -o " + sfm_matchesDir + " -d " + reconstructParam.CAMERA_DATABASE_PATH)
# 1.1 Check intrinsic
# ( if camera parameter not specified then replace with fixed camera.
# and set appropriate width and height)
with open(os.path.join(sfm_matchesDir, "sfm_data.json")) as sfm_data_file:
sfm_data = json.load(sfm_data_file)
hImg = sfm_data["views"][0]['value']['ptr_wrapper']['data']["height"]
wImg = sfm_data["views"][0]['value']['ptr_wrapper']['data']["width"]
if len(sfm_data["intrinsics"]) == 0:
for view in sfm_data["views"]:
view["value"]["ptr_wrapper"]["data"]["id_intrinsic"] = 0;
sfm_data["intrinsics"].append({})
sfm_data["intrinsics"][0]["key"] = 0
sfm_data["intrinsics"][0]["values"] = {}
# sfm_data["intrinsics"][0]["values"]["polymorphic_name"] = "pinhole_radial_k3"
sfm_data["intrinsics"][0]["values"]["polymorphic_name"] = "pinhole"
sfm_data["intrinsics"][0]["values"]["polymorphic_id"] = 2147999999
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"] = {}
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["id"] = 2147483660
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"] = {}
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"]["width"] = wImg
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"]["height"] = hImg
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"]["focal_length"] = reconstructParam.focalLength
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"]["disto_k3"] = [0, 0, 0]
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"]["principal_point"] = [wImg / 2, hImg / 2]
with open(os.path.join(sfm_matchesDir, "sfm_data.json"), "w") as sfm_data_file:
json.dump(sfm_data, sfm_data_file)
# 2 - Features computation and matching
# ( Compute a list of features & descriptors for each image)
os.system(reconstructParam.EXTRACT_FEATURE_MATCH_PROJECT_PATH + \
" " + sfm_matchesDir + \
" -mf=" + str(reconstructParam.maxTrackletMatchDistance) + \
" -mm=" + str(reconstructParam.minMatchToRetain) + \
" -f=" + str(reconstructParam.extFeatDistRatio) + \
" -r=" + str(reconstructParam.extFeatRansacRound))
# OpenMVG assumes matches.e.txt for global reconstruction, matches.f.txt for incremental reconstruction
os.system("cp " + os.path.join(sfm_matchesDir, "matches.f.txt") + " " + os.path.join(sfm_matchesDir, "matches.e.txt"))
# 3 - Global reconstruction
countRecon = 1
while not os.path.isfile(os.path.join(sfm_globalDir, "sfm_data.json")) and countRecon < reconstructParam.rerunRecon:
os.system("openMVG_main_GlobalSfM -i " + os.path.join(sfm_matchesDir, "sfm_data.json") + " -m " + sfm_matchesDir + " -o " + sfm_globalDir)
countRecon = countRecon + 1
time.sleep(1)
if not os.path.isfile(os.path.join(sfm_globalDir, "sfm_data.json")):
continue
# 4 - Color the pointcloud
os.system("openMVG_main_ComputeSfM_DataColor -i " + os.path.join(sfm_globalDir, "sfm_data.json") + " -o " + os.path.join(sfm_globalDir, "colorized.ply"))
# 4.5 remove part of reconstruction where it is incorrect
# Specifically,sometimes when their matching is not adequate,
# the reconstructed model will be divided into two or more models
# with different scale and a "jump" between pose translation.
# This function detects such jump and retain the the largest
# beginning or ending part of reconstruction, while the rest
# should be reconstructed separately by cleanSfM.
countCut = 0
# keep cutting until no more cut
while cleanSfM.cutSfMDataJump(os.path.join(sfm_globalDir, "sfm_data.json"), bufferFrame=reconstructParam.bufferFrame):
countCut = countCut + 1
os.rename(os.path.join(sfm_globalDir, "sfm_data_BC.json"),
os.path.join(sfm_globalDir, "sfm_data_BC" + str(countCut) + ".json"))
os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + \
" " + os.path.join(sfm_globalDir, "sfm_data.json") + \
" " + os.path.join(sfm_globalDir, "sfm_data.json") + \
" -c=" + "rs,rst,rsti")
os.system("openMVG_main_ComputeSfM_DataColor -i " + os.path.join(sfm_globalDir, "sfm_data.json") + " -o " + os.path.join(sfm_globalDir, "colorized_AC.ply"))
# 5 - Clean sfm_data by removing viewID of frames that are not used
# in reconstruction and put them in another folder and reconstruct them again
# note that sfm_data.json in matches folder is renamed and kept as reference
unusedImg = cleanSfM.cleanSfM(os.path.join(sfm_globalDir, "sfm_data.json"),
[os.path.join(sfm_matchesDir, "matches.putative.txt"),
os.path.join(sfm_matchesDir, "matches.e.txt"),
os.path.join(sfm_matchesDir, "matches.f.txt")])
# 6. move unused images, csv files into a new folder unless they have less than x images
for i in range(0, len(unusedImg)):
listUnused = unusedImg[i]
if len(listUnused) < reconstructParam.minUnusedImgLength:
continue
# set name for new video
if i == 0:
newVidName = video + "_front"
elif i == 1:
newVidName = video + "_back"
else:
# this should not be called
continue
# set path
pathNewVid = os.path.join(inputPath, newVidName)
# skip if there is already this folder
if os.path.isdir(pathNewVid):
continue
print "Extract unused part of " + video + " into " + newVidName
FileUtils.makedir(pathNewVid)
csvNewVid = os.path.join(pathNewVid, "csv")
imgNewVid = os.path.join(pathNewVid, "inputImg")
FileUtils.makedir(csvNewVid)
FileUtils.makedir(imgNewVid)
# copy image in list and csv file
os.system("cp -s " + os.path.join(sfm_inputDir, "csv", "*.csv") + " " + csvNewVid)
for unusedFilename in listUnused:
os.system("cp -s " + os.path.join(sfm_inputImgDir, unusedFilename) + " " + imgNewVid)
# append the folder into reconstruction queue
listVideo.append(newVidName)
# train bag of words model, and extract bag of words feature for all images
if USE_BOW and not os.path.isfile(os.path.join(outputPath, "merge_result", "Output", "matches", "BOWfile.yml")):
outputBowPath = os.path.join(outputPath, "merge_result", "Output", "matches")
if not os.path.isdir(outputBowPath):
FileUtils.makedir(outputBowPath)
print "Execute Training BOW : " + reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + outputPath + " " + \
os.path.join(outputBowPath, "BOWfile.yml") + " -p=" + os.path.join(outputBowPath, "PCAfile.yml")
os.system(reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + outputPath + " " + \
os.path.join(outputBowPath, "BOWfile.yml") + " -p=" + os.path.join(outputBowPath, "PCAfile.yml"))
# load graph structure from "mergeGraph.txt" if it exists
# create new graph structure if it does not exist
if os.path.isfile(os.path.join(outputPath, "merge_result", "Output", "SfM", "reconstruction", "mergeGraph.txt")):
if USE_BOW:
sfmGraph = sfmMergeGraphBOW.sfmGraphBOW.load(os.path.join(outputPath, "merge_result", "Output", "SfM", "reconstruction", "mergeGraph.txt"))
else:
sfmGraph = sfmMergeGraph.sfmGraph.load(os.path.join(outputPath, "merge_result", "Output", "SfM", "reconstruction", "mergeGraph.txt"))
sfmGraph.workspacePath = reconstructParam.WORKSPACE_DIR
#### start of manually adding new model code ####
# In current code, you cannot add new 3D model once you start merging.
# Enable following commented code to add new 3D model after you already started merging.
'''
newModelToAdd = []
for newModelName in newModelToAdd:
addModel(newModelName,os.path.join(inputPath,newModelName),os.path.join(outputPath,newModelName))
sfmGraph.clearBadMatches()
'''
### end of manually adding new model code ###
else:
if USE_BOW:
sfmGraph = sfmMergeGraphBOW.sfmGraphBOW(inputPath,
outputPath,
os.path.join(outputPath, "merge_result", "Input"),
os.path.join(outputPath, "merge_result", "Output", "SfM", "reconstruction"),
os.path.join(outputPath, "merge_result", "Output", "matches"),
os.path.join(outputPath, "merge_result", "Input", "csv"),
os.path.join(outputPath, "merge_result", "Input", "inputImg"),
reconstructParam.WORKSPACE_DIR,
reconstructParam.minReconFrameToAdd)
else:
sfmGraph = sfmMergeGraph.sfmGraph(inputPath,
outputPath,
os.path.join(outputPath, "merge_result", "Input"),
os.path.join(outputPath, "merge_result", "Output", "SfM", "reconstruction"),
os.path.join(outputPath, "merge_result", "Output", "matches"),
os.path.join(outputPath, "merge_result", "Input", "csv"),
os.path.join(outputPath, "merge_result", "Input", "inputImg"),
reconstructParam.WORKSPACE_DIR,
reconstructParam.minReconFrameToAdd)
if USE_BOW:
sfmGraph.mergeModel(os.path.join(outputPath, listVideo[0], "matches", "image_describer.txt"),
inputPath,
outputPath,
reconParam=reconstructParam,
reconBOWParam=reconstructBOWParam)
else:
sfmGraph.mergeModel(os.path.join(outputPath, listVideo[0], "matches", "image_describer.txt"),
inputPath,
outputPath,
reconParam=reconstructParam)
# select largest 3D model and save it
SfMDataUtils.saveFinalSfM(PROJECT_PATH)
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():
# set default parameter
reconstructParam = ReconstructParam.ReconstructParam
reconstructBOWParam = ReconstructBOWParam.ReconstructBOWParam
# parse parameters
description = 'This script is for reconstruct 3D models from multiple videos and merge to one 3D model. ' + \
'BOW is used for accelerating 3D model merge. ' + \
'Please prepare multiple videos in Input folder.'
parser = argparse.ArgumentParser(description=description)
parser.add_argument('project_path', action='store', nargs=None, const=None, \
default=None, type=str, choices=None, metavar=None, \
help='Directory path where your 3D model project is stored.')
parser.add_argument('-k', '--path-camera-file', action='store', nargs='?', const=None, \
default=None, type=str, choices=None, metavar=None, \
help='File path where camera matrix is stored in Numpy text format. (default: focal length ' + \
str(reconstructParam.focalLength) + ' will be used)')
parser.add_argument('--bow', action='store_true', default=False, \
help='Use BOW to accelerate 3D model merge if this flag is set (default: False)')
args = parser.parse_args()
PROJECT_PATH = args.project_path
USE_BOW = args.bow
PATH_CAMERA_FILE = args.path_camera_file
if PATH_CAMERA_FILE:
if os.path.exists(PATH_CAMERA_FILE):
with open(PATH_CAMERA_FILE, "r") as camMatFile:
K = np.loadtxt(camMatFile)
if K.shape[0] != 3 or K.shape[1] != 3:
print "Error : invalid camera matrix size = " + str(K)
sys.exit()
print "Focal length " + str(K[0][0]) + " is set for reconstruction"
reconstructParam.focalLength = K[0][0]
else:
print "Error : invalid camera matrix file = " + PATH_CAMERA_FILE
sys.exit()
# get paths
inputPath = os.path.join(PROJECT_PATH, "Input")
outputPath = os.path.join(PROJECT_PATH, "Output")
FileUtils.makedir(outputPath)
# reconstruct all videos
listVideo = sorted(os.listdir(inputPath))
for video in listVideo:
if not os.path.isdir(os.path.join(inputPath, video)):
continue
print "Begin reconstructing video : " + video
sfm_mainDir = os.path.join(outputPath, video)
sfm_inputDir = os.path.join(inputPath, video)
sfm_inputImgDir = os.path.join(sfm_inputDir, "inputImg")
sfm_matchesDir = os.path.join(sfm_mainDir, "matches")
sfm_sfmDir = os.path.join(sfm_mainDir, "SfM")
sfm_reconstructDir = os.path.join(sfm_sfmDir, "reconstruction")
sfm_globalDir = os.path.join(sfm_reconstructDir, "global")
FileUtils.makedir(sfm_mainDir)
FileUtils.makedir(sfm_inputImgDir)
FileUtils.makedir(sfm_matchesDir)
FileUtils.makedir(sfm_sfmDir)
FileUtils.makedir(sfm_reconstructDir)
FileUtils.makedir(sfm_globalDir)
if not os.path.isfile(os.path.join(sfm_globalDir, "sfm_data.json")):
# list images
os.system("openMVG_main_SfMInit_ImageListing -i " +
sfm_inputImgDir + " -o " + sfm_matchesDir + " -d " +
reconstructParam.CAMERA_DATABASE_PATH)
# 1.1 Check intrinsic
# ( if camera parameter not specified then replace with fixed camera.
# and set appropriate width and height)
with open(os.path.join(sfm_matchesDir,
"sfm_data.json")) as sfm_data_file:
sfm_data = json.load(sfm_data_file)
hImg = sfm_data["views"][0]['value']['ptr_wrapper']['data'][
"height"]
wImg = sfm_data["views"][0]['value']['ptr_wrapper']['data'][
"width"]
if len(sfm_data["intrinsics"]) == 0:
for view in sfm_data["views"]:
view["value"]["ptr_wrapper"]["data"][
"id_intrinsic"] = 0
sfm_data["intrinsics"].append({})
sfm_data["intrinsics"][0]["key"] = 0
sfm_data["intrinsics"][0]["values"] = {}
# sfm_data["intrinsics"][0]["values"]["polymorphic_name"] = "pinhole_radial_k3"
sfm_data["intrinsics"][0]["values"][
"polymorphic_name"] = "pinhole"
sfm_data["intrinsics"][0]["values"][
"polymorphic_id"] = 2147999999
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"] = {}
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"][
"id"] = 2147483660
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"][
"data"] = {}
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"][
"width"] = wImg
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"][
"height"] = hImg
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"][
"focal_length"] = reconstructParam.focalLength
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"][
"disto_k3"] = [0, 0, 0]
sfm_data["intrinsics"][0]["values"]["ptr_wrapper"]["data"][
"principal_point"] = [wImg / 2, hImg / 2]
with open(os.path.join(sfm_matchesDir, "sfm_data.json"),
"w") as sfm_data_file:
json.dump(sfm_data, sfm_data_file)
# 2 - Features computation and matching
# ( Compute a list of features & descriptors for each image)
os.system(reconstructParam.EXTRACT_FEATURE_MATCH_PROJECT_PATH + \
" " + sfm_matchesDir + \
" -mf=" + str(reconstructParam.maxTrackletMatchDistance) + \
" -mm=" + str(reconstructParam.minMatchToRetain) + \
" -f=" + str(reconstructParam.extFeatDistRatio) + \
" -r=" + str(reconstructParam.extFeatRansacRound))
# OpenMVG assumes matches.e.txt for global reconstruction, matches.f.txt for incremental reconstruction
os.system("cp " + os.path.join(sfm_matchesDir, "matches.f.txt") +
" " + os.path.join(sfm_matchesDir, "matches.e.txt"))
# 3 - Global reconstruction
countRecon = 1
while not os.path.isfile(
os.path.join(sfm_globalDir, "sfm_data.json")
) and countRecon < reconstructParam.rerunRecon:
os.system("openMVG_main_GlobalSfM -i " +
os.path.join(sfm_matchesDir, "sfm_data.json") +
" -m " + sfm_matchesDir + " -o " + sfm_globalDir)
countRecon = countRecon + 1
time.sleep(1)
if not os.path.isfile(os.path.join(sfm_globalDir,
"sfm_data.json")):
continue
# 4 - Color the pointcloud
os.system("openMVG_main_ComputeSfM_DataColor -i " +
os.path.join(sfm_globalDir, "sfm_data.json") + " -o " +
os.path.join(sfm_globalDir, "colorized.ply"))
# 4.5 remove part of reconstruction where it is incorrect
# Specifically,sometimes when their matching is not adequate,
# the reconstructed model will be divided into two or more models
# with different scale and a "jump" between pose translation.
# This function detects such jump and retain the the largest
# beginning or ending part of reconstruction, while the rest
# should be reconstructed separately by cleanSfM.
countCut = 0
# keep cutting until no more cut
while cleanSfM.cutSfMDataJump(
os.path.join(sfm_globalDir, "sfm_data.json"),
bufferFrame=reconstructParam.bufferFrame):
countCut = countCut + 1
os.rename(
os.path.join(sfm_globalDir, "sfm_data_BC.json"),
os.path.join(sfm_globalDir,
"sfm_data_BC" + str(countCut) + ".json"))
os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + \
" " + os.path.join(sfm_globalDir, "sfm_data.json") + \
" " + os.path.join(sfm_globalDir, "sfm_data.json") + \
" -c=" + "rs,rst,rsti")
os.system("openMVG_main_ComputeSfM_DataColor -i " +
os.path.join(sfm_globalDir, "sfm_data.json") + " -o " +
os.path.join(sfm_globalDir, "colorized_AC.ply"))
# 5 - Clean sfm_data by removing viewID of frames that are not used
# in reconstruction and put them in another folder and reconstruct them again
# note that sfm_data.json in matches folder is renamed and kept as reference
unusedImg = cleanSfM.cleanSfM(
os.path.join(sfm_globalDir, "sfm_data.json"), [
os.path.join(sfm_matchesDir, "matches.putative.txt"),
os.path.join(sfm_matchesDir, "matches.e.txt"),
os.path.join(sfm_matchesDir, "matches.f.txt")
])
# 6. move unused images, csv files into a new folder unless they have less than x images
for i in range(0, len(unusedImg)):
listUnused = unusedImg[i]
if len(listUnused) < reconstructParam.minUnusedImgLength:
continue
# set name for new video
if i == 0:
newVidName = video + "_front"
elif i == 1:
newVidName = video + "_back"
else:
# this should not be called
continue
# set path
pathNewVid = os.path.join(inputPath, newVidName)
# skip if there is already this folder
if os.path.isdir(pathNewVid):
continue
print "Extract unused part of " + video + " into " + newVidName
FileUtils.makedir(pathNewVid)
csvNewVid = os.path.join(pathNewVid, "csv")
imgNewVid = os.path.join(pathNewVid, "inputImg")
FileUtils.makedir(csvNewVid)
FileUtils.makedir(imgNewVid)
# copy image in list and csv file
os.system("cp -s " +
os.path.join(sfm_inputDir, "csv", "*.csv") + " " +
csvNewVid)
for unusedFilename in listUnused:
os.system("cp -s " +
os.path.join(sfm_inputImgDir, unusedFilename) +
" " + imgNewVid)
# append the folder into reconstruction queue
listVideo.append(newVidName)
# train bag of words model, and extract bag of words feature for all images
if USE_BOW and not os.path.isfile(
os.path.join(outputPath, "merge_result", "Output", "matches",
"BOWfile.yml")):
outputBowPath = os.path.join(outputPath, "merge_result", "Output",
"matches")
if not os.path.isdir(outputBowPath):
FileUtils.makedir(outputBowPath)
print "Execute Training BOW : " + reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + outputPath + " " + \
os.path.join(outputBowPath, "BOWfile.yml") + " -p=" + os.path.join(outputBowPath, "PCAfile.yml")
os.system(reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + outputPath + " " + \
os.path.join(outputBowPath, "BOWfile.yml") + " -p=" + os.path.join(outputBowPath, "PCAfile.yml"))
# load graph structure from "mergeGraph.txt" if it exists
# create new graph structure if it does not exist
if os.path.isfile(
os.path.join(outputPath, "merge_result", "Output", "SfM",
"reconstruction", "mergeGraph.txt")):
if USE_BOW:
sfmGraph = sfmMergeGraphBOW.sfmGraphBOW.load(
os.path.join(outputPath, "merge_result", "Output", "SfM",
"reconstruction", "mergeGraph.txt"))
else:
sfmGraph = sfmMergeGraph.sfmGraph.load(
os.path.join(outputPath, "merge_result", "Output", "SfM",
"reconstruction", "mergeGraph.txt"))
sfmGraph.workspacePath = reconstructParam.WORKSPACE_DIR
#### start of manually adding new model code ####
# In current code, you cannot add new 3D model once you start merging.
# Enable following commented code to add new 3D model after you already started merging.
'''
newModelToAdd = []
for newModelName in newModelToAdd:
addModel(newModelName,os.path.join(inputPath,newModelName),os.path.join(outputPath,newModelName))
sfmGraph.clearBadMatches()
'''
### end of manually adding new model code ###
else:
if USE_BOW:
sfmGraph = sfmMergeGraphBOW.sfmGraphBOW(
inputPath, outputPath,
os.path.join(outputPath, "merge_result", "Input"),
os.path.join(outputPath, "merge_result", "Output", "SfM",
"reconstruction"),
os.path.join(outputPath, "merge_result", "Output", "matches"),
os.path.join(outputPath, "merge_result", "Input", "csv"),
os.path.join(outputPath, "merge_result", "Input",
"inputImg"), reconstructParam.WORKSPACE_DIR,
reconstructParam.minReconFrameToAdd)
else:
sfmGraph = sfmMergeGraph.sfmGraph(
inputPath, outputPath,
os.path.join(outputPath, "merge_result", "Input"),
os.path.join(outputPath, "merge_result", "Output", "SfM",
"reconstruction"),
os.path.join(outputPath, "merge_result", "Output", "matches"),
os.path.join(outputPath, "merge_result", "Input", "csv"),
os.path.join(outputPath, "merge_result", "Input",
"inputImg"), reconstructParam.WORKSPACE_DIR,
reconstructParam.minReconFrameToAdd)
if USE_BOW:
sfmGraph.mergeModel(os.path.join(outputPath, listVideo[0], "matches",
"image_describer.txt"),
inputPath,
outputPath,
reconParam=reconstructParam,
reconBOWParam=reconstructBOWParam)
else:
sfmGraph.mergeModel(os.path.join(outputPath, listVideo[0], "matches",
"image_describer.txt"),
inputPath,
outputPath,
reconParam=reconstructParam)
# select largest 3D model and save it
SfMDataUtils.saveFinalSfM(PROJECT_PATH)
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")
def main():
SfmDataUtils.saveGlobalSfM(os.path.join("../data","sfm_data.json"), os.path.join("../data","Amat.txt"), os.path.join("../data","sfm_data_global.json"))
os.system("openMVG_main_ComputeSfM_DataColor -i " + os.path.join("../data","sfm_data.json") + " -o " + os.path.join("../data","sfm_data.ply"))
os.system("openMVG_main_ComputeSfM_DataColor -i " + os.path.join("../data","sfm_data_global.json") + " -o " + os.path.join("../data","sfm_data_global.ply"))
