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"))
예제 #2
0
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"))
예제 #6
0
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"))