From b9174ae0f4e4a6835151da8d1751beb2db5174ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Oct 2020 11:24:26 -0400 Subject: [PATCH 1/3] Changed name to avoid template confusion in VC 2016 --- gtsam/geometry/Point3.cpp | 12 +++++------- gtsam/geometry/Point3.h | 4 ++-- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce4ceee895..8665f77169 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -75,17 +75,15 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } -Point3Pair mean(const std::vector &abPointPairs) { +Point3Pair means(const std::vector &abPointPairs) { const size_t n = abPointPairs.size(); - Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + Point3 aSum(0, 0, 0), bSum(0, 0, 0); for (const Point3Pair &abPair : abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; + aSum += abPair.first; + bSum += abPair.second; } const double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; - return make_pair(aCentroid, bCentroid); + return {aSum * f, bSum * f}; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7f58497e90..33b5836f82 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -67,8 +67,8 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { return sum / points.size(); } -/// mean of Point3 pair -GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); +/// mean of Point3 pair +GTSAM_EXPORT Point3Pair means(const std::vector &abPointPairs); template struct Range; From eb4f5288e9b31dd749d8173dd287eac2f1a94a71 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Oct 2020 12:01:33 -0400 Subject: [PATCH 2/3] Clean up code and tests and use "means" --- gtsam/geometry/Pose3.cpp | 65 ++++++++++++------------- gtsam/geometry/tests/testPoint3.cpp | 16 +++--- gtsam_unstable/geometry/Similarity3.cpp | 49 +++++++++++-------- 3 files changed, 69 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ea822b7968..22849d4f58 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -24,10 +24,11 @@ #include #include -using namespace std; - namespace gtsam { +using std::vector; +using Point3Pairs = vector; + /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); @@ -212,18 +213,20 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (std::abs(phi)>nearZeroThreshold) { - const double sinPhi = sin(phi), cosPhi = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + const Matrix3 WVW = W * V * W; + if (std::abs(phi) > nearZeroThreshold) { + const double s = sin(phi), c = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, + phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); - } - else { - Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - - 1./24.*(W*W*V + V*W*W - 3*W*V*W) - + 1./120.*(W*V*W*W + W*W*V*W); + Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + + (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * + (WVW * W + W * WVW); + } else { + Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - + 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + + 1. / 120. * (WVW * W + W * WVW); } #endif @@ -381,39 +384,33 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, } /* ************************************************************************* */ -boost::optional Pose3::Align(const std::vector& abPointPairs) { +boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { const size_t n = abPointPairs.size(); - if (n < 3) - return boost::none; // we need at least three pairs + if (n < 3) { + return boost::none; // we need at least three pairs + } // calculate centroids - Point3 aCentroid(0,0,0), bCentroid(0,0,0); - for(const Point3Pair& abPair: abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; - } - double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; + const auto centroids = means(abPointPairs); // Add to form H matrix Matrix3 H = Z_3x3; - for(const Point3Pair& abPair: abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; + for (const Point3Pair &abPair : abPointPairs) { + const Point3 da = abPair.first - centroids.first; + const Point3 db = abPair.second - centroids.second; H += da * db.transpose(); - } + } // ClosestTo finds rotation matrix closest to H in Frobenius sense - Rot3 aRb = Rot3::ClosestTo(H); - Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + const Rot3 aRb = Rot3::ClosestTo(H); + const Point3 aTb = centroids.first - aRb * centroids.second; return Pose3(aRb, aTb); } -boost::optional align(const vector& baPointPairs) { - vector abPointPairs; - for (const Point3Pair& baPair: baPointPairs) { - abPointPairs.push_back(make_pair(baPair.second, baPair.first)); +boost::optional align(const Point3Pairs &baPointPairs) { + Point3Pairs abPointPairs; + for (const Point3Pair &baPair : baPointPairs) { + abPointPairs.emplace_back(baPair.second, baPair.first); } return Pose3::Align(abPointPairs); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a481a80722..a655011a03 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -166,22 +166,22 @@ TEST (Point3, normalize) { //************************************************************************* TEST(Point3, mean) { - Point3 expected_a_mean(2, 2, 2); + Point3 expected(2, 2, 2); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); std::vector a_points{a1, a2, a3}; - Point3 actual_a_mean = mean(a_points); - EXPECT(assert_equal(expected_a_mean, actual_a_mean)); + Point3 actual = mean(a_points); + EXPECT(assert_equal(expected, actual)); } TEST(Point3, mean_pair) { Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0); - Point3Pair expected_mean = std::make_pair(a_mean, b_mean); + Point3Pair expected = std::make_pair(a_mean, b_mean); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); - std::vector point_pairs{{a1,b1},{a2,b2},{a3,b3}}; - Point3Pair actual_mean = mean(point_pairs); - EXPECT(assert_equal(expected_mean.first, actual_mean.first)); - EXPECT(assert_equal(expected_mean.second, actual_mean.second)); + std::vector point_pairs{{a1, b1}, {a2, b2}, {a3, b3}}; + Point3Pair actual = means(point_pairs); + EXPECT(assert_equal(expected.first, actual.first)); + EXPECT(assert_equal(expected.second, actual.second)); } //************************************************************************* diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index b2d7dc080d..819c51fee5 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,10 +23,14 @@ namespace gtsam { +using std::vector; +using PointPairs = vector; + namespace { /// Subtract centroids from point pairs. -static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3Pair& centroids) { - std::vector d_abPointPairs; +static PointPairs subtractCentroids(const PointPairs &abPointPairs, + const Point3Pair ¢roids) { + PointPairs d_abPointPairs; for (const Point3Pair& abPair : abPointPairs) { Point3 da = abPair.first - centroids.first; Point3 db = abPair.second - centroids.second; @@ -36,7 +40,8 @@ static std::vector subtractCentroids(const std::vector& } /// Form inner products x and y and calculate scale. -static const double calculateScale(const std::vector& d_abPointPairs, const Rot3& aRb) { +static const double calculateScale(const PointPairs &d_abPointPairs, + const Rot3 &aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { @@ -50,7 +55,7 @@ static const double calculateScale(const std::vector& d_abPointPairs } /// Form outer product H. -static Matrix3 calculateH(const std::vector& d_abPointPairs) { +static Matrix3 calculateH(const PointPairs &d_abPointPairs) { Matrix3 H = Z_3x3; for (const Point3Pair& d_abPair : d_abPointPairs) { H += d_abPair.first * d_abPair.second.transpose(); @@ -59,7 +64,8 @@ static Matrix3 calculateH(const std::vector& d_abPointPairs) { } /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. -static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { +static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb, + const Point3Pair ¢roids) { const double s = calculateScale(d_abPointPairs, aRb); const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; return Similarity3(aRb, aTb, s); @@ -67,8 +73,9 @@ static Similarity3 align(const std::vector& d_abPointPairs, const Ro /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { - auto centroids = mean(abPointPairs); +static Similarity3 alignGivenR(const PointPairs &abPointPairs, + const Rot3 &aRb) { + auto centroids = means(abPointPairs); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); return align(d_abPointPairs, aRb, centroids); } @@ -147,10 +154,12 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -Similarity3 Similarity3::Align(const std::vector& abPointPairs) { - // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 - if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); - auto centroids = mean(abPointPairs); +Similarity3 Similarity3::Align(const PointPairs &abPointPairs) { + // Refer to Chapter 3 of + // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + if (abPointPairs.size() < 3) + throw std::runtime_error("input should have at least 3 pairs of points"); + auto centroids = means(abPointPairs); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); Matrix3 H = calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense @@ -158,17 +167,18 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { return align(d_abPointPairs, aRb, centroids); } -Similarity3 Similarity3::Align(const std::vector& abPosePairs) { +Similarity3 Similarity3::Align(const vector &abPosePairs) { const size_t n = abPosePairs.size(); - if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); + if (n < 2) + throw std::runtime_error("input should have at least 2 pairs of poses"); // calculate rotation vector rotations; - vector abPointPairs; + PointPairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); Pose3 wTa, wTb; - for (const Pose3Pair& abPair : abPosePairs) { + for (const Pose3Pair &abPair : abPosePairs) { std::tie(wTa, wTb) = abPair; rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); @@ -178,7 +188,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { return alignGivenR(abPointPairs, aRb); } -Matrix4 Similarity3::wedge(const Vector7& xi) { +Matrix4 Similarity3::wedge(const Vector7 &xi) { // http://www.ethaneade.org/latex2html/lie/node29.html const auto w = xi.head<3>(); const auto u = xi.segment<3>(3); @@ -217,12 +227,13 @@ Matrix3 Similarity3::GetV(Vector3 w, double lambda) { W = 1.0 / 24.0 - theta2 / 720.0; } const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda; + const double expMinLambda = exp(-lambda); double A, alpha = 0.0, beta, mu; if (lambda2 > 1e-9) { - A = (1.0 - exp(-lambda)) / lambda; + A = (1.0 - expMinLambda) / lambda; alpha = 1.0 / (1.0 + theta2 / lambda2); - beta = (exp(-lambda) - 1 + lambda) / lambda2; - mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3; + beta = (expMinLambda - 1 + lambda) / lambda2; + mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3; } else { A = 1.0 - lambda / 2.0 + lambda2 / 6.0; beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0; From cef937e09d8505068ad503ab7a6248a194190536 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 1 Oct 2020 20:21:14 -0400 Subject: [PATCH 3/3] Update Point3.h Fix doxygen comment --- gtsam/geometry/Point3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b425af8a40..57188fc5ee 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { return sum / points.size(); } -/// mean of Point3 pair +/// Calculate the two means of a set of Point3 pairs GTSAM_EXPORT Point3Pair means(const std::vector &abPointPairs); template