def plot_pose2_on_axes(axes, pose: Pose2, axis_length: float = 0.1, covariance: np.ndarray = None) -> None: """ Plot a 2D pose on given axis `axes` with given `axis_length`. The ellipse is scaled in such a way that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. pose: The pose to be plotted. axis_length: The length of the camera axes. covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() origin = t # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], 'r-') y_axis = origin + gRp[:, 1] * axis_length line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], 'g-') if covariance is not None: pPp = covariance[0:2, 0:2] gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) plot_covariance_ellipse_2d(axes, origin, gPp)
def setUp(self): # Create poses for the source map s_pose1 = Pose2(0, Point2(0, 0)) s_pose2 = Pose2(math.radians(-90), Point2(0, 2.5)) s_poses = [s_pose1, s_pose2] # Create points for the source map s_point1 = Point2(0.5, 0.5) s_point2 = Point2(2, 0.5) s_point3 = Point2(2, 2) s_point4 = Point2(0.5, 2) s_points = [s_point1, s_point2, s_point3, s_point4] # Create the source map self.s_map = (s_poses, s_points) # Create poses for the destination map d_pose1 = Pose2(math.radians(90), Point2(10, 10)) d_pose2 = Pose2(0, Point2(5, 10)) d_poses = [d_pose1, d_pose2] # Create points for the destination map d_point1 = Point2(9, 11) d_point2 = Point2(9, 14) d_point3 = Point2(6, 14) d_point4 = Point2(6, 11) d_points = [d_point1, d_point2, d_point3, d_point4] d_map = (d_poses, d_points) # Create the destination map self.d_map = (d_poses, d_points)
def rect_of_pose2(worldTcenter : Pose2, width=1, height=1): """Create a rectangle artist at a given Pose2 `worldTcenter`.""" centerTcorner = Pose2(-width/2, -height/2,0) worldTcorner = worldTcenter.compose(centerTcorner) rect = mpatches.Rectangle([worldTcorner.x(),worldTcorner.y()], width, height, angle = math.degrees(worldTcorner.theta())) return rect
def poe(self, q): """ Forward kinematics. Takes numpy array of joint angles, in radians. """ l1Zl1 = Pose2.Expmap(self.xi1 * q[0]) l2Zl2 = Pose2.Expmap(self.xi2 * q[1]) l3Zl3 = Pose2.Expmap(self.xi3 * q[2]) return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
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_ik(self): """Check iterative inverse kinematics function.""" # at rest actual = self.arm.ik(Pose2(0, 2 * 3.5 + 2.5, math.radians(90))) np.testing.assert_array_almost_equal(actual, Q0, decimal=2) # -30, -45, -90 sTt_desired = Pose2(5.78, 1.52, math.radians(-75)) actual = self.arm.ik(sTt_desired) self.assertPose2Equals(self.arm.fk(actual), sTt_desired) np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
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_poe_arm(self): """Make sure POE is correct for some known test configurations.""" # at rest expected = Pose2(0, 2 * 3.5 + 2.5, math.radians(90)) sTt = self.arm.poe(Q0) self.assertIsInstance(sTt, Pose2) self.assertPose2Equals(sTt, expected) # -30, -45, -90 expected = Pose2(5.78, 1.52, math.radians(-75)) sTt = self.arm.poe(Q1) self.assertPose2Equals(sTt, expected)
def robot2world(robotPose, objectPose): worldPose = Pose2(vector3(0, 0, 0)) rx, ry, rth = robotPose ox, oy, oth = objectPose rPos = Pose2(vector3(rx, ry, 0)) rAngle = Pose2(vector3(0, 0, np.radians(rth))) oPose = Pose2(vector3(ox, oy, np.radians(oth))) rPose = compose(rPos, rAngle) newPose = rPose.transformFrom(Point2(ox, oy)) x, y = newPose.x(), newPose.y() return x, y, (rth - oth) % 360
def test_rect_of_pose2_simple(self): """Test creation of a rectangle artist at a given Pose2.""" pose = Pose2(4, 5, 0) rect = rect_of_pose2(pose, width=2, height=3) self.assertEqual(rect.get_width(), 2) self.assertEqual(rect.get_height(), 3) self.gtsamAssertEquals(pose2_of_rect(rect), pose)
def run_example(): """ Use trajectory interpolation and then trajectory tracking a la Murray to move a 3-link arm on a straight line. """ # Create arm arm = ThreeLinkArm() # Get initial pose using forward kinematics q = np.radians(vector3(30, -30, 45)) sTt_initial = arm.fk(q) # Create interpolated trajectory in task space to desired goal pose sTt_goal = Pose2(2.4, 4.3, math.radians(0)) poses = trajectory(sTt_initial, sTt_goal, 50) # Setup figure and plot initial pose fignum = 0 fig = plt.figure(fignum) axes = fig.gca() axes.set_xlim(-5, 5) axes.set_ylim(0, 10) gtsam_plot.plot_pose2(fignum, arm.fk(q)) # For all poses in interpolated trajectory, calculate dq to move to next pose. # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). for pose in poses: sTt = arm.fk(q) error = delta(sTt, pose) J = arm.jacobian(q) q += np.dot(np.linalg.inv(J), error) arm.plot(fignum, q) plt.pause(0.01) plt.pause(10)
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_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_call(self): expected_pose = Pose2(1, 1, 0) def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = ge.CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) v = Values() v.insert(0, Pose2(1, 0, 0)) e = cf.error(v) self.assertEqual(e, 0.5)
def __init__(self): self.L1 = 3.5 self.L2 = 3.5 self.L3 = 2.5 self.xi1 = vector3(0, 0, 1) self.xi2 = vector3(self.L1, 0, 1) self.xi3 = vector3(self.L1 + self.L2, 0, 1) self.sXt0 = Pose2(0, self.L1 + self.L2 + self.L3, math.radians(90))
def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: | X---X | | | X---X ------------------ | | O | O | | | O---O """ pts_a = [ Point2(1, -3), Point2(1, -5), Point2(-1, -5), Point2(-1, -3), ] pts_b = [ Point2(3, 1), Point2(1, 1), Point2(1, 3), Point2(3, 3), ] # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) aTb = Pose2.Align(ab_pairs) self.assertIsNotNone(aTb) for pt_a, pt_b in zip(pts_a, pts_b): pt_a_ = aTb.transformFrom(pt_b) np.testing.assert_allclose(pt_a, pt_a_) # Matrix version A = np.array(pts_a).T B = np.array(pts_b).T aTb = Pose2.Align(A, B) self.assertIsNotNone(aTb) for pt_a, pt_b in zip(pts_a, pts_b): pt_a_ = aTb.transformFrom(pt_b) np.testing.assert_allclose(pt_a, pt_a_)
def plot(self, fignum, q): """ Plot arm. Takes figure number, and numpy array of joint angles, in radians. """ fig = plt.figure(fignum) axes = fig.gca() sXl1 = Pose2(0, 0, math.radians(90)) p1 = sXl1.translation() gtsam_plot.plot_pose2_on_axes(axes, sXl1) def plot_line(p, g, color): q = g.translation() line = np.append(p[np.newaxis], q[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], color) return q l1Zl1 = Pose2(0, 0, q[0]) l1Xl2 = Pose2(self.L1, 0, 0) sTl2 = compose(sXl1, l1Zl1, l1Xl2) p2 = plot_line(p1, sTl2, 'r-') gtsam_plot.plot_pose2_on_axes(axes, sTl2) l2Zl2 = Pose2(0, 0, q[1]) l2Xl3 = Pose2(self.L2, 0, 0) sTl3 = compose(sTl2, l2Zl2, l2Xl3) p3 = plot_line(p2, sTl3, 'g-') gtsam_plot.plot_pose2_on_axes(axes, sTl3) l3Zl3 = Pose2(0, 0, q[2]) l3Xt = Pose2(self.L3, 0, 0) sTt = compose(sTl3, l3Zl3, l3Xt) plot_line(p3, sTt, 'b-') gtsam_plot.plot_pose2_on_axes(axes, sTt)
def test_printing(self): """Tests if the factor result matches the GTSAM Pose2 unit test""" gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) noise_model = gtsam.noiseModel.Unit.Create(3) from gtsam.symbol_shorthand import X cf = CustomFactor(noise_model, [X(0), X(1)], error_func) cf_string = """CustomFactor on x0, x1 noise model: unit (3) """ self.assertEqual(cf_string, repr(cf))
def test_call(self): """Test if calling the factor works (only error)""" expected_pose = Pose2(1, 1, 0) def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) v = Values() v.insert(0, Pose2(1, 0, 0)) e = cf.error(v) self.assertEqual(e, 0.5)
def plot_pose2_on_axes(axes, pose: Pose2, axis_length: float = 0.1, covariance: np.ndarray = None) -> None: """ Plot a 2D pose on given axis `axes` with given `axis_length`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. pose: The pose to be plotted. axis_length: The length of the camera axes. covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() origin = t # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], 'r-') y_axis = origin + gRp[:, 1] * axis_length line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], 'g-') if covariance is not None: pPp = covariance[0:2, 0:2] gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) w, v = np.linalg.eig(gPp) # 5 sigma corresponds to 99.9996%, see note above k = 5.0 angle = np.arctan2(v[1, 0], v[0, 0]) e1 = patches.Ellipse(origin, np.sqrt(w[0] * k), np.sqrt(w[1] * k), np.rad2deg(angle), fill=False) axes.add_patch(e1)
def test_optimization(self): """Tests if a factor graph with a CustomFactor can be properly optimized""" gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) expected = Pose2(2, 2, np.pi / 2) def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated :param v: Values object :param H: list of references to the Jacobian arrays :return: the non-linear error """ key0 = this.keys()[0] key1 = this.keys()[1] gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) if H is not None: result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) fg = gtsam.NonlinearFactorGraph() fg.add(cf) fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model)) v = Values() v.insert(0, Pose2(0, 0, 0)) v.insert(1, Pose2(0, 0, 0)) params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params) result = optimizer.optimize() self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5) self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
def test_jacobian(self): """Tests if the factor result matches the GTSAM Pose2 unit test""" gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) expected = Pose2(2, 2, np.pi / 2) def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """ the custom error function. One can freely use variables captured from the outside scope. Or the variables can be acquired by indexing `v`. Jacobian is passed by modifying the H array of numpy matrices. """ key0 = this.keys()[0] key1 = this.keys()[1] gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) if H is not None: result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) v = Values() v.insert(0, gT1) v.insert(1, gT2) bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model) gf = cf.linearize(v) gf_b = bf.linearize(v) J_cf, b_cf = gf.jacobian() J_bf, b_bf = gf_b.jacobian() np.testing.assert_allclose(J_cf, J_bf) np.testing.assert_allclose(b_cf, b_bf)
def test_no_jacobian(self): """Tests that we will not calculate the Jacobian if not requested""" gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) expected = Pose2(2, 2, np.pi / 2) def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated :param v: Values object :param H: list of references to the Jacobian arrays :return: the non-linear error """ key0 = this.keys()[0] key1 = this.keys()[1] gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) self.assertTrue( H is None) # Should be true if we only request the error if H is not None: result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) v = Values() v.insert(0, gT1) v.insert(1, gT2) bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model) e_cf = cf.error(v) e_bf = bf.error(v) np.testing.assert_allclose(e_cf, e_bf)
def pose(self, pose): """ Calculate the similarity transform of a Pose2 Parameters: pose - Pose2 object Returns: Pose2 object """ R = self._R.compose(pose.rotation()) translation = self._s * \ self._R.rotate(pose.translation()).vector() + self._t.vector() return Pose2(R.theta(), Point2(translation))
def world2robot(robotPose: tuple, objectPose: tuple) -> tuple: #transforms objectPose from world frame to robot frame #objectPose: pose of object in robot pose #robotPose: pose of robot base in world frame rx, ry, rth = robotPose ox, oy, oth = objectPose rPos = Pose2(vector3( rx, ry, )) rAngle = Pose2(vector3(0, 0, np.radians(rth))) oPose = Pose2(vector3(ox, oy, np.radians(oth))) rPose = compose(rPos, rAngle) newPose = rPose.transformTo(Point2(ox, oy)) x, y = newPose.x(), newPose.y() print(newPose) return x, y, (oth - rth) % 360
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 trajectory(g0, g1, N=20): """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately. g0 and g1 are the initial and final pose, respectively. N is the number of *intervals* Returns N+1 poses """ e = delta(g0, g1) return [ Pose2(g0.x() + e[0] * t, g0.y() + e[1] * t, g0.theta() + e[2] * t) for t in np.linspace(0, 1, N) ]
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_transformFrom(self): """Test transformFrom method.""" pose = Pose2(2, 4, -math.pi / 2) actual = pose.transformFrom(Point2(2, 1)) expected = Point2(3, 2) self.gtsamAssertEquals(actual, expected, 1e-6) # multi-point version points = np.stack([Point2(2, 1), Point2(2, 1)]).T actual_array = pose.transformFrom(points) self.assertEqual(actual_array.shape, (2, 2)) expected_array = np.stack([expected, expected]).T np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)