Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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
graph.add(smartfactor)
# 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))
graph.add(PriorFactorPose3(0, poses[0], pose_noise))
# 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):
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)
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()
graph.add(PriorFactorPose2(1, prior_mean, prior_noise))
# 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)
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():
graph.add(item)
# 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))
graph.add(PriorFactorPose3(0, poses[0], pose_noise))
# 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):
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()
graph.add(PriorFactorPose2(1, prior_mean, prior_noise))
# 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))
# - 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()
graph.add(NonlinearEqualityPose3(x1, first_pose))
# 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))
## Add measurements
# pose 1
graph.add(GenericStereoFactor3D(
StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
graph.add(GenericStereoFactor3D(
StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
graph.add(GenericStereoFactor3D(
StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
# pose 2
graph.add(GenericStereoFactor3D(
StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
graph.add(GenericStereoFactor3D(
StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K))
graph.add(GenericStereoFactor3D(
# 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
graph.add(
BearingRangeFactorPose2Point2(x1, l1,
bearing11, range11, measurement_noise))
graph.add(
BearingRangeFactorPose2Point2(x2, l1,
bearing21, range21, measurement_noise))