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)