def run(args: Namespace) -> None:
"""Run LAGO on input data stored in g2o file."""
g2oFile = gtsam.findExampleDataFile(
"noisyToyGraph.txt") if args.input is None else args.input
graph = gtsam.NonlinearFactorGraph()
graph, initial = gtsam.readG2o(g2oFile)
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8))
graph.add(PriorFactorPose2(0, Pose2(), priorModel))
print(graph)
print("Computing LAGO estimate")
estimateLago: Values = gtsam.lago.initialize(graph)
print("done!")
if args.output is None:
estimateLago.print("estimateLago")
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel = gtsam.NonlinearFactorGraph()
graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
gtsam.writeG2o(graphNoKernel, estimateLago, outputFile)
print("Done! ")
def gtsamOpt(inputfname, outputfname):
graph, initial = gtsam.readG2o(inputfname, True)
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(
gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
print("Writing results to file: ", outputfname)
graphNoKernel, _ = gtsam.readG2o(inputfname, True)
gtsam.writeG2o(graphNoKernel, result, outputfname)
print("Done!")
def main():
"""Main runner."""
parser = argparse.ArgumentParser(
description="A 3D Pose SLAM example that reads input from g2o, and "
"initializes Pose3")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument(
'-o',
'--output',
help="the path to the output file with optimized graph")
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
else args.input
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel.Diagonal.Variances(
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print("Done!")
if args.plot:
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()
def run(args):
"""Run Shonan averaging and then recover translations linearly before saving result."""
# Get input file
if args.input_file:
input_file = args.input_file
else:
if args.named_dataset == "":
raise ValueError(
"You must either specify a named dataset or an input file")
input_file = gtsam.findExampleDataFile(args.named_dataset)
if args.dimension == 2:
print("Running Shonan averaging for SO(2) on ", input_file)
shonan = gtsam.ShonanAveraging2(input_file)
if shonan.nrUnknowns() == 0:
raise ValueError("No 2D pose constraints found, try -d 3.")
initial = shonan.initializeRandomly()
rotations, _ = shonan.run(initial, 2, 10)
factors = gtsam.parse2DFactors(input_file)
elif args.dimension == 3:
print("Running Shonan averaging for SO(3) on ", input_file)
shonan = gtsam.ShonanAveraging3(input_file)
if shonan.nrUnknowns() == 0:
raise ValueError("No 3D pose constraints found, try -d 2.")
initial = shonan.initializeRandomly()
rotations, _ = shonan.run(initial, 3, 10)
factors = gtsam.parse3DFactors(input_file)
else:
raise ValueError("Can only run SO(2) or SO(3) averaging")
print("Recovering translations")
poses = estimate_poses_given_rot(factors, rotations, args.dimension)
print("Writing result to ", args.output_file)
gtsam.writeG2o(gtsam.NonlinearFactorGraph(), poses, args.output_file)
plot.plot_trajectory(1, poses, scale=0.2)
plot.set_axes_equal(1)
plt.show()
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()
problem._graph = input_graph
from gtsam import writeG2o
def vector6(x, y, z, a, b, c):
"""Create 6d double numpy array."""
return np.array([x, y, z, a, b, c], dtype=np.float)
poses = gtsam.Values()
graph = gtsam.NonlinearFactorGraph()
keys = problem._poses.keys()
import utils
for i in range(keys.size()):
rand = utils.random_omega(1)
randr = utils.random_omega(1)
original: gtsam.Pose3 = input_poses.atPose3(keys.at(i))
# poses.insert(keys.at(i), gtsam.Pose3(gtsam.Rot3.RzRyRx(randr[0], randr[1],randr[2]), gtsam.Point3(rand[0], rand[1], rand[2])))
poses.insert(keys.at(i), original.retract(np.concatenate([rand, randr])))
for i in range(problem._graph.size()):
factor = gtsam.dynamic_cast_BetweenFactorPose3_NonlinearFactor(
problem._graph.at(i))
model = gtsam.noiseModel_Isotropic.Sigma(6, 1.0)
graph.add(
gtsam.BetweenFactorPose3(factor.keys().at(0),
factor.keys().at(1), factor.measured(),
model))
writeG2o(graph, poses, g2oFile)
def main():
"""Main runner."""
parser = argparse.ArgumentParser(
description="A 2D Pose SLAM example that reads input from g2o, "
"converts it to a factor graph and does the optimization. "
"Output is written on a file, in g2o format")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument(
'-o',
'--output',
help="the path to the output file with optimized graph")
parser.add_argument('-m',
'--maxiter',
type=int,
help="maximum number of iterations for optimizer")
parser.add_argument('-k',
'--kernel',
choices=['none', 'huber', 'tukey'],
default="none",
help="Type of kernel used")
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
else args.input
maxIterations = 100 if args.maxiter is None else args.maxiter
is3D = False
graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel.Diagonal.Variances(
gtsam.Point3(1e-6, 1e-6, 1e-8))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination")
params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
# ... and optimize
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("\nFactor Graph:\n{}".format(graph))
print("\nInitial Estimate:\n{}".format(initial))
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print("Done!")
if args.plot:
resultPoses = gtsam.utilities.extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()
def hook(optimizer, error: float):
writer.add_scalar("optimizer/loss", error, optimizer.iterations())
writer.add_scalar("optimizer/lambda", optimizer.lambda_(),
optimizer.iterations())
writer.flush()
gtsam_optimize(optimizer, params, hook)
result = optimizer.values()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
# graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graph, result, outputFile)
print("Done!")
if args.plot:
resultPoses = gtsam.utilities_allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()