Cleaned up and tested exmpap
dellaert
parent
d7ed19dc21
commit
f64d17f4f0
|
|
@ -38,6 +38,10 @@ Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
|
|||
R_(R), t_(t), s_(s) {
|
||||
}
|
||||
|
||||
Similarity3::Similarity3(const Matrix4& T) :
|
||||
R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) {
|
||||
}
|
||||
|
||||
bool Similarity3::equals(const Similarity3& other, double tol) const {
|
||||
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
|
||||
&& s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
|
||||
|
|
@ -100,7 +104,7 @@ Matrix7 Similarity3::AdjointMap() const {
|
|||
// http://www.ethaneade.org/latex2html/lie/node30.html
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 t = t_.vector();
|
||||
Matrix3 A = s_ * skewSymmetric(t) * R;
|
||||
const Matrix3 A = s_ * skewSymmetric(t) * R;
|
||||
Matrix7 adj;
|
||||
adj <<
|
||||
R, Z_3x3, Matrix31::Zero(), // 3*7
|
||||
|
|
@ -110,91 +114,86 @@ Matrix7 Similarity3::AdjointMap() const {
|
|||
}
|
||||
|
||||
Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
|
||||
Matrix3 wx = skewSymmetric(w[0], w[1], w[2]);
|
||||
double lambdasquared = lambda * lambda;
|
||||
double thetasquared = w.transpose() * w;
|
||||
double theta = sqrt(thetasquared);
|
||||
double X, Y, Z, W, alpha, beta, gama, mu, upsilon, A, B, C;
|
||||
if (thetasquared > 1e-9 && lambdasquared > 1e-9) {
|
||||
X = sin(theta) / theta;
|
||||
Y = (1 - cos(theta)) / thetasquared;
|
||||
Z = (1 - X) / thetasquared;
|
||||
W = (0.5 - Y) / thetasquared;
|
||||
alpha = lambdasquared / (lambdasquared + thetasquared);
|
||||
beta = (exp(-lambda) - 1 + lambda) / lambdasquared;
|
||||
gama = Y - (lambda * Z);
|
||||
mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda))
|
||||
/ (lambdasquared * lambda);
|
||||
upsilon = Z - (lambda * W);
|
||||
// http://www.ethaneade.org/latex2html/lie/node29.html
|
||||
double lambda2 = lambda * lambda;
|
||||
double theta2 = w.transpose() * w;
|
||||
double theta = sqrt(theta2);
|
||||
double A, B, C;
|
||||
// TODO(frank): eliminate copy/paste
|
||||
if (theta2 > 1e-9 && lambda2 > 1e-9) {
|
||||
const double X = sin(theta) / theta;
|
||||
const double Y = (1 - cos(theta)) / theta2;
|
||||
const double Z = (1 - X) / theta2;
|
||||
const double W = (0.5 - Y) / theta2;
|
||||
const double alpha = lambda2 / (lambda2 + theta2);
|
||||
const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
|
||||
/ (lambda2 * lambda);
|
||||
const double upsilon = Z - (lambda * W);
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
B = alpha * (beta - gama) + gama;
|
||||
B = alpha * (beta - gamma) + gamma;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
} else if (thetasquared <= 1e-9 && lambdasquared > 1e-9) {
|
||||
} else if (theta2 <= 1e-9 && lambda2 > 1e-9) {
|
||||
//Taylor series expansions
|
||||
X = 1;
|
||||
Y = 0.5 - thetasquared / 24.0;
|
||||
Z = 1.0 / 6.0 - thetasquared / 120.0;
|
||||
W = 1.0 / 24.0 - thetasquared / 720.0;
|
||||
alpha = lambdasquared / (lambdasquared + thetasquared);
|
||||
beta = (exp(-lambda) - 1 + lambda) / lambdasquared;
|
||||
gama = Y - (lambda * Z);
|
||||
mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda))
|
||||
/ (lambdasquared * lambda);
|
||||
upsilon = Z - (lambda * W);
|
||||
const double Y = 0.5 - theta2 / 24.0;
|
||||
const double Z = 1.0 / 6.0 - theta2 / 120.0;
|
||||
const double W = 1.0 / 24.0 - theta2 / 720.0;
|
||||
const double alpha = lambda2 / (lambda2 + theta2);
|
||||
const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
|
||||
/ (lambda2 * lambda);
|
||||
const double upsilon = Z - (lambda * W);
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
B = alpha * (beta - gama) + gama;
|
||||
B = alpha * (beta - gamma) + gamma;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
} else if (thetasquared > 1e-9 && lambdasquared <= 1e-9) {
|
||||
X = sin(theta) / theta;
|
||||
Y = (1 - cos(theta)) / thetasquared;
|
||||
Z = (1 - X) / thetasquared;
|
||||
W = (0.5 - Y) / thetasquared;
|
||||
alpha = lambdasquared / (lambdasquared + thetasquared);
|
||||
beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0
|
||||
- (lambda * lambdasquared) / 120;
|
||||
gama = Y - (lambda * Z);
|
||||
mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120
|
||||
- (lambda * lambdasquared) / 720;
|
||||
upsilon = Z - (lambda * W);
|
||||
} else if (theta2 > 1e-9 && lambda2 <= 1e-9) {
|
||||
const double X = sin(theta) / theta;
|
||||
const double Y = (1 - cos(theta)) / theta2;
|
||||
const double Z = (1 - X) / theta2;
|
||||
const double W = (0.5 - Y) / theta2;
|
||||
const double alpha = lambda2 / (lambda2 + theta2);
|
||||
const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0
|
||||
- (lambda * lambda2) / 120;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120
|
||||
- (lambda * lambda2) / 720;
|
||||
const double upsilon = Z - (lambda * W);
|
||||
if (lambda < 1e-9) {
|
||||
A = 1 - lambda / 2.0 + lambdasquared / 6.0;
|
||||
A = 1 - lambda / 2.0 + lambda2 / 6.0;
|
||||
} else {
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
}
|
||||
B = alpha * (beta - gama) + gama;
|
||||
B = alpha * (beta - gamma) + gamma;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
} else {
|
||||
X = 1;
|
||||
Y = 0.5 - thetasquared / 24.0;
|
||||
Z = 1.0 / 6.0 - thetasquared / 120.0;
|
||||
W = 1.0 / 24.0 - thetasquared / 720.0;
|
||||
alpha = lambdasquared / (lambdasquared + thetasquared);
|
||||
beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0
|
||||
- (lambda * lambdasquared) / 120;
|
||||
gama = Y - (lambda * Z);
|
||||
mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120
|
||||
- (lambda * lambdasquared) / 720;
|
||||
upsilon = Z - (lambda * W);
|
||||
const double Y = 0.5 - theta2 / 24.0;
|
||||
const double Z = 1.0 / 6.0 - theta2 / 120.0;
|
||||
const double W = 1.0 / 24.0 - theta2 / 720.0;
|
||||
const double gamma = Y - (lambda * Z);
|
||||
const double upsilon = Z - (lambda * W);
|
||||
if (lambda < 1e-9) {
|
||||
A = 1 - lambda / 2.0 + lambdasquared / 6.0;
|
||||
A = 1 - lambda / 2.0 + lambda2 / 6.0;
|
||||
} else {
|
||||
A = (1 - exp(-lambda)) / lambda;
|
||||
}
|
||||
B = gama;
|
||||
B = gamma;
|
||||
C = upsilon;
|
||||
}
|
||||
return A * I_3x3 + B * wx + C * wx * wx;
|
||||
const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]);
|
||||
return A * I_3x3 + B * Wx + C * Wx * Wx;
|
||||
}
|
||||
|
||||
Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
||||
// To get the logmap, calculate w and lambda, then solve for u as show at ethaneade.org
|
||||
Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) {
|
||||
// To get the logmap, calculate w and lambda, then solve for u as shown by Ethan at
|
||||
// www.ethaneade.org/latex2html/lie/node29.html
|
||||
Vector3 w = Rot3::Logmap(s.R_);
|
||||
double lambda = log(s.s_);
|
||||
const Vector3 w = Rot3::Logmap(T.R_);
|
||||
const double lambda = log(T.s_);
|
||||
Vector7 result;
|
||||
result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda;
|
||||
result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda;
|
||||
if (Hm) {
|
||||
// incomplete
|
||||
throw std::runtime_error("Similarity3::Logmap: derivative not implemented");
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
|
@ -202,10 +201,9 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
|||
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
|
||||
const auto w = v.head<3>();
|
||||
const auto u = v.segment<3>(3);
|
||||
double lambda = v[6];
|
||||
const double lambda = v[6];
|
||||
if (Hm) {
|
||||
// Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>());
|
||||
// incomplete
|
||||
throw std::runtime_error("Similarity3::Expmap: derivative not implemented");
|
||||
}
|
||||
const Matrix3 V = GetV(w, lambda);
|
||||
return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda));
|
||||
|
|
|
|||
|
|
@ -60,6 +60,9 @@ public:
|
|||
/// Construct from Eigen types
|
||||
Similarity3(const Matrix3& R, const Vector3& t, double s);
|
||||
|
||||
/// Construct from matrix [R t; 0 s^-1]
|
||||
Similarity3(const Matrix4& T);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
@ -118,10 +121,10 @@ public:
|
|||
/// Chart at the origin
|
||||
struct ChartAtOrigin {
|
||||
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
|
||||
return Similarity3::Expmap(v);
|
||||
return Similarity3::Expmap(v, H);
|
||||
}
|
||||
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
|
||||
return Similarity3::Logmap(other);
|
||||
return Similarity3::Logmap(other, H);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -183,15 +186,17 @@ private:
|
|||
static Matrix3 GetV(Vector3 w, double lambda);
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {
|
||||
};
|
||||
|
||||
inline Matrix wedge<Similarity3>(const Vector& xi) {
|
||||
return Similarity3::wedge(xi);
|
||||
}
|
||||
|
||||
template<>
|
||||
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
|
||||
|
||||
template<>
|
||||
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -39,15 +40,16 @@ using symbol_shorthand::X;
|
|||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
|
||||
|
||||
static Point3 P(0.2, 0.7, -2);
|
||||
static Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
|
||||
static double s = 4;
|
||||
static Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
|
||||
static Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1);
|
||||
static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
|
||||
static Similarity3 T4(R, P, s);
|
||||
static Similarity3 T5(R, P, 10);
|
||||
static Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
|
||||
static const Point3 P(0.2, 0.7, -2);
|
||||
static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
|
||||
static const double s = 4;
|
||||
static const Similarity3 id;
|
||||
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
|
||||
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1);
|
||||
static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
|
||||
static const Similarity3 T4(R, P, s);
|
||||
static const Similarity3 T5(R, P, 10);
|
||||
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Concepts) {
|
||||
|
|
@ -197,10 +199,10 @@ TEST(Similarity3, Matrix) {
|
|||
//*****************************************************************************
|
||||
// Exponential and log maps
|
||||
TEST(Similarity3, ExpLogMap) {
|
||||
Vector7 expected;
|
||||
expected << 0.1,0.2,0.3,0.4,0.5,0.6,0.7;
|
||||
Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(expected));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
Vector7 delta;
|
||||
delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7;
|
||||
Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta));
|
||||
EXPECT(assert_equal(delta, actual));
|
||||
|
||||
Vector7 zeros;
|
||||
zeros << 0,0,0,0,0,0,0;
|
||||
|
|
@ -211,14 +213,8 @@ TEST(Similarity3, ExpLogMap) {
|
|||
Similarity3 ident = Similarity3::identity();
|
||||
EXPECT(assert_equal(expZero, ident));
|
||||
|
||||
// Compare to matrix exponential calculated using matlab expm
|
||||
Rot3 Rexpm(0.9358, -0.2832, 0.2102,
|
||||
0.3029, 0.9506, -0.0680,
|
||||
-0.1805, 0.1273, 0.9753);
|
||||
Point3 texpm(0.2724, 0.3825, 0.4213);
|
||||
Similarity3 Sexpm(Rexpm, texpm, 2.0138);
|
||||
Similarity3 Scalc = Similarity3::Expmap(expected);
|
||||
EXPECT(assert_equal(Sexpm, Scalc, 1e-3));
|
||||
// Compare to matrix exponential, using expm in Lie.h
|
||||
EXPECT(assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -387,6 +383,31 @@ TEST(Similarity3, AlignScaledPointClouds) {
|
|||
// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3|
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3 , Invariants) {
|
||||
Similarity3 id;
|
||||
|
||||
EXPECT(check_group_invariants(id,id));
|
||||
EXPECT(check_group_invariants(id,T3));
|
||||
EXPECT(check_group_invariants(T2,id));
|
||||
EXPECT(check_group_invariants(T2,T3));
|
||||
|
||||
EXPECT(check_manifold_invariants(id,id));
|
||||
EXPECT(check_manifold_invariants(id,T3));
|
||||
EXPECT(check_manifold_invariants(T2,id));
|
||||
EXPECT(check_manifold_invariants(T2,T3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3 , LieGroupDerivatives) {
|
||||
Similarity3 id;
|
||||
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue