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 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
self.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
self.data_directory = 'mapping/sim_match_data/'
min_obersvation_number = 2
filter_bad_landmarks_enable = True
self.num_images = 3
self.back_end = MappingBackEnd(
self.data_directory, self.num_images, calibration, self.pose_estimates, measurement_noise, pose_prior_noise, filter_bad_landmarks_enable, min_obersvation_number) # pylint: disable=line-too-long
self.result = self.back_end.bundle_adjustment()
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 run():
"""Execution."""
# Undistortion
if run_undistortion:
feature_matcher = FeatureMatcher(basedir, image_extension,
source_image_size, number_images)
feature_matcher.undistortion(distort_calibration_matrix,
distortion_coefficients, resize_output)
# Feature Extraction
feature_matcher = FeatureMatcher(basedir, image_extension,
source_image_size, number_images)
if run_feature_extraction:
feature_matcher.feature_extraction(undistort_img_size, feature_type)
# Feature Matching
if run_feature_matching:
feature_matcher.feature_matching(undistort_img_size, feature_type,
matching_type, calibration_matrix)
if run_bundle_adjustment:
# Create pose estimates
pose_estimates = pose_estimate_generator_rectangle(
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)
measurement_noise = gtsam.noiseModel_Robust(
gtsam.noiseModel_mEstimator_Huber(1.345),
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
back_end = MappingBackEnd(basedir, number_images, calibration_matrix,
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')
# print(sfm_result)
# Plot Result
plot_with_result(sfm_result, 30, 30, 30, 0.5)
# Save map data
if save_result:
back_end.save_map_to_file(sfm_result)
back_end.save_poses_to_file(sfm_result)
class TestMappingBackEnd(GtsamTestCase):
"""Unit tests for mapping back end."""
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
self.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
self.data_directory = 'mapping/sim_match_data/'
min_obersvation_number = 2
filter_bad_landmarks_enable = True
self.num_images = 3
self.back_end = MappingBackEnd(
self.data_directory, self.num_images, calibration, self.pose_estimates, measurement_noise, pose_prior_noise, filter_bad_landmarks_enable, min_obersvation_number) # pylint: disable=line-too-long
self.result = self.back_end.bundle_adjustment()
def assert_landmark_map_equal(self, actual_landmark_map, expected_landmark_map):
"""Helper function to test landmark map."""
for item in expected_landmark_map.items():
assert (
item[0] in actual_landmark_map), "FAIL: Test create landmark map."
for i, observation in enumerate(expected_landmark_map[item[0]]):
self.assertEqual(
actual_landmark_map[item[0]][i][0], observation[0])
self.gtsamAssertEquals(
actual_landmark_map[item[0]][i][1], observation[1])
def test_load_points(self):
"""Test load points."""
pts3d = load_points()
self.assertEqual(len(pts3d), 90)
def test_load_features(self):
"""Test load image features"""
image_features, _ = self.back_end.load_features(0)
self.assertIsInstance(image_features, list)
def test_load_matches(self):
"""Test get matches"""
matches = self.back_end.load_matches(0, 1)
self.assertEqual(matches[0][0], 0)
self.assertEqual(matches[0][2], 1)
self.assertIsInstance(matches, list)
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 test_create_initial_estimate(self):
"""test create initial estimate"""
initial_estimate = self.back_end.create_initial_estimate()
for i, pose in enumerate(self.pose_estimates):
self.gtsamAssertEquals(initial_estimate.atPose3((X(i))), pose)
def test_bundle_adjustment(self):
"""Test bundle adjustment"""
# """Test pose output"""
delta_z = 1
num_frames = 3
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0)
for i in range(num_frames):
# Create ground truth poses
expected_pose_i = Pose3(wRc, Point3(0, delta_z*i, 2))
# Get actual poses
actual_pose_i = self.result.atPose3(X(i))
self.gtsamAssertEquals(actual_pose_i, expected_pose_i, 1e-6)
# """Test landmark output"""
expected_points = load_points()
for i, expected_point_i in enumerate(expected_points):
actual_point_i = self.result.atPoint3(P(i))
self.gtsamAssertEquals(actual_point_i, expected_point_i, 1e-4)
def test_get_landmark_descriptor(self):
"""Test get landmark descriptor"""
landmark_map = self.back_end.get_landmark_map()
actual_desc = self.back_end.get_landmark_descriptor(landmark_map[0])
expected_desc = [-0.0228, 0.0062, -0.0301, 0.036, -0.0042, 0.0606, -0.0554, 0.0104, 0.0241, -0.035, -0.0211, -0.0809, -0.0347, -0.0648, -0.057, 0.0412, 0.0136, -0.054, -0.1114, 0.0901, -0.0057, -0.0057, 0.1536, 0.0046, 0.0693, 0.014, 0.0236, 0.0228, -0.0224, 0.1264, -0.0837, -0.0236, -0.0631, -0.0292, -0.079, 0.033, 0.0204, -0.0499, -0.021, 0.0188, 0.0961, -0.0149, 0.1638, 0.0308, 0.0588, -0.0109, 0.0628, -0.0412, -0.0603, 0.1131, -0.0276, -0.1137, -0.0436, 0.0407, 0.074, -0.0027, 0.1288, 0.0492, 0.015, -0.038, -0.0786, 0.071, -0.0096, -0.0719, 0.0424, -0.0402, -0.03, 0.0149, -0.02, 0.0198, -0.0398, 0.0264, -0.0646, 0.0226, -0.1117, 0.0384, -0.0536, 0.0803, 0.0263, -0.0287, 0.0501, -0.084, 0.0477, 0.0513, -0.0853, 0.069, 0.0149, 0.0205, -0.0866, -0.0932, 0.0007, 0.0516, -0.0772, 0.0433, -0.0196, 0.0588, -0.1192, 0.0434, -0.1432, -0.0494, -0.0591, -0.0849, -0.0109, 0.0259, -0.0516, 0.0327, 0.0501, 0.0841, 0.1331, -0.0449, -0.1234, -0.1025, -0.0441, -0.0664, -0.0375, 0.0967, 0.0053, -0.0094, -0.0837, -0.0522, -0.0298, 0.0471, -0.0443, -0.0133, -0.0289, 0.1113, -0.0001,
0.017, -0.0362, -0.0649, 0.0312, 0.0098, -0.056, -0.0203, -0.1457, 0.0829, -0.089, -0.0392, -0.0005, 0.0221, 0.1029, 0.0557, -0.0487, -0.0617, 0.0521, 0.0139, -0.0726, 0.0166, 0.0683, -0.074, -0.1101, 0.0131, 0.0443, 0.0267, 0.092, -0.0104, -0.0051, 0.0572, 0.1161, 0.0068, -0.0206, 0.0114, -0.0884, -0.0064, 0.0044, 0.017, 0.0119, 0.0685, -0.0887, 0.0374, 0.0613, 0.0047, 0.0824, 0.0412, -0.0018, -0.0425, 0.0399, -0.0303, -0.0843, 0.0117, 0.0592, 0.0628, 0.0256, 0.0666, -0.021, -0.0388, 0.1168, -0.0384, -0.087, -0.0219, 0.0143, -0.018, 0.0789, 0.0609, -0.0085, -0.1109, 0.0361, 0.0202, -0.0423, 0.0313, -0.0234, -0.1378, -0.0772, 0.129, -0.0511, 0.0378, 0.1046, 0.0658, -0.1097, -0.0898, -0.1398, -0.0301, -0.0137, -0.0905, -0.03, -0.0905, -0.0365, -0.0711, 0.0941, 0.0047, 0.0003, 0.022, 0.0143, -0.0393, -0.0196, -0.024, 0.0045, -0.0532, -0.0117, 0.0089, -0.0144, -0.0027, -0.0225, 0.0857, 0.0125, 0.1007, 0.0446, 0.0132, -0.055, -0.154, -0.0789, 0.0045, -0.0907, 0.0156, 0.0707, 0.0667, -0.1578, 0.0206, -0.0072, 0.0477, -0.0862, 0.0817, -0.0221, -0.0816, 0.0652, 0.0355]
np.testing.assert_almost_equal(actual_desc, expected_desc, 5)
def test_save_map_to_file(self):
"""Test save map to file."""
self.back_end.save_map_to_file(self.result)
file_exist = path.exists(self.data_directory+"result/map.dat")
self.assertEqual(file_exist, True)
def test_load_map_from_file(self):
"""Test load map from file."""
file_name = self.data_directory+"result/map.dat"
landmark_points, descriptors = load_map_from_file(file_name)
actual_landmark = landmark_points[0]
actual_desc = descriptors[0]
expected_landmark = [-4.999999944544284425e+00,
4.999999945095506604e+01, 1.000000011114795528e+00]
expected_desc = [-0.0228, 0.0062, -0.0301, 0.036, -0.0042, 0.0606, -0.0554, 0.0104, 0.0241, -0.035, -0.0211, -0.0809, -0.0347, -0.0648, -0.057, 0.0412, 0.0136, -0.054, -0.1114, 0.0901, -0.0057, -0.0057, 0.1536, 0.0046, 0.0693, 0.014, 0.0236, 0.0228, -0.0224, 0.1264, -0.0837, -0.0236, -0.0631, -0.0292, -0.079, 0.033, 0.0204, -0.0499, -0.021, 0.0188, 0.0961, -0.0149, 0.1638, 0.0308, 0.0588, -0.0109, 0.0628, -0.0412, -0.0603, 0.1131, -0.0276, -0.1137, -0.0436, 0.0407, 0.074, -0.0027, 0.1288, 0.0492, 0.015, -0.038, -0.0786, 0.071, -0.0096, -0.0719, 0.0424, -0.0402, -0.03, 0.0149, -0.02, 0.0198, -0.0398, 0.0264, -0.0646, 0.0226, -0.1117, 0.0384, -0.0536, 0.0803, 0.0263, -0.0287, 0.0501, -0.084, 0.0477, 0.0513, -0.0853, 0.069, 0.0149, 0.0205, -0.0866, -0.0932, 0.0007, 0.0516, -0.0772, 0.0433, -0.0196, 0.0588, -0.1192, 0.0434, -0.1432, -0.0494, -0.0591, -0.0849, -0.0109, 0.0259, -0.0516, 0.0327, 0.0501, 0.0841, 0.1331, -0.0449, -0.1234, -0.1025, -0.0441, -0.0664, -0.0375, 0.0967, 0.0053, -0.0094, -0.0837, -0.0522, -0.0298, 0.0471, -0.0443, -0.0133, -0.0289, 0.1113, -0.0001,
0.017, -0.0362, -0.0649, 0.0312, 0.0098, -0.056, -0.0203, -0.1457, 0.0829, -0.089, -0.0392, -0.0005, 0.0221, 0.1029, 0.0557, -0.0487, -0.0617, 0.0521, 0.0139, -0.0726, 0.0166, 0.0683, -0.074, -0.1101, 0.0131, 0.0443, 0.0267, 0.092, -0.0104, -0.0051, 0.0572, 0.1161, 0.0068, -0.0206, 0.0114, -0.0884, -0.0064, 0.0044, 0.017, 0.0119, 0.0685, -0.0887, 0.0374, 0.0613, 0.0047, 0.0824, 0.0412, -0.0018, -0.0425, 0.0399, -0.0303, -0.0843, 0.0117, 0.0592, 0.0628, 0.0256, 0.0666, -0.021, -0.0388, 0.1168, -0.0384, -0.087, -0.0219, 0.0143, -0.018, 0.0789, 0.0609, -0.0085, -0.1109, 0.0361, 0.0202, -0.0423, 0.0313, -0.0234, -0.1378, -0.0772, 0.129, -0.0511, 0.0378, 0.1046, 0.0658, -0.1097, -0.0898, -0.1398, -0.0301, -0.0137, -0.0905, -0.03, -0.0905, -0.0365, -0.0711, 0.0941, 0.0047, 0.0003, 0.022, 0.0143, -0.0393, -0.0196, -0.024, 0.0045, -0.0532, -0.0117, 0.0089, -0.0144, -0.0027, -0.0225, 0.0857, 0.0125, 0.1007, 0.0446, 0.0132, -0.055, -0.154, -0.0789, 0.0045, -0.0907, 0.0156, 0.0707, 0.0667, -0.1578, 0.0206, -0.0072, 0.0477, -0.0862, 0.0817, -0.0221, -0.0816, 0.0652, 0.0355]
np.testing.assert_almost_equal(actual_landmark, expected_landmark, 5)
np.testing.assert_almost_equal(actual_desc, expected_desc, 5)
def test_save_poses_to_file(self):
"""Test save poses to file."""
self.back_end.save_poses_to_file(self.result)
file_exist = path.exists(self.data_directory+"result/poses.dat")
self.assertEqual(file_exist, True)
def test_load_poses_from_file(self):
"""Test load psoes from file."""
file_name = self.data_directory+"result/poses.dat"
poses = load_poses_from_file(file_name)
# """Test pose output"""
delta_z = 1
num_frames = 3
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0)
for i in range(num_frames):
# Create ground truth poses
expected_pose_i = Pose3(wRc, Point3(0, delta_z*i, 2))
# Get actual poses
rotation = Rot3(np.array(poses[i][3:]).reshape(3, 3))
actual_pose_i = Pose3(rotation, Point3(np.array(poses[i][0:3])))
self.gtsamAssertEquals(actual_pose_i, expected_pose_i, 1e-6)