def test_align_poses_along_straight_line(self) -> None:
"""Test Align Pose2Pairs method.
Scenario:
3 object poses
same scale (no gauge ambiguity)
world frame has poses rotated about 180 degrees.
world and egovehicle frame translated by 15 meters w.r.t. each other
"""
R180 = Rot2.fromDegrees(180)
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose2(Rot2(), np.array([5, 0]))
eTo1 = Pose2(Rot2(), np.array([10, 0]))
eTo2 = Pose2(Rot2(), np.array([15, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world "w" frame)
wTo0 = Pose2(R180, np.array([-10, 0]))
wTo1 = Pose2(R180, np.array([-5, 0]))
wTo2 = Pose2(R180, np.array([0, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Scenario:
3 object poses
with gauge ambiguity (2x scale)
world frame has poses rotated by 90 degrees.
world and egovehicle frame translated by 11 meters w.r.t. each other
"""
R90 = Rot2.fromDegrees(90)
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose2(Rot2(), np.array([1, 0]))
eTo1 = Pose2(Rot2(), np.array([2, 0]))
eTo2 = Pose2(Rot2(), np.array([4, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose2(R90, np.array([0, 12]))
wTo1 = Pose2(R90, np.array([0, 14]))
wTo2 = Pose2(R90, np.array([0, 18]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
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 test_rotation(self) -> None:
"""Ensure rotation component is returned properly."""
R = Rot2.fromDegrees(90)
t = np.array([1, 2])
bSa = Similarity2(R=R, t=t, s=3.0)
# evaluates to [[0, -1], [1, 0]]
expected_R = Rot2.fromDegrees(90)
np.testing.assert_allclose(expected_R.matrix(), bSa.rotation().matrix())
def test_constructorBetweenFactorPose2s(self) -> None:
"""Check if ShonanAveraging2 constructor works when not initialized from g2o file.
GT pose graph:
| cam 1 = (0,4)
--o
| .
. .
. .
| |
o-- ... o--
cam 0 cam 2 = (4,0)
(0,0)
"""
num_images = 3
wTi_list = [
Pose2(Rot2.fromDegrees(0), np.array([0, 0])),
Pose2(Rot2.fromDegrees(90), np.array([0, 4])),
Pose2(Rot2.fromDegrees(0), np.array([4, 0])),
]
edges = [(0, 1), (1, 2), (0, 2)]
i2Ri1_dict = {(i1, i2):
wTi_list[i2].inverse().compose(wTi_list[i1]).rotation()
for (i1, i2) in edges}
lm_params = LevenbergMarquardtParams.CeresDefaults()
shonan_params = ShonanAveragingParameters2(lm_params)
shonan_params.setUseHuber(False)
shonan_params.setCertifyOptimality(True)
noise_model = gtsam.noiseModel.Unit.Create(3)
between_factors = gtsam.BetweenFactorPose2s()
for (i1, i2), i2Ri1 in i2Ri1_dict.items():
i2Ti1 = Pose2(i2Ri1, np.zeros(2))
between_factors.append(
BetweenFactorPose2(i2, i1, i2Ti1, noise_model))
obj = ShonanAveraging2(between_factors, shonan_params)
initial = obj.initializeRandomly()
result_values, _ = obj.run(initial, min_p=2, max_p=100)
wRi_list = [result_values.atRot2(i) for i in range(num_images)]
thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
# map all angles to [0,360)
thetas_deg = thetas_deg % 360
thetas_deg -= thetas_deg[0]
expected_thetas_deg = np.array([0.0, 90.0, 0.0])
np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
def test_scale(self) -> None:
"""Ensure the scale factor is returned properly."""
bRa = Rot2()
bta = np.array([1, 2])
bsa = 3.0
bSa = Similarity2(R=bRa, t=bta, s=bsa)
self.assertEqual(bSa.scale(), 3.0)
def animate(i): global pose rect = rect_of_rot2(Rot2(i*omega), width=0.4, height=0.4) ax.clear() ax.set_xlim((-size, size)) ax.set_ylim((-size, size)) ax.add_artist(rect)
def test_align_case_3(self):
"""Test generating similarity transform with gtsam pose2 align test case 3 - transformation of a triangle."""
# Create expected sim2
expected_R = Rot2(math.radians(120))
expected_s = 1
expected_t = Point2(10, 10)
expected = Pose2(math.radians(120), expected_t)
# Create source points
s_point1 = Point2(0, 0)
s_point2 = Point2(10, 0)
s_point3 = Point2(10, 10)
# Create destination points
d_point1 = expected.transform_from(s_point1)
d_point2 = expected.transform_from(s_point2)
d_point3 = expected.transform_from(s_point3)
# Align
point_pairs = [[s_point1, d_point1], [s_point2, d_point2],
[s_point3, d_point3]]
sim2 = Similarity2()
sim2.align(point_pairs)
# Check actual sim2 equals to expected sim2
assert (expected_R.equals(sim2._R, 1e-6))
self.assert_gtsam_equals(sim2._t, expected_t)
self.assertAlmostEqual(sim2._s, expected_s, delta=1e-6)
def test_translation(self) -> None:
"""Ensure translation component is returned properly."""
R = Rot2.fromDegrees(90)
t = np.array([1, 2])
bSa = Similarity2(R=R, t=t, s=3.0)
expected_t = np.array([1, 2])
np.testing.assert_allclose(expected_t, bSa.translation())
def test_constructor(self) -> None:
"""Sim(2) to perform p_b = bSa * p_a"""
bRa = Rot2()
bta = np.array([1, 2])
bsa = 3.0
bSa = Similarity2(R=bRa, t=bta, s=bsa)
self.assertIsInstance(bSa, Similarity2)
np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix())
np.testing.assert_allclose(bSa.translation(), bta)
np.testing.assert_allclose(bSa.scale(), bsa)
def test_transform(self):
"""Test similarity transform on poses and points."""
expected_poses, expected_points = self.d_map
sim2 = Similarity2(Rot2(math.radians(90)), Point2(10, 10), 2)
poses, points = self.s_map
for i, point in enumerate(points):
point = sim2.point(point)
self.assert_gtsam_equals(expected_points[i], point)
for i, pose in enumerate(poses):
pose = sim2.pose(pose)
self.assert_gtsam_equals(expected_poses[i], pose)
def test_align_poses_scaled_squares(self):
"""Test if Align Pose2Pairs method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
Scenario:
4 object poses
with gauge ambiguity (2.5x scale)
"""
# 0, 90, 180, and 270 degrees yaw
R0 = Rot2.fromDegrees(0)
R90 = Rot2.fromDegrees(90)
R180 = Rot2.fromDegrees(180)
R270 = Rot2.fromDegrees(270)
aTi0 = Pose2(R0, np.array([2, 3]))
aTi1 = Pose2(R90, np.array([12, 3]))
aTi2 = Pose2(R180, np.array([12, 13]))
aTi3 = Pose2(R270, np.array([2, 13]))
aTi_list = [aTi0, aTi1, aTi2, aTi3]
bTi0 = Pose2(R0, np.array([4, 3]))
bTi1 = Pose2(R90, np.array([8, 3]))
bTi2 = Pose2(R180, np.array([8, 7]))
bTi3 = Pose2(R270, np.array([4, 7]))
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity2.Align(ab_pairs)
for aTi, bTi in zip(aTi_list, bTi_list):
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
def test_align(self):
"""Test generating similarity transform with Point2 pairs."""
# Create expected sim2
expected_R = Rot2(math.radians(90))
expected_s = 2
expected_t = Point2(10, 10)
# Create source points
s_point1 = Point2(0.5, 0.5)
s_point2 = Point2(2, 0.5)
# Create destination points
d_point1 = Point2(9, 11)
d_point2 = Point2(9, 14)
# Align
point_pairs = [[s_point1, d_point1], [s_point2, d_point2]]
sim2 = Similarity2()
sim2.align(point_pairs)
# Check actual sim2 equals to expected sim2
assert (expected_R.equals(sim2._R, 1e-6))
self.assertEqual(sim2._s, expected_s)
self.assert_gtsam_equals(sim2._t, expected_t)
def test_align_case_1(self):
"""Test generating similarity transform with gtsam pose2 align test case 1 - translation only."""
# Create expected sim2
expected_R = Rot2(math.radians(0))
expected_s = 1
expected_t = Point2(10, 10)
# Create source points
s_point1 = Point2(0, 0)
s_point2 = Point2(20, 10)
# Create destination points
d_point1 = Point2(10, 10)
d_point2 = Point2(30, 20)
# Align
point_pairs = [[s_point1, d_point1], [s_point2, d_point2]]
sim2 = Similarity2()
sim2.align(point_pairs)
# Check actual sim2 equals to expected sim2
assert (expected_R.equals(sim2._R, 1e-6))
self.assert_gtsam_equals(sim2._t, expected_t)
self.assertEqual(sim2._s, expected_s)
def align(self, point_pairs):
"""
Generate similarity transform with Point2 pairs.
This function is developed based on gtsam::Pose2::align() with modification to calculate the scale.
*************************************************************************
It finds the angle using a linear method:
q = Pose2::transform_from(p) = t + R*p
Subtract the centroid from each point
q1 = R*p1+t -> (q1-c_q) = R * (p1-c_p)
c_q = R*c_p+t
using dp=[dpx;dpy] and q=[dqx;dqy] we have
|dqx| |c -s| |dpx| |dpx -dpy| |c|
| | = | | * | | = | | * | | = H_i*cs
|dqy| |s c| |dpy| |dpy dpx| |s|
where the Hi are the 2*2 matrices. Then we will minimize the criterion
J = \sum_i norm(q_i - H_i * cs)
Taking the derivative with respect to cs and setting to zero we have
cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
\sum_i H_i'*H_i = (\sum_i dpx_i^2+dpy_i^2) |1 0|
| |
|0 1|
The hessian is diagonal and just divides by a constant.
i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
The scale is the square root of the determinant of the rotation matrix because the rotation matrix is an orthogonal matrix.
The translation is then found from the centroids
as they also satisfy cq = t + sR*cp, hence t = cq - sR*cp
"""
n = len(point_pairs)
assert n >= 2 # we need at least two pairs
# calculate centroids
s_center = Point2(0, 0)
d_center = Point2(0, 0)
for point_pair in point_pairs:
s_center = Point2(s_center.x() + point_pair[0].x(),
s_center.y() + point_pair[0].y())
d_center = Point2(d_center.x() + point_pair[1].x(),
d_center.y() + point_pair[1].y())
f = 1.0 / n
s_center = Point2(s_center.x() * f, s_center.y() * f)
d_center = Point2(d_center.x() * f, d_center.y() * f)
# calculate cos and sin
c = 0
s = 0
constant = 0
for point_pair in point_pairs:
s_d = Point2(point_pair[0].x() - s_center.x(),
point_pair[0].y() - s_center.y())
d_d = Point2(point_pair[1].x() - d_center.x(),
point_pair[1].y() - d_center.y())
c += s_d.x() * d_d.x() + s_d.y() * d_d.y()
s += -s_d.y() * d_d.x() + s_d.x() * d_d.y()
constant += s_d.x() * s_d.x() + s_d.y() * s_d.y()
c /= constant
s /= constant
# calculate angle, scale, and translation
theta = math.atan2(s, c)
R = Rot2.fromAngle(theta)
determinant = np.linalg.det(np.array([[c, -s], [s, c]]))
if (determinant >= 0):
scale = determinant**(1 / 2)
elif (determinant < 0):
scale = -(-determinant)**(1 / 2)
t = R.rotate(s_center)
t = Point2(d_center.x() - scale * t.x(), d_center.y() - scale * t.y())
self._R = R
self._t = t
self._s = scale
def test_is_eq(self) -> None:
"""Ensure object equality works properly (are equal)."""
bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
self.gtsamAssertEquals(bSa, bSa_)
def __init__(self, rotation=Rot2(), translation=Point2(0, 0), scale=1):
"""Construct from rotation, translation, and scale."""
self._R = rotation
self._t = translation
self._s = scale
def test_not_eq_translation(self) -> None:
"""Ensure object equality works properly (not equal translation)."""
bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
self.assertNotEqual(bSa, bSa_)
def rect_of_rot2(worldRcenter : Rot2, width=1, height=1): """Create a rectangle artist at a given Pose2 `worldTcenter`.""" worldTcenter = Pose2(0, 0, worldRcenter.theta()) return rect_of_pose2(worldTcenter)
def test_not_eq_rotation(self) -> None:
"""Ensure object equality works properly (not equal rotation)."""
bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3)
self.assertNotEqual(bSa, bSa_)
def init(): rect = rect_of_rot2(Rot2(0), width=0.4, height=0.4) return (rect,)
self.assertEqual(rect.get_height(), 3)
self.gtsamAssertEquals(pose2_of_rect(rect), pose)
def test_rect_of_pose2_rotated(self):
"""Test creation of a rectangle artist at a given Pose2."""
pose = Pose2(4, 5, math.radians(30))
rect = rect_of_pose2(pose, width=2, height=3)
self.gtsamAssertEquals(pose2_of_rect(rect), pose)
if __name__ == '__main__':
unittest.main(argv=['first-arg-is-ignored'], exit=False)
## $SO(2)$
The simplest of all groups is $SO(2)$, the group of rotations in the plane. In GTSAM, these are represented using `Rot2`.
R=Rot2(math.radians(45)); R.matrix()
fig, ax = plt.subplots()
size=1
ax.set_xlim((-size, size))
ax.set_ylim((-size, size))
ax.add_artist(rect_of_rot2(Rot2(math.radians(20))))
plt.show()
# First set up the figure, the axis, and the plot element we want to animate
fig, ax = plt.subplots()
plt.close()
N=32
size=1.5
ax.set_xlim((-size, size))
ax.set_ylim((-size, size))
def test_not_eq_scale(self) -> None:
"""Ensure object equality works properly (not equal scale)."""
bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0)
self.assertNotEqual(bSa, bSa_)