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

Replace rotAveraging with gtsam::FindKarcherMean.

release/4.3a0
alexma3312 2020-08-21 13:51:21 -04:00
parent c80cfe068f
commit e94aae10bf
3 changed files with 5 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -140,23 +140,6 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
return Similarity3(aRb, aTb, s);
}
// Use the geodesic L2 mean to solve the mean of rotations,
// Refer to: http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf (on page 18)
Rot3 Similarity3::rotationAveraging(const std::vector<Rot3>& rotations, double error) {
Rot3 R = rotations[0];
const size_t n = rotations.size();
Vector3 r;
r << 0, 0, 0;
while (1) {
for (const Rot3 R_i : rotations) {
r += Rot3::Logmap(R.inverse().compose(R_i));
}
r /= n;
if (r.norm() < error) return R;
R = R.compose(Rot3::Expmap(r));
}
}
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
const size_t n = abPosePairs.size();
if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs
@ -172,7 +155,7 @@ Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
const double f = 1.0 / n;
aCentroid *= f;
bCentroid *= f;
Rot3 aRb = Similarity3::rotationAveraging(rotationList);
const Rot3 aRb = FindKarcherMean<Rot3>(rotationList);
// Calculate scale
double x = 0;

10
gtsam_unstable/geometry/Similarity3.h
View File

@ -23,6 +23,9 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam_unstable/dllexport.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
#include <gtsam/slam/KarcherMeanFactor.h>
namespace gtsam {
@ -112,12 +115,7 @@ public:
* Create Similarity3 by aligning at least three point pairs
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
/**
* Calculate the average rotation from a list of rotations
*/
GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector<Rot3>& rotations, double error = 1e-10);
/**
* Create Similarity3 by aligning at least two pose pairs
*/

9
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -321,15 +321,6 @@ TEST(Similarity3, AlignPoint3_3) {
EXPECT(assert_equal(expected_aSb, actual_aSb));
}
//******************************************************************************
// Rotation Averaging
TEST(Similarity3, RotationAveraging) {
Rot3 expected = Rot3::Rx(90 * degree);
vector<Rot3> rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)};
Rot3 actual = Similarity3::rotationAveraging(rotations);
EXPECT(assert_equal(expected, actual));
}
//******************************************************************************
// Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) {