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