# How to use the gtsam.vec 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.

spillai / pybot / deprecated / mapping / test_gtsam.py View on Github
``````measurement = camera.project(pointj)
print measurement

# call add() function to add measurement into a single factor, here we need to add:
#    1. the 2D measurement
#    2. the corresponding camera's key
#    3. camera noise model
#    4. camera calibration
smartfactor.add_single(measurement, i, measurement_noise, K)

# insert the smart factor in the graph

# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
pose_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.3, 0.1, 0.1, 0.1))

# Because the structure-from-motion problem has a scale ambiguity, the problem is
# still under-constrained. Here we add a prior on the second pose x1, so this will
# fix the scale by indicating the distance between x0 and x1.
# Because these two are fixed, the rest of the poses will be also be fixed.
graph.add(PriorFactorPose3(1, poses[1], pose_noise)) # add directly to graph

graph.printf("Factor Graph:\n")

# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = Values()
delta = Pose3(Rot3.rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))

for i, posei in enumerate(poses):``````
spillai / pybot / deprecated / mapping / test_gtsam.py View on Github
``````def test_PlanarSLAMExample():
print("test_PlanarSLAMExample\n")
print("=================================")

# Create a factor graph
graph = NonlinearFactorGraph()

# Create the keys we need for this simple example
x1, x2, x3 = symbol('x', 1), symbol('x', 2), symbol('x', 3)
l1, l2 = symbol('l', 1), symbol('l', 2)

# Add a prior on pose x1 at the origin. A prior factor consists of a mean
# and a noise model (covariance matrix)
prior = Pose2(0.0, 0.0, 0.0) #  prior mean is at origin
prior_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True) #  30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactorPose2(x1, prior, prior_noise)) #  add directly to graph

# Add two odometry factors
odometry = Pose2(2.0, 0.0, 0.0) #  create a measurement for both factors (the same in this case)
odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True) #  20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(x1, x2, odometry, odometry_noise))
graph.add(BetweenFactorPose2(x2, x3, odometry, odometry_noise))

# Add Range-Bearing measurements to two different landmarks
# create a noise model for the landmark measurements
measurement_noise = Diagonal.Sigmas(vec(0.1, 0.2), True) #  0.1 rad std on bearing, 20cm on range

# create the measurement values - indices are (pose id, landmark id)
bearing11 = Rot2.fromDegrees(45)
bearing21 = Rot2.fromDegrees(90)
bearing32 = Rot2.fromDegrees(90)``````
spillai / pybot / deprecated / mapping / test_gtsam.py View on Github
``````def test_odometryExample():
print("test_odmetryExample\n")
print("=================================")

# 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)
prior_mean = Pose2(0, 0, 0)
prior_noise  = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True)

graph = NonlinearFactorGraph()

# Add odometry factors
odometry = Pose2(2., 0., 0.)

# For simplicity, we will use the same noise model for each odometry factor
odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True)

# Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactorPose2(1, 2, odometry, odometry_noise))
graph.add(BetweenFactorPose2(2, 3, odometry, odometry_noise))

block_str = '================'
graph.printf(s="%s\nFactor Graph:\n" % block_str)``````
spillai / pybot / deprecated / mapping / test_smartfactors.py View on Github
``````for i, posei in enumerate(poses):
# generate the 2D measurement
camera = SimpleCamera(posei, K)
for j, pointj in enumerate(points):
measurement = camera.project(pointj)
print measurement

sfactors[j].add_single(measurement, i, measurement_noise, K)

# insert the smart factor in the graph
for item in sfactors.itervalues():

# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
pose_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.3, 0.1, 0.1, 0.1))

# Because the structure-from-motion problem has a scale ambiguity, the problem is
# still under-constrained. Here we add a prior on the second pose x1, so this will
# fix the scale by indicating the distance between x0 and x1.
# Because these two are fixed, the rest of the poses will be also be fixed.
graph.add(PriorFactorPose3(1, poses[1], pose_noise)) # add directly to graph

graph.printf("Factor Graph:\n")

# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = Values()
delta = Pose3(Rot3.rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))

for i, posei in enumerate(poses):``````
spillai / pybot / deprecated / mapping / test_gtsam.py View on Github
``````print("test_odmetryExample\n")
print("=================================")

# 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)
prior_mean = Pose2(0, 0, 0)
prior_noise  = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True)

graph = NonlinearFactorGraph()

# Add odometry factors
odometry = Pose2(2., 0., 0.)

# For simplicity, we will use the same noise model for each odometry factor
odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True)

# Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactorPose2(1, 2, odometry, odometry_noise))
graph.add(BetweenFactorPose2(2, 3, odometry, odometry_noise))

block_str = '================'
graph.printf(s="%s\nFactor Graph:\n" % block_str)

# Create the data structure to hold the initialEstimate estimate to the
# solution For illustrative purposes, these have been deliberately set to
# incorrect values
initial = Values()
initial.insert(1, Pose2(0.5, 0.0, 0.2))
initial.insert(2, Pose2(2.3, 0.1, -0.2))
initial.insert(3, Pose2(4.1, 0.1, 0.1))``````
spillai / pybot / deprecated / mapping / test_gtsam.py View on Github
``````#  - x1 is fixed with a constraint, x2 is initialized with noisy values
#  - No noise on measurements
x1, x2 = symbol('x', 1), symbol('x', 2)
l1, l2, l3 = symbol('l', 1), symbol('l', 2), symbol('l', 3)

# Create a factor graph
graph = NonlinearFactorGraph()

# add a constraint on the starting pose
first_pose = Pose3()

# Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
stereo_model = Diagonal.Sigmas(vec(1.0, 1.0, 1.0))

# pose 1
StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
StereoPoint2(120,  80, 440), stereo_model, x1, l2, K))
StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))

# pose 2
StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K))
spillai / pybot / deprecated / mapping / test_gtsam.py View on Github
``````# Add a prior on pose x1 at the origin. A prior factor consists of a mean
# and a noise model (covariance matrix)
prior = Pose2(0.0, 0.0, 0.0) #  prior mean is at origin
prior_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True) #  30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactorPose2(x1, prior, prior_noise)) #  add directly to graph

# Add two odometry factors
odometry = Pose2(2.0, 0.0, 0.0) #  create a measurement for both factors (the same in this case)
odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True) #  20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(x1, x2, odometry, odometry_noise))
graph.add(BetweenFactorPose2(x2, x3, odometry, odometry_noise))

# Add Range-Bearing measurements to two different landmarks
# create a noise model for the landmark measurements
measurement_noise = Diagonal.Sigmas(vec(0.1, 0.2), True) #  0.1 rad std on bearing, 20cm on range

# create the measurement values - indices are (pose id, landmark id)
bearing11 = Rot2.fromDegrees(45)
bearing21 = Rot2.fromDegrees(90)
bearing32 = Rot2.fromDegrees(90)
range11 = np.sqrt(4.0+4.0)
range21 = 2.0
range32 = 2.0

# Add Bearing-Range factors
BearingRangeFactorPose2Point2(x1, l1,
bearing11, range11, measurement_noise))
BearingRangeFactorPose2Point2(x2, l1,
bearing21, range21, measurement_noise))``````

## gtsam

Georgia Tech Smoothing And Mapping library

BSD-2-Clause