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)))
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)))
dt = 0.01
t = np.arange(0, 1, dt)