Ejemplo n.º 1
0
    def mergeModel(self, listbeacon, image_descFile, inputPath, outputPath, reconParam=ReconstructParam, 
                   reconIBeaconParam=ReconstructIBeaconParam, reconBOWParam=ReconstructBOWParam, 
                   mergeCoocThresRat=0.25, mergeCoocThresFrame=30):
        print "Begin merging models"
        
        normBeaconApproach = reconIBeaconParam.normApproach
        
        FileUtils.makedir(self.mInputImgPath)
        FileUtils.makedir(self.mCsvPath)
        FileUtils.makedir(self.mMatchesPath)
        FileUtils.makedir(self.mSfMPath)
        
        # generate beacon.txt file for all models
        # we need generate every time to synchronize with sfm_data.json
        for video in self.sfmModel:
            print "Generating beacon.txt for " + video.name
            # write beacon file
            IBeaconUtils.exportBeaconDataForSfmImageFrames(video.csvFolLoc, video.sfm_dataLoc, 
                                                           listbeacon, video.beaconFileLoc, normBeaconApproach)
            
            if (not os.path.isfile(video.beaconFileLoc)):
                print("Error: beacon file %s is not created" % video.beaconFileLoc)
                return
        
        # create symbolic links to all images, csv, and descriptor/feature files
        os.system("cp --remove-destination -s " + os.path.join(inputPath,"*","inputImg","*") + " " + self.mInputImgPath)
        os.system("cp --remove-destination -s " + os.path.join(inputPath,"*","csv","*") + " " + self.mCsvPath)
        os.system("cp --remove-destination -s " + os.path.join(outputPath,"*","matches","*.desc") + " " + self.mMatchesPath)
        os.system("cp --remove-destination -s " + os.path.join(outputPath,"*","matches","*.feat") + " " + self.mMatchesPath)
        os.system("cp --remove-destination -s " + os.path.join(outputPath,"*","matches","*.bow") + " " + self.mMatchesPath)
        
        # copy listbeacon.txt and image_describer.txt
        os.system("cp --remove-destination " + listbeacon + " " + self.mInputPath)
        os.system("cp --remove-destination " + image_descFile + " " + self.mMatchesPath)
         
        listLead = range(0,len(self.sfmModel)) # list of model indexes which can initiate merge (list of model indexes which did not fail merge yet)
        listBye = [] # list of model indexes which will not be used to initiate merge (list of model indexes which already failed merge)
        baseVideo = -1
        mergeCandidatesRemainsForBaseVideo = True
        calBeaconSim = False
        
        while True:
            # update model indexes which are not used to initiate merge
            if not mergeCandidatesRemainsForBaseVideo:
                listBye.append(self.sfmModel[baseVideo].name)
            
            listName = [(x,self.sfmModel[x].name) for x in range(0,len(self.sfmModel))]
            listLead = [x[0] for x in listName if x[1] not in listBye]
            
            # if there was a merge, recalculate the cooccurence graph
            if mergeCandidatesRemainsForBaseVideo:
                # calculate cooccurence graph
                if not calBeaconSim:
                    beaconCoocRat, beaconCoocFrame = self.calCooccurenceGraph(coocThres=reconIBeaconParam.coocThres)
                    calBeaconSim = True
                    
                print "graph edges : " + str(beaconCoocRat)
                print "SfM model names : " + str([x.name for x in self.sfmModel])
                connectionGraph = np.logical_or(beaconCoocRat > mergeCoocThresRat,beaconCoocFrame > mergeCoocThresFrame)
                
                # calculate connected component on graph
                ccLabel = scipy.sparse.csgraph.connected_components(
                    connectionGraph,
                    directed=False)[1]
                        
            # if nore more mergable components
            if len(np.unique(ccLabel)) == len(ccLabel):
                print "No more mergable components. Exiting."
                return
            
            # sort the length of reconstructed frames in each video 
            # from small to large to find the base Video
            reconFrameLenList = [len(self.sfmModel[i].reconFrame) for i in range(0,len(self.sfmModel))]
            reconFrameLenIdx = [x[0] for x in sorted(enumerate(reconFrameLenList), key=lambda y:y[1])]

            # find first base video that has a connected component
            baseVideo = ""
            for video in reconFrameLenIdx:
                if np.sum(ccLabel==ccLabel[video]) > 1 and video in listLead:
                    baseVideo = video
                    break
                
            # this should never be called since program should exit 
            # if there is no connected components in grap 
            if baseVideo == "":
                print "Cannot find connected component to merge. Exiting."
                return

            # get videos that connect to this baseVideo
            # and sort the from smallest to largest as merge order
            neighborVec = np.where(connectionGraph[baseVideo,:])[0]
            neighborVec = neighborVec[neighborVec!=baseVideo] # prevent selecting itself to merge
            mergeCandidate = neighborVec.tolist()
            nReconFrameMergeCand = [len(self.sfmModel[x].reconFrame) for x in mergeCandidate]
            orderMergeCand = [x[0] for x in sorted(enumerate(nReconFrameMergeCand), key=lambda y:y[1])]
            mergeCandidateModel = [self.sfmModel[mergeCandidate[i]] for i in orderMergeCand]

            mergedModel = self.sfmModel[baseVideo]
            
            print "Based model: " + mergedModel.name
            print "To merge with: " + str([x.name for x in mergeCandidateModel])
            mergeCandidatesRemainsForBaseVideo = False            
            for video in mergeCandidateModel:
                
                # check if failed localization has been performed on this pair before
                # if so, skip this localization
                if self.isBadMatch(video,mergedModel):
                    continue
                
                # swap order so small model is merged to larger model
                swap = False
                if len(mergedModel.reconFrame) < len(video.reconFrame):
                    tmp = mergedModel
                    mergedModel = video
                    video = tmp
                    swap = True
                
                # attempt merge
                mergeResult, mergedModelTmp = self.mergeOneModel(mergedModel,video,reconParam,reconIBeaconParam,reconBOWParam)
                
                if mergeResult:
                    mergedModel.update(mergedModelTmp)
                    videoIdx = self.sfmModel.index(video)
                    del self.sfmModel[videoIdx]
                    
                    # update beacon
                    beaconCoocRat = np.delete(beaconCoocRat,videoIdx,0)
                    beaconCoocRat = np.delete(beaconCoocRat,videoIdx,1)
                    beaconCoocFrame = np.delete(beaconCoocFrame,videoIdx,0)
                    beaconCoocFrame = np.delete(beaconCoocFrame,videoIdx,1)
                    beaconCoocRat, beaconCoocFrame = self.updateCooccurenceGraph(beaconCoocRat, beaconCoocFrame, [self.sfmModel.index(mergedModel)], coocThres=reconIBeaconParam.coocThres)
                    
                    self.nMergedModel = self.nMergedModel+1
                    self.save(os.path.join(self.mSfMPath,"global" + str(self.nMergedModel-1),"mergeGraph.txt"))
                    self.save(os.path.join(self.mSfMPath,"mergeGraph.txt"))
                    mergeCandidatesRemainsForBaseVideo = True
                    
                    # reset listBye to allow small model to merge to new large model
                    listBye = []
                    
                    # write result log file
                    with open(os.path.join(self.mSfMPath,"logRecon.txt"),"a") as outLogFile:
                        outLogFile.write(str(self.nMergedModel-1) + " " + mergedModel.name + "\n")
                    
                    # start again
                    break
                else:
                    # add to bad matches
                    self.badMatches.append([video.name,mergedModel.name])
                
                    # save
                    self.save(os.path.join(self.mSfMPath,"mergeGraph.txt"))
                
                    if swap:
                        # swap back if not merged
                        mergedModel = video
Ejemplo n.º 2
0
    def mergeOneModel(self, model1, model2, reconParam, reconIBeaconParam, reconBOWParam):
        
        sfmOutPath = os.path.join(self.mSfMPath,"global"+str(self.nMergedModel))
        
        # modified by T. IShihara 2016.06.14
        # fix file name too long issue
        # 
        # create a temporary folder for reconstructed image of model2
        #inputImgTmpFolder = os.path.join(self.mSfMPath,"inputImgTmp","inputImgTmp"+model2.name)        
        inputImgTmpFolder = os.path.join(self.mSfMPath,"inputImgTmp","inputImgTmpModel2")
        if os.path.isdir(inputImgTmpFolder):
            FileUtils.removedir(inputImgTmpFolder)
        
        # copy reconstructed image fom model2 to tmp folder
        sfm_data2 = FileUtils.loadjson(model2.sfm_dataLoc)
        if not os.path.isdir(inputImgTmpFolder):
            listReconFrameName = [sfm_data2["views"][x]["value"]["ptr_wrapper"]["data"]["filename"] for x in range(0,len(sfm_data2["views"])) if sfm_data2["views"][x]["value"]["ptr_wrapper"]["data"]["id_view"] in model2.reconFrame]
            FileUtils.makedir(inputImgTmpFolder)
            for reconFrameName in listReconFrameName:
                os.system("cp -s " + os.path.join(model2.imgFolLoc,reconFrameName) + " " + inputImgTmpFolder)
        
        
        # remove all old localization result
        FileUtils.removedir(model2.locFolLoc) 
        FileUtils.makedir(model2.locFolLoc)

        # localize the images from model2 on model1
        if self.useBow:
            os.system(reconIBeaconParam.LOCALIZE_PROJECT_PATH + \
                      " " + inputImgTmpFolder + \
                      " " + os.path.dirname(model1.sfm_dataLoc) + \
                      " " + self.mMatchesPath + \
                      " " + model2.locFolLoc + \
                      " -f=" + str(reconParam.locFeatDistRatio) + \
                      " -r=" + str(reconParam.locRansacRound) + \
                      " -b=" + model1.beaconFileLoc + \
                      " -e=" + model2.csvFolLoc + \
                      " -k=" + str(reconIBeaconParam.locKNNnum) + \
                      " -c=" + str(reconIBeaconParam.coocThres) + \
                      " -i=" + str(reconParam.locSkipFrame) + \
                      " -v=" + str(reconIBeaconParam.locSkipSelKNN) + \
                      " -n=" + str(reconIBeaconParam.normApproach) + \
                      " -kb=" + str(reconBOWParam.locKNNnum) + \
                      " -a=" + os.path.join(self.mMatchesPath, "BOWfile.yml") + \
                      " -p=" + os.path.join(self.mMatchesPath, "PCAfile.yml"))                                  
        else:
            os.system(reconIBeaconParam.LOCALIZE_PROJECT_PATH + \
                      " " + inputImgTmpFolder + \
                      " " + os.path.dirname(model1.sfm_dataLoc) + \
                      " " + self.mMatchesPath + \
                      " " + model2.locFolLoc + \
                      " -f=" + str(reconParam.locFeatDistRatio) + \
                      " -r=" + str(reconParam.locRansacRound) + \
                      " -b=" + model1.beaconFileLoc + \
                      " -e=" + model2.csvFolLoc + \
                      " -k=" + str(reconIBeaconParam.locKNNnum) + \
                      " -c=" + str(reconIBeaconParam.coocThres) + \
                      " -i=" + str(reconParam.locSkipFrame) + \
                      " -v=" + str(reconIBeaconParam.locSkipSelKNN) + \
                      " -n=" + str(reconIBeaconParam.normApproach))
                  
        # remove temporary image folder
        # removedir(inputImgTmpFolder)
        
        # extract centers from all json file and write to a file
        fileLoc = open(os.path.join(model2.locFolLoc,"center.txt"),"w")
        countLocFrame = 0
        for filename in sorted(os.listdir(model2.locFolLoc)):
            if filename[-4:]!="json":
                continue
            
            countLocFrame = countLocFrame + 1
            with open(os.path.join(model2.locFolLoc,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() 
        
        # get inlier matches
        FileUtils.makedir(sfmOutPath)
        resultSfMDataFile = os.path.join(sfmOutPath,"sfm_data.json")
        # below also checks if the ratio between first and last svd of M[0:3,0:3] 
        # is good or not. If not then reject
        # TODO : revisit ransacRound parameter, use number of reconstruction frame to determine structure points transform seems small
        nMatchPointsTmp, nInlierTmp, M = mergeSfM.mergeModel(model1.sfm_dataLoc,
                            model2.sfm_dataLoc,
                            model2.locFolLoc,
                            resultSfMDataFile,
                            ransacThres=model1.ransacStructureThres,
                            mergePointThres=model1.mergeStructureThres,
                            ransacRoundMul=reconParam.ransacRoundMul,
                            inputImgDir=self.mInputImgPath,
                            minLimit=reconParam.min3DnInliers)
        
        ratioInlierMatchPoints = 0.0
        if nMatchPointsTmp>0:
            ratioInlierMatchPoints = float(nInlierTmp)/nMatchPointsTmp
        
        # 3. perform test whether merge is good
        sfm_merge_generated = True
        countFileAgree = 0
        countFileLoc = 1
        if os.path.isfile(resultSfMDataFile):
            os.system(reconParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + resultSfMDataFile + " " + resultSfMDataFile)
            countFileLoc, countFileAgree = mergeSfM.modelMergeCheckLocal(resultSfMDataFile, model2.locFolLoc, model1.validMergeRansacThres)
        else:
            sfm_merge_generated = False
        
        ratioAgreeFrameReconFrame = 0.0
        if (len(model2.reconFrame)>0):
            ratioAgreeFrameReconFrame = float(countFileAgree)/len(model2.reconFrame)
        ratioAgreeFrameLocFrame = 0.0
        if (countFileLoc>0):
            ratioAgreeFrameLocFrame = float(countFileAgree)/countFileLoc
        
        # write log file
        with open(os.path.join(self.mSfMPath,"global"+str(self.nMergedModel),"log.txt"),"a") as filelog:
            filelog.write(("M1: " + model1.name + "\n" + \
                          "M2: " + model2.name + "\n" + \
                          "nMatchedPoints: " + str(nMatchPointsTmp) + "\n" + \
                          "nInliers: " + str(nInlierTmp) + "\n" + \
                          "ratioInlierWithMatchedPoints: " + str(ratioInlierMatchPoints) + "\n" + \
                          "countLocFrame: " + str(countLocFrame) + "\n" + \
                          "nReconFrame M2: " + str(len(model2.reconFrame)) + "\n" + \
                          "countFileAgree: " + str(countFileAgree) + "\n" + \
                          "countFileLoc: " + str(countFileLoc) + "\n" + \
                          "not sfm_merge_generated: " + str(not sfm_merge_generated) + "\n" + \
                          # obsolete condition by T. Ishihara 2015.11.10
                          #"nInlierTmp > "+str(reconParam.vldMergeRatioInliersFileagree)+"*countFileAgree: " + str(nInlierTmp > reconParam.vldMergeRatioInliersFileagree*countFileAgree) + "\n" + \
                          "countFileAgree > "+str(reconParam.vldMergeMinCountFileAgree)+": " + str(countFileAgree > reconParam.vldMergeMinCountFileAgree) + "\n" + \
                          # obsolete condition by T. Ishihara 2016.04.02
                          #"countFileAgree > "+str(reconParam.vldMergeSmallMinCountFileAgree)+": " + str(countFileAgree > reconParam.vldMergeSmallMinCountFileAgree) + "\n" + \
                          # obsolete condition by T. Ishihara 2016.04.02
                          #"countFileLoc < countFileAgree*" +str(reconParam.vldMergeShortRatio)+ ": " + str(countFileLoc < countFileAgree*reconParam.vldMergeShortRatio) + "\n" + \
                          "ratioLocAgreeWithReconFrame: " + str(ratioAgreeFrameReconFrame) + "\n" + \
                          "ratioLocAgreeWithReconFrame > " + str(reconParam.vldMergeRatioAgrFReconF) + ": " + str(ratioAgreeFrameReconFrame > reconParam.vldMergeRatioAgrFReconF) + "\n" + \
                          "ratioLocAgreeWithLocFrame: " + str(ratioAgreeFrameLocFrame) + "\n" + \
                          "ratioLocAgreeWithLocFrame > " + str(reconParam.vldMergeRatioAgrFLocF) + ": " + str(ratioAgreeFrameLocFrame > reconParam.vldMergeRatioAgrFLocF) + "\n" + \
                          str(M) + "\n\n"))
       
        # rename the localization folder to save localization result
        '''
        if os.path.isdir(model2.locFolLoc+model1.name):
            FileUtils.removedir(model2.locFolLoc+model1.name)
        os.rename(model2.locFolLoc,model2.locFolLoc+model1.name)
        '''
        
        # obsolete merge condition
        '''
        if not sfm_merge_generated or \
            not (nInlierTmp > reconParam.vldMergeRatioInliersFileagree*countFileAgree and \
            ((countFileAgree > reconParam.vldMergeMinCountFileAgree or (countFileAgree > reconParam.vldMergeSmallMinCountFileAgree and countFileLoc < countFileAgree*reconParam.vldMergeShortRatio)) and \
            ((nInlierTmp > reconParam.vldMergeNInliers and float(countFileAgree)/len(model2.reconFrame) > reconParam.vldMergeRatioAgrFReconFNInliers) or float(countFileAgree)/countFileLoc > reconParam.vldMergeRatioAgrFLocF) and
            (float(countFileAgree)/len(model2.reconFrame) > reconParam.vldMergeRatioAgrFReconF))):
        '''
        # update merge condition by T. Ishihara 2015.11.10
        '''
        if not sfm_merge_generated or \
            not (countFileAgree > reconParam.vldMergeMinCountFileAgree and \
                 countFileAgree > reconParam.vldMergeSmallMinCountFileAgree and \
                 countFileLoc < countFileAgree*reconParam.vldMergeShortRatio and \
                 ((nInlierTmp > reconParam.vldMergeNInliers and ratioAgreeFrameReconFrame > reconParam.vldMergeRatioAgrFReconFNInliers) or \
                    ratioAgreeFrameReconFrame > reconParam.vldMergeRatioAgrFReconF) and \
                 ratioAgreeFrameLocFrame > reconParam.vldMergeRatioAgrFLocF):
        '''
        # update merge condition by T. Ishihara 2016.04.02
        '''
        if not sfm_merge_generated or \
            not (countFileAgree > reconParam.vldMergeMinCountFileAgree and \
                 ((nInlierTmp > reconParam.vldMergeNInliers and ratioAgreeFrameReconFrame > reconParam.vldMergeRatioAgrFReconFNInliers) or \
                    ratioAgreeFrameReconFrame > reconParam.vldMergeRatioAgrFReconF) and \
                 ratioAgreeFrameLocFrame > reconParam.vldMergeRatioAgrFLocF):
        '''
        # update merge condition by T. Ishihara 2016.06.09
        '''
        if not sfm_merge_generated or \
            not (countFileAgree > reconParam.vldMergeMinCountFileAgree and \
                 ratioAgreeFrameLocFrame > reconParam.vldMergeRatioAgrFLocF and \
                 nInlierTmp > reconParam.min3DnInliers and \
                 ratioInlierMatchPoints > reconParam.vldMergeRatioInliersMatchPoints):
        '''
        # update merge condition by T. Ishihara 2016.06.20
        if not sfm_merge_generated or \
            not (countFileAgree > reconParam.vldMergeMinCountFileAgree and \
                 ratioAgreeFrameLocFrame > reconParam.vldMergeRatioAgrFLocF and \
                 nInlierTmp > reconParam.min3DnInliers):        
            print "Transformed locations do not agree with localization. Skip merge between " + model1.name + " and " + model2.name + "."
            
            '''
            if os.path.isfile(os.path.join(sfmOutPath,"sfm_data.json")):
                os.rename(os.path.join(sfmOutPath,"sfm_data.json"), \
                          os.path.join(sfmOutPath,"sfm_data_("+model1.name + "," + model2.name+").json"))
            '''
                            
            # move to next video
            return False, sfmModelIBeacon("","","","","","","",validMergeRansacThres=0,validMergeRansacThresK=0,
                                          ransacStructureThres=0, ransacStructureThresK=0, 
                                          mergeStructureThres=0, mergeStructureThresK=0)
                
        # generate colorized before bundle adjustment for comparison
        os.system("openMVG_main_ComputeSfM_DataColor " +
            " -i " + os.path.join(sfmOutPath,"sfm_data.json") +
            " -o " + os.path.join(sfmOutPath,"colorized_pre.ply"))        
        
        # TODO : try computing structure from know pose here
        # https://github.com/openMVG/openMVG/issues/246
        # http://openmvg.readthedocs.io/en/latest/software/SfM/ComputeStructureFromKnownPoses/
        
        # TODO : revisit the order of bundle adjustment
        # perform bundle adjustment
        '''
        os.system(reconParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + os.path.join(sfmOutPath,"sfm_data.json") + " " + os.path.join(sfmOutPath,"sfm_data.json") + \
                  " -c=" + "rs,rst,rsti" + " -r=" + "1")
        '''
        os.system(reconParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + os.path.join(sfmOutPath,"sfm_data.json") + " " + os.path.join(sfmOutPath,"sfm_data.json") + \
                  " -c=" + "rst,rsti" + " -r=" + "1")
        
        os.system("openMVG_main_ComputeSfM_DataColor " +
            " -i " + os.path.join(sfmOutPath,"sfm_data.json") +
            " -o " + os.path.join(sfmOutPath,"colorized.ply"))
        
        # write new beacon file
        IBeaconUtils.exportBeaconDataForSfmImageFrames(self.mCsvPath, resultSfMDataFile, os.path.join(self.mInputPath,"listbeacon.txt"),
                                                       os.path.join(sfmOutPath,"beacon.txt"), reconIBeaconParam.normApproach)
        
        return True, sfmModelIBeacon("A" + model1.name + "," + model2.name +"Z", self.mInputImgPath, self.mCsvPath, 
                                     os.path.join(sfmOutPath,"beacon.txt"), self.mMatchesPath, os.path.join(sfmOutPath,"loc"), 
                                     resultSfMDataFile, validMergeRansacThres=model1.validMergeRansacThres,
                                     ransacStructureThres=model1.ransacStructureThres, 
                                     mergeStructureThres=model1.mergeStructureThres)
def main():
    # set parameter
    reconstructParam = ReconstructParam.ReconstructParam
    reconstructIBeaconParam = ReconstructIBeaconParam.ReconstructIBeaconParam
    reconstructBOWParam = ReconstructBOWParam.ReconstructBOWParam

    # parse parameters
    description = 'This script is for reconstruct 3D models from multiple videos and merge to one 3D model. ' + \
                'iBeacon 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 per image a list of features & descriptors )
            IBeaconUtils.exportBeaconDataForSfmImageFrames(os.path.join(sfm_inputDir, "csv"), os.path.join(sfm_matchesDir, "sfm_data.json"), 
                                                           os.path.join(inputPath, "listbeacon.txt"), os.path.join(sfm_matchesDir, "beacon.txt"), 
                                                           reconstructIBeaconParam.normApproach)
            
            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)
                # for OpenMVG 1.0
                #os.system("openMVG_main_ConvertSfM_DataFormat -i " + os.path.join(sfm_globalDir, "sfm_data.bin") + " -o " + os.path.join(sfm_globalDir, "sfm_data.json"))
                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"))
                # modified by T. Ishihara 2016.06.14
                # change bundle adjustment parameter for cutted model
                '''                
                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(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + \
                          " " + os.path.join(sfm_globalDir, "sfm_data.json") + \
                          " " + os.path.join(sfm_globalDir, "sfm_data.json") + \
                          " -c=" + "rst,rsti" + " -r=" + "1")
            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")):
        sfmGraph = sfmMergeGraphIBeacon.sfmGraphIBeacon.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:
        sfmGraph = sfmMergeGraphIBeacon.sfmGraphIBeacon(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,
                                          USE_BOW,
                                          validMergeRansacThresK=reconstructParam.vldMergeAgrFrameThresK,                                          
                                          minReconFrame=reconstructParam.minReconFrameToAdd)
    
    sfmGraph.mergeModel(os.path.join(inputPath, "listbeacon.txt"),
                        os.path.join(outputPath, listVideo[0], "matches", "image_describer.txt"),
                        inputPath,
                        outputPath,
                        reconParam=reconstructParam,
                        reconIBeaconParam=reconstructIBeaconParam,
                        reconBOWParam=reconstructBOWParam)
    
    # select largest 3D model and save it
    SfMDataUtils.saveFinalSfM(PROJECT_PATH)
def main():
    description = 'This script is for merging multiple SfM output models to one SfM model.' + \
                'Please prepare multiple OpenMVG projects which have output SfM models, and matrix to convert to global coordinate.'
    parser = argparse.ArgumentParser(description=description)
    parser.add_argument('input_csv', action='store', nargs=None, const=None, \
                        default=None, type=str, choices=None, metavar=None, \
                        help='Input CSV file which lists OpenMVG projects which will be merged.')
    parser.add_argument('output_dir', action='store', nargs=None, const=None, \
                        default=None, type=str, choices=None, metavar=None, \
                        help='Output directory path where merged model will be saved.')
    args = parser.parse_args()
    input_csv = args.input_csv
    output_dir = args.output_dir
        
    # load reconstruct parameters
    reconstructParam = ReconstructParam.ReconstructParam
    reconstructIBeaconParam = ReconstructIBeaconParam.ReconstructIBeaconParam
    
    # read projects list
    projectList = []
    with open(input_csv, "r") as f:
        reader = csv.reader(f)
        for row in reader:
            project = {}
            project["dir"]  = row[0]
            project["sfm_data"]  = row[1]
            project["A"] = row[2]
            projectList.append(project)
    
    # copy source files to output directory
    for project in projectList:
        copyOriginalFiles(project["dir"], output_dir)
    
    # load beacon settings
    mergeBeaconmap = None
    for project in projectList:
        beacon_file = os.path.join(project["dir"], "Input", "listbeacon.txt")
        if os.path.exists(beacon_file):
            beaconmap = iBeaconUtils.parseBeaconSetting(beacon_file)
            if mergeBeaconmap is None:
                mergeBeaconmap = beaconmap
            else:
                if mergeBeaconmap!=beaconmap:
                    print "invalid find listbeacon.txt for project data : " + project["dir"]
                    print "listbeacon.txt should be same for all project data"
                    sys.exit()
                else:
                    print "valid listbeacon.txt for project data : " + project["dir"]
    
    # prepare output directory
    if not os.path.isdir(os.path.join(output_dir,"Ref")):
        FileUtils.makedir(os.path.join(output_dir,"Ref"))
    if not os.path.isdir(os.path.join(output_dir,"Ref","loc")):
        FileUtils.makedir(os.path.join(output_dir,"Ref","loc"))
    if not os.path.isdir(os.path.join(output_dir,"Output","SfM")):
        FileUtils.makedir(os.path.join(output_dir,"Output","SfM"))
    if not os.path.isdir(os.path.join(output_dir,"Output","SfM","reconstruction")):
        FileUtils.makedir(os.path.join(output_dir,"Output","SfM","reconstruction"))
    if not os.path.isdir(os.path.join(output_dir,"Output","SfM","reconstruction","global")):
        FileUtils.makedir(os.path.join(output_dir,"Output","SfM","reconstruction","global"))
    
    sfmDataList = []
    for project in projectList:
        if not os.path.exists(project["sfm_data"]):
            print "cannot find sfm data : " + project["sfm_data"]
            sys.exit()
        with open(project["sfm_data"]) as jsonFile:
            sfmDataList.append(json.load(jsonFile))
    
    AList = []
    for project in projectList:
        AList.append(np.loadtxt(project["A"]))
        print "load mat : " + project["A"]
        print (np.loadtxt(project["A"]))
    
    print "Load 3D points"
    pointIdList = []
    pointList = []
    for sfmData in sfmDataList:
        pointId, point = mergeSfM.getAll3DPointloc(sfmData)
        pointn = np.asarray(point, dtype=np.float).T
        
        pointIdList.append(pointId)
        pointList.append(pointn)
    
    # merge models
    mergeSfmData = None
    mergePointId = None
    mergePointn = None
    for idx in range(0, len(sfmDataList)):
        if idx==0:
            mergeSfmData = sfmDataList[0]
            mergeSfM.transform_sfm_data(mergeSfmData, AList[0])
        else:
            mergePointThres = mergeSfM.findMedianStructurePointsThres(mergeSfmData, reconstructParam.mergePointThresMul)
            print "thres to merge 3D points : " + str(mergePointThres)
            
            inlierMap = findInliersByKnownTransform(mergePointId, pointIdList[idx], mergePointn, pointList[idx], AList[idx], mergePointThres)
            print "number of points in base model : " + str(len(mergePointn[0]))
            print "number of points in model " + str(idx) + " : " + str(len(pointList[idx]))
            print "number of inliers : " + str(len(inlierMap))
            
            mergeSfM.merge_sfm_data(mergeSfmData, sfmDataList[idx], AList[idx], {x[0]: x[1] for x in inlierMap})
        
        mergePointId, mergePoint = mergeSfM.getAll3DPointloc(mergeSfmData)
        mergePointn = np.asarray(mergePoint, dtype=np.float).T
    
    # go back to coordinate of the first model
    _invA = np.linalg.inv(AList[0][0:3,0:3])
    invA = np.c_[_invA, -np.dot(_invA,AList[0][:,3])]
    mergeSfM.transform_sfm_data(mergeSfmData, invA)
    
    mergeSfmData["root_path"] = os.path.join(output_dir,"Input","inputImg")
    
    resultSfMDataFile = os.path.join(output_dir,"Output","SfM","reconstruction","global","sfm_data.json")
    with open(resultSfMDataFile,"w") as jsonfile:
        json.dump(mergeSfmData, jsonfile)
    
    # write new beacon file
    if mergeBeaconmap is not None:
        iBeaconUtils.exportBeaconDataForSfmImageFrames(os.path.join(output_dir,"Input","csv"), resultSfMDataFile, 
                                                       os.path.join(output_dir,"Input","listbeacon.txt"),
                                                       os.path.join(output_dir,"Output","SfM","reconstruction","global","beacon.txt"), 
                                                       reconstructIBeaconParam.normApproach)
    
    '''
    os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + resultSfMDataFile + " " + resultSfMDataFile)
    '''
    os.system(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + " " + resultSfMDataFile + " " + resultSfMDataFile + \
              " -c=" + "rst,rsti" + " -r=" + "1")
    
    Amat = AList[0]
    with open(os.path.join(output_dir,"Ref","Amat.txt"),"w") as AmatFile:
        np.savetxt(AmatFile,Amat)
    FileUtils.convertNumpyMatTxt2OpenCvMatYml(os.path.join(output_dir,"Ref","Amat.txt"), os.path.join(output_dir,"Ref","Amat.yml"), "A")
    
    # To create same directory structure before merging, create sfm_data.json without structure information in matches directory
    with open(resultSfMDataFile) as fpr:
        sfmData = json.load(fpr)
        sfmData["extrinsics"] = []
        sfmData["control_points"] = []
        sfmData["structure"] = []
        with open(os.path.join(output_dir,"Output","matches","sfm_data.json"),"w") as fpw:
            json.dump(sfmData, fpw)
    
    print "Execute : " + reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + os.path.join(output_dir,"Output") + " " + \
              os.path.join(output_dir,"Output", "matches", "BOWfile.yml") + " -p=" + os.path.join(output_dir,"Output", "matches", "PCAfile.yml")
    os.system(reconstructParam.WORKSPACE_DIR + "/TrainBoW/Release/TrainBoW " + os.path.join(output_dir,"Output") + " " + \
              os.path.join(output_dir,"Output", "matches", "BOWfile.yml") + " -p=" + os.path.join(output_dir,"Output", "matches", "PCAfile.yml"))
    
    os.system("openMVG_main_ComputeSfM_DataColor -i " + resultSfMDataFile + \
              " -o " + os.path.join(output_dir,"Output","SfM","reconstruction","global","colorized.ply"))
Ejemplo n.º 5
0
def main():
    # set parameter
    reconstructParam = ReconstructParam.ReconstructParam
    reconstructIBeaconParam = ReconstructIBeaconParam.ReconstructIBeaconParam
    reconstructBOWParam = ReconstructBOWParam.ReconstructBOWParam

    # parse parameters
    description = 'This script is for reconstruct 3D models from multiple videos and merge to one 3D model. ' + \
                'iBeacon 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 per image a list of features & descriptors )
            IBeaconUtils.exportBeaconDataForSfmImageFrames(
                os.path.join(sfm_inputDir, "csv"),
                os.path.join(sfm_matchesDir, "sfm_data.json"),
                os.path.join(inputPath, "listbeacon.txt"),
                os.path.join(sfm_matchesDir, "beacon.txt"),
                reconstructIBeaconParam.normApproach)

            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)
                # for OpenMVG 1.0
                #os.system("openMVG_main_ConvertSfM_DataFormat -i " + os.path.join(sfm_globalDir, "sfm_data.bin") + " -o " + os.path.join(sfm_globalDir, "sfm_data.json"))
                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"))
                # modified by T. Ishihara 2016.06.14
                # change bundle adjustment parameter for cutted model
                '''                
                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(reconstructParam.BUNDLE_ADJUSTMENT_PROJECT_PATH + \
                          " " + os.path.join(sfm_globalDir, "sfm_data.json") + \
                          " " + os.path.join(sfm_globalDir, "sfm_data.json") + \
                          " -c=" + "rst,rsti" + " -r=" + "1")
            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")):
        sfmGraph = sfmMergeGraphIBeacon.sfmGraphIBeacon.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:
        sfmGraph = sfmMergeGraphIBeacon.sfmGraphIBeacon(
            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,
            USE_BOW,
            validMergeRansacThresK=reconstructParam.vldMergeAgrFrameThresK,
            minReconFrame=reconstructParam.minReconFrameToAdd)

    sfmGraph.mergeModel(os.path.join(inputPath, "listbeacon.txt"),
                        os.path.join(outputPath, listVideo[0], "matches",
                                     "image_describer.txt"),
                        inputPath,
                        outputPath,
                        reconParam=reconstructParam,
                        reconIBeaconParam=reconstructIBeaconParam,
                        reconBOWParam=reconstructBOWParam)

    # select largest 3D model and save it
    SfMDataUtils.saveFinalSfM(PROJECT_PATH)