def optimizeTimePoly(self): graph = minisam.FactorGraph() #loss = minisam.CauchyLoss.Cauchy(0.) # TODO: Options Struct loss = None graph.add( TimePolyFactor(minisam.key('p', 0), self.bezier, self.minSpd, self.maxSpd, self.maxGs, loss, ts=self.tspan[0], tf=self.tspan[1])) init_values = minisam.Variables() opt_param = minisam.LevenbergMarquardtOptimizerParams() #opt_param.verbosity_level = minisam.NonlinearOptimizerVerbosityLevel.ITERATION opt = minisam.LevenbergMarquardtOptimizer(opt_param) values = minisam.Variables() init_values.add(minisam.key('p', 0), np.ones((2, ))) opt.optimize(graph, init_values, values) self.timePolyCoeffs = Flight.gen5thTimePoly( values.at(minisam.key('p', 0)), self.duration)
def optimize(graph, initials): """ Choose an solver from CHOLESKY, // Eigen Direct LDLt factorization CHOLMOD, // SuiteSparse CHOLMOD QR, // SuiteSparse SPQR CG, // Eigen Classical Conjugate Gradient Method CUDA_CHOLESKY, // cuSolverSP Cholesky factorization """ # optimize by GN opt_param = sam.GaussNewtonOptimizerParams() opt_param.max_iterations = 1000 opt_param.min_rel_err_decrease = 1e-10 opt_param.min_abs_err_decrease = 1e-10 opt_param.linear_solver_type = sam.LinearSolverType.CHOLMOD # opt_param.verbosity_level = sam.NonlinearOptimizerVerbosityLevel.SUBITERATION print(opt_param) opt = sam.GaussNewtonOptimizer(opt_param) all_timer = sam.global_timer().getTimer("Pose graph all") all_timer.tic() results = sam.Variables() status = opt.optimize(graph, initials, results) all_timer.toc() if status != sam.NonlinearOptimizationStatus.SUCCESS: print("optimization error: ", status) sam.global_timer().print() return results
def get_g2o_data(filename): graph = sam.FactorGraph() initials = sam.Variables() _ = sam.loadG2O(filename, graph, initials) return graph, initials
def optimizePoseGraph(self): self.graph_optimized = minisam.Variables() status = self.opt.optimize(self.graph_factors, self.graph_initials, self.graph_optimized) if status != minisam.NonlinearOptimizationStatus.SUCCESS: print("optimization error: ", status) # correct current pose pose_trans, pose_rot = getGraphNodePose(self.graph_optimized, self.curr_node_idx) self.curr_se3[:3, :3] = pose_rot self.curr_se3[:3, 3] = pose_trans
def __init__(self): self.prior_cov = minisam.DiagonalLoss.Sigmas( np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) self.const_cov = np.array([0.5, 0.5, 0.5, 0.1, 0.1, 0.1]) self.odom_cov = minisam.DiagonalLoss.Sigmas(self.const_cov) self.loop_cov = minisam.DiagonalLoss.Sigmas(self.const_cov) self.graph_factors = minisam.FactorGraph() self.graph_initials = minisam.Variables() self.opt_param = minisam.LevenbergMarquardtOptimizerParams() self.opt = minisam.LevenbergMarquardtOptimizer(self.opt_param) self.curr_se3 = None self.curr_node_idx = None self.prev_node_idx = None self.graph_optimized = None
def _optimize_graph(self): """Optimize the factor graph.""" if not self.new_node_guessed: raise RuntimeError('Missing initial guess for newly added node!') self.optimized_results = ms.Variables() status = self.opt.optimize(self.graph, self.initials, self.optimized_results) # Record last optimized SE2 (pose) self.last_optimized_se2 = self.get_result(self._idc_in_graph[-1]) # Use the results as the initials for the next iteration self.initials = self.optimized_results # After optimization at the current time step, set this flag to false so the first # factor added at the next time step can be ensured to be a odom (between) factor self.odom_added = False if status != ms.NonlinearOptimizationStatus.SUCCESS: print("optimization error: ", status)
def __init__(self, px, pcf, config, expected_lane_extractor, expected_pole_extractor, expected_rs_stop_extractor, first_node_idx=0): """Constructor method. Args: px (float): Distance from rear axle to front bumper. pcf (flaot): Distace from camera to front bumper. config (dict): Container for all configurations related to factor graph based localization. This should be read from the configuration .yaml file. expected_lane_extractor: Expected lane extractor for lane boundary factor. This is for lane boundary factors to query the map for expected lane boundaries. expected_pole_extractor: Expected pole extractor for pole factor. first_node_idx (int): Index of the first node. Sometimes it is more convenient to let indices consistent with recorded data. Especially when performing localization using only part of data that don't start from the beginning of the recording. """ self.px = px self.pcf = pcf # Config self.config = config # Initialize factors LaneBoundaryFactor.initialize(expected_lane_extractor, px) GNNLaneBoundaryFactor.initialize(expected_lane_extractor, px) PoleFactor.initialize(self.px, self.pcf) RSStopFactor.initialize(expected_rs_stop_extractor, px) # Flag to be burried into MMPriorFactor. # It indicates if MMPriorFactor experiences a mode switching. self.prior_switch_flag = SwitchFlag(False) # Instead of performing map pole queries in pole factors, graph manager # queries map poles in advance and only feeds map poles in the neighborhood # to the relevant pole factor. The pole factors are responsible for transforming # map poles into their own frame. self.expected_pole_extractor = expected_pole_extractor # Factor graph self.graph = ms.FactorGraph() # Initial guess self.initials = ms.Variables() # Window size self.win_size = config['graph']['win_size'] #: bool: True to use previous a posteriori as a priori self.use_prev_posteriori = config['graph']['use_prev_posteriori'] # Optimizer parameter self.opt_params = ms.LevenbergMarquardtOptimizerParams() self.opt_params.verbosity_level = ms.NonlinearOptimizerVerbosityLevel.ITERATION self.opt_params.max_iterations = 5 # Optimizer self.opt = ms.LevenbergMarquardtOptimizer(self.opt_params) # # Optimizer parameter # self.opt_params = ms.GaussNewtonOptimizerParams() # self.opt_params.verbosity_level = ms.NonlinearOptimizerVerbosityLevel.ITERATION # self.opt_params.max_iterations = 5 # # Optimizer # self.opt = ms.GaussNewtonOptimizer(self.opt_params) # Marginal covarnaince solver self.mcov_solver = ms.MarginalCovarianceSolver() #: int: Index for the prior node self.prior_node_idx = first_node_idx #: int: Index for the next node to be added self.next_node_idx = first_node_idx + 1 #: deque of pose node indices (including prior node) self._idc_in_graph = deque() #: deque of tuples: Each contains the state and noise model of the a posteriori of the last pose of each step # Tuple: (index, ms.SE2, ms.GaussianModel) self._history_a_posteriori = deque(maxlen=self.win_size-1) #: ms.Variables(): Result of optimization self.optimized_results = None #: ms.sophus.SE2: Last optimized SE2 pose self.last_optimized_se2 = None #: np.ndarray: Covariance matrix of last optimized pose self.last_optimized_cov = None #: np.ndarray: Stores the covariance matrix of the new node predicted using CTRV model. # This is used for gating and computing weights for data association self.pred_cov = None #: bool: True if new pose node already has a corresponding initial guess self.new_node_guessed = False #: bool: True if odom factor has been added at the current time step. # As in the current implementation, a odom factor should be added as the first operation # at every time step to introduce a new node. This flag is used to check the abovementioned # is not violated. # This flag is set to False after optimization is carried out in the end of every time step. # When trying to add a factor other than a odom factor without already adding a odom factor, # an Exceptio will be raised. self.odom_added = False
def optimizeBezierPath(self): if (self.bezier.order == 3 and self.optParams.init != None and self.optParams.final != None): self.bezier = Bezier.constructBezierPath( self.startPose, self.endPose, self.bezier.order, self.duration * np.array([ self.optParams.init / self.bezier.order, self.optParams.final / self.bezier.order ])) else: graph = minisam.FactorGraph() #loss = minisam.CauchyLoss.Cauchy(0.) # TODO: Options Struct loss = None graph.add( BezierCurveFactor(minisam.key('p', 0), self.startPose, self.endPose, self.bezier.order, self.duration, loss, optParams=self.optParams)) init_values = minisam.Variables() opt_param = minisam.LevenbergMarquardtOptimizerParams() #opt_param.verbosity_level = minisam.NonlinearOptimizerVerbosityLevel.ITERATION opt = minisam.LevenbergMarquardtOptimizer(opt_param) values = minisam.Variables() linePts = self.startPose.getTranslation() + \ np.arange(0,1+1/(self.bezier.order),1/(self.bezier.order))*(self.endPose.getTranslation()- self.startPose.getTranslation()) #pdb.set_trace() if (self.optParams.init != None and self.optParams.final == None): # TODO: Initial Conditions initialGuess = np.hstack((linePts[3:-2].reshape((1, -1)), 1)) #init_values.add(minisam.key('p', 0), np.ones((1+self.dimension*(self.bezier.order-3),))) init_values.add(minisam.key('p', 0), initialGuess) opt.optimize(graph, init_values, values) d = np.array([self.optParams.init / self.bezier.order]) self.bezier = Bezier.constructBezierPath( self.startPose, self.endPose, self.bezier.order, np.hstack((d, values.at(minisam.key('p', 0))))) elif (self.optParams.init != None and self.optParams.final != None): print("Both Constrained") initialGuess = linePts[:, 2:-2].reshape((1, -1)) d = self.duration * np.array([ self.optParams.init / self.bezier.order, self.optParams.final / self.bezier.order ]) unit = np.zeros((self.dimension)) unit[0] = 1 pos2 = self.startPose * (d[0] * unit) pos3 = self.endPose * (-d[1] * unit) v = (pos3 - pos2) / (self.bezier.order - 2) initialGuess = pos2 + np.multiply( np.arange(1, self.bezier.order - 3 + 1), v) initialGuess = initialGuess.reshape((1, -1)) init_values.add(minisam.key('p', 0), np.squeeze(initialGuess)) #init_values.add(minisam.key('p', 0), np.ones((self.dimension*(self.bezier.order-3),))) opt.optimize(graph, init_values, values) #pdb.set_trace() self.bezier = Bezier.constructBezierPath( self.startPose, self.endPose, self.bezier.order, np.hstack((d, values.at(minisam.key('p', 0))))) else: initialGuess = np.hstack((linePts[3:-2].reshape((1, -1)))) #init_values.add(minisam.key('p', 0), np.ones((2+self.dimension*(self.bezier.order-3),))) init_values.add(minisam.key('p', 0), initialGuess) opt.optimize(graph, init_values, values) self.bezier = Bezier.constructBezierPath( self.startPose, self.endPose, self.bezier.order, values.at(minisam.key('p', 0)))
loss = None start = SE2() theta = np.pi / 4 R = SE2.rotationMatrix(theta) x = np.array([[6], [15]]) end = SE2(R=R, x=x) order = 6 graph = minisam.FactorGraph() graph.add(BezierCurveFactor(minisam.key('p', 0), start, end, order, loss)) init_values = minisam.Variables() init_values.add(minisam.key('p', 0), np.ones((2 + 2 * (order - 3), ))) print("initial curve parameters :", init_values.at(minisam.key('p', 0))) opt_param = minisam.LevenbergMarquardtOptimizerParams() #opt_param.verbosity_level = minisam.NonlinearOptimizerVerbosityLevel.ITERATION opt = minisam.LevenbergMarquardtOptimizer(opt_param) values = minisam.Variables() tic = perf_counter() status = opt.optimize(graph, init_values, values) toc = perf_counter() print("opitmized curve parameters :", values.at(minisam.key('p', 0)))