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 addOdometryFactor(self, odom_transform):
self.graph_initials.add(minisam.key('x', self.curr_node_idx),
minisam.SE3(self.curr_se3))
self.graph_factors.add(
minisam.BetweenFactor(minisam.key('x', self.prev_node_idx),
minisam.key('x', self.curr_node_idx),
minisam.SE3(odom_transform), self.odom_cov))
def visualise(results, mcov_solver, nvert):
fig, ax = plt.subplots()
for i in range(nvert):
pose = results.at(key('x', i + 1))
cov = mcov_solver.marginalCovariance(key('x', i + 1))
plotSE2WithCov(pose, cov)
plt.axis('equal')
plt.show()
def addPriorFactor(self):
self.curr_node_idx = 0
self.prev_node_idx = 0
self.curr_se3 = np.eye(4)
self.graph_initials.add(minisam.key('x', self.curr_node_idx),
minisam.SE3(self.curr_se3))
self.graph_factors.add(
minisam.PriorFactor(minisam.key('x', self.curr_node_idx),
minisam.SE3(self.curr_se3), self.prior_cov))
def main():
filename = sys.argv[1]
graph, initials = get_g2o_data(filename)
# add a prior factor to first pose to fix the whole system
priorloss = sam.ScaleLoss.Scale(1) # value will be broadcasted to the dimension of pose. I guess.
graph.add(sam.PriorFactor(sam.key('x', 0), initials.at(sam.key('x', 0)), priorloss))
results = optimize(graph, initials)
# print(graph)
visualise(graph, initials, results)
def initialise_variables():
'''
We will give an initial value to our pose variables
'''
initials = Variables()
initials.add(key('x', 1), SE2(SO2(0.2), np.array([0.2, -0.3])))
initials.add(key('x', 2), SE2(SO2(-0.1), np.array([5.1, 0.3])))
initials.add(key('x', 3), SE2(SO2(-1.57 - 0.2), np.array([9.9, -0.1])))
initials.add(key('x', 4), SE2(SO2(-3.14 + 0.1), np.array([10.2, -5.0])))
initials.add(key('x', 5), SE2(SO2(1.57 - 0.1), np.array([5.1, -5.1])))
return initials
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 add_gnss_factor(self, point, add_init_guess=False):
"""Add GNSS factor to the last pose node (excluding the prior node).
Args:
point: Numpy.ndarray of measured x-y coordinate.
add_init_guess (bool): True to use gnss as initial guess.
This can be used when odom or imu is not applicable.
"""
if not self.odom_added:
raise RuntimeError(
'Between (odom) factor should be added first at every time step.')
node_key = ms.key('x', self._idc_in_graph[-1])
self.graph.add(GNSSFactor(node_key,
point,
self.config['gnss']))
if add_init_guess:
if self.last_optimized_se2 is not None:
# Use the difference from last pose to guess heading
last_trans = self.last_optimized_se2.translation()
trans_diff = point - last_trans
theta_guess = atan2(trans_diff[1], trans_diff[0])
else:
# Simply guess 0 as heading
theta_guess = 0.0
self.initials.add(node_key,
sophus.SE2(sophus.SO2(theta_guess), point))
self.new_node_guessed = True
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 addOdometryFactor(self, odom_transform, second_last=-1):
'''
-1代表当前帧和上一帧产生约束，-2代表当前帧和上上一帧产生约束
注意：先进行和上一帧的约束，在进行上上一帧的约束，顺序不能乱
'''
if second_last == -1:
self.graph_initials.add(minisam.key('x', self.curr_node_idx), minisam.SE3(self.curr_se3))
self.graph_factors.add(minisam.BetweenFactor(
minisam.key('x', self.prev_node_idx),
minisam.key('x', self.curr_node_idx),
minisam.SE3(odom_transform),
self.odom_cov))
elif second_last == -2:
self.graph_factors.add(minisam.BetweenFactor(
minisam.key('x', self.sec_prev_node_idx),
minisam.key('x', self.curr_node_idx),
minisam.SE3(odom_transform),
self.odom_cov))
def add_prior_factor(self, x, y, theta, prior_cov=None):
"""Add prior factor to first pose node.
The first pose node, so-called prior node, in the sliding window only has a prior factor apart
from the between factor, which connects the prior node and the second pose node.
Initial guess is automatically added to the prior node using the same values as the prior.
Args:
x: X coordinate. (m)
y: Y coordinate. (m)
theta: Heading (rad)
prior_cov: Covariance for prior.
"""
prior_node_key = ms.key('x', self.prior_node_idx)
prior_pose = ms.sophus.SE2(ms.sophus.SO2(theta), np.array([x, y]))
if self.config['prior']['max_mixture']:
self.prior_switch_flag.flag = False
self.graph.add(MMPriorFactor(prior_node_key,
prior_pose,
self.prior_switch_flag,
self.config['prior'],
prior_cov))
else:
if prior_cov is None:
prior_noise_model = ms.DiagonalLoss.Sigmas(np.array([self.config['prior']['stddev_x'],
self.config['prior']['stddev_y'],
self.config['prior']['stddev_theta']]))
else:
prior_noise_model = ms.GaussianLoss.Covariance(prior_cov)
self.graph.add(ms.PriorFactor(prior_node_key,
prior_pose,
prior_noise_model))
# Add prior node's index into queue if it is the first node added
if not self._idc_in_graph:
self._idc_in_graph.appendleft(self.prior_node_idx)
# Increment prior node index for later use if a posteriori is to be used as a priori later on
if self.use_prev_posteriori:
self.prior_node_idx += 1
# Add initial guess
self.initials.add(prior_node_key, prior_pose)
self.new_node_guessed = True
def add_rs_stop_factor(self, detected_rs_stop_dist, z):
"""Add road surfacc stop sign factor.
Args:
detected_rs_stop_dist (float): Distance to the detected rs stop sign.
z (float): Z-coordinate of the vehilce. This is to query the map at the correct level.
"""
if not self.odom_added:
raise RuntimeError(
'Between (odom) factor should be added first at every time step.')
node_key = ms.key('x', self._idc_in_graph[-1])
self.graph.add(RSStopFactor(node_key,
detected_rs_stop_dist,
z,
self.pred_cov,
self.config['rs_stop']))
def add_gnn_lane_factor(self, lane_marking_detection, z):
"""Add GNN lane factor to the graph.
Args:
lane_marking_detection (MELaneDetection): Lane marking detection object.
z (float): Z-coordinate of the vehilce. This is to query the map at the correct level.
"""
if not self.odom_added:
raise RuntimeError(
'Between (odom) factor should be added first at every time step.')
node_key = ms.key('x', self._idc_in_graph[-1])
self.graph.add(GNNLaneBoundaryFactor(node_key,
lane_marking_detection,
z,
self.pred_cov,
self.config['lane']))
def add_lane_factor(self, detected_marking, z):
"""Add max-mixture PDA lane factor to the graph.
Args:
detected_marking (MELaneMarking): Detected lane marking.
z (float): Z-coordinate of the vehilce. This is to query the map at the correct level.
"""
if not self.odom_added:
raise RuntimeError(
'Between (odom) factor should be added first at every time step.')
node_key = ms.key('x', self._idc_in_graph[-1])
self.graph.add(LaneBoundaryFactor(node_key,
detected_marking,
z,
self.pred_cov,
self.config['lane']))
def add_pole_factor(self, detected_pole):
"""Add pole factor to the graph.
Args:
detected_pole (Pole): Pole object.
"""
if not self.odom_added:
raise RuntimeError(
'Between (odom) factor should be added first at every time step.')
node_key = ms.key('x', self._idc_in_graph[-1])
# Retrieve the last initial guessed pose
init_guess = self.initials.at(node_key)
yaw = init_guess.so2().theta()
# Matrix to transform points in ego (rear axle) frame to world frame
tform_e2w = np.zeros((3, 3))
tform_e2w[0:2, 0:2] = np.array([[cos(yaw), -sin(yaw)],
[sin(yaw), cos(yaw)]])
tform_e2w[0:2, 2] = init_guess.translation()
tform_e2w[2, 2] = 1
# Get detected pole in world frame
det_pole_world = tform_e2w @ np.array(
[detected_pole.x, detected_pole.y, 1])
# Query map poles in the detected pole's neighborhood
# The results are wrt the world frame
neighbor_poles = self.expected_pole_extractor.extract(
(det_pole_world[0], det_pole_world[1]), self.config['pole']['query_radius'])
self.graph.add(PoleFactor(node_key,
detected_pole,
neighbor_poles,
self.pred_cov,
self.config['pole']))
def setup_graph():
'''
Here we will setup our factor graph
We have 3 factors in this example:
- Prior Factor: Anchors the inital position
- Odometry Factor: Between every pose
- Loop Closure Factor: It is what the name suggests
'''
graph = FactorGraph()
# We will now losses for each factor. This is basically a loss(Mahalanobis distance)
# specified by a covariance matrix.
priorLoss = DiagonalLoss.Sigmas(np.array([0.1, 0.1, 0.01]))
odomLoss = DiagonalLoss.Sigmas(np.array([0.5, 0.5, 0.1]))
loopLoss = DiagonalLoss.Sigmas(np.array([0.5, 0.5, 0.1]))
# We will now define factors between nodes
# Prior Factor
graph.add(
PriorFactor(key('x', 1), SE2(SO2(0), np.array([0, 0])), priorLoss))
# Odom Factor
graph.add(
BetweenFactor(key('x', 1), key('x', 2), SE2(SO2(0), np.array([5, 0])),
odomLoss))
graph.add(
BetweenFactor(key('x', 2), key('x', 3),
SE2(SO2(-1.57), np.array([5, 0])), odomLoss))
graph.add(
BetweenFactor(key('x', 3), key('x', 4),
SE2(SO2(-1.57), np.array([5, 0])), odomLoss))
graph.add(
BetweenFactor(key('x', 4), key('x', 5),
SE2(SO2(-1.57), np.array([5, 0])), odomLoss))
# Loop Closure Factor
graph.add(
BetweenFactor(key('x', 5), key('x', 2),
SE2(SO2(-1.57), np.array([5, 0])), loopLoss))
return graph
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=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()
status = opt.optimize(graph, init_values, values)
toc = perf_counter()
def addLoopFactor(self, loop_transform, loop_idx):
self.graph_factors.add(
minisam.BetweenFactor(minisam.key('x', loop_idx),
minisam.key('x', self.curr_node_idx),
minisam.SE3(loop_transform), self.loop_cov))
def add_ctrv_between_factor(self, vx, yaw_rate, delta_t, add_init_guess=True):
"""Add CTRV based between factor.
This method extends the graph by adding a new ctrv between factor to the last pose node.
This method should be the first operation at every time step to introduce a new node into
the graph in most cases. An exception is the very first time step where a prior node should
be added beforehand.
Args:
vx: Velocity in x. (m/s)
yaw_rate: Yaw rate. (rad/s)
delta_t: Timd difference. (sec)
add_init_guess (bool): True to use the motion model as the source of initial guess.
This should be True for most cases. For first node where odom or imu is not applicable,
choose to add init guess using with gnss factor instead.
"""
if not self._idc_in_graph:
raise RuntimeError(
'Between factor cannot be added to an empty graph!')
if self.odom_added:
raise RuntimeError(
'Between factor already added for this time step. \
Optimize the current factor graph to proceed to the next time step.')
last_node_idx = self._idc_in_graph[-1]
last_node_key = ms.key('x', last_node_idx)
new_node_key = ms.key('x', self.next_node_idx)
# Create a BetweenFactor obj based on CTRV model and add it to the graph
between, motion = create_ctrv_between_factor(last_node_key, new_node_key,
vx, yaw_rate, delta_t,
self.config['ctrv'])
self.graph.add(between)
# Add the new node index into the queue
self._idc_in_graph.append(self.next_node_idx)
# Increment the index of the next node to be added
self.odom_added = True
self.next_node_idx += 1
self.new_node_guessed = False
# Predict uncertainty of the new pose node from last pose and odom.
# This will be used for gating and data association for other types of factors.
# Note since minisam always considers uncertainty wrt local frames, theta used for
# computing F is 0 as the last pose has the coordinate (x=0, y=0, theta=0) wrt its own frame.
F = compute_F(0., vx, yaw_rate, delta_t)
motion_uncert = motion[3]
self.pred_cov = F @ self.last_optimized_cov @ F.T + motion_uncert
# Add CTRV based initial guess for the new pose node
if add_init_guess:
# Predict pose a priori as initial guess using CTRV model
last_trans = self.last_optimized_se2.translation()
last_x, last_y = last_trans[0], last_trans[1]
last_theta = self.last_optimized_se2.so2().theta()
# The delta motion is wrt the local frame of the last pose, must make it wrt the global frame
delta_x, delta_y, delta_theta = motion[0:3]
rotm = np.array([[cos(last_theta), -sin(last_theta)],
[sin(last_theta), cos(last_theta)]])
delta = rotm @ np.array([delta_x, delta_y])
delta_x_global = delta[0]
delta_y_global = delta[1]
# Predict pose a priori
prior_x = last_x + delta_x_global
prior_y = last_y + delta_y_global
prior_theta = last_theta + delta_theta
# Use prior as initial guess
guessed_pose = sophus.SE2(sophus.SO2(
prior_theta), np.array([prior_x, prior_y]))
self.initials.add(new_node_key, guessed_pose)
self.new_node_guessed = True
def get_result(self, idx):
"""Get optimied SE2 pose result of the node with the specified index."""
return copy_se2(self.optimized_results.at(ms.key('x', idx)))
def get_marignal_cov_matrix(self, idx):
"""Get marginal covariance matrix of the node with the specified index.."""
return self.mcov_solver.marginalCovariance(ms.key('x', idx))
def getGraphNodePose(graph, idx):
pose = graph.at(minisam.key('x', idx))
pose_trans = pose.translation()
pose_rot = pose.so3().matrix()
return pose_trans, pose_rot
# error = y - exp(m * x + c);
def error(self, variables):
params = variables.at(self.keys()[0])
return np.array(
[self.p_[1] - np.exp(params[0] * self.p_[0] + params[1])])
data = loadFromFile(
"/home/varun/minisam/examples/data/exp_curve_fitting_data.txt")
loss = minisam.CauchyLoss.Cauchy(1.0)
#loss=None
graph = minisam.FactorGraph()
for pair in data:
graph.add(ExpCurveFittingFactor(minisam.key('p', 0), pair, loss))
init_values = minisam.Variables()
init_values.add(minisam.key('p', 0), np.array([0, 0]))
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()
status = opt.optimize(graph, init_values, values)
print("opitmized curve parameters :", values.at(minisam.key('p', 0)))
