Beispiel #1
0
    def getInitialGuesses(self, cameras):

        if not self.G:
            raise RuntimeError("Graph is uninitialized!")

        #################################################################
        ## STEP 0: check if all cameras in the chain are connected
        ##         through common target point observations
        ##         (=all vertices connected?)
        #################################################################
        if not self.isGraphConnected():
            sm.logError(
                "The cameras are not connected through mutual target observations! "
                "Please provide another dataset...")

            self.plotGraph()
            sys.exit(0)

        #################################################################
        ## STEP 1: get baseline initial guesses by calibrating good
        ##         camera pairs using a stereo calibration
        ##
        #################################################################

        #first we need to find the best camera pairs to obtain the initial guesses
        #--> use the pairs that share the most common observed target corners
        #The graph is built with weighted edges that represent the number of common
        #target corners, so we can use dijkstras algorithm to get the best pair
        #configuration for the initial pair calibrations
        weights = [1.0 / commonPoints for commonPoints in self.G.es["weight"]]

        #choose the cam with the least edges as base_cam
        outdegrees = self.G.vs.outdegree()
        base_cam_id = outdegrees.index(min(outdegrees))

        #solve for shortest path  (=optimal transformation chaining)
        edges_on_path = self.G.get_shortest_paths(0,
                                                  weights=weights,
                                                  output="epath")

        self.optimal_baseline_edges = set(
            [item for sublist in edges_on_path for item in sublist])

        #################################################################
        ## STEP 2: solve stereo calibration problem for the baselines
        ##         (baselines are always from lower_id to higher_id cams!)
        #################################################################

        #calibrate all cameras in pairs
        for baseline_edge_id in self.optimal_baseline_edges:

            #get the cam_nrs from the graph edge (calibrate from low to high id)
            vertices = self.G.es[baseline_edge_id].tuple
            if vertices[0] < vertices[1]:
                camL_nr = vertices[0]
                camH_nr = vertices[1]
            else:
                camL_nr = vertices[1]
                camH_nr = vertices[0]

            print "\t initializing camera pair ({0},{1})...  ".format(
                camL_nr, camH_nr)

            #run the pair extrinsic calibration
            obs_list = self.obs_db.getAllObsTwoCams(camL_nr, camH_nr)
            success, baseline_HL = kcc.stereoCalibrate(cameras[camL_nr],
                                                       cameras[camH_nr],
                                                       obs_list,
                                                       distortionActive=False)

            if success:
                sm.logDebug("baseline_{0}_{1}={2}".format(
                    camL_nr, camH_nr, baseline_HL.T()))
            else:
                sm.logError(
                    "initialization of camera pair ({0},{1}) failed  ".format(
                        camL_nr, camH_nr))
                sm.logError("estimated baseline_{0}_{1}={2}".format(
                    camL_nr, camH_nr, baseline_HL.T()))

            #store the baseline in the graph
            self.G.es[self.G.get_eid(camL_nr,
                                     camH_nr)]["baseline_HL"] = baseline_HL

        #################################################################
        ## STEP 3: transform from the "optimal" baseline chain to camera chain ordering
        ##         (=> baseline_0 = T_c1_c0 |
        #################################################################

        #construct the optimal path graph
        G_optimal_baselines = self.G.copy()

        eid_not_optimal_path = set(range(0, len(G_optimal_baselines.es)))
        for eid in self.optimal_baseline_edges:
            eid_not_optimal_path.remove(eid)
        G_optimal_baselines.delete_edges(eid_not_optimal_path)

        #now we convert the arbitary baseline graph to baselines starting from
        # cam0 and traverse the chain (cam0->cam1->cam2->camN)
        baselines = []
        for baseline_id in range(0, self.numCams - 1):
            #find the shortest path on the graph
            path = G_optimal_baselines.get_shortest_paths(
                baseline_id, baseline_id + 1)[0]

            #get the baseline from cam with id baseline_id to baseline_id+1
            baseline_HL = sm.Transformation()
            for path_idx in range(0, len(path) - 1):
                source_vert = path[path_idx]
                target_vert = path[path_idx + 1]
                T_edge = self.G.es[self.G.get_eid(source_vert,
                                                  target_vert)]["baseline_HL"]

                #correct the direction (baselines always from low to high cam id!)
                T_edge = T_edge if source_vert < target_vert else T_edge.inverse(
                )

                #chain up
                baseline_HL = T_edge * baseline_HL

            #store in graph
            baselines.append(baseline_HL)

        #################################################################
        ## STEP 4: refine guess in full batch
        #################################################################
        success, baselines = kcc.solveFullBatch(cameras, baselines, self)

        if not success:
            sm.logWarn("Full batch refinement failed!")

        return baselines
Beispiel #2
0
    def getInitialGuesses(self, cameras):
        
        if not self.G:
            raise RuntimeError("Graph is uninitialized!")
        
        #################################################################
        ## STEP 0: check if all cameras in the chain are connected
        ##         through common target point observations
        ##         (=all vertices connected?)
        #################################################################
        if not self.isGraphConnected():
            sm.logError("The cameras are not connected through mutual target observations! " 
                        "Please provide another dataset...")
            
            self.plotGraph()
            sys.exit(0)
        
        #################################################################
        ## STEP 1: get baseline initial guesses by calibrating good 
        ##         camera pairs using a stereo calibration
        ## 
        #################################################################

        #first we need to find the best camera pairs to obtain the initial guesses
        #--> use the pairs that share the most common observed target corners
        #The graph is built with weighted edges that represent the number of common
        #target corners, so we can use dijkstras algorithm to get the best pair
        #configuration for the initial pair calibrations
        weights = [1.0/commonPoints for commonPoints in self.G.es["weight"]]

        #choose the cam with the least edges as base_cam
        outdegrees = self.G.vs.outdegree()
        base_cam_id = outdegrees.index(min(outdegrees))

        #solve for shortest path  (=optimal transformation chaining)
        edges_on_path = self.G.get_shortest_paths(0, weights=weights, output="epath")
        
        self.optimal_baseline_edges = set([item for sublist in edges_on_path for item in sublist])
        
        
        #################################################################
        ## STEP 2: solve stereo calibration problem for the baselines
        ##         (baselines are always from lower_id to higher_id cams!)
        #################################################################
        
        #calibrate all cameras in pairs
        for baseline_edge_id in self.optimal_baseline_edges:

            #get the cam_nrs from the graph edge (calibrate from low to high id)
            vertices = self.G.es[baseline_edge_id].tuple
            if vertices[0]<vertices[1]:
                camL_nr = vertices[0]
                camH_nr = vertices[1]
            else:
                camL_nr = vertices[1]
                camH_nr = vertices[0]
            
            print "\t initializing camera pair ({0},{1})...  ".format(camL_nr, camH_nr)          

            #run the pair extrinsic calibration
            obs_list = self.obs_db.getAllObsTwoCams(camL_nr, camH_nr)
            success, baseline_HL = kcc.stereoCalibrate(cameras[camL_nr], 
                                                       cameras[camH_nr], 
                                                       obs_list,
                                                       distortionActive=False)
            
            if success:
                sm.logDebug("baseline_{0}_{1}={2}".format(camL_nr, camH_nr, baseline_HL.T()))
            else:
                sm.logError("initialization of camera pair ({0},{1}) failed  ".format(camL_nr, camH_nr))
                sm.logError("estimated baseline_{0}_{1}={2}".format(camL_nr, camH_nr, baseline_HL.T()))
        
            #store the baseline in the graph
            self.G.es[ self.G.get_eid(camL_nr, camH_nr) ]["baseline_HL"] = baseline_HL
        
        #################################################################
        ## STEP 3: transform from the "optimal" baseline chain to camera chain ordering
        ##         (=> baseline_0 = T_c1_c0 | 
        #################################################################
        
        #construct the optimal path graph
        G_optimal_baselines = self.G.copy()
        
        eid_not_optimal_path = set(range(0,len(G_optimal_baselines.es)))
        for eid in self.optimal_baseline_edges:
            eid_not_optimal_path.remove(eid)
        G_optimal_baselines.delete_edges( eid_not_optimal_path )
        
        #now we convert the arbitary baseline graph to baselines starting from 
        # cam0 and traverse the chain (cam0->cam1->cam2->camN)
        baselines = []
        for baseline_id in range(0, self.numCams-1):
            #find the shortest path on the graph
            path = G_optimal_baselines.get_shortest_paths(baseline_id, baseline_id+1)[0]
            
            #get the baseline from cam with id baseline_id to baseline_id+1
            baseline_HL = sm.Transformation()
            for path_idx in range(0, len(path)-1):
                source_vert = path[path_idx]
                target_vert = path[path_idx+1]
                T_edge = self.G.es[ self.G.get_eid(source_vert, target_vert) ]["baseline_HL"]
            
                #correct the direction (baselines always from low to high cam id!)
                T_edge = T_edge if source_vert<target_vert else T_edge.inverse()
            
                #chain up
                baseline_HL = T_edge * baseline_HL
            
            #store in graph
            baselines.append(baseline_HL)
 
        #################################################################
        ## STEP 4: refine guess in full batch
        #################################################################
        success, baselines = kcc.solveFullBatch(cameras, baselines, self)
        
        if not success:
            sm.logWarn("Full batch refinement failed!")
    
        return baselines