def test_TriangulationExample(self):
""" Tests triangulation with shared Cal3_S2 calibration"""
# Some common constants
sharedCal = (1500, 1200, 0, 640, 480)
measurements, _ = self.generate_measurements(Cal3_S2,
PinholeCameraCal3_S2,
(sharedCal, sharedCal))
triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal),
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
# Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements_noisy = Point2Vector()
measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal),
measurements_noisy,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
def test_values(self):
values = Values()
E = EssentialMatrix(Rot3(), Unit3())
tol = 1e-9
values.insert(0, Point2(0, 0))
values.insert(1, Point3(0, 0, 0))
values.insert(2, Rot2())
values.insert(3, Pose2())
values.insert(4, Rot3())
values.insert(5, Pose3())
values.insert(6, Cal3_S2())
values.insert(7, Cal3DS2())
values.insert(8, Cal3Bundler())
values.insert(9, E)
values.insert(10, imuBias_ConstantBias())
# Special cases for Vectors and Matrices
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
# floating point numbers in column-major (Fortran style) storage order,
# whereas by default, numpy.array is in row-major order and the type is
# in whatever the number input type is, e.g. np.array([1,2,3])
# will have 'int' type.
#
# The wrapper will automatically fix the type and storage order for you,
# but for performance reasons, it's recommended to specify the correct
# type and storage order.
# for vectors, the order is not important, but dtype still is
vec = np.array([1., 2., 3.])
values.insert(11, vec)
mat = np.array([[1., 2.], [3., 4.]], order='F')
values.insert(12, mat)
# Test with dtype int and the default order='C'
# This still works as the wrapper converts to the correct type and order for you
# but is nornally not recommended!
mat2 = np.array([[1, 2, ], [3, 5]])
values.insert(13, mat2)
self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol))
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
self.assertTrue(values.atimuBias_ConstantBias(
10).equals(imuBias_ConstantBias(), tol))
# special cases for Vector and Matrix:
actualVector = values.atVector(11)
self.assertTrue(np.allclose(vec, actualVector, tol))
actualMatrix = values.atMatrix(12)
self.assertTrue(np.allclose(mat, actualMatrix, tol))
actualMatrix2 = values.atMatrix(13)
self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
def run():
"""Execution."""
# Input images(undistorted) calibration
fov, w, h = 60, 1280, 720
calibration = Cal3_S2(fov, w, h)
# Camera to world rotation
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
# Create pose estimates
pose_estimates = [Pose3(wRc, Point3(0, i, 2)) for i in range(3)]
# Create measurement noise for bundle adjustment
sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# Create pose prior noise
rotation_sigma = np.radians(60)
translation_sigma = 1
pose_noise_sigmas = np.array([
rotation_sigma, rotation_sigma, rotation_sigma, translation_sigma,
translation_sigma, translation_sigma
])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
# Create MappingBackEnd instance
data_directory = 'mapping/datasets/sim_match_data/'
num_images = 3
min_landmark_seen = 3
back_end = MappingBackEnd(data_directory, num_images, calibration,
pose_estimates, measurement_noise,
pose_prior_noise, min_landmark_seen)
# Bundle Adjustment
tic_ba = time.time()
sfm_result, poses, points = back_end.bundle_adjustment()
toc_ba = time.time()
print('BA spents ', toc_ba - tic_ba, 's')
# Plot Result
plot_sfm_result(sfm_result, poses, points)
def setUp(self):
"""Create mapping Back-end and read csv file."""
# Input images(undistorted) calibration
fov, w, h = 60, 1280, 720
calibration = Cal3_S2(fov, w, h)
# Create pose priors
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # camera to world rotation
pose_estimates = [Pose3(wRc, Point3(0, i, 2)) for i in range(3)]
# Create measurement noise for bundle adjustment
sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# Create pose prior noise
rotation_sigma = np.radians(60)
translation_sigma = 1
pose_noise_sigmas = np.array([
rotation_sigma, rotation_sigma, rotation_sigma, translation_sigma,
translation_sigma, translation_sigma
])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
# Create MappingBackEnd instance
data_directory = 'mapping/sim_match_data/'
min_landmark_seen = 3
self.num_images = 3
self.back_end = MappingBackEnd(
data_directory, self.num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, min_landmark_seen) # pylint: disable=line-too-long
def test_back_projection(self):
"""Test back projection"""
fov, width, height = 60, 1280, 720
calibration = Cal3_S2(fov, width, height)
actual = back_projection(
calibration, Point2(640, 360),
Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3()), 20)
expected = Point3(0, 20, 0)
self.gtsamAssertEquals(actual, expected)
fov, width, height = 60, 640, 480
calibration = Cal3_S2(fov, width, height)
actual = back_projection(
calibration, Point2(320, 240),
Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3()), 20)
expected = Point3(0, 20, 0)
self.gtsamAssertEquals(actual, expected)
def __init__(self, triangle: bool = False, nrCameras: int = 3, K=Cal3_S2()) -> None:
"""
Options to generate test scenario
@param triangle: generate a triangle scene with 3 points if True, otherwise
a cube with 8 points
@param nrCameras: number of cameras to generate
@param K: camera calibration object
"""
self.triangle = triangle
self.nrCameras = nrCameras
def run():
"""Execution"""
basedir = 'calibration/undistort_images/distort_images/'
img_extension = '.jpg'
output_dir = 'calibration/undistort_images/undistort_images/'
output_prefix = 'undist_image'
calibration = Cal3_S2(fx=347.820593,
fy=329.096945,
s=0,
u0=295.717950,
v0=222.964889).matrix()
distortion = np.array([-0.284322, 0.055723, 0.006772, 0.005264, 0.000000])
undist_calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912).matrix()
undistort(basedir, img_extension, output_dir, output_prefix, calibration,
distortion, undist_calibration)
def run():
"""Execution."""
# Input images(undistorted) calibration
calibration = Cal3_S2(
fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912)
# Camera to world rotation
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
# Create pose estimates
pose_estimates = [Pose3(wRc, Point3(1.58*i, 0, 1.2)) for i in range(6)]
# Create measurement noise for bundle adjustment
sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# Create pose prior noise
rotation_sigma = np.radians(60)
translation_sigma = 1
pose_noise_sigmas = np.array([rotation_sigma, rotation_sigma, rotation_sigma,
translation_sigma, translation_sigma, translation_sigma])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
#"""Use 4D agri matching"""
data_directory = 'mapping/datasets/Klaus_4d_agri_match_data/'
num_images = 6
min_obersvation_number = 6
filter_bad_landmarks_enable = True
prob = 0.9
threshold = 3
backprojection_depth = 15
back_end = MappingBackEnd(data_directory, num_images, calibration,
pose_estimates, measurement_noise, pose_prior_noise, filter_bad_landmarks_enable, min_obersvation_number, prob, threshold, backprojection_depth)
# Bundle Adjustment
tic_ba = time.time()
sfm_result1 = back_end.bundle_adjustment()
toc_ba = time.time()
print('BA spents ', toc_ba-tic_ba, 's')
# Save map data
back_end.save_map_to_file(sfm_result1)
back_end.save_poses_to_file(sfm_result1)
#"""Use Superpoint matching"""
# Create MappingBackEnd instance
data_directory = 'mapping/datasets/Klaus_Superpoint_match_data/'
# data_directory = 'feature_matcher/Klaus_filter_match_data/'
num_images = 6
min_obersvation_number = 3
back_end = MappingBackEnd(data_directory, num_images, calibration,
pose_estimates, measurement_noise, pose_prior_noise, min_obersvation_number)
# Bundle Adjustment
tic_ba = time.time()
sfm_result2 = back_end.bundle_adjustment()
toc_ba = time.time()
print('BA spents ', toc_ba-tic_ba, 's')
# # Plot Result
plot_with_results(sfm_result1, sfm_result2, 30, 30, 30)
def run():
"""Execution."""
# Input images(undistorted) calibration
calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912)
# Create pose estimates
theta = 45
delta_x = 1
delta_y = -0.5
delta_z = 1.2
rows = 2
cols = 2
angles = 8
prior1_delta = [0, -1, 1.2, 0]
prior2_delta = [1, -1, 1.2, 0]
pose_estimates = pose_estimate_generator(theta, delta_x, delta_y, delta_z,
prior1_delta, prior2_delta, rows,
cols, angles)
# Create measurement noise for bundle adjustment
sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# Create pose prior noise
rotation_sigma = np.radians(60)
translation_sigma = 1
pose_noise_sigmas = np.array([
rotation_sigma, rotation_sigma, rotation_sigma, translation_sigma,
translation_sigma, translation_sigma
])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
# Create MappingBackEnd instance
data_directory = 'mapping/datasets/library_data/library_4X8/undistort_images/features/'
num_images = 34
filter_bad_landmarks_enable = True
min_obersvation_number = 3
prob = 0.9
threshold = 3
backprojection_depth = 2
back_end = MappingBackEnd(data_directory, num_images, calibration,
pose_estimates, measurement_noise,
pose_prior_noise, filter_bad_landmarks_enable,
min_obersvation_number, prob, threshold,
backprojection_depth)
# Bundle Adjustment
tic_ba = time.time()
sfm_result = back_end.bundle_adjustment()
toc_ba = time.time()
print('BA spents ', toc_ba - tic_ba, 's')
# Plot Result
plot_with_result(sfm_result, 5, 5, 5, 0.5)
def setUp(self):
# Camera to world rotation
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
initial_pose = Pose3(wRc, Point3(0, 0, 1.2))
file_name = ""
self.calibration = Cal3_S2(fx=1, fy=1, s=0, u0=320, v0=240)
self.image_size = (640, 480)
l2_thresh = 0.7
distance_thresh = [5, 5]
self.trajectory_estimator = TrajectoryEstimator(
initial_pose, file_name, self.calibration, self.image_size,
l2_thresh, distance_thresh)
def setUp(self):
image_directory_path = 'superpoint_descriptor/undistort_images/'
image_extension = '*.jpg'
image_size = (640, 480)
nn_thresh = 0.7
# Input images(undistorted) calibration
self.calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912).matrix()
self.front_end = SuperpointWrapper(image_directory_path,
image_extension, image_size,
nn_thresh)
def run():
"""Execution."""
# Camera to world rotation
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
initial_pose = Pose3(wRc, Point3(1.58, 0, 1.2))
directory_name = "localization/data_sets/Klaus_1X6_Localization/"
calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912)
image_size = (640, 480)
l2_thresh = 0.7
distance_thresh = [5, 5]
trajectory_estimator = TrajectoryEstimator(initial_pose, directory_name,
calibration, image_size,
l2_thresh, distance_thresh)
camid = 1
skip = 1
start_index = 0
img_glob = "*.jpg"
distort_calibration = Cal3_S2(fx=347.820593,
fy=329.096945,
s=0,
u0=295.717950,
v0=222.964889).matrix()
distortion = np.array([-0.284322, 0.055723, 0.006772, 0.005264, 0.000000])
trajectory = trajectory_estimator.trajectory_generator(
distort_calibration, distortion, directory_name, camid, skip, img_glob,
start_index)
actual_poses = load_poses_from_file(directory_name + "poses.dat")
plot_trajectory_verification(trajectory_estimator.map.landmarks,
actual_poses, trajectory)
def test_computation_graph(self):
"""Test the dask computation graph execution using a valid collection of relative unit-translations."""
"""Test a simple case with 8 camera poses.
The camera poses are arranged on the circle and point towards the center
of the circle. The poses of 8 cameras are obtained from SFMdata and the
unit translations directions between some camera pairs are computed from their global translations.
This test is copied from GTSAM's TranslationAveragingExample.
"""
fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
expected_wTi_list = SFMdata.createPoses(Cal3_S2(fx, fy, s, u0, v0))
wRi_list = [x.rotation() for x in expected_wTi_list]
# create relative translation directions between a pose index and the
# next two poses
i2Ui1_dict = {}
for i1 in range(len(expected_wTi_list) - 1):
for i2 in range(i1 + 1, min(len(expected_wTi_list), i1 + 3)):
# create relative translations using global R and T.
i2Ui1_dict[(i1, i2)] = Unit3(expected_wTi_list[i2].between(
expected_wTi_list[i1]).translation())
# use the `run` API to get expected results
expected_wti_list = self.obj.run(len(wRi_list), i2Ui1_dict, wRi_list)
expected_wTi_list = [
Pose3(wRi, wti) if wti is not None else None
for (wRi, wti) in zip(wRi_list, expected_wti_list)
]
# form computation graph and execute
i2Ui1_graph = dask.delayed(i2Ui1_dict)
wRi_graph = dask.delayed(wRi_list)
computation_graph = self.obj.create_computation_graph(
len(wRi_list), i2Ui1_graph, wRi_graph)
with dask.config.set(scheduler="single-threaded"):
wti_list = dask.compute(computation_graph)[0]
wTi_list = [
Pose3(wRi, wti) if wti is not None else None
for (wRi, wti) in zip(wRi_list, wti_list)
]
# compare the entries
self.assertTrue(
geometry_comparisons.compare_global_poses(wTi_list,
expected_wTi_list))
def run():
"""Execution."""
basedir = 'superpoint_descriptor/'
image_extension = '*.jpg'
image_size = (640, 480)
number_images = 6
# Input images(undistorted) calibration
calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912).matrix()
feature_matcher = FeatureMatcher(basedir, image_extension, image_size,
number_images)
feature_matcher.feature_matching(image_size, 'Superpoint', 'FLANN',
calibration)
def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None:
self.K = K
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [Pose3()] * nrCameras
# Set Noise parameters
self.noiseModels = Data.NoiseModels()
self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
# noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
# np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
def generate_data(options) -> Tuple[Data, GroundTruth]:
""" Generate ground-truth and measurement data. """
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
# Generate simulated data
if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10
for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints
truth.points[j] = Point3(
r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),
Point3(-10, -10, 10), Point3(10, -10, 10),
Point3(10, 10, -10), Point3(-10, 10, -10),
Point3(-10, -10, -10), Point3(10, -10, -10)
]
# Create camera cameras on a circle around the triangle
height = 10
r = 40
for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras
t = Point3(r * math.cos(theta), r * math.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
Point3(0, 0, 0),
Point3(0, 0, 1),
truth.K)
# Create measurements
for j in range(nrPoints):
# All landmarks seen in every frame
data.Z[i][j] = truth.cameras[i].project(truth.points[j])
data.J[i][j] = j
# Calculate odometry between cameras
for i in range(1, options.nrCameras):
data.odometry[i] = truth.cameras[i - 1].pose().between(
truth.cameras[i].pose())
return data, truth
def setUp(self):
# Camera to world rotation
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
self.initial_pose = Pose3(wRc, Point3(0, 0, 1.2))
self.directory_name = "localization/data_sets/Klaus_1/"
calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912)
image_size = (640, 480)
l2_thresh = 0.7
distance_thresh = [5, 5]
self.trajectory_estimator = TrajectoryEstimator(
self.initial_pose, self.directory_name, calibration, image_size,
l2_thresh, distance_thresh)
def test_trajectory_generator(self):
"""Test initial pose recover"""
camid = 1
skip = 1
start_index = 0
img_glob = "*.jpg"
distort_calibration = Cal3_S2(fx=347.820593,
fy=329.096945,
s=0,
u0=295.717950,
v0=222.964889).matrix()
distortion = np.array(
[-0.284322, 0.055723, 0.006772, 0.005264, 0.000000])
trajectory = self.trajectory_estimator.trajectory_generator(
distort_calibration, distortion, self.directory_name, camid, skip,
img_glob, start_index)
for pose in trajectory:
print(pose)
actual_poses = load_poses_from_file(self.directory_name + "poses.dat")
plot_trajectory_verification(self.trajectory_estimator.map.landmarks,
actual_poses, trajectory)
self.gtsamAssertEquals(trajectory[-1], self.initial_pose, tol=0.1)
def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None:
self.K = K
self.cameras = [Pose3()] * nrCameras
self.points = [Point3(0, 0, 0)] * nrPoints
"""Unit tests for comparison functions for geometry types.
Authors: Ayush Baid
"""
import unittest
from typing import List
from unittest.mock import patch
import numpy as np
from gtsam import Cal3_S2, Point3, Pose3, Rot3, Similarity3, Unit3
from gtsam.examples import SFMdata
import gtsfm.utils.geometry_comparisons as geometry_comparisons
import tests.data.sample_poses as sample_poses
POSE_LIST = SFMdata.createPoses(Cal3_S2())
ROT3_EULER_ANGLE_ERROR_THRESHOLD = 1e-2
POINT3_RELATIVE_ERROR_THRESH = 1e-1
POINT3_ABS_ERROR_THRESH = 1e-2
def rot3_compare(R: Rot3, R_: Rot3, msg=None) -> bool:
return np.allclose(R.xyz(), R_.xyz(), atol=1e-2)
def point3_compare(t: Point3, t_: Point3, msg=None) -> bool:
return np.allclose(t,
t_,
rtol=POINT3_RELATIVE_ERROR_THRESH,
atol=POINT3_ABS_ERROR_THRESH)
from gtsam import Cal3_S2, PinholeCameraCal3_S2
from gtsam.examples import SFMdata
import gtsfm.utils.overlap_frustums as overlap_frustums_utils
CUBE_SIZE = 4
CUBE_RESOLUTION = 128
# set the dummy image size as 400x300
IMAGE_W = 400
IMAGE_H = 300
# set dummy camera intrinsics
CAMERA_INTRINSICS = Cal3_S2(
fx=100.0,
fy=100.0,
s=0.0,
u0=IMAGE_W // 2,
v0=IMAGE_H // 2,
)
# set dummy camera poses as described in GTSAM example
CAMERA_POSES = SFMdata.createPoses(CAMERA_INTRINSICS)
# set dummy camera instances
CAMERAS = [
PinholeCameraCal3_S2(CAMERA_POSES[i], CAMERA_INTRINSICS)
for i in range(len(CAMERA_POSES))
]
# set dummy sphere grid center
SPHERE_CENTER = np.array([-1.5, -1.5, -1.5])
# set dummy sphere grid radius
SPHERE_RADIUS = 0.2
LineSegment3D(points[8], points[9])
]
semantics = [0.8, 0.8,
0.8, 0.8,
1.0, 1.0,
1.0, 1.0,
1.0, 1.0]
lines_array = []
for i in range(step):
l = np.append(points[i*2], points[i*2 + 1])
lines_array.append(l)
return [points, lines, semantics]
K = Cal3_S2(320., 320., 0.0, 320., 240.)
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3])) # RPY - XYZ
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
pl = createPointsLines()
poses = createPoses(K)
points = pl[0]
sems = pl[2]
# Create a factor graph
graph = NonlinearFactorGraph()
#Add Pose Prior
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
Each variable in the system (poses and landmarks) must be identified with a unique key.
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
Here we will use Symbols
In GTSAM, measurement functions are represented as 'factors'. Several common factors
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
Here we will use Projection factors to model the camera's landmark observations.
Also, we will initialize the robot at some location using a Prior factor.
When the factors are created, we will add them to a Factor Graph. As the factors we are using
are nonlinear factors, we will need a Nonlinear Factor Graph.
Finally, once all of the factors have been added to our factor graph, we will want to
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
to update the linearization point. This happens repeatedly until the solver converges
to a consistent set of variable values. This requires us to specify an initial guess
for each variable, held in a Values container.
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1 * np.random.randn(6, 1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1 * np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
def run_matching(superpoint_wrapper):
"""Create Feature Matching after Feature Extraction."""
# Create matches and save both the information and the images(with matches displayed on the origin images)
calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0,
u0=325.3452, v0=240.2912).matrix()
superpoint_wrapper.get_all_feature_matches(calibration, 1)
# path for data used in this test
DATA_ROOT_PATH = Path(__file__).resolve().parent.parent / "data"
DOOR_TRACKS_PATH = DATA_ROOT_PATH / "tracks2d_door.pickle"
DOOR_DATASET_PATH = DATA_ROOT_PATH / "set1_lund_door"
# focal length set to 50 px, with `px`, `py` set to zero
CALIBRATION = Cal3Bundler(50, 0, 0, 0, 0)
# Generate 8 camera poses arranged in a circle of radius 40 m
CAMERAS = {
i: PinholeCameraCal3Bundler(pose, CALIBRATION)
for i, pose in enumerate(
SFMdata.createPoses(
Cal3_S2(
CALIBRATION.fx(),
CALIBRATION.fx(),
0,
CALIBRATION.px(),
CALIBRATION.py(),
)))
}
LANDMARK_POINT = Point3(0.0, 0.0, 0.0)
MEASUREMENTS = [
SfmMeasurement(i, cam.project(LANDMARK_POINT))
for i, cam in CAMERAS.items()
]
def get_track_with_one_outlier() -> List[SfmMeasurement]:
"""Generates a track with outlier measurement."""
# perturb one measurement
idx_to_perturb = 5
wTi_list: global poses.
pair_indices: pairs (i1, i2) to construct relative poses for.
Returns:
Dictionary (i1, i2) -> i2Ti1 for all requested pairs.
"""
return {(i1, i2): wTi_list[i2].between(wTi_list[i1])
for i1, i2 in pair_indices}
"""4 poses in the circle of radius 5m, all looking at the center of the circle.
For relative poses, each pose has just 2 edges, connecting to the immediate neighbors.
"""
CIRCLE_TWO_EDGES_GLOBAL_POSES = SFMdata.createPoses(
Cal3_S2(fx=1, fy=1, s=0, u0=0, v0=0))[::2]
CIRCLE_TWO_EDGES_RELATIVE_POSES = generate_relative_from_global(
CIRCLE_TWO_EDGES_GLOBAL_POSES, [(1, 0), (2, 1), (3, 2), (0, 3)])
"""4 poses in the circle of radius 5m, all looking at the center of the circle.
For relative poses, each pose is connected to every other (3) pose.
"""
CIRCLE_ALL_EDGES_GLOBAL_POSES = copy.copy(CIRCLE_TWO_EDGES_GLOBAL_POSES)
CIRCLE_ALL_EDGES_RELATIVE_POSES = generate_relative_from_global(
CIRCLE_TWO_EDGES_GLOBAL_POSES, [(0, 1), (0, 2), (0, 3), (1, 2), (1, 3),
(2, 3)])
"""3 poses on a line, simulating forward motion, with large translations and no relative rotation.
For relative poses, we have a fully connected graph.
def main():
"""
A structure-from-motion example with landmarks
- The landmarks form a 10 meter cube
- The robot rotates around the landmarks, always facing towards the cube
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a NonlinearISAM object which will relinearize and reorder the variables
# every "reorderInterval" updates
isam = NonlinearISAM(reorderInterval=3)
# Create a Factor Graph and Values to hold the new data
graph = NonlinearFactorGraph()
initial_estimate = Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise,
X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM.
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j] + noise
initial_estimate.insert(L(j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
from gtsfm.densify.patchmatchnet_data import PatchmatchNetData
# set the default image size as 800x600, with 3 channels
DEFAULT_IMAGE_W = 800
DEFAULT_IMAGE_H = 600
DEFAULT_IMAGE_C = 3
# set default track points, the coordinates are in the world frame
DEFAULT_NUM_TRACKS = 100
DEFAULT_TRACK_POINTS = [Point3(5, 5, float(i)) for i in range(DEFAULT_NUM_TRACKS)]
# set default camera intrinsics
DEFAULT_CAMERA_INTRINSICS = Cal3_S2(
fx=100.0,
fy=100.0,
s=1.0,
u0=DEFAULT_IMAGE_W // 2,
v0=DEFAULT_IMAGE_H // 2,
)
# set default camera poses as described in GTSAM example
DEFAULT_CAMERA_POSES = SFMdata.createPoses(DEFAULT_CAMERA_INTRINSICS)
# set default camera instances
DEFAULT_CAMERAS = [
PinholeCameraCal3_S2(DEFAULT_CAMERA_POSES[i], DEFAULT_CAMERA_INTRINSICS) for i in range(len(DEFAULT_CAMERA_POSES))
]
DEFAULT_NUM_CAMERAS = len(DEFAULT_CAMERAS)
# the number of valid images should be equal to the number of cameras (with estimated pose)
DEFAULT_NUM_IMAGES = DEFAULT_NUM_CAMERAS
# build dummy image dictionary with default image shape
DEFAULT_DUMMY_IMAGE_DICT = {
run_feature_extraction = False
run_feature_matching = False
run_bundle_adjustment = True
save_result = True
basedir = "mapping/datasets/flann_klaus_1x1x8/"
image_extension = ".jpg"
source_image_size = (640, 480)
# Undistortion
# resize_output = True, output will be resized to the same shape as the input image
# resize_output = False, output image will shrink due to rectification of radian distort
resize_output = False
distort_calibration_matrix = Cal3_S2(fx=347.820593,
fy=329.096945,
s=0,
u0=295.717950,
v0=222.964889).matrix()
distortion_coefficients = np.array(
[-0.284322, 0.055723, 0.006772, 0.005264, 0.000000])
# calibration_matrix = Cal3_S2(fx=232.0542, fy=252.8620, s=0,
# u0=325.3452, v0=240.2912).matrix()
# Resize
calibration_matrix = Cal3_S2(fx=211.8927,
fy=197.7030,
s=0,
u0=281.1168,
v0=179.2954)
undistort_img_size = (583, 377)
number_images = 10
def point_on_line(a, b, p):
ap = p - a
ab = b - a
result = a + np.dot(ap, ab) / np.dot(ab, ab) * ab
return result
# p = Point2(1,1)
#
# a = Point2(1,1)
# b = Point2(-1,-1)
# dis = point_on_line(a,b, p)
K = Cal3_S2(320, 320, 0.0, 320, 240)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(20, 0, 0)
up = gtsam.Point3(0, 0, 1) # NED -> NORTH_EAST_DOWN ???
cam = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
pw = Point3(10.0, 3.0, 0.0)
# pi_groundtruth = cam.project(pw)
# pi,Dpose,Dpoint = cal_PointProject_Jacobians(cam,pw)
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
d = 1 / q[2]
#
