saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 16:40:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rename Quaternion to QuaternionManifold.

Browse Source 
Also rename EigenQuaternion to EigenQuaternionManiold.

Change-Id: I7095fa61cfabf97a328b02fb027f5a2a8dd499ae
This commit is contained in:
Sameer Agarwal
2022-01-22 11:07:52 -08:00
parent ca6d841c25
commit 19eef54fc2
3 changed files with 59 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/manifold.h
+13 -13
View File
@@ -354,8 +354,8 @@ class CERES_EXPORT ProductManifold : public Manifold {
//
// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
//
// The tangent space is R^3, which relates to the ambient space through
// the Plus and Minus operations defined as:
// The tangent space is R^3, which relates to the ambient space through the Plus
// and Minus operations defined as:
//
// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x
// Minus(y, x) = to_delta(y * x^{-1})
@@ -364,10 +364,10 @@ class CERES_EXPORT ProductManifold : public Manifold {
// (|q|=1), q^-1 = [q0; -q1; -q2; -q3]
//
// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0)
class CERES_EXPORT Quaternion : public Manifold {
class CERES_EXPORT QuaternionManifold : public Manifold {
public:
Quaternion() = default;
virtual ~Quaternion() = default;
QuaternionManifold() = default;
virtual ~QuaternionManifold() = default;
int AmbientSize() const override { return 4; }
int TangentSize() const override { return 3; }
@@ -382,18 +382,18 @@ class CERES_EXPORT Quaternion : public Manifold {
};
// Implements the quaternion manifold for Eigen's representation of the Hamilton
// quaternion. Geometrically it is exactly the same as the Quaternion manifold
// quaternion. Geometrically it is exactly the same as the QuaternionManifold
// defined above. However, Eigen uses a different internal memory layout for the
// elements of the quaternion than what is commonly used. It stores the
// quaternion in memory as [q1, q2, q3, q0] or [x, y, z, w] where the
// real (scalar) part is last.
// quaternion in memory as [q1, q2, q3, q0] or [x, y, z, w] where the real
// (scalar) part is last.
//
// Since Ceres operates on parameter blocks which are raw double pointers
// this difference is important and requires a different manifold.
class CERES_EXPORT EigenQuaternion : public Manifold {
// Since Ceres operates on parameter blocks which are raw double pointers this
// difference is important and requires a different manifold.
class CERES_EXPORT EigenQuaternionManifold : public Manifold {
public:
EigenQuaternion() = default;
virtual ~EigenQuaternion() = default;
EigenQuaternionManifold() = default;
virtual ~EigenQuaternionManifold() = default;
int AmbientSize() const override { return 4; }
int TangentSize() const override { return 3; }
internal/ceres/manifold.cc
+19 -16
View File
@@ -402,50 +402,53 @@ bool ProductManifold::MinusJacobian(const double* x,
return true;
}
bool Quaternion::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
bool QuaternionManifold::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
QuaternionPlusImpl<CeresQuaternionOrder>(x, delta, x_plus_delta);
return true;
}
bool Quaternion::PlusJacobian(const double* x, double* jacobian) const {
bool QuaternionManifold::PlusJacobian(const double* x, double* jacobian) const {
QuaternionPlusJacobianImpl<CeresQuaternionOrder>(x, jacobian);
return true;
}
bool Quaternion::Minus(const double* y,
const double* x,
double* y_minus_x) const {
bool QuaternionManifold::Minus(const double* y,
const double* x,
double* y_minus_x) const {
QuaternionMinusImpl<CeresQuaternionOrder>(y, x, y_minus_x);
return true;
}
bool Quaternion::MinusJacobian(const double* x, double* jacobian) const {
bool QuaternionManifold::MinusJacobian(const double* x,
double* jacobian) const {
QuaternionMinusJacobianImpl<CeresQuaternionOrder>(x, jacobian);
return true;
}
bool EigenQuaternion::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
bool EigenQuaternionManifold::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
QuaternionPlusImpl<EigenQuaternionOrder>(x, delta, x_plus_delta);
return true;
}
bool EigenQuaternion::PlusJacobian(const double* x, double* jacobian) const {
bool EigenQuaternionManifold::PlusJacobian(const double* x,
double* jacobian) const {
QuaternionPlusJacobianImpl<EigenQuaternionOrder>(x, jacobian);
return true;
}
bool EigenQuaternion::Minus(const double* y,
const double* x,
double* y_minus_x) const {
bool EigenQuaternionManifold::Minus(const double* y,
const double* x,
double* y_minus_x) const {
QuaternionMinusImpl<EigenQuaternionOrder>(y, x, y_minus_x);
return true;
}
bool EigenQuaternion::MinusJacobian(const double* x, double* jacobian) const {
bool EigenQuaternionManifold::MinusJacobian(const double* x,
double* jacobian) const {
QuaternionMinusJacobianImpl<EigenQuaternionOrder>(x, jacobian);
return true;
}
internal/ceres/manifold_test.cc
+27 -27
View File
@@ -273,8 +273,8 @@ TEST(ProductManifold, EuclideanAndZeroTangentSize) {
}
}
TEST(Quaternion, PlusPiBy2) {
Quaternion manifold;
TEST(QuaternionManifold, PlusPiBy2) {
QuaternionManifold manifold;
Vector x = Vector::Zero(4);
x[0] = 1.0;
@@ -307,12 +307,12 @@ TEST(Quaternion, PlusPiBy2) {
}
}
// Compute the expected value of Quaternion::Plus via functions in rotation.h
// and compares it to the one computed by Quaternion::Plus.
MATCHER_P2(QuaternionPlusIsCorrectAt, x, delta, "") {
// Compute the expected value of QuaternionManifold::Plus via functions in
// rotation.h and compares it to the one computed by QuaternionManifold::Plus.
MATCHER_P2(QuaternionManifoldPlusIsCorrectAt, x, delta, "") {
// This multiplication by 2 is needed because AngleAxisToQuaternion uses
// |delta|/2 as the angle of rotation where as in the implementation of
// Quaternion for historical reasons we use |delta|.
// QuaternionManifold for historical reasons we use |delta|.
const Vector two_delta = delta * 2;
Vector delta_q(4);
AngleAxisToQuaternion(two_delta.data(), delta_q.data());
@@ -343,46 +343,46 @@ Vector RandomQuaternion() {
return x;
}
TEST(Quaternion, GenericDelta) {
Quaternion manifold;
TEST(QuaternionManifold, GenericDelta) {
QuaternionManifold manifold;
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = RandomQuaternion();
const Vector y = RandomQuaternion();
Vector delta = Vector::Random(3);
EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
EXPECT_THAT(manifold, QuaternionManifoldPlusIsCorrectAt(x, delta));
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}
TEST(Quaternion, SmallDelta) {
Quaternion manifold;
TEST(QuaternionManifold, SmallDelta) {
QuaternionManifold manifold;
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = RandomQuaternion();
const Vector y = RandomQuaternion();
Vector delta = Vector::Random(3);
delta.normalize();
delta *= 1e-6;
EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
EXPECT_THAT(manifold, QuaternionManifoldPlusIsCorrectAt(x, delta));
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}
TEST(Quaternion, DeltaJustBelowPi) {
Quaternion manifold;
TEST(QuaternionManifold, DeltaJustBelowPi) {
QuaternionManifold manifold;
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = RandomQuaternion();
const Vector y = RandomQuaternion();
Vector delta = Vector::Random(3);
delta.normalize();
delta *= (M_PI - 1e-6);
EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
EXPECT_THAT(manifold, QuaternionManifoldPlusIsCorrectAt(x, delta));
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}
// Compute the expected value of EigenQuaternion::Plus using Eigen and compares
// it to the one computed by Quaternion::Plus.
MATCHER_P2(EigenQuaternionPlusIsCorrectAt, x, delta, "") {
// Compute the expected value of EigenQuaternionManifold::Plus using Eigen and
// compares it to the one computed by QuaternionManifold::Plus.
MATCHER_P2(EigenQuaternionManifoldPlusIsCorrectAt, x, delta, "") {
// This multiplication by 2 is needed because AngleAxisToQuaternion uses
// |delta|/2 as the angle of rotation where as in the implementation of
// Quaternion for historical reasons we use |delta|.
@@ -414,39 +414,39 @@ MATCHER_P2(EigenQuaternionPlusIsCorrectAt, x, delta, "") {
return true;
}
TEST(EigenQuaternion, GenericDelta) {
EigenQuaternion manifold;
TEST(EigenQuaternionManifold, GenericDelta) {
EigenQuaternionManifold manifold;
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = RandomQuaternion();
const Vector y = RandomQuaternion();
Vector delta = Vector::Random(3);
EXPECT_THAT(manifold, EigenQuaternionPlusIsCorrectAt(x, delta));
EXPECT_THAT(manifold, EigenQuaternionManifoldPlusIsCorrectAt(x, delta));
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}
TEST(EigenQuaternion, SmallDelta) {
EigenQuaternion manifold;
TEST(EigenQuaternionManifold, SmallDelta) {
EigenQuaternionManifold manifold;
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = RandomQuaternion();
const Vector y = RandomQuaternion();
Vector delta = Vector::Random(3);
delta.normalize();
delta *= 1e-6;
EXPECT_THAT(manifold, EigenQuaternionPlusIsCorrectAt(x, delta));
EXPECT_THAT(manifold, EigenQuaternionManifoldPlusIsCorrectAt(x, delta));
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}
TEST(EigenQuaternion, DeltaJustBelowPi) {
EigenQuaternion manifold;
TEST(EigenQuaternionManifold, DeltaJustBelowPi) {
EigenQuaternionManifold manifold;
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = RandomQuaternion();
const Vector y = RandomQuaternion();
Vector delta = Vector::Random(3);
delta.normalize();
delta *= (M_PI - 1e-6);
EXPECT_THAT(manifold, EigenQuaternionPlusIsCorrectAt(x, delta));
EXPECT_THAT(manifold, EigenQuaternionManifoldPlusIsCorrectAt(x, delta));
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}