# How to use the gtsam.Pose2 function in gtsam

## To help you get started, we’ve selected a few gtsam examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

borglab / gtsam / python / gtsam_examples / OdometryExample.py View on Github
``````# 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.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))
initial.print("\nInitial Estimate:\n")

# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
result.print("\nFinal Result:\n")``````
borglab / gtsam / cython / gtsam / examples / PlanarManipulatorExample.py View on Github
``````def fk(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)``````
borglab / gtsam / cython / gtsam / examples / PlanarManipulatorExample.py View on Github
``````def fk(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)``````
borglab / gtsam / cython / gtsam / examples / PlanarManipulatorExample.py View on Github
``````def plot_line(p, g, color):
t = g.translation()
q = np.array([t.x(), t.y()])
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], color)
return q

l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
p2 = plot_line(p1, sTl2, 'r-')
gtsam_plot.plot_pose2_on_axes(axes, sTl2)

l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
p3 = plot_line(p2, sTl3, 'g-')
gtsam_plot.plot_pose2_on_axes(axes, sTl3)

l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
sTt = compose(sTl3, l3Zl3, l3Xt)
plot_line(p3, sTt, 'b-')
gtsam_plot.plot_pose2_on_axes(axes, sTt)``````
borglab / gtsam / cython / gtsam / examples / Pose2SLAMExample.py View on Github
``````# 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:
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))  # print

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

# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
# The optimizer accepts an optional set of configuration parameters,
# controlling things like convergence criteria, the type of linear
# system solver to use, and the amount of information displayed during
# optimization. We will set a few parameters as a demonstration.
parameters = gtsam.GaussNewtonParams()

# Stop iterating once the change in error between steps is less than this value
parameters.setRelativeErrorTol(1e-5)
# Do not perform more than N iteration steps
parameters.setMaxIterations(100)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)``````
borglab / gtsam / cython / gtsam_unstable / examples / FixedLagSmootherExample.py View on Github
``````# 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)
new_values.insert(current_key, current_pose)

# Add odometry factors from two different sources with different error
# stats
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.1, 0.1, 0.05]))
new_factors.push_back(gtsam.BetweenFactorPose2(
previous_key, current_key, odometry_measurement_1, odometry_noise_1
))

odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05]))
new_factors.push_back(gtsam.BetweenFactorPose2(
previous_key, current_key, odometry_measurement_2, odometry_noise_2
))

# Update the smoothers with the new factors. In this case,
# one iteration must pass for Levenberg-Marquardt to accurately
# estimate
if time >= 0.50:
smoother_batch.update(new_factors, new_values, new_timestamps)
print("Timestamp = " + str(time) + ", Key = " + str(current_key))
print(smoother_batch.calculateEstimatePose2(current_key))

new_timestamps.clear()
new_values.clear()``````
PoseNet-Mobile-Robot / Mobile-Robotics / gtsamSolver.py View on Github
``````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.initialValues.insert(X(self.currentKey), priorMean)

return``````
borglab / gtsam / cython / gtsam / examples / PlanarManipulatorExample.py View on Github
``````def fk(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)``````
borglab / gtsam / cython / gtsam / examples / Pose2SLAMExample.py View on Github
``````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:
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))  # print

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

# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
# The optimizer accepts an optional set of configuration parameters,
# controlling things like convergence criteria, the type of linear
# system solver to use, and the amount of information displayed during
# optimization. We will set a few parameters as a demonstration.
parameters = gtsam.GaussNewtonParams()

# Stop iterating once the change in error between steps is less than this value
parameters.setRelativeErrorTol(1e-5)
# Do not perform more than N iteration steps
parameters.setMaxIterations(100)``````
borglab / gtsam / cython / gtsam / examples / PlanarManipulatorExample.py View on Github
``````def trajectory(g0, g1, N=20):
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
g0 and g1 are the initial and final pose, respectively.
N is the number of *intervals*
Returns N+1 poses
"""
e = delta(g0, g1)
return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]``````

## gtsam

Georgia Tech Smoothing And Mapping library

BSD-2-Clause

74 / 100