diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index cc7f8e474e..96bdce69b9 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -309,54 +309,77 @@ double Pose2::range(const Pose2& pose, } /* ************************************************************************* - * New explanation, from scan.ml - * It finds the angle using a linear method: - * q = Pose2::transformFrom(p) = t + R*p + * Align finds the angle using a linear method: + * a = Pose2::transformFrom(b) = t + R*b * We need to remove the centroids from the data to find the rotation - * using dp=[dpx;dpy] and q=[dqx;dqy] we have - * |dqx| |c -s| |dpx| |dpx -dpy| |c| + * using db=[dbx;dby] and a=[dax;day] we have + * |dax| |c -s| |dbx| |dbx -dby| |c| * | | = | | * | | = | | * | | = H_i*cs - * |dqy| |s c| |dpy| |dpy dpx| |s| + * |day| |s c| |dby| |dby dbx| |s| * where the Hi are the 2*2 matrices. Then we will minimize the criterion - * J = \sum_i norm(q_i - H_i * cs) + * J = \sum_i norm(a_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) + * cs = (\sum_i H_i' * a_i)/(\sum H_i'*H_i) * The hessian is diagonal and just divides by a constant, but this * normalization constant is irrelevant, since we take atan2. - * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy) + * i.e., cos ~ sum(dbx*dax + dby*day) and sin ~ sum(-dby*dax + dbx*day) * The translation is then found from the centroids - * as they also satisfy cq = t + R*cp, hence t = cq - R*cp + * as they also satisfy ca = t + R*cb, hence t = ca - R*cb */ -boost::optional align(const vector& pairs) { - - size_t n = pairs.size(); - if (n<2) return boost::none; // we need at least two pairs +boost::optional Pose2::Align(const Point2Pairs &ab_pairs) { + const size_t n = ab_pairs.size(); + if (n < 2) { + return boost::none; // we need at least 2 pairs + } // calculate centroids - Point2 cp(0,0), cq(0,0); - for(const Point2Pair& pair: pairs) { - cp += pair.first; - cq += pair.second; + Point2 ca(0, 0), cb(0, 0); + for (const Point2Pair& pair : ab_pairs) { + ca += pair.first; + cb += pair.second; } - double f = 1.0/n; - cp *= f; cq *= f; + const double f = 1.0/n; + ca *= f; + cb *= f; // calculate cos and sin - double c=0,s=0; - for(const Point2Pair& pair: pairs) { - Point2 dp = pair.first - cp; - Point2 dq = pair.second - cq; - c += dp.x() * dq.x() + dp.y() * dq.y(); - s += -dp.y() * dq.x() + dp.x() * dq.y(); + double c = 0, s = 0; + for (const Point2Pair& pair : ab_pairs) { + Point2 da = pair.first - ca; + Point2 db = pair.second - cb; + c += db.x() * da.x() + db.y() * da.y(); + s += -db.y() * da.x() + db.x() * da.y(); } // calculate angle and translation - double theta = atan2(s,c); - Rot2 R = Rot2::fromAngle(theta); - Point2 t = cq - R*cp; + const double theta = atan2(s, c); + const Rot2 R = Rot2::fromAngle(theta); + const Point2 t = ca - R*cb; return Pose2(R, t); } +boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { + if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) { + throw std::invalid_argument( + "Pose2:Align expects 2*N matrices of equal shape."); + } + Point2Pairs ab_pairs; + for (size_t j=0; j < a.cols(); j++) { + ab_pairs.emplace_back(a.col(j), b.col(j)); + } + return Pose2::Align(ab_pairs); +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +boost::optional align(const Point2Pairs& ba_pairs) { + Point2Pairs ab_pairs; + for (const Point2Pair &baPair : ba_pairs) { + ab_pairs.emplace_back(baPair.second, baPair.first); + } + return Pose2::Align(ab_pairs); +} +#endif + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 1e79836f5e..0d75113a2d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -92,6 +92,18 @@ class Pose2: public LieGroup { *this = Expmap(v); } + /** + * Create Pose2 by aligning two point pairs + * A pose aTb is estimated between pairs (a_point, b_point) such that + * a_point = aTb * b_point + * Note this allows for noise on the points but in that case the mapping + * will not be exact. + */ + static boost::optional Align(const Point2Pairs& abPointPairs); + + // Version of Pose2::Align that takes 2 matrices. + static boost::optional Align(const Matrix& a, const Matrix& b); + /// @} /// @name Testable /// @{ @@ -331,12 +343,15 @@ inline Matrix wedge(const Vector& xi) { return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval(); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** + * @deprecated Use static constructor (with reversed pairs!) * Calculate pose between a vector of 2D point correspondences (p,q) * where q = Pose2::transformFrom(p) = t + R*p */ -typedef std::pair Point2Pair; -GTSAM_EXPORT boost::optional align(const std::vector& pairs); +GTSAM_EXPORT boost::optional +GTSAM_DEPRECATED align(const Point2Pairs& pairs); +#endif template <> struct traits : public internal::LieGroup {}; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5369475976..b88d812a45 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -479,6 +479,7 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { return Pose3::Align(abPointPairs); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 boost::optional align(const Point3Pairs &baPointPairs) { Point3Pairs abPointPairs; for (const Point3Pair &baPair : baPointPairs) { @@ -486,6 +487,7 @@ boost::optional align(const Point3Pairs &baPointPairs) { } return Pose3::Align(abPointPairs); } +#endif /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 5aeac37ec5..858270c005 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -372,6 +372,9 @@ class Pose2 { Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); Pose2(Vector v); + static boost::optional Align(const gtsam::Point2Pairs& abPointPairs); + static boost::optional Align(const gtsam::Matrix& a, const gtsam::Matrix& b); + // Testable void print(string s = "") const; bool equals(const gtsam::Pose2& pose, double tol) const; @@ -424,8 +427,6 @@ class Pose2 { void serialize() const; }; -boost::optional align(const gtsam::Point2Pairs& pairs); - #include class Pose3 { // Standard Constructors diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 0df858aa8b..de779cc750 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -717,81 +717,75 @@ TEST( Pose2, range_pose ) /* ************************************************************************* */ TEST(Pose2, align_1) { - Pose2 expected(Rot2::fromAngle(0), Point2(10,10)); - - vector correspondences; - Point2Pair pq1(make_pair(Point2(0,0), Point2(10,10))); - Point2Pair pq2(make_pair(Point2(20,10), Point2(30,20))); - correspondences += pq1, pq2; - - boost::optional actual = align(correspondences); - EXPECT(assert_equal(expected, *actual)); + Pose2 expected(Rot2::fromAngle(0), Point2(10, 10)); + Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)}, + {Point2(30, 20), Point2(20, 10)}}; + boost::optional aTb = Pose2::Align(ab_pairs); + EXPECT(assert_equal(expected, *aTb)); } TEST(Pose2, align_2) { - Point2 t(20,10); + Point2 t(20, 10); Rot2 R = Rot2::fromAngle(M_PI/2.0); Pose2 expected(R, t); - vector correspondences; - Point2 p1(0,0), p2(10,0); - Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2); - EXPECT(assert_equal(Point2(20,10),q1)); - EXPECT(assert_equal(Point2(20,20),q2)); - Point2Pair pq1(make_pair(p1, q1)); - Point2Pair pq2(make_pair(p2, q2)); - correspondences += pq1, pq2; + Point2 b1(0, 0), b2(10, 0); + Point2Pairs ab_pairs {{expected.transformFrom(b1), b1}, + {expected.transformFrom(b2), b2}}; - boost::optional actual = align(correspondences); - EXPECT(assert_equal(expected, *actual)); + boost::optional aTb = Pose2::Align(ab_pairs); + EXPECT(assert_equal(expected, *aTb)); } namespace align_3 { - Point2 t(10,10); + Point2 t(10, 10); Pose2 expected(Rot2::fromAngle(2*M_PI/3), t); - Point2 p1(0,0), p2(10,0), p3(10,10); - Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3); + Point2 b1(0, 0), b2(10, 0), b3(10, 10); + Point2 a1 = expected.transformFrom(b1), + a2 = expected.transformFrom(b2), + a3 = expected.transformFrom(b3); } TEST(Pose2, align_3) { using namespace align_3; - vector correspondences; - Point2Pair pq1(make_pair(p1, q1)); - Point2Pair pq2(make_pair(p2, q2)); - Point2Pair pq3(make_pair(p3, q3)); - correspondences += pq1, pq2, pq3; + Point2Pairs ab_pairs; + Point2Pair ab1(make_pair(a1, b1)); + Point2Pair ab2(make_pair(a2, b2)); + Point2Pair ab3(make_pair(a3, b3)); + ab_pairs += ab1, ab2, ab3; - boost::optional actual = align(correspondences); - EXPECT(assert_equal(expected, *actual)); + boost::optional aTb = Pose2::Align(ab_pairs); + EXPECT(assert_equal(expected, *aTb)); } namespace { /* ************************************************************************* */ // Prototype code to align two triangles using a rigid transform /* ************************************************************************* */ - struct Triangle { size_t i_,j_,k_;}; + struct Triangle { size_t i_, j_, k_;}; - boost::optional align2(const Point2Vector& ps, const Point2Vector& qs, + boost::optional align2(const Point2Vector& as, const Point2Vector& bs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; - vector correspondences; - correspondences += make_pair(ps[t1.i_],qs[t2.i_]), make_pair(ps[t1.j_],qs[t2.j_]), make_pair(ps[t1.k_],qs[t2.k_]); - return align(correspondences); + Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]}, + {as[t1.j_], bs[t2.j_]}, + {as[t1.k_], bs[t2.k_]}}; + return Pose2::Align(ab_pairs); } } TEST(Pose2, align_4) { using namespace align_3; - Point2Vector ps,qs; - ps += p1, p2, p3; - qs += q3, q1, q2; // note in 3,1,2 order ! + Point2Vector as, bs; + as += a1, a2, a3; + bs += b3, b1, b2; // note in 3,1,2 order ! Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; - boost::optional actual = align2(ps, qs, make_pair(t1,t2)); + boost::optional actual = align2(as, bs, {t1, t2}); EXPECT(assert_equal(expected, *actual)); } diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 860487db2e..d3a51d6382 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -70,27 +70,36 @@ def test_align(self) -> None: O---O """ pts_a = [ - Point2(3, 1), - Point2(1, 1), - Point2(1, 3), - Point2(3, 3), - ] - pts_b = [ 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))) - bTa = gtsam.align(ab_pairs) - aTb = bTa.inverse() - assert aTb is not None + 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) - assert np.allclose(pt_a, pt_a_) + np.testing.assert_allclose(pt_a, pt_a_) if __name__ == "__main__":