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 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 get_T(q, L):
T = []
T.append(Pose3(Rot3.Ypr(q[0], 0.0, 0.0), Point3(0, 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, q[1], 0.0), Point3(L[0], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, q[2]), Point3(L[1], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, q[3], 0.0), Point3(L[2], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, q[4]), Point3(L[3], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, q[5], 0.0), Point3(L[4], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, q[6]), Point3(L[5], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(L[6], 0, 0)))
return T
def test_range(self):
l1 = Point3(1, 0, 0)
l2 = Point3(1, 1, 0)
x1 = Pose3()
xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
# establish range is indeed zero
self.assertEqual(1, x1.range(point=l1))
# establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
# establish range is indeed zero
self.assertEqual(1, x1.range(pose=xl1))
# establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
def interpolate(current, goal, N):
diff = delta(current, goal)
return [
Pose3(
Rot3.Ypr(0.0, 0.0, 0.0),
Point3(current.translation().x() + diff[0] * t,
current.translation().y() + diff[1] * t,
current.translation().z() + diff[2] * t))
for t in np.linspace(0, 1, N)
]
def get_pose3_vector(num_poses: int) -> Pose3Vector:
"""Generate camera 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)))
# Add third camera slightly rotated
rotatedCamera = Rot3.Ypr(0.1, 0.2, 0.1)
pose3 = pose1.compose(Pose3(rotatedCamera, Point3(0.1, -2, -0.1)))
available_poses = [pose1, pose2, pose3]
pose3_vec = Pose3Vector()
for i in range(num_poses):
pose3_vec.append(available_poses[i])
return pose3_vec
def get_fk(L, q):
T = list()
T.append(Pose3(Rot3.Ypr(q[0], 0.0, 0.0), Point3(0, 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, q[1], 0.0), Point3(L[0], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, q[2]), Point3(L[1], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, q[3], 0.0), Point3(L[2], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, q[4]), Point3(L[3], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, q[5], 0.0), Point3(L[4], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, q[6]), Point3(L[5], 0, 0)))
T.append(Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(L[6], 0, 0)))
fk = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))
for i in range(0, 8):
fk = compose(fk, T[i])
return fk
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 get_Jacobian(T, si):
J = np.array([[0, 0, 0, 0, 0, 0]]).T
for i in range(0, 7):
G = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))
for j in range(0, i):
G = compose(G, T[j])
if i == 0:
J = np.c_[J, np.expand_dims(si[i], axis=1)]
else:
J = np.c_[J, np.expand_dims(G.Adjoint(si[i]), axis=1)]
J = np.delete(J, 0, 1)
return J
def setUp(self):
""" Set up two camera poses """
# Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-np.pi / 2, 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)))
# 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 main():
# Lengths of manipulator links
lengths = [1, 1, 1, 1, 1, 1, 1]
#q = [0, 0, 0, 0, 0, 0, math.pi/4]
q = [0, 0, 0, 0, 0, 0, 0]
v = np.array([[0, 0, 1, 0, 0, 0]]).T
x_goal = [2, 5, 0]
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(7, 0, 0))
#G = compose(*(g + [gtool]))
#fk_1 = get_fk(lengths, q)
#fk_1.between(G)
# Jacobian
#check jacobian forward
#q_dot = np.array([0.5, 0, 1, 0, 0, -1, 0]).T
#J.dot(q_dot)
J = get_Jacobian(T, si)
Jinv = np.linalg.pinv(J)
d = np.matmul(Jinv, v)
q += np.squeeze(d)
fk = get_fk(lengths, q)
print fk
def test_outliers_and_far_landmarks(self) -> None:
"""Check safe triangulation function."""
pose1, pose2 = self.poses
K1 = Cal3_S2(1500, 1200, 0, 640, 480)
# create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
camera1 = PinholeCameraCal3_S2(pose1, K1)
# create second camera 1 meter to the right of first camera
K2 = Cal3_S2(1600, 1300, 0, 650, 440)
camera2 = PinholeCameraCal3_S2(pose2, K2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(self.landmark)
z2 = camera2.project(self.landmark)
cameras = CameraSetCal3_S2()
measurements = Point2Vector()
cameras.append(camera1)
cameras.append(camera2)
measurements.append(z1)
measurements.append(z2)
landmarkDistanceThreshold = 10 # landmark is closer than that
# all default except landmarkDistanceThreshold:
params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
actual: TriangulationResult = gtsam.triangulateSafe(
cameras, measurements, params)
self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
self.assertTrue(actual.valid())
landmarkDistanceThreshold = 4 # landmark is farther than that
params2 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params2)
self.assertTrue(actual.farPoint())
# 3. Add a slightly rotated third camera above with a wrong measurement
# (OUTLIER)
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
K3 = Cal3_S2(700, 500, 0, 640, 480)
camera3 = PinholeCameraCal3_S2(pose3, K3)
z3 = camera3.project(self.landmark)
cameras.append(camera3)
measurements.append(z3 + Point2(10, -10))
landmarkDistanceThreshold = 10 # landmark is closer than that
outlierThreshold = 100 # loose, the outlier is going to pass
params3 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params3)
self.assertTrue(actual.valid())
# now set stricter threshold for outlier rejection
outlierThreshold = 5 # tighter, the outlier is not going to pass
params4 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params4)
self.assertTrue(actual.outlier())
def test_pose3_roundtrip(self):
obj = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
self.assertEqualityOnPickleRoundtrip(obj)
def test_triangulation_robust_three_poses(self) -> None:
"""Ensure triangulation with a robust model works."""
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
z1: Point2 = camera1.project(landmark)
z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark)
poses = gtsam.Pose3Vector([pose1, pose2, pose3])
measurements = Point2Vector([z1, z2, z3])
# noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
# Add outlier
measurements[0] += Point2(100, 120) # very large pixel noise!
# now estimate does not match landmark
actual2 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
# DLT is surprisingly robust, but still off (actual error is around 0.26m)
self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
# Again with nonlinear optimization
actual3 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=True)
# result from nonlinear (but non-robust optimization) is close to DLT and still off
self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
# Again with nonlinear optimization, this time with robust loss
model = gtsam.noiseModel.Robust.Create(
gtsam.noiseModel.mEstimator.Huber.Create(1.345),
gtsam.noiseModel.Unit.Create(2))
actual4 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=True,
model=model)
# using the Huber loss we now have a quite small error!! nice!
self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
Cal3_S2,
Cal3Bundler,
CameraSetCal3_S2,
CameraSetCal3Bundler,
PinholeCameraCal3_S2,
PinholeCameraCal3Bundler,
Point2,
Point2Vector,
Point3,
Pose3,
Pose3Vector,
Rot3,
)
from gtsam.utils.test_case import GtsamTestCase
UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
class TestTriangulationExample(GtsamTestCase):
"""Tests for triangulation with shared and individual calibrations"""
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)
def get_fk(L, q):
T = get_T(q, L)
fk = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))
for i in T:
fk = compose(fk, i)
return fk
def main():
# Lengths of manipulator links
lengths = [1, 1, 1, 1, 1, 1, 1]
#q = [0, 0, 0, 0, 0, 0, math.pi/4]
# v = np.array([[0, 0, 1, 0, 0, 0]]).T
q = [0, 0, 0, 0, 0, 0, 0]
# Number of steps on trajectory
N = 40
y = list()
si = get_si()
T = get_T(q, lengths)
# FK init
fk = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))
goal = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(3, 0, 0))
current = get_fk(lengths, q)
poses = interpolate(current, goal, N)
min_err = 100.
for pose in poses:
current = get_fk(lengths, q)
error = delta(current, pose)
T = get_T(q, lengths)
J = get_Jacobian(T, si)
pinv_J = np.linalg.pinv(J)
#thresh_inds = pinv_J < 1e-4
#pinv_J[thresh_inds] = 0.
val_pre = np.squeeze(
np.matmul(pinv_J,
np.array([[0, 0, 0, error[0], error[1], error[2]]]).T))
ctc_vec = np.vectorize(clamp_to_circle)
val = ctc_vec(val_pre)
q += val
#print "\n-------------------------------------- current
l2_err = sum(delta(current, goal).tolist())**2
print "\n", l2_err
print current.translation()
if l2_err < min_err and l2_err != 0.:
min_err = l2_err
y.append(current.translation())
#print "\n=================== val"
#print val.tolist()
#print "\n******************* q"
#print q.tolist()
# print get_fk(lengths,q)
print "min_err:", min_err
vector = map(lambda x: list(x.vector()), y)
xs = map(lambda x: x[0], vector)
ys = map(lambda x: x[1], vector)
zs = map(lambda x: x[2], vector)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(xs, ys, zs, label='Tool Trajectory')
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
plt.show()