diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index fcaf0c874..58bace0f1 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -26,7 +26,7 @@ namespace gtsam { using std::vector; -namespace { +namespace internal { /// Subtract centroids from point pairs. static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, const Point3Pair ¢roids) { @@ -79,10 +79,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, const Rot3 &aRb) { auto centroids = means(abPointPairs); - auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); return align(d_abPointPairs, aRb, centroids); } -} // namespace +} // namespace internal Similarity3::Similarity3() : t_(0,0,0), s_(1) { @@ -163,11 +163,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &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); - Matrix3 H = calculateH(d_abPointPairs); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); + Matrix3 H = internal::calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return align(d_abPointPairs, aRb, centroids); + return internal::align(d_abPointPairs, aRb, centroids); } Similarity3 Similarity3::Align(const vector &abPosePairs) { @@ -190,7 +190,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { } const Rot3 aRb_estimate = FindKarcherMean(rotations); - return alignGivenR(abPointPairs, aRb_estimate); + return internal::alignGivenR(abPointPairs, aRb_estimate); } Matrix4 Similarity3::wedge(const Vector7 &xi) { @@ -281,7 +281,7 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) { return os; } -const Matrix4 Similarity3::matrix() const { +Matrix4 Similarity3::matrix() const { Matrix4 T; T.topRows<3>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 0ef787b05..27fdba6d7 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -180,15 +180,15 @@ public: /// @{ /// Calculate 4*4 matrix group equivalent - GTSAM_EXPORT const Matrix4 matrix() const; + GTSAM_EXPORT Matrix4 matrix() const; /// Return a GTSAM rotation - const Rot3& rotation() const { + Rot3 rotation() const { return R_; } /// Return a GTSAM translation - const Point3& translation() const { + Point3 translation() const { return t_; }