12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Clean up code and tests and use "means"

release/4.3a0
Frank Dellaert 2020-10-01 12:01:33 -04:00
parent b9174ae0f4
commit eb4f5288e9
3 changed files with 69 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam/geometry/Pose3.cpp
View File

@ -24,10 +24,11 @@
#include <limits> #include <limits>
#include <string> #include <string>
using namespace std;
namespace gtsam { namespace gtsam {
using std::vector;
using Point3Pairs = vector<Point3Pair>;
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3);
@ -212,18 +213,20 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
#else #else
// The closed-form formula in Barfoot14tro eq. (102) // The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm(); double phi = w.norm();
if (std::abs(phi)>nearZeroThreshold) { const Matrix3 WVW = W * V * W;
const double sinPhi = sin(phi), cosPhi = cos(phi); if (std::abs(phi) > nearZeroThreshold) {
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; 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 // 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) Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
+ (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
- 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
} (WVW * W + W * WVW);
else { } else {
Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
- 1./24.*(W*W*V + V*W*W - 3*W*V*W) 1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
+ 1./120.*(W*V*W*W + W*W*V*W); 1. / 120. * (WVW * W + W * WVW);
} }
#endif #endif
@ -381,39 +384,33 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) { boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
const size_t n = abPointPairs.size(); const size_t n = abPointPairs.size();
if (n < 3) if (n < 3) {
return boost::none; // we need at least three pairs return boost::none; // we need at least three pairs
}
// calculate centroids // calculate centroids
Point3 aCentroid(0,0,0), bCentroid(0,0,0); const auto centroids = means(abPointPairs);
for(const Point3Pair& abPair: abPointPairs) {
aCentroid += abPair.first;
bCentroid += abPair.second;
}
double f = 1.0 / n;
aCentroid *= f;
bCentroid *= f;
// Add to form H matrix // Add to form H matrix
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
for(const Point3Pair& abPair: abPointPairs) { for (const Point3Pair &abPair : abPointPairs) {
Point3 da = abPair.first - aCentroid; const Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - bCentroid; const Point3 db = abPair.second - centroids.second;
H += da * db.transpose(); H += da * db.transpose();
} }
// ClosestTo finds rotation matrix closest to H in Frobenius sense // ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot3 aRb = Rot3::ClosestTo(H); const Rot3 aRb = Rot3::ClosestTo(H);
Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); const Point3 aTb = centroids.first - aRb * centroids.second;
return Pose3(aRb, aTb); return Pose3(aRb, aTb);
} }
boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) { boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
vector<Point3Pair> abPointPairs; Point3Pairs abPointPairs;
for (const Point3Pair& baPair: baPointPairs) { for (const Point3Pair &baPair : baPointPairs) {
abPointPairs.push_back(make_pair(baPair.second, baPair.first)); abPointPairs.emplace_back(baPair.second, baPair.first);
} }
return Pose3::Align(abPointPairs); return Pose3::Align(abPointPairs);
} }

16
gtsam/geometry/tests/testPoint3.cpp
View File

@ -166,22 +166,22 @@ TEST (Point3, normalize) {
//************************************************************************* //*************************************************************************
TEST(Point3, mean) { 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); Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
std::vector<Point3> a_points{a1, a2, a3}; std::vector<Point3> a_points{a1, a2, a3};
Point3 actual_a_mean = mean(a_points); Point3 actual = mean(a_points);
EXPECT(assert_equal(expected_a_mean, actual_a_mean)); EXPECT(assert_equal(expected, actual));
} }
TEST(Point3, mean_pair) { TEST(Point3, mean_pair) {
Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0); 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 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); Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
std::vector<Point3Pair> point_pairs{{a1,b1},{a2,b2},{a3,b3}}; std::vector<Point3Pair> point_pairs{{a1, b1}, {a2, b2}, {a3, b3}};
Point3Pair actual_mean = mean(point_pairs); Point3Pair actual = means(point_pairs);
EXPECT(assert_equal(expected_mean.first, actual_mean.first)); EXPECT(assert_equal(expected.first, actual.first));
EXPECT(assert_equal(expected_mean.second, actual_mean.second)); EXPECT(assert_equal(expected.second, actual.second));
} }
//************************************************************************* //*************************************************************************

49
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -23,10 +23,14 @@
namespace gtsam { namespace gtsam {
using std::vector;
using PointPairs = vector<Point3Pair>;
namespace { namespace {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3Pair& centroids) { static PointPairs subtractCentroids(const PointPairs &abPointPairs,
std::vector<Point3Pair> d_abPointPairs; const Point3Pair &centroids) {
PointPairs d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) { for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first; Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second; Point3 db = abPair.second - centroids.second;
@ -36,7 +40,8 @@ static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>&
} }
/// Form inner products x and y and calculate scale. /// Form inner products x and y and calculate scale.
static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) { static const double calculateScale(const PointPairs &d_abPointPairs,
const Rot3 &aRb) {
double x = 0, y = 0; double x = 0, y = 0;
Point3 da, db; Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) { for (const Point3Pair& d_abPair : d_abPointPairs) {
@ -50,7 +55,7 @@ static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs
} }
/// Form outer product H. /// Form outer product H.
static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) { static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) { for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose(); H += d_abPair.first * d_abPair.second.transpose();
@ -59,7 +64,8 @@ static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
} }
/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. /// 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<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
const Point3Pair &centroids) {
const double s = calculateScale(d_abPointPairs, aRb); const double s = calculateScale(d_abPointPairs, aRb);
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s); return Similarity3(aRb, aTb, s);
@ -67,8 +73,9 @@ static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Ro
/// This method estimates the similarity transform from point pairs, given a known or estimated rotation. /// 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 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
static Similarity3 alignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) { static Similarity3 alignGivenR(const PointPairs &abPointPairs,
auto centroids = mean(abPointPairs); const Rot3 &aRb) {
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids); return align(d_abPointPairs, aRb, centroids);
} }
@ -147,10 +154,12 @@ Point3 Similarity3::operator*(const Point3& p) const {
return transformFrom(p); return transformFrom(p);
} }
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) { Similarity3 Similarity3::Align(const PointPairs &abPointPairs) {
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 // Refer to Chapter 3 of
if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
auto centroids = mean(abPointPairs); 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); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
Matrix3 H = calculateH(d_abPointPairs); Matrix3 H = calculateH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense // ClosestTo finds rotation matrix closest to H in Frobenius sense
@ -158,17 +167,18 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
return align(d_abPointPairs, aRb, centroids); return align(d_abPointPairs, aRb, centroids);
} }
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) { Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
const size_t n = abPosePairs.size(); 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 // calculate rotation
vector<Rot3> rotations; vector<Rot3> rotations;
vector<Point3Pair> abPointPairs; PointPairs abPointPairs;
rotations.reserve(n); rotations.reserve(n);
abPointPairs.reserve(n); abPointPairs.reserve(n);
Pose3 wTa, wTb; Pose3 wTa, wTb;
for (const Pose3Pair& abPair : abPosePairs) { for (const Pose3Pair &abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair; std::tie(wTa, wTb) = abPair;
rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
abPointPairs.emplace_back(wTa.translation(), wTb.translation()); abPointPairs.emplace_back(wTa.translation(), wTb.translation());
@ -178,7 +188,7 @@ Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
return alignGivenR(abPointPairs, aRb); return alignGivenR(abPointPairs, aRb);
} }
Matrix4 Similarity3::wedge(const Vector7& xi) { Matrix4 Similarity3::wedge(const Vector7 &xi) {
// http://www.ethaneade.org/latex2html/lie/node29.html // http://www.ethaneade.org/latex2html/lie/node29.html
const auto w = xi.head<3>(); const auto w = xi.head<3>();
const auto u = xi.segment<3>(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; W = 1.0 / 24.0 - theta2 / 720.0;
} }
const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda; const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda;
const double expMinLambda = exp(-lambda);
double A, alpha = 0.0, beta, mu; double A, alpha = 0.0, beta, mu;
if (lambda2 > 1e-9) { if (lambda2 > 1e-9) {
A = (1.0 - exp(-lambda)) / lambda; A = (1.0 - expMinLambda) / lambda;
alpha = 1.0 / (1.0 + theta2 / lambda2); alpha = 1.0 / (1.0 + theta2 / lambda2);
beta = (exp(-lambda) - 1 + lambda) / lambda2; beta = (expMinLambda - 1 + lambda) / lambda2;
mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3; mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3;
} else { } else {
A = 1.0 - lambda / 2.0 + lambda2 / 6.0; A = 1.0 - lambda / 2.0 + lambda2 / 6.0;
beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0; beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;