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 __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 __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)))
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))) print("Optimization took: ", toc - tic) b = Bezier.constructBezierPath(start, end, order, values.at(minisam.key('p', 0)))