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;
namespace {
namespace internal {
/// Subtract centroids from point pairs.
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) {
@ -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<Pose3Pair> &abPosePairs) {
@ -190,7 +190,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
}
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(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_;

6
gtsam/geometry/Similarity3.h
View File

@ -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_;
}