def getTargetPoseGuess(self, timestamp, cameras, baselines_HL=[]): #go through all camera that see this target at the given time #and take the one with the most target points camids = list() numcorners = list() for cam_id in self.obs_db.getCamIdsAtTimestamp(timestamp): camids.append(cam_id) numcorners.append( len(self.obs_db.getCornerIdsAtTime(timestamp, cam_id))) #get the pnp solution of the cam that sees most target corners max_idx = numcorners.index(max(numcorners)) cam_id_max = camids[max_idx] #solve the pnp problem camera_geomtry = cameras[cam_id_max].geometry success, T_t_cN = camera_geomtry.estimateTransformation( self.obs_db.getObservationAtTime(timestamp, cam_id_max)) if not success: sm.logWarn( "getTargetPoseGuess: solvePnP failed with solution: {0}". format(T_t_cN)) #transform it back to cam0 (T_t_cN --> T_t_c0) T_cN_c0 = sm.Transformation() for baseline_HL in baselines_HL[0:cam_id_max]: T_cN_c0 = baseline_HL * T_cN_c0 T_t_c0 = T_t_cN * T_cN_c0 return T_t_c0
def getTargetPoseGuess(self, timestamp, cameras, baselines_HL=[]): #go through all camera that see this target at the given time #and take the one with the most target points camids = list() numcorners = list() for cam_id in self.obs_db.getCamIdsAtTimestamp(timestamp): camids.append(cam_id) numcorners.append( len(self.obs_db.getCornerIdsAtTime(timestamp, cam_id)) ) #get the pnp solution of the cam that sees most target corners max_idx = numcorners.index(max(numcorners)) cam_id_max = camids[max_idx] #solve the pnp problem camera_geomtry = cameras[cam_id_max].geometry success, T_t_cN = camera_geomtry.estimateTransformation(self.obs_db.getObservationAtTime(timestamp, cam_id_max)) if not success: sm.logWarn("getTargetPoseGuess: solvePnP failed with solution: {0}".format(T_t_cN)) #transform it back to cam0 (T_t_cN --> T_t_c0) T_cN_c0 = sm.Transformation() for baseline_HL in baselines_HL[0:cam_id_max]: T_cN_c0 = baseline_HL * T_cN_c0 T_t_c0 = T_t_cN * T_cN_c0 return T_t_c0
def __generateExtrinsicsInitialGuess(self): """Estimate the pose of the camera with a PnP solver. Call after initializing the intrinsics""" # estimate and set T_c in the observations for idx, observation in enumerate(self.__observations): (success, T_t_c) = self.__camera.estimateTransformation(observation) if (success): observation.set_T_t_c(T_t_c) else: sm.logWarn("Could not estimate T_t_c for observation at index" . idx) return
def __generateExtrinsicsInitialGuess(self): """Estimate the pose of the camera with a PnP solver. Call after initializing the intrinsics""" # estimate and set T_c in the observations for idx, observation in enumerate(self.__observations): (success, T_t_c) = self.__camera.estimateTransformation(observation) if (success): observation.set_T_t_c(T_t_c) else: sm.logWarn("Could not estimate T_t_c for observation at index {0}".format(idx)) return
def truncateIndicesFromTime(self, indices, bag_from_to): #get the timestamps timestamps = list() for idx in self.indices: topic, data, stamp = self.bag._read_message( self.index[idx].position) timestamp = data.header.stamp.secs + data.header.stamp.nsecs / 1.0e9 timestamps.append(timestamp) bagstart = min(timestamps) baglength = max(timestamps) - bagstart #some value checking if bag_from_to[0] >= bag_from_to[1]: raise RuntimeError( "Bag start time must be bigger than end time.".format( bag_from_to[0])) if bag_from_to[0] < 0.0: sm.logWarn("Bag start time of {0} s is smaller 0".format( bag_from_to[0])) if bag_from_to[1] > baglength: sm.logWarn( "Bag end time of {0} s is bigger than the total length of {1} s" .format(bag_from_to[1], baglength)) #find the valid timestamps valid_indices = [] for idx, timestamp in enumerate(timestamps): if timestamp >= (bagstart + bag_from_to[0]) and timestamp <= ( bagstart + bag_from_to[1]): valid_indices.append(idx) sm.logWarn("BagImageDatasetReader: truncated {0} / {1} images.".format( len(indices) - len(valid_indices), len(indices))) return valid_indices
def truncateIndicesFromTime(self, indices, bag_from_to): # get the timestamps timestamps = list() for idx in self.indices: topic, data, stamp = self.bag._read_message(self.index[idx].position) timestamp = data.header.stamp.secs + data.header.stamp.nsecs / 1.0e9 timestamps.append(timestamp) bagstart = min(timestamps) baglength = max(timestamps) - bagstart # some value checking if bag_from_to[0] >= bag_from_to[1]: raise RuntimeError("Bag start time must be bigger than end time.".format(bag_from_to[0])) if bag_from_to[0] < 0.0: sm.logWarn("Bag start time of {0} s is smaller 0".format(bag_from_to[0])) if bag_from_to[1] > baglength: sm.logWarn("Bag end time of {0} s is bigger than the total length of {1} s".format( bag_from_to[1], baglength)) # find the valid timestamps valid_indices = [] for idx, timestamp in enumerate(timestamps): if timestamp >= (bagstart + bag_from_to[0]) and timestamp <= (bagstart + bag_from_to[1]): valid_indices.append(idx) sm.logWarn( "BagImageDatasetReader: truncated {0} / {1} images.".format(len(indices) - len(valid_indices), len(indices))) return valid_indices
def truncateIndicesFromFreq(self, indices, freq): # some value checking if freq < 0.0: raise RuntimeError("Frequency {0} Hz is smaller 0".format(freq)) # find the valid timestamps timestamp_last = -1 valid_indices = [] for idx in self.indices: topic, data, stamp = self.bag._read_message( self.index[idx].position) timestamp = data.header.stamp.secs + data.header.stamp.nsecs / 1.0e9 if timestamp_last < 0.0: timestamp_last = timestamp valid_indices.append(idx) continue if (timestamp - timestamp_last) >= 1.0 / freq: timestamp_last = timestamp valid_indices.append(idx) sm.logWarn( "BagImageDatasetReader: truncated {0} / {1} images (frequency)". format(len(indices) - len(valid_indices), len(indices))) return valid_indices
def figure_setup(scenes, plots=set(['scene', 'expectations_vs_demonstrations', 'feature_weights_progress', 'occupancy', \ 'feature_histogram', 'spline_parameter_distribution', 'spline_parameter_autocorrelation', \ 'feature_value_autocorrelation', 'parameter_trace', 'feature_expectation_trace'])): figuresPerScene = [] axesPerScene = [] dec = int(np.floor(np.log10(len(scenes)))) + 1 for cnt, scene in enumerate(scenes): figuresThisScene = [] axes = {} if 'scene' in plots: figuresThisScene.append( pl.figure(figsize=(20., 12.))) # new figure per demonstration figuresThisScene[-1].canvas.set_window_title('scene_{0}'.format( str(cnt).zfill(dec))) ax = figuresThisScene[-1].add_subplot(111) ax.grid('on') ax.set_xlabel('x [m]') ax.set_ylabel('y [m]') grid = None for o in reversed(scene.getObservations()): grid = o.occupancyGrid if not grid is None: break if not grid is None: try: from planner_algorithms import distanceTransform dt = distanceTransform(grid) pplot.plotGrid(dt, ax, cmap='Greys_r', interpolation='none', mask_value=np.NAN, rotate_args={ 'mode': 'constant', 'order': 0, 'cval': np.NAN }) except ImportError: sm.logWarn( "Cannot import planner_algorithms, not showing distance transform" ) pplot.plotScene(scene, ax, show_radii=False, show_observations=True, show_observation_ellipses=False, show_gridmaps=True, colormap=cmAgents, alpha_obs_ellipses=0.05, traj_args={'linewidth': 2.0}, \ arrow_head_width=0.2, grid_args={'plot_border': True, 'plot_origin': True}, grid_rotate_args={'mode': 'constant', 'order': 0, 'cval': pp.OccupancyValue.FREE}) ax.legend(loc='best', numpoints=1, fancybox=True, framealpha=0.5) ax.set_aspect('equal') axes['scene'] = ax if 'expectations_vs_demonstrations' in plots: figuresThisScene.append( pl.figure(figsize=(20., 12.))) # new figure per demonstration figuresThisScene[-1].canvas.set_window_title( 'expectations_vs_demonstrations_scene{0}'.format( str(cnt).zfill(dec))) ax = figuresThisScene[-1].add_subplot(111) # Expectations ax.grid('on') ax.set_ylabel('feature value') axes['expectations_vs_demonstrations'] = ax if 'feature_weights_progress' in plots and cnt == 0: # Note: This will be the same for all plots figuresThisScene.append( pl.figure(figsize=(20., 12.))) # new figure per demonstration figuresThisScene[-1].canvas.set_window_title( 'feature_weights_progress') ax = figuresThisScene[-1].add_subplot(111) # Gradient norm ax.grid('on', which='both', axis='y') ax.grid('on', which='major', axis='x') ax.set_xlabel('iteration') ax.set_ylabel('gradient norm') ax.set_yscale('log') ax.get_xaxis().set_major_locator(pl.MaxNLocator(integer=True)) ax.tick_params(axis='y', which='both') ax.yaxis.set_major_formatter(pl.FormatStrFormatter("%.1f")) ax.yaxis.set_minor_formatter(pl.FormatStrFormatter("%.1f")) ax2 = ax.twinx() ax2.set_ylabel('feature weight') ax2.grid('on', which='major', axis='y') axes['feature_weights_progress'] = [ax, ax2] if 'occupancy' in plots: figuresThisScene.append(pl.figure(figsize=(20., 12.))) figuresThisScene[-1].canvas.set_window_title('occupancy') ax = figuresThisScene[-1].add_subplot(111) ax.grid('on') ax.set_xlabel('x') ax.set_ylabel('y') axes['occupancy'] = ax if 'feature_histogram' in plots: figuresThisScene.append(pl.figure(figsize=(20., 12.))) figuresThisScene[-1].canvas.set_window_title( 'feature_histogram_scene{0}'.format(str(cnt).zfill(dec))) ax = figuresThisScene[-1].add_subplot(111) ax.grid('on') ax.set_xlabel('feature value') ax.set_ylabel('probability') axes['feature_histogram'] = ax if 'spline_parameter_distribution' in plots: nAxes = scene.numActiveSplineParameters fig1, axs = pplot.squareTileAxesLayout(nAxes, figsize=(20., 12.)) figuresThisScene.append(fig1) figuresThisScene[-1].canvas.set_window_title( 'spline_parameter_distribution_scene{0}'.format( str(cnt).zfill(dec))) axs = putil.flatten(axs) axes['spline_parameter_distribution'] = axs figuresThisScene[-1].tight_layout() if 'spline_parameter_autocorrelation' in plots: nAxes = scene.numActiveSplineParameters fig2, axs = pplot.squareTileAxesLayout(nAxes, figsize=(20., 12.)) figuresThisScene.append(fig2) axs = putil.flatten(axs) for ax in axs: ax.grid('on') figuresThisScene[-1].canvas.set_window_title( 'spline_parameter_autocorrelation_scene{0}'.format( str(cnt).zfill(dec))) axes['spline_parameter_autocorrelation'] = axs figuresThisScene[-1].tight_layout() if 'feature_value_autocorrelation' in plots: nAxes = len(featureContainer.getContainer()) fig3, axs = pplot.squareTileAxesLayout(nAxes, figsize=(20., 12.)) figuresThisScene.append(fig3) axs = putil.flatten(axs) for ax in axs: ax.grid('on') figuresThisScene[-1].canvas.set_window_title( 'feature_value_autocorrelation_scene{0}'.format( str(cnt).zfill(dec))) axes['feature_value_autocorrelation'] = axs figuresThisScene[-1].tight_layout() if 'parameter_trace' in plots: nAxes = scene.numActiveSplineParameters fig1, axs = pplot.squareTileAxesLayout(nAxes, figsize=(20., 12.)) figuresThisScene.append(fig1) figuresThisScene[-1].canvas.set_window_title( 'parameter_trace_scene{0}'.format(str(cnt).zfill(dec))) axs = putil.flatten(axs) axes['parameter_trace'] = axs figuresThisScene[-1].tight_layout() if 'feature_expectation_trace' in plots: nAxes = len(featureContainer.getContainer()) fig1, axs = pplot.squareTileAxesLayout(nAxes, figsize=(20., 12.)) figuresThisScene.append(fig1) figuresThisScene[-1].canvas.set_window_title( 'feature_expectation_trace_scene{0}'.format( str(cnt).zfill(dec))) axs = putil.flatten(axs) for ax in axs: ax.grid('on') axes['feature_expectation_trace'] = axs figuresThisScene[-1].tight_layout() axesPerScene.append(axes) figuresPerScene.append(figuresThisScene) return figuresPerScene, axesPerScene
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
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
def __buildOptimizationProblem(self, W): """Build the optimisation problem""" problem = inc.CalibrationOptimizationProblem() # Initialize all design variables. self.__initPoseDesignVariables(problem) ##### ## build error terms and add to problem # store all frames self.__frames = [] self.__reprojection_errors = [] # This code assumes that the order of the landmarks in the observations # is invariant across all observations. At least for the chessboards it is true. ##### # add all the landmarks once landmarks = [] landmarks_expr = {} keypoint_ids0 = self.__observations[0].getCornersIdx() for idx, landmark in enumerate(self.__observations[0].getCornersTargetFrame()): # design variable for landmark landmark_w_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(landmark)) landmark_w_dv.setActive(self.__config.estimateParameters['landmarks']); landmarks.append(landmark_w_dv) landmarks_expr[keypoint_ids0[idx]] = landmark_w_dv.toExpression() problem.addDesignVariable(landmark_w_dv, CALIBRATION_GROUP_ID) ##### # activate design variables self.__camera_dv.setActive( self.__config.estimateParameters['intrinsics'], self.__config.estimateParameters['distortion'], self.__config.estimateParameters['shutter'] ) ##### # Add design variables # add the camera design variables last for optimal sparsity patterns problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) ##### # Regularization term / motion prior motionError = asp.BSplineMotionError(self.__poseSpline_dv, W) problem.addErrorTerm(motionError) ##### # add a reprojection error for every corner of each observation for frameid, observation in enumerate(self.__observations): # only process successful observations of a pattern if (observation.hasSuccessfulObservation()): # add a frame frame = self.__cameraModelFactory.frameType() frame.setGeometry(self.__camera) frame.setTime(observation.time()) self.__frames.append(frame) ##### # add an error term for every observed corner for index, point in enumerate(observation.getCornersImageFrame()): # keypoint time offset by line delay as expression type keypoint_time = self.__camera_dv.keypointTime(frame.time(), point) # from target to world transformation. T_w_t = self.__poseSpline_dv.transformationAtTime( keypoint_time, self.__config.timeOffsetConstantSparsityPattern, self.__config.timeOffsetConstantSparsityPattern ) T_t_w = T_w_t.inverse() # we only have the the first image's design variables # so any landmark that is not in that frame won't be in the problem # thus we must skip those measurements that are of a keypoint that isn't visible keypoint_ids = observation.getCornersIdx() if not np.any(keypoint_ids[index]==keypoint_ids0): sm.logWarn("landmark {0} in frame {1} not in first frame".format(keypoint_ids[index],frameid)) continue # transform target point to camera frame p_t = T_t_w * landmarks_expr[keypoint_ids[index]] # create the keypoint keypoint_index = frame.numKeypoints() keypoint = acv.Keypoint2() keypoint.setMeasurement(point) inverseFeatureCovariance = self.__config.inverseFeatureCovariance; keypoint.setInverseMeasurementCovariance(np.eye(len(point)) * inverseFeatureCovariance) keypoint.setLandmarkId(keypoint_index) frame.addKeypoint(keypoint) # create reprojection error reprojection_error = self.__buildErrorTerm( frame, keypoint_index, p_t, self.__camera_dv, self.__poseSpline_dv ) self.__reprojection_errors.append(reprojection_error) problem.addErrorTerm(reprojection_error) return problem