def __init__(self,image_directory_path = 'datasets/4.5/mapping/source_images/', image_extension = '*.jpg', image_size = (160,120), down_sample = 1, nn_thresh = 0.7):
self.basedir = image_directory_path
self.img_extension = image_extension
self.nn_thresh = nn_thresh
self.fe = SuperPointFrontend(weights_path="SuperPointPretrainedNetwork/superpoint_v1.pth",
nms_dist=4,
conf_thresh=0.015,
nn_thresh=0.7,
cuda=False)
self.point_tracker = PointTracker(5, self.nn_thresh)
self.img_size = image_size
self.scale = down_sample
# fov_in_degrees = 128, image_width = 640, image_height = 480)
# fx = w/2*math.tan(fov*3.14/360)
# How resizing an image affect the intrinsic camera matrix:https://dsp.stackexchange.com/questions/6055/how-does-resizing-an-image-affect-the-intrinsic-camera-matrix
self.image_transform = np.array([[0.25,0,0.25-0.5],[0,0.25,0.25-0.5],[0,0,1]])
# self.calibration = gtsam.Cal3_S2(fx=156.3, fy=236,s=0,u0=320, v0=240).matrix()
self.calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6).matrix()
self.calibration = np.dot(self.image_transform,self.calibration)
self.distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000])
# self.cal = gtsam.Cal3_S2(fx=156.3, fy=236,s=0,u0=320, v0=240)
# self.cal = gtsam.Cal3_S2(fx=19.54, fy=29.5,s=0,u0=39.625, v0=29.625)
self.cal = gtsam.Cal3_S2(fx=83.35, fy=78.675,s=0,u0=75.65, v0=61.65)
self.keypoint_list = []
self.descriptor_list = []
self.correspondence_table = []
self.img_pose=[]
self.landmark=[]
def test_insertProjectionFactors(self):
"""Test insertProjectionFactors."""
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2())
self.assertEqual(graph.size(), 2)
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(),
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)))
self.assertEqual(graph.size(), 2)
def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
""""Returns global rotations and unit translation directions between 8 cameras
that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
and the unit translations directions between some camera pairs are computed from their
global translations. """
fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0))
# Rotations of the cameras in the world frame.
wRc_values = gtsam.Values()
# Normalized translation directions from camera i to camera j
# in the coordinate frame of camera i.
i_iZj_list = []
for i in range(0, len(wTc_list) - 2):
# Add the rotation.
wRi = wTc_list[i].rotation()
wRc_values.insert(i, wRi)
# Create unit translation measurements with next two poses.
for j in range(i + 1, i + 3):
# Compute the translation from pose i to pose j, in the world reference frame.
w_itj = wTc_list[j].translation() - wTc_list[i].translation()
# Obtain the translation in the camera i's reference frame.
i_itj = wRi.unrotate(w_itj)
# Compute the normalized unit translation direction.
i_iZj = gtsam.Unit3(i_itj)
i_iZj_list.append(
gtsam.BinaryMeasurementUnit3(
i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
# Add the last two rotations.
wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation())
wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation())
return wRc_values, i_iZj_list
def __init__(self, atrium_map, downsample_width, downsample_height, l2_thresh, distance_thresh):
""" A class to estimate the current camera pose with prebuilt map, an image, and a trajectory included the past pose.
Args:
atrium_map - a map object that includes a N*3 numpy array of gtsam.Point3 and a N*M numpy array of M dimensions descriptors
downsample_width - image width downsample factor
downsample_height - image height downsample factor
l2_thresh - l2 distance threshold of two descriptors
distance_thresh - feature coordinate distance threshold
"""
# Camera settings
self.image_width = 640
self.image_height = 480
self.fov_in_degrees = 128
self.calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6)
# gtsam.Cal3_S2(
# self.fov_in_degrees, self.image_width, self.image_height)
self.distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000])
self.projection = np.array([[226.994629 ,0.000000, 311.982613, 0.000000],[0.000000, 245.874146, 250.410089, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]])
self.atrium_map = atrium_map
self.downsample_w = downsample_width
self.downsample_h = downsample_height
self.l2_threshold = l2_thresh
self.distance_threshold = distance_thresh
def undistort_image(basedir, img_extension):
search = os.path.join(basedir, img_extension)
img_paths = glob.glob(search)
img_paths.sort()
print("Number of Images: ", len(img_paths))
maxlen = len(img_paths)
if maxlen == 0:
raise IOError(
'No images were found (maybe wrong \'image extension\' parameter?)'
)
calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7, s=0, u0=303.6,
v0=247.6).matrix()
distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000])
projection = np.array([[226.994629, 0.000000, 311.982613, 0.000000],
[0.000000, 245.874146, 250.410089, 0.000000],
[0.000000, 0.000000, 1.000000, 0.000000]])
# calibration = gtsam.Cal3_S2(fx=343.555173, fy=327.221818,s=0,u0=295.979699, v0=261.530851).matrix()
# distortion = np.array([-0.305247, 0.064438, -0.007641, 0.006581, 0.000000])
# projection = np.array([[235.686951, 0.000000, 304.638818, 0.000000],[0.000000, 256.651520, 265.858792, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]])
for i, img_path in enumerate(img_paths):
img = cv2.imread(img_path, 0)
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
calibration, distortion, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, calibration, distortion, None, projection)
# dst = cv2.undistort(img,calibration ,distortion , None, calibration)
# cv2.imshow(dst)
output_path = '%d_frame_undist.jpg' % i
output_path = os.path.join(basedir, output_path)
cv2.imwrite(output_path, dst)
def __init__(self,image_directory_path = 'datasets/5x5x8/source_1x5x1/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7, row = 1, col = 6, angles = 1, directions = 1):
# def __init__(self,image_directory_path = 'datasets/5x5x8/source/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7, row = 5, col = 5, angles = 8, directions = 3):
# def __init__(self,image_directory_path = 'datasets/4.5/mapping/source_images/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7, row = 1, col = 6, angles = 1, directions = 1):
self.basedir = image_directory_path
self.img_extension = image_extension
self.nn_thresh = nn_thresh
self.fe = SuperPointFrontend(weights_path="SuperPointPretrainedNetwork/superpoint_v1.pth",
nms_dist=4,
conf_thresh=0.015,
nn_thresh=0.7,
cuda=False)
self.point_tracker = PointTracker(5, self.nn_thresh)
self.img_size = image_size
self.scale = down_sample
self.cal= gtsam.Cal3_S2(fx=232.0542, fy=252.8620,s=0,u0=325.3452, v0=240.2912)
self.calibration = self.cal.matrix()
self.keypoint_list = []
self.descriptor_list = []
self.correspondence_table = []
self.row = row
self.col = col
self.angles = angles
self.directions = directions
self.img_pose = np.array([])
self.landmark=[]
self.landmark_descriptor = []
def undistort_image(basedir,img_extension):
"""A function to undistort the distorted images."""
search = os.path.join(basedir, img_extension)
img_paths = glob.glob(search)
img_paths.sort()
print("Number of Images: ",len(img_paths))
maxlen = len(img_paths)
if maxlen == 0:
raise IOError('No images were found (maybe wrong \'image extension\' parameter?)')
calibration = gtsam.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])
rectification = np.identity(3)
projection = np.array([[240.446564 ,0.000000, 302.423680, 0.000000],[0.000000, 265.140778, 221.096494, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]])
count = 0
img_idx = 0
for i,img_path in enumerate(img_paths):
img = cv2.imread(img_path, 1)
if count == 8:
count = 0
img_idx += 1
dst = cv2.undistort(img, calibration, distortion, np.eye(3), projection)
output_path = '%d_'%img_idx+'%d_frame_undist.jpg'%count
output_path = os.path.join(basedir+'source/', output_path)
cv2.imwrite(output_path,dst)
count += 1
def test_find_dsf(self):
"""Test functions related to DSF"""
# """Test find bad matches."""
data_directory = 'mapping/datasets/dsf_test_data/'
num_images = 3
filter_bad_landmarks_enable = False
min_obersvation_number = 3
back_end = MappingBackEnd(
data_directory, num_images, gtsam.Cal3_S2(), [], [], [], filter_bad_landmarks_enable, min_obersvation_number)
bad_matches = back_end.find_bad_matches()
self.assertEqual(bad_matches, {(2, 6), (0, 4)})
# """Test create landmark map."""
actual_landmark_map, dsf = back_end.create_landmark_map(False)
expected_landmark_map = {(0, 1): [(0, Point2(1, 1)), (1, Point2(2, 1)), (2, Point2(3, 1))], (2, 0): [(2, Point2(0, 0))], (0, 0): [(0, Point2(0, 0))], (0, 5): [(0, Point2(1, 5)), (0, Point2(1, 6)), (2, Point2(3, 6))], (0, 4): [
(0, Point2(1, 4)), (2, Point2(3, 3)), (2, Point2(3, 5))], (1, 0): [(1, Point2(0, 0))], (0, 3): [(0, Point2(1, 3)), (1, Point2(2, 2)), (2, Point2(3, 2))], (0, 2): [(0, Point2(1, 2)), (2, Point2(3, 4))]}
self.assert_landmark_map_equal(
actual_landmark_map, expected_landmark_map)
# """Test generate dsf."""
landmark_representative = dsf.find(gtsam.IndexPair(2, 1))
key = (landmark_representative.i(), landmark_representative.j())
self.assertEqual(key, (0, 1))
# """Test filter bad landmarks."""
actual_landmark_map = back_end.filter_bad_landmarks(
expected_landmark_map, dsf, True)
expected_landmark_map = [[(0, Point2(1, 1)), (1, Point2(2, 1)), (2, Point2(3, 1))], [
(0, Point2(1, 3)), (1, Point2(2, 2)), (2, Point2(3, 2))]]
# self.assert_landmark_map_equal(actual_landmark_map, expected_landmark_map)
for i, observations in enumerate(expected_landmark_map):
for j, observation in enumerate(observations):
self.assertEqual(actual_landmark_map[i][j][0], observation[0])
self.gtsamAssertEquals(
actual_landmark_map[i][j][1], observation[1])
def __init__(self, data_directory, num_images=3, delta_z=1):
"""Construct by reading from a data directory."""
self.basedir = data_directory
self.nrimages = num_images
# Camera calibration
fov, w, h = 60, 1280, 720
self._calibration = gtsam.Cal3_S2(fov, w, h)
# Pose distance along z axis
# fps = 30
# velocity = 10 # m/s
# self.delta_z = velocity / fps
self.delta_z = delta_z
self._min_landmark_seen = 3
def get_image_features(image_index):
""" Load features
features - keypoints:A Nx2 list of (x,y). Descriptors: A Nx256 list of desc.
"""
feat_file = os.path.join(self.basedir,
"{0:07}.key".format(image_index))
keypoints, descriptors = load_features_list(feat_file)
return ImageFeature(keypoints, descriptors)
self._image_features = [
get_image_features(image_index)
for image_index in range(self.nrimages)
]
self._image_matches = self.create_image_matches()
self.image_points, self._landmark_estimates, self.image_poses = self.data_association(
)
def load_calibration():
""" calculates the intrinsic camera calibration """
pixel_width = 320; pixel_height = 240
vfov = 45; hfov = 60
fx = (pixel_width/2.0)/math.tan(math.radians(hfov/2.0))
fy = (pixel_height/2.0)/math.tan(math.radians(vfov/2.0))
s = 0.0; cx = pixel_width/2.0; cy = pixel_height/2.0
camera_intrinsics = gtsam.Cal3_S2(fx, fy, s, cx, cy)
return camera_intrinsics
def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
"""
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 test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([
k.calibration().calibrate(pt)
for k, pt in zip(self.cameras, self.measurements)
])
shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses,
shared_cal,
rectified,
rank_tol=1e-9,
optimize=False)
self.gtsamAssertEquals(triangulated, self.origin)
def __init__(self, nrCameras, nrPoints, fov_in_degrees, image_width,
image_height):
"""
Parameters:
nrCameras -- Number of cameras
nrPoints -- Number of landmarks
fov_in_degrees -- Based on the official document of Runcam Swift 2: horizontal FOV = 128, Vertical FOV = 91, Diagonal FOV = 160
image_width, image_height -- camera output image [640,480]. Superpoint can extract features with downsampled images, therefore the output of Superpoint is flexible.
"""
self.nrCameras = nrCameras
self.nrPoints = nrPoints
self.calibration = gtsam.Cal3_S2(fov_in_degrees, image_width,
image_height)
def test_reprojectionErrors(self):
"""Test reprojectionErrors."""
pixels = np.asarray([[20, 30], [20, 30]])
I = [1, 2]
K = gtsam.Cal3_S2()
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K)
values = gtsam.Values()
values.insert(0, gtsam.Pose3())
cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K)
gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10)
errors = gtsam.utilities.reprojectionErrors(graph, values)
np.testing.assert_allclose(errors, np.zeros((2, 2)))
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [gtsam.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 undistort_image(basedir, img_extension):
"""A function to undistort the distorted images."""
search = os.path.join(basedir, img_extension)
img_paths = glob.glob(search)
img_paths.sort()
print("Number of Images: ", len(img_paths))
maxlen = len(img_paths)
if maxlen == 0:
raise IOError(
'No images were found (maybe wrong \'image extension\' parameter?)'
)
calibration = gtsam.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])
rectification = np.identity(3)
projection = np.array([[240.446564, 0.000000, 302.423680, 0.000000],
[0.000000, 265.140778, 221.096494, 0.000000],
[0.000000, 0.000000, 1.000000, 0.000000]])
# calibration = gtsam.Cal3_S2(fx=331.0165, fy=310.4791,s=0,u0=332.7372, v0=248.5307).matrix()
# distortion = np.array([-0.3507, 0.1112, 8.6304e-04, -0.0018, 0.000000])
# rectification = np.identity(3)
# calibration = gtsam.Cal3_S2(fx=332.8312, fy=312.3082,s=-1.1423,u0=330.2353, v0=249.5842).matrix()
# distortion = np.array([-0.3718, 0.1623, 5.4112e-04, -8.9232e-04, -0.0362])
# rectification = np.identity(3)
count = 0
img_idx = 0
for i, img_path in enumerate(img_paths):
img = cv2.imread(img_path, 1)
# h, w = img.shape[:2]
# newcameramtx, roi=cv2.getOptimalNewCameraMatrix(calibration,distortion,(w,h),1,(w,h))
# dst = cv2.undistort(img, calibration, distortion, np.eye(3),projection)
dst = cv2.undistort(img, calibration, distortion)
output_path = 'frame_%d.jpg' % count
output_path = os.path.join(basedir + 'matlab_undist/', output_path)
cv2.imwrite(output_path, dst)
count += 1
def generate_data(options):
""" Generate ground-truth and measurement data. """
K = gtsam.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] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10),
gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10),
gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10),
gtsam.Point3(-10, -10, -10), gtsam.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 = gtsam.Point3(r * cos(theta), r * sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
gtsam.Point3(),
gtsam.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 create_map(fignum):
calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6)
distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000])
rectification = np.identity(3)
projection = np.array([[226.994629 ,0.000000, 311.982613, 0.000000],[0.000000, 245.874146, 250.410089, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]])
# Create matched feature data and desc data
data = Data(5,10,calibration.matrix(),distortion,projection)
data.create_feature_data(0)
data.create_descriptor_data()
data.undistort_feature_points()
# Mapping
mapping = Mapping(5, 10, calibration, 640, 480)
sfm_result = mapping.atrium_sfm(data,30,1.58)
atrium_map = mapping.create_atrium_map(sfm_result, data.desc)
return sfm_result, atrium_map
def undistort_image(basedir,img_extension):
search = os.path.join(basedir, img_extension)
img_paths = glob.glob(search)
img_paths.sort()
print("Number of Images: ",len(img_paths))
maxlen = len(img_paths)
if maxlen == 0:
raise IOError('No images were found (maybe wrong \'image extension\' parameter?)')
calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6).matrix()
projection = np.array([[226.994629 ,0.000000, 311.982613, 0.000000],[0.000000, 245.874146, 250.410089, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]])
distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000])
for i,img_path in enumerate(img_paths):
img = cv2.imread(img_path, 0)
dst = cv2.undistort(img,calibration ,distortion , None, calibration)
# cv2.imshow(dst)
output_path = '%d_frame_undist.jpg'%i
output_path = os.path.join(basedir, output_path)
cv2.imwrite(output_path,dst)
def __init__(self, fov, width, height, nr_points):
"""
Key Arguments:
calibration - the calibration of a camera
poses - a list of the camera poses, each pose is a gtsam.Pose3 object
vision_area_list - a list of vision area of each pose, back project the vertices of the image to distance 10m to get its vision area at 10m
projected_features - a list of features, each element is a collection of features which are landmarks that projected to the current pose
superpoint_features - a list of features, each element is a Synthetic superpoint output features contains key point and normalized descriptor.
match_features - a list of features, each element is a matched feature
visible_map - a list of Map, each element is a landmark point in the map
"""
self.calibration = gtsam.Cal3_S2(fov, width, height)
self.poses = []
self.past_poses = []
self.vision_area_list = []
self.projected_features = []
self.map_indices = []
self.superpoint_features = []
self.matched_features = []
self.visible_map = []
def __init__(self,
pose0=np.eye(4),
pose0_to_pose1_range=1.0,
K=np.eye(3),
relinearizeThreshold=0.1,
relinearizeSkip=10,
proj_noise_val=1.0):
self.graph = NonlinearFactorGraph()
# Add a prior on pose x0. 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]))
x0factor = PriorFactorPose3(iSAM2Wrapper.get_key('x', 0),
gtsam.gtsam.Pose3(pose0), pose_noise)
self.graph.push_back(x0factor)
# Set scale between pose 0 and pose 1 to Unity
x0x1_noise = gtsam.noiseModel_Isotropic.Sigma(1, 0.1)
x1factor = RangeFactorPose3(iSAM2Wrapper.get_key('x', 0),
iSAM2Wrapper.get_key('x', 1),
pose0_to_pose1_range, x0x1_noise)
self.graph.push_back(x1factor)
iS2params = gtsam.ISAM2Params()
iS2params.setRelinearizeThreshold(relinearizeThreshold)
iS2params.setRelinearizeSkip(relinearizeSkip)
self.isam2 = gtsam.ISAM2(iS2params)
self.projection_noise = gtsam.noiseModel_Isotropic.Sigma(
2, proj_noise_val)
self.K = gtsam.Cal3_S2(iSAM2Wrapper.CameraMatrix_to_Cal3_S2(K))
#self.opt_params = gtsam.DoglegParams()
#self.opt_params.setVerbosity('Error')
#self.opt_params.setErrorTol(0.1)
self.initial_estimate = gtsam.Values()
self.initial_estimate.insert(iSAM2Wrapper.get_key('x', 0),
gtsam.gtsam.Pose3(pose0))
self.lm_factor_ids = set()
def setUp(self):
# self.calibration = gtsam.Cal3_S2(160, 640, 480).matrix()
# self.calibration = gtsam.Cal3_S2(fx=156.3, fy=236,s=1,u0=320, v0=240).matrix()
self.image_transform = np.array([[0.25, 0, 0.125 - 0.5],
[0, 0.25, 0.125 - 0.5], [0, 0, 1]])
self.calibration = gtsam.Cal3_S2(fx=333.4,
fy=327.22,
s=0,
u0=303.6,
v0=247.6).matrix()
# self.calibration = gtsam.Cal3_S2(fx=343.55, fy=314.7,s=0,u0=295.97, v0=261.53).matrix()
# self.calibration = np.dot(self.image_transform,self.calibration2)
self.distortion = np.array(
[-0.282548, 0.054412, -0.001882, 0.004796, 0.000000])
# self.distortion = np.array([-0.305247, 0.064438, -0.007641, 0.006581, 0.000000])
self.rectification = np.identity(3)
self.projection = np.array(
[[226.994629, 0.000000, 311.982613, 0.000000],
[0.000000, 245.874146, 250.410089, 0.000000],
[0.000000, 0.000000, 1.000000, 0.000000]])
# self.projection = np.array([[235.686951, 0.000000, 304.638818, 0.000000],[0.000000, 256.651520, 265.858792, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]])
# self.projection = np.dot(self.image_transform,self.projection2)
self.calibration_inv = np.linalg.inv(self.calibration)
self.R = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
# self.t = np.array([[-0.79],[-1.36832],[0]])
self.t = np.array([[1.58], [0], [0]])
# self.t = np.array([[-0.61],[-0.78832],[0]])
self.t_skew_symmetric = np.array([[0, -self.t[2], self.t[1]],
[self.t[2], 0, -self.t[0]],
[-self.t[1], self.t[0], 0]])
self.E = np.dot(self.t_skew_symmetric, self.R)
self.F = np.dot(np.dot(self.calibration_inv.T, self.E),
self.calibration_inv)
# http://answers.opencv.org/question/31421/opencv-3-essentialmatrix-and-recoverpose/
# self.P_0 = np.dot(self.calibration,np.hstack((np.identity(3),np.zeros((3,1)))))
# self.P_1 = np.dot(self.calibration,np.hstack((self.R.T,-np.dot(self.R.T,self.t))))
self.P_0 = np.hstack((np.identity(3), np.zeros((3, 1))))
self.P_1 = np.hstack((self.R.T, -np.dot(self.R.T, self.t)))
def __init__(self,image_directory_path = 'datasets/4.5/mapping/source_images/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7):
self.basedir = image_directory_path
self.img_extension = image_extension
self.nn_thresh = nn_thresh
self.fe = SuperPointFrontend(weights_path="SuperPointPretrainedNetwork/superpoint_v1.pth",
nms_dist=4,
conf_thresh=0.015,
nn_thresh=0.7,
cuda=False)
self.point_tracker = PointTracker(5, self.nn_thresh)
self.img_size = image_size
self.scale = down_sample
# self.calibration = gtsam.Cal3_S2(fx=240.446564, fy=265.140778,s=0,u0=302.423680, v0=221.096494).matrix()
# self.cal = gtsam.Cal3_S2(fx=240.446564, fy=265.140778,s=0,u0=302.423680, v0=221.096494)
self.cal = gtsam.Cal3_S2(fx=232.0542, fy=252.8620,s=0,u0=325.3452, v0=240.2912)
self.calibration = self.cal.matrix()
self.keypoint_list = []
self.descriptor_list = []
self.correspondence_table = []
self.img_pose=[]
self.landmark=[]
def __init__(self):
fx = Config().fx
fy = Config().fy
cx = Config().cx
cy = Config().cy
bf = Config().bf
# Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline
baseline = bf / fx
self.K_stereo = gt.Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline)
self.K_mono = gt.Cal3_S2(fx, fy, 0.0, cx, cy)
self.deltaMono = np.sqrt(5.991)
self.deltaStereo = np.sqrt(7.815)
self.depth_threshold = bf / fx * 60
# Create graph container and add factors to it
self.graph = gt.NonlinearFactorGraph()
# Create initial estimate for camera poses and landmarks
self.initialEstimate = gt.Values()
# add a constraint on the starting pose
# first_pose = gt.Pose3()
# self.graph.add(gt.NonlinearEqualityPose3(X(1), first_pose))
self.inv_lvl_sigma2 = np.zeros((8, ), dtype=np.float)
for idx in np.arange(8):
self.inv_lvl_sigma2[idx] = 1. / 1.2**(2 * idx - 2)
# point counter for landmarks and octave container
self.counter = 1
self.octave = []
self.is_stereo = []
def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters
K = gtsam.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 an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
# to maintain proper linearization and efficient variable ordering, iSAM2
# performs partial relinearization/reordering at each step. A parameter
# structure is available that allows the user to set various properties, such
# as the relinearization threshold and type of linear solver. For this
# example, we we set the relinearization threshold small so the iSAM2 result
# will approach the batch result.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.01)
parameters.setRelinearizeSkip(1)
isam = gtsam.ISAM2(parameters)
# Create a Factor Graph and Values to hold the new data
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
# Add factors for each landmark observation
for j, point in enumerate(points):
camera = gtsam.PinholeCameraCal3_S2(pose, K)
measurement = camera.project(point)
graph.push_back(
gtsam.GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
initial_estimate.insert(
X(i),
pose.compose(
gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25),
gtsam.Point3(0.05, -0.10, 0.20))))
# 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
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1,
0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
graph.push_back(
gtsam.PriorFactorPoint3(L(0), points[0],
point_noise)) # add directly to graph
# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initial_estimate.insert(
L(j),
gtsam.Point3(point.x() - 0.25,
point.y() + 0.20,
point.z() + 0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
# If accuracy is desired at the expense of time, update(*) can be called additional
# times to perform multiple optimizer iterations every step.
isam.update()
current_estimate = isam.calculateEstimate()
print("****************************************************")
print("Frame", i, ":")
for j in range(i + 1):
print(X(j), ":", current_estimate.atPose3(X(j)))
for j in range(len(points)):
print(L(j), ":", current_estimate.atPoint3(L(j)))
visual_ISAM2_plot(current_estimate)
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
plt.ioff()
plt.show()
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [gtsam.Point3()] * nrPoints
import unittest
import numpy as np
import gtsam
import gtsam_unstable
from gtsam.utils.test_case import GtsamTestCase
pose1 = gtsam.Pose3()
pose2 = gtsam.Pose3(
np.array([[0.9999375, 0.00502487, 0.00998725, 0.1],
[-0.00497488, 0.999975, -0.00502487, 0.02],
[-0.01001225, 0.00497488, 0.9999375, 1.], [0., 0., 0., 1.]]))
point = np.array([2, 0, 15])
point_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(2))
cal = gtsam.Cal3_S2()
body_p_sensor = gtsam.Pose3()
alpha = 0.1
measured = np.array([0.13257015, 0.0004157])
class TestProjectionFactorRollingShutter(GtsamTestCase):
def test_constructor(self):
'''
test constructor for the ProjectionFactorRollingShutter
'''
factor = gtsam_unstable.ProjectionFactorRollingShutter(
measured, alpha, point_noise, 0, 1, 2, cal)
factor = gtsam_unstable.ProjectionFactorRollingShutter(
measured, alpha, point_noise, 0, 1, 2, cal, body_p_sensor)
factor = gtsam_unstable.ProjectionFactorRollingShutter(
def full_bundle_adjustment_update(self, sfm_map: SfmMap):
# Variable symbols for camera poses.
X = gtsam.symbol_shorthand.X
# Variable symbols for observed 3D point "landmarks".
L = gtsam.symbol_shorthand.L
# Create a factor graph.
graph = gtsam.NonlinearFactorGraph()
# Extract the first two keyframes (which we will assume has ids 0 and 1).
kf_0 = sfm_map.get_keyframe(0)
kf_1 = sfm_map.get_keyframe(1)
# We will in this function assume that all keyframes have a common, given (constant) camera model.
common_model = kf_0.camera_model()
calibration = gtsam.Cal3_S2(common_model.fx(), common_model.fy(), 0,
common_model.cx(), common_model.cy())
# Unfortunately, the dataset does not contain any information on uncertainty in the observations,
# so we will assume a common observation uncertainty of 3 pixels in u and v directions.
obs_uncertainty = gtsam.noiseModel.Isotropic.Sigma(2, 3.0)
# Add measurements.
for keyframe in sfm_map.get_keyframes():
for keypoint_id, map_point in keyframe.get_observations().items():
obs_point = keyframe.get_keypoint(keypoint_id).point()
factor = gtsam.GenericProjectionFactorCal3_S2(
obs_point, obs_uncertainty, X(keyframe.id()),
L(map_point.id()), calibration)
graph.push_back(factor)
# Set prior on the first camera (which we will assume defines the reference frame).
no_uncertainty_in_pose = gtsam.noiseModel.Constrained.All(6)
factor = gtsam.PriorFactorPose3(X(kf_0.id()), gtsam.Pose3(),
no_uncertainty_in_pose)
graph.push_back(factor)
# Set prior on distance to next camera.
no_uncertainty_in_distance = gtsam.noiseModel.Constrained.All(1)
prior_distance = 1.0
factor = gtsam.RangeFactorPose3(X(kf_0.id()), X(kf_1.id()),
prior_distance,
no_uncertainty_in_distance)
graph.push_back(factor)
# Set initial estimates from map.
initial_estimate = gtsam.Values()
for keyframe in sfm_map.get_keyframes():
pose_w_c = gtsam.Pose3(keyframe.pose_w_c().to_matrix())
initial_estimate.insert(X(keyframe.id()), pose_w_c)
for map_point in sfm_map.get_map_points():
point_w = gtsam.Point3(map_point.point_w().squeeze())
initial_estimate.insert(L(map_point.id()), point_w)
# Optimize the graph.
params = gtsam.GaussNewtonParams()
params.setVerbosity('TERMINATION')
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
# Update map with results.
for keyframe in sfm_map.get_keyframes():
updated_pose_w_c = result.atPose3(X(keyframe.id()))
keyframe.update_pose_w_c(SE3.from_matrix(
updated_pose_w_c.matrix()))
for map_point in sfm_map.get_map_points():
updated_point_w = result.atPoint3(L(map_point.id())).reshape(3, 1)
map_point.update_point_w(updated_point_w)
sfm_map.has_been_updated()
if __name__ == '__main__':
# declare noise estimates
ODOM_SIGMA = 0.01
BOX_SIGMA = 3
# define shortcuts for symbols
X = lambda i: int(gtsam.symbol(ord('x'), i))
Q = lambda i: int(gtsam.symbol(ord('q'), i))
# create empty graph / estimate
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# define calibration
calibration = gtsam.Cal3_S2(525.0, 525.0, 0.0, 160.0, 120.0)
# define noise models
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([1e-1] * 6, dtype=np.float))
odometry_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([ODOM_SIGMA] * 3 + [ODOM_SIGMA] * 3, dtype=np.float))
bbox_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([BOX_SIGMA] * 4, dtype=np.float))
# define poses
poses = []
poses.append(
gtsam.SimpleCamera.Lookat(gtsam.Point3(10, 0, 0), gtsam.Point3(),
gtsam.Point3(0, 0, 1)).pose())
poses.append(
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values()
# 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
noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t # simulation time
if i == 0: # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))
elif i >= 2: # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))
if i > 0:
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = gtsam.BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()
