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)))
