How to use the gtsam.PriorFactorPose2 function in gtsam

def initialize(self, priorMean=[0,0,0], priorCov=[0,0,0]):
        # init the prior
        priorMean = gtsam.Pose2(priorMean[0], priorMean[1], priorMean[2])
        priorCov = gtsam.noiseModel_Diagonal.Sigmas(np.array(priorCov))
        self.graph.add(gtsam.PriorFactorPose2(X(self.currentKey), priorMean, priorCov))
        self.initialValues.insert(X(self.currentKey), priorMean)

        return

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
graphWithPrior = graph
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graphWithPrior.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(graphWithPrior, 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:

def vector3(x, y, z):
    """Create 3d double numpy array."""
    return np.array([x, y, z], dtype=np.float)


# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))

# 1. Create a factor graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()

# 2a. Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))

# 2b. Add odometry factors
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
    2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
    3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
    4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))

# 2c. Add the loop closure constraint
# This factor encodes the fact that we have returned to the same pose. In real
# systems, these constraints may be identified in many ways, such as appearance-based
# techniques with camera images. We will use another Between Factor to enforce this constraint:
graph.add(gtsam.BetweenFactorPose2(

PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))

# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()

# Create the keys corresponding to unknown variables in the factor graph
X1 = gtsam.symbol(ord('x'), 1)
X2 = gtsam.symbol(ord('x'), 2)
X3 = gtsam.symbol(ord('x'), 3)
L1 = gtsam.symbol(ord('l'), 4)
L2 = gtsam.symbol(ord('l'), 5)

# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))

# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(gtsam.BetweenFactorPose2(
    X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
    X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))

# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(gtsam.BearingRangeFactor2D(
    X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
    X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
    X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))

# Print graph

# Define a batch fixed lag smoother, which uses
    # Levenberg-Marquardt to perform the nonlinear optimization
    lag = 2.0
    smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)

    # Create containers to store the factors and linearization points
    # that will be sent to the smoothers
    new_factors = gtsam.NonlinearFactorGraph()
    new_values = gtsam.Values()
    new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()

    # Create  a prior on the first pose, placing it at the origin
    prior_mean = gtsam.Pose2(0, 0, 0)
    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
    X1 = 0
    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
    new_values.insert(X1, prior_mean)
    new_timestamps.insert(_timestamp_key_value(X1, 0.0))

    delta_time = 0.25
    time = 0.25

    while time <= 3.0:
        previous_key = 1000 * (time - delta_time)
        current_key = 1000 * time

        # assign current key to the current timestamp
        new_timestamps.insert(_timestamp_key_value(current_key, time))

        # Add a guess for this pose to the new values
        # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
        current_pose = gtsam.Pose2(time * 2, 0, 0)

#!/usr/bin/env python
from __future__ import print_function
import gtsam
import numpy as np

# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()

# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))

# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
# For simplicity, we will use the same noise model for each odometry factor
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
graph.print("\nFactor Graph:\n")

# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))

import gtsam

import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot

# Create noise models
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))

# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()

# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))

# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
# For simplicity, we will use the same noise model for each odometry factor
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))

# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
print("\nInitial Estimate:\n{}".format(initial))