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

Similarity3 fixes

release/4.3a0
Varun Agrawal 2022-02-06 00:09:06 -05:00
parent bf668e5869
commit a57465ee64
2 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/Similarity3.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
using std::vector; using std::vector;
namespace { namespace internal {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) { const Point3Pair &centroids) {
@ -79,10 +79,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) { const Rot3 &aRb) {
auto centroids = means(abPointPairs); auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids); return align(d_abPointPairs, aRb, centroids);
} }
} // namespace } // namespace internal
Similarity3::Similarity3() : Similarity3::Similarity3() :
t_(0,0,0), s_(1) { t_(0,0,0), s_(1) {
@ -163,11 +163,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
if (abPointPairs.size() < 3) if (abPointPairs.size() < 3)
throw std::runtime_error("input should have at least 3 pairs of points"); throw std::runtime_error("input should have at least 3 pairs of points");
auto centroids = means(abPointPairs); auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
Matrix3 H = calculateH(d_abPointPairs); Matrix3 H = internal::calculateH(d_abPointPairs);
// 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); Rot3 aRb = Rot3::ClosestTo(H);
return align(d_abPointPairs, aRb, centroids); return internal::align(d_abPointPairs, aRb, centroids);
} }
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) { Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@ -190,7 +190,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
} }
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations); const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb_estimate); return internal::alignGivenR(abPointPairs, aRb_estimate);
} }
Matrix4 Similarity3::wedge(const Vector7 &xi) { Matrix4 Similarity3::wedge(const Vector7 &xi) {
@ -281,7 +281,7 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
return os; return os;
} }
const Matrix4 Similarity3::matrix() const { Matrix4 Similarity3::matrix() const {
Matrix4 T; Matrix4 T;
T.topRows<3>() << R_.matrix(), t_; T.topRows<3>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;

6
gtsam/geometry/Similarity3.h
View File

@ -180,15 +180,15 @@ public:
/// @{ /// @{
/// Calculate 4*4 matrix group equivalent /// Calculate 4*4 matrix group equivalent
GTSAM_EXPORT const Matrix4 matrix() const; GTSAM_EXPORT Matrix4 matrix() const;
/// Return a GTSAM rotation /// Return a GTSAM rotation
const Rot3& rotation() const { Rot3 rotation() const {
return R_; return R_;
} }
/// Return a GTSAM translation /// Return a GTSAM translation
const Point3& translation() const { Point3 translation() const {
return t_; return t_;
} }