def __init__(self):
super(PoseGraphOptimization, self).__init__()
solver = g2o.BlockSolverSE2(g2o.LinearSolverCholmodSE2())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
super(PoseGraphOptimization, self).set_algorithm(solver)
self.first_pose_vertex_id = -1 # used to create pose-pose and pose-landmark edges
self.last_pose_vertex_id = -1 # used to create pose-pose and pose-landmark edges
self.landmark_vertex_id = -1 # single landmark vertex for this assignment
self.next_vertex_id = 0 # generic vertex id incrementer, used to generate ids for both pose and landmark vertices
def main():
#solver = g2o.BlockSolverX(g2o.LinearSolverCholmodX())
# solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.BlockSolverSE2(g2o.LinearSolverCholmodSE2())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer = g2o.SparseOptimizer()
optimizer.set_verbose(True)
optimizer.set_algorithm(solver)
optimizer.load(args.input)
print('num vertices:', len(optimizer.vertices()))
print('num edges:', len(optimizer.edges()), end='\n\n')
optimizer.initialize_optimization()
optimizer.optimize(args.max_iterations)
if len(args.output) > 0:
optimizer.save(args.output)
def __init__(self):
super(PoseGraphOptimization, self).__init__()
# solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
solver = g2o.BlockSolverSE2(g2o.LinearSolverCholmodSE2())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
super(PoseGraphOptimization, self).set_algorithm(solver)
def __init__(self):
self.optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE2(g2o.LinearSolverCholmodSE2())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
self.optimizer.set_algorithm(solver)