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()