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 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 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 gtsamOpt2PoseStampedarray(inputfname, pose_array):
pointID = 0
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))
resultPoses = gtsam.allPose3s(result)
keys = resultPoses.keys()
for i in range(keys.size()):
posetmp = gstamPose2Transform(resultPoses.atPose3(keys.at(i)))
posestampedtmp = PoseStamped()
posestampedtmp.header.frame_id = str(keys.at(i))
posestampedtmp.pose = posetmp
pose_array.poseArray.append(posestampedtmp)
def test_initializePoses(self):
g2oFile = gtsam.findExampleDataFile("pose3example-grid")
is3D = True
inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
priorModel = gtsam.noiseModel.Unit.Create(6)
inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
initial = gtsam.InitializePose3.initialize(inputGraph)
# TODO(frank): very loose !!
self.gtsamAssertEquals(initial, expectedValues, 0.1)
def test_initialize(self) -> None:
"""Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file."""
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt")
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))
estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values)
def __read_lidar_pose_priors(self) -> Dict[int, Pose3]:
"""Read the poses for the IMU for rig indices."""
filepath = str(self._base_folder / LIDAR_POSE_RELATIVE_PATH)
_, values = gtsam.readG2o(filepath, is3D=True)
lidar_keys = values.keys()
logger.info("Number of keys in g2o file: %d", len(lidar_keys))
w_T_imu: Dict[int, Pose3] = {}
for rig_idx in range(self.num_rig_poses):
if rig_idx in lidar_keys:
w_T_imu[rig_idx] = values.atPose3(rig_idx)
return w_T_imu
def main():
"""Main runner."""
# Read graph from file
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add prior on the first key. TODO: assumes first key ios z
priorModel = gtsam.noiseModel.Diagonal.Variances(
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
# Initializing Pose3 - chordal relaxation
initialization = gtsam.InitializePose3.initialize(graph)
print(initialization)
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")
required=True)
args = parser.parse_args()
g2oFile = args.output
inputFile = args.input
import sys, os
sys.path.append(os.path.join(sys.path[0], 'shonan', 'python'))
from random_pose_slam_problem import RandomPoseSLAMProblem
from sfm_problem import SFMProblem
# problem = RandomPoseSLAMProblem(nr_poses=1000, nr_measurements=1000, radius=10.0, sigma=0.8, model_sigma=1.0)
problem = lambda: None
input_graph, input_poses = gtsam.readG2o(inputFile, True)
problem._poses = input_poses
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
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()
help="the path to the output file with optimized graph")
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
parser.add_argument('-l',
'--logdir',
help='where to store tensorboard log',
required=True)
parser.add_argument('-e', '--prefix', help='log name prefix', required=True)
args = parser.parse_args()
g2oFile = args.input
is3D = True
graph_in, initial = gtsam.readG2o(g2oFile, is3D)
graph = gtsam.NonlinearFactorGraph()
for i in range(graph_in.size()):
factor = gtsam.dynamic_cast_BetweenFactorPose3_NonlinearFactor(
graph_in.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))
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1, 1, 1, 1, 1, 1))
print("Adding prior to g2o file ")
def test_readG2o3D(self):
"""Test reading directly into factor graph."""
is3D = True
graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
self.assertEqual(graph.size(), 6)
self.assertEqual(initial.size(), 5)
