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