def _remove_unary_factors_from_first_node(self):
"""Remove unary factors related to the first node.
If the a posteriori of the current first node is recorded in a previous step, where it
was the last node, and used as a priori in this step, all factors already taken into
account to obtain the a posteriori must be removed. Otherwise, the same information will
contirbutes twice and leads to over-confident estimation.
Since the current implementation supports only in order factor adding, factors to be removed
are supposed to be unary in this localization case.
It is done by creating a new FactorGraph without factors that have to be removed.
"""
# Remove initial for the prior node
first_node_key = ms.key('x', self._idc_in_graph[0])
self.initials.erase(first_node_key)
# Create a new graph without factors connected to the first node
new_graph = ms.FactorGraph()
for factor in self.graph:
keep = True
# Check if it's a unary factor and connects to the first node
if len(factor.keys()) == 1 and first_node_key == factor.keys()[0]:
keep = False
if keep:
new_graph.add(factor)
# Replace the new graph
self.graph = new_graph
def get_g2o_data(filename):
graph = sam.FactorGraph()
initials = sam.Variables()
_ = sam.loadG2O(filename, graph, initials)
return graph, initials
def _truncate_first_node(self):
"""Truncate the first node in the current graph.
Used when the graph size exceeds the specified window size. All factors
related to the first node are simply deleted. It is done by creating a new
FactorGraph without factors connected to the first pose node.
"""
# Remove initial for the prior node
first_node_key = ms.key('x', self._idc_in_graph[0])
self.initials.erase(first_node_key)
# Create a new graph without factors connected to the first node
new_graph = ms.FactorGraph()
for factor in self.graph:
keep = True
# Check if this factor connects to the first node
for k in factor.keys():
if k == first_node_key:
keep = False
break
if keep:
new_graph.add(factor)
# Replace the new graph
self.graph = new_graph
# Remove the left most node index from queue
self._idc_in_graph.popleft()
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)))
return np.array([Bezier.costFunctionCurvDev(b)])
#loss=minisam.CauchyLoss.Cauchy(1.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()