def create_initial_estimate(self, pose_indices):
"""Create initial estimate from ???."""
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # camera to world rotation
depth = 20
initial_estimate = gtsam.Values()
# Create dictionary for initial estimation
for img_idx, features in enumerate(self._image_features):
for kp_idx, keypoint in enumerate(features.keypoints):
if self.image_points[img_idx].kp_3d_exist(kp_idx):
landmark_id = self.image_points[img_idx].kp_3d(kp_idx)
# Filter invalid landmarks
if self._landmark_estimates[
landmark_id].seen >= self._min_landmark_seen:
key_point = Point2(keypoint[0], keypoint[1])
key = P(landmark_id)
if not initial_estimate.exists(key):
# do back-projection
pose = Pose3(wRc,
Point3(0, self.delta_z * img_idx, 2))
landmark_3d_point = self.back_projection(
key_point, pose, depth)
initial_estimate.insert(P(landmark_id),
landmark_3d_point)
# Initial estimate for poses
for idx in pose_indices:
pose_i = Pose3(wRc, Point3(0, self.delta_z * idx, 2))
initial_estimate.insert(X(idx), pose_i)
return initial_estimate
def test_sim3_pose(self):
"""Test generating similarity transform with Pose3 pairs."""
# Create expected sim3
expected_R = Rot3.Ry(math.radians(180))
expected_s = 2
expected_t = Point3(4, 6, 10)
print(expected_R)
# Create source poses
s_pose1 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])),
Point3(0, 0, 0))
s_pose2 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, 1]])),
Point3(4, 0, 0))
# Create destination poses
d_pose1 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])),
Point3(4, 6, 10))
d_pose2 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, -1]])),
Point3(-4, 6, 10))
# Align
pose_pairs = [[s_pose1, d_pose1], [s_pose2, d_pose2]]
sim3 = Similarity3()
sim3.sim3_pose(pose_pairs)
# Check actual sim3 equals to expected sim3
assert (expected_R.equals(sim3._R, 1e-6))
self.assertAlmostEqual(sim3._s, expected_s, delta=1e-6)
self.assert_gtsam_equals(sim3._t, expected_t)
def test_transform_from(self):
"""Test transform form"""
T = Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3(1, 2, 3))
pose = Pose3(Rot3(), Point3(1, 2, 3))
actual = transform_from(T, pose)
expected = Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3(2, 5, 1))
self.gtsamAssertEquals(actual, expected)
def main():
q = [0, 0, 0, 0, 0, 0, 0]
# q = [1, 1, 1, 1, 1, 1, 1]
lengths = [10, 20, 10, 20, 10, 10, 20]
si = get_si()
T = get_T(q, lengths)
# FK init
fk = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))
# expMap
g = get_expMap(q, si)
gtool = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(100, 0, 0))
G = compose(*(g + [gtool]))
fk_1 = get_fk(lengths, q)
fk_1.between(G)
# Jacobian
J = get_Jacobian(T, si)
Jinv = np.linalg.pinv(J)
#check jacobian forward
q_dot = np.array([0.5, 0, 1, 0, 0, -1, 0]).T
J.dot(q_dot)
# check with movement
v = np.array([[0, 0, 0, 0, 0, 1]]).T
d = np.matmul(Jinv, v)
q += np.squeeze(d)
fk = get_fk(lengths, q)
print fk
def createPointsLines() -> Tuple[List[Point3],List[LineSegment3D], List[np.array]]:
step = 5
points = [
Point3(-1, -1, 0), Point3(-1, 1, 0),
Point3(1, -1, 0), Point3(1, 1, 0),
Point3(-1.5, 1, -1), Point3(1.5, 1, -1),
Point3(-1.5, -1, -1), Point3(1.5, -1, -1),
Point3(-1.5, 0, -0.5), Point3(1.5, 0, -0.5)
]
lines = [
LineSegment3D(points[0], points[1]),
LineSegment3D(points[2], points[3]),
LineSegment3D(points[4], points[5]),
LineSegment3D(points[6], points[7]),
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]
def test_between(self):
"""Test between method."""
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
expected = T2.inverse().compose(T3)
actual = T2.between(T3)
self.gtsamAssertEquals(actual, expected, 1e-6)
def test_back_projection(self):
"""Test back projection"""
actual = self.back_end.back_projection(
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)
def test_back_projection(self):
"""test back projection"""
cal = Cal3DS2(fx=1, fy=1, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)
point = Point2(320, 240)
pose = Pose3(Rot3(np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]]).T),
Point3(0, 2, 1.2))
actual_point = back_projection(cal, point, pose)
actual_point.equals(Point3(10, 2, 1.2), tol=1e-9)
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 define_microphones():
"""Create microphones."""
height = 0.5
microphones = []
microphones.append(Point3(0, 0, height))
microphones.append(Point3(403 * CM, 0, height))
microphones.append(Point3(403 * CM, 403 * CM, height))
microphones.append(Point3(0, 403 * CM, 2 * height))
return microphones
def test_compute_translation_to_direction_angle_is_zero(self):
i2Ui1_measured = Unit3(Point3(1, 0, 0))
wTi2_estimated = Pose3(Rot3(), Point3(0, 0, 0))
wTi1_estimated = Pose3(Rot3(), Point3(2, 0, 0))
self.assertEqual(
geometry_comparisons.compute_translation_to_direction_angle(
i2Ui1_measured, wTi2_estimated, wTi1_estimated),
0.0,
)
def create_points():
# Create the set of ground-truth landmarks
points = [
Point3(10.0, 0.0, 0.0),
Point3(10.0, 5.0, 0.0),
Point3(10.0, 2.5, 2.5),
Point3(10.0, 0.0, 5.0),
Point3(10.0, 5.0, 5.0)
]
return points
def run(self):
reached = 0
r = rospy.Rate(10)
while not rospy.is_shutdown():
goal = self.goals[self.goalindex]
self.sTg = Pose3(Rot3.Rz(goal[3]), Point3(goal[0], goal[1],
goal[2]))
R = quaternion_matrix([
self.sTcurr_ros.pose.orientation.x,
self.sTcurr_ros.pose.orientation.y,
self.sTcurr_ros.pose.orientation.z,
self.sTcurr_ros.pose.orientation.w
])[0:3, 0:3]
self.sTcurr = Pose3(
Rot3(R),
Point3(self.sTcurr_ros.pose.position.x,
self.sTcurr_ros.pose.position.y,
self.sTcurr_ros.pose.position.z))
currTg = self.sTcurr.between(self.sTg)
u_pitch = self.kp_x * currTg.x() + self.kd_x * (
currTg.x() - self.lastT.x()) + min(
max(-0.2, self.ki_x * self.ix), 0.2)
u_roll = self.kp_y * currTg.y() + self.kd_y * (currTg.y() -
self.lastT.y())
currTg_yaw = currTg.rotation().rpy()[2]
u_yaw = self.kp_yaw * currTg_yaw + self.kd_yaw * (currTg_yaw -
self.lastyaw)
u_z = self.kp_x * currTg.z() + self.kd_z * (currTg.z() -
self.lastT.z())
self.lastT = currTg
self.lastyaw = currTg_yaw
self.ix += currTg.x()
self.iy += currTg.y()
self.iz += currTg.z()
self.iyaw += currTg_yaw
self.move_cmd.linear.x = min(max(-1, u_pitch), 1)
self.move_cmd.linear.y = min(max(-1, u_roll), 1)
self.move_cmd.linear.z = min(max(-1, u_z), 1)
self.move_cmd.angular.z = min(max(-1, u_yaw), 1)
self.velPub.publish(self.move_cmd)
reached = self.check(currTg)
if (reached == 1):
reached = 0
self.goalindex += 1
r.sleep()
def setUp(self):
"""Set up the data association object and test data."""
super().setUp()
self.obj = DummyDataAssociation(0.5, 2)
# set up ground truth data for comparison
self.dummy_corr_idxs_dict = {
(0, 1): np.array([[0, 2]]),
(1, 2): np.array([[2, 3], [4, 5], [7, 9]]),
(0, 2): np.array([[1, 8]]),
}
self.keypoints_list = [
Keypoints(coordinates=np.array([[12, 16], [13, 18], [0, 10]])),
Keypoints(coordinates=np.array([
[8, 2],
[16, 14],
[22, 23],
[1, 6],
[50, 50],
[16, 12],
[82, 121],
[39, 60],
])),
Keypoints(coordinates=np.array([
[1, 1],
[8, 13],
[40, 6],
[82, 21],
[1, 6],
[12, 18],
[15, 14],
[25, 28],
[7, 10],
[14, 17],
])),
]
# Generate two poses for use in triangulation tests
# Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
pose1 = Pose3(upright, Point3(0, 0, 1))
# create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
self.poses = Pose3Vector()
self.poses.append(pose1)
self.poses.append(pose2)
# landmark ~5 meters infront of camera
self.expected_landmark = Point3(5, 0.5, 1.2)
def test_level2(self):
# Create a level camera, looking in Y-direction
pose2 = Pose2(0.4, 0.3, math.pi / 2.0)
camera = SimpleCamera.Level(K, pose2, 0.1)
# expected
x = Point3(1, 0, 0)
y = Point3(0, 0, -1)
z = Point3(0, 1, 0)
wRc = Rot3(x, y, z)
expected = Pose3(wRc, Point3(0.4, 0.3, 0.1))
self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
def test_transformFrom(self):
"""Test transformFrom method."""
pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi / 2), Point3(2, 4, 0))
actual = pose.transformFrom(Point3(2, 1, 10))
expected = Point3(3, 2, 10)
self.gtsamAssertEquals(actual, expected, 1e-6)
# multi-point version
points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
actual_array = pose.transformFrom(points)
self.assertEqual(actual_array.shape, (3, 2))
expected_array = np.stack([expected, expected]).T
np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
def setUp(self):
"""Set up two camera poses"""
# Looking along X-axis, 1 meter above ground plane (x-y)
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
# create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
# twoPoses
self.poses = Pose3Vector()
self.poses.append(pose1)
self.poses.append(pose2)
# landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2)
def createPoses(K: Cal3_S2) -> List[Pose3]:
"""Generate a set of ground-truth camera poses arranged in a circle about the origin."""
radius = 4.0
height = -2.0
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
up = Point3(0, 0, 1) # NED -> NORTH_EAST_DOWN ???
target = Point3(0, 0, 0)
poses = []
for theta in angles:
position = Point3(radius * np.cos(theta), radius * np.sin(theta), height)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses
def plot_trajectory_verification(landmarks,
poses,
trajectory,
x_axe=30,
y_axe=30,
z_axe=30,
axis_length=2,
figure_number=0):
"""Plot the map, mapping pose results, and the generated trajectory.
Parameters:
landmarks - a list of [x,y,z]
poses - a list of [x,y,z,1*9 rotation matrix]
trajectory - a list of Pose3 object
"""
# Declare an id for the figure
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot landmark points
for landmark in landmarks:
gtsam_plot.plot_point3(figure_number,
Point3(landmark[0], landmark[1], landmark[2]),
'rx')
# Plot mapping pose results
for pose in poses:
rotation = Rot3(np.array(pose[3:]).reshape(3, 3))
actual_pose_i = Pose3(rotation, Point3(np.array(pose[0:3])))
gtsam_plot.plot_pose3(figure_number, actual_pose_i, axis_length)
gRp = actual_pose_i.rotation().matrix() # rotation from pose to global
t = actual_pose_i.translation()
axes.scatter([t.x()], [t.y()], [t.z()],
s=20,
color='red',
alpha=1,
marker="^")
# Plot cameras
for pose in trajectory:
gtsam_plot.plot_pose3(figure_number, pose, axis_length)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
axes.scatter([t.x()], [t.y()], [t.z()], s=20, color='blue', alpha=1)
# draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(-z_axe, z_axe)
plt.legend()
plt.show()
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)
def createPointsLines() -> Tuple[List[Point3], List[Line3D]]:
points = [
Point3(-1, -1, 0), Point3(-1, 1, 0),
Point3(1, -1, 0), Point3(1, 1, 0),
Point3(-1.5, 1, -1), Point3(1.5, 1, -1),
Point3(-1.5, -1, -1), Point3(1.5, -1, -1),
Point3(-1.5, 0, -0.5), Point3(1.5, 0, -0.5)
]
lines = [Line3D(points[0], points[1]), Line3D(points[0], points[1]),
Line3D(points[2], points[3]), Line3D(points[2], points[3]),
Line3D(points[4], points[5]), Line3D(points[4], points[5]),
Line3D(points[6], points[7]), Line3D(points[6], points[7]),
Line3D(points[8], points[9]), Line3D(points[8], points[9]),]
return [points, lines]
def landmark_projection(self, pose):
""" Project landmark points in the map to the given camera pose.
And filter landmark points outside the view of the current camera pose.
Parameters:
pose - gtsam.Point3, the pose of a camera
self.map - A Landmarks Object
Returns:
observed_landmarks - A Landmarks Object
"""
# Check if the atrium map is empty
assert self.map.get_length(), "the map is empty"
observed_landmarks = Landmarks([], [])
for i, landmark_point in enumerate(self.map.landmarks):
camera = gtsam.PinholeCameraCal3_S2(pose, self.calibration)
# feature is gtsam.Point2 object
landmark_point = Point3(landmark_point[0], landmark_point[1],
landmark_point[2])
feature_point = camera.project(landmark_point)
# Check if the projected feature is within the field of view.
if (feature_point.x() >= 0 and feature_point.x() < self.width
and feature_point.y() >= 0
and feature_point.y() < self.height):
observed_landmarks.append(
self.map.landmarks[i], self.map.descriptors[i],
[feature_point.x(), feature_point.y()])
return observed_landmarks
def test_serialization(self):
"""Test if serialization is working normally"""
expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
actual = Pose3()
serialized = expected.serialize()
actual.deserialize(serialized)
self.gtsamAssertEquals(expected, actual, 1e-10)
def test_compute_translation_to_direction_angle_is_nonzero(self):
rz = np.deg2rad(90)
wRi2 = Rot3.RzRyRx(0, 0,
rz) # x-axis of i2 points along y in world frame
wTi2_estimated = Pose3(wRi2, Point3(0, 0, 0))
wTi1_estimated = Pose3(Rot3(), Point3(
-1, 0,
0)) # At (0, 1, 0) in i2 frame, rotation of i1 is irrelevant here.
i2Ui1_measured = Unit3(Point3(1, 0, 0))
# Estimated relative translation of i1 in i2 frame is (0, 1, 0), and the measurement in i2 frame is (1, 0, 0).
# Expected angle between the two is 90 degrees.
self.assertTrue(
geometry_comparisons.compute_translation_to_direction_angle(
i2Ui1_measured, wTi2_estimated, wTi1_estimated),
90.0,
)
def verify_with_exact_intrinsics(
self,
keypoints_i1: Keypoints,
keypoints_i2: Keypoints,
match_indices: np.ndarray,
camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler,
) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray]:
"""Estimates the essential matrix and verifies the feature matches.
Note: this function is preferred when camera intrinsics are approximate (i.e from image size/exif). The feature
coordinates are used to compute the fundamental matrix, which is then converted to the essential matrix.
Args:
keypoints_i1: detected features in image #i1.
keypoints_i2: detected features in image #i2.
match_indices: matches as indices of features from both images, of shape (N3, 2), where N3 <= min(N1, N2).
camera_intrinsics_i1: intrinsics for image #i1.
camera_intrinsics_i2: intrinsics for image #i2.
Returns:
Estimated rotation i2Ri1, or None if it cannot be estimated.
Estimated unit translation i2Ui1, or None if it cannot be estimated.
Indices of verified correspondences, of shape (N, 2) with N <= N3. These are subset of match_indices.
"""
v_inlier_idxs = np.array([], dtype=np.uint32)
# check if we don't have the minimum number of points
if match_indices.shape[0] < self.min_pts:
return None, None, v_inlier_idxs
# set a random seed using descriptor data for repeatability
np.random.seed(
int(1000 * (match_indices[0, 0] + match_indices[0, 1]) %
(UINT32_MAX)))
# get the number of entries in the input
num_matches = match_indices.shape[0]
# get the number of verified_pts we will output
num_verifier_pts = np.random.randint(low=0, high=num_matches)
# randomly sample the indices for matches which will be verified
v_inlier_idxs = np.random.choice(num_matches,
num_verifier_pts,
replace=False).astype(np.uint32)
# use a random 3x3 matrix if the number of verified points are less that
if num_verifier_pts >= self.min_pts:
# generate random rotation and translation for essential matrix
rotation_angles = np.random.uniform(low=0.0,
high=2 * np.pi,
size=(3, ))
i2Ri1 = Rot3.RzRyRx(rotation_angles[0], rotation_angles[1],
rotation_angles[2])
i2Ti1 = Point3(np.random.uniform(low=-1.0, high=1.0, size=(3, )))
return i2Ri1, Unit3(i2Ti1), match_indices[v_inlier_idxs]
else:
return None, None, match_indices[v_inlier_idxs]
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 run(args: Namespace) -> None:
"""Run LAGO on input data stored in g2o file."""
g2oFile = gtsam.findExampleDataFile(
"noisyToyGraph.txt") if args.input is None else args.input
graph = gtsam.NonlinearFactorGraph()
graph, initial = gtsam.readG2o(g2oFile)
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8))
graph.add(PriorFactorPose2(0, Pose2(), priorModel))
print(graph)
print("Computing LAGO estimate")
estimateLago: Values = gtsam.lago.initialize(graph)
print("done!")
if args.output is None:
estimateLago.print("estimateLago")
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel = gtsam.NonlinearFactorGraph()
graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
gtsam.writeG2o(graphNoKernel, estimateLago, outputFile)
print("Done! ")
def test_landmark_projection(self):
"""Test landmark projection."""
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
pose = Pose3(wRc, Point3(0, 0, 1.2))
depth = 10
# Calculate the camera field of view area
vision_area = calculate_vision_area(self.calibration, pose, depth,
self.image_size)
points = [
vision_area[0], vision_area[1], vision_area[2], vision_area[3],
[1, 10, 10], [10, 10, 6000]
]
map_data = Landmarks(
points,
[[0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1], [1, 1, 0]])
self.trajectory_estimator.load_map(map_data)
expected_observed_landmarks = Landmarks(
points[:5],
[[0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1]])
actual_observed_landmarks = self.trajectory_estimator.landmark_projection(
pose)
if (expected_observed_landmarks == actual_observed_landmarks) is False:
print("Expected_observed_landmarks: ",
expected_observed_landmarks.landmarks,
expected_observed_landmarks.descriptors)
print("Actual_observed_landmarks: ",
actual_observed_landmarks.landmarks,
actual_observed_landmarks.descriptors)
self.assertEqual(
expected_observed_landmarks == actual_observed_landmarks, True)
def test_data(self):
"""Test functions in SfmData"""
# Create new track with 3 measurements
i1, i2, i3 = 3, 5, 6
uv_i1 = Point2(21.23, 45.64)
# translating along X-axis
uv_i2 = Point2(45.7, 45.64)
uv_i3 = Point2(68.35, 45.64)
# add measurements and arbitrary point to the track
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = Point3(1.0, 6.0, 2.0)
track2 = SfmTrack(pt)
track2.addMeasurement(i1, uv_i1)
track2.addMeasurement(i2, uv_i2)
track2.addMeasurement(i3, uv_i3)
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)
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)
