Fixed equality, transform_from, AdjointMap, and added wedge (for testing AdjointMap)
dellaert
parent
95f4d14d5e
commit
d7ed19dc21
|
|
@ -38,13 +38,13 @@ Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
|
|||
R_(R), t_(t), s_(s) {
|
||||
}
|
||||
|
||||
bool Similarity3::equals(const Similarity3& sim, double tol) const {
|
||||
return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) && s_ < (sim.s_ + tol)
|
||||
&& s_ > (sim.s_ - tol);
|
||||
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);
|
||||
}
|
||||
|
||||
bool Similarity3::operator==(const Similarity3& other) const {
|
||||
return equals(other, 1e-9);
|
||||
return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
|
||||
}
|
||||
|
||||
void Similarity3::print(const std::string& s) const {
|
||||
|
|
@ -70,40 +70,47 @@ Similarity3 Similarity3::inverse() const {
|
|||
|
||||
Point3 Similarity3::transform_from(const Point3& p, //
|
||||
OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
|
||||
Point3 q = R_ * p + t_;
|
||||
if (H1) {
|
||||
const Matrix3 R = R_.matrix();
|
||||
Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
*H1 << DR, R, R * p.vector();
|
||||
print("From Derivative");
|
||||
// TODO(frank): explain the derivative in lambda
|
||||
*H1 << DR, s_ * R, s_ * p.vector();
|
||||
}
|
||||
if (H2)
|
||||
*H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
|
||||
return R_ * (s_ * p) + t_;
|
||||
// TODO: Effect of scale change is this, right?
|
||||
// No, this is incorrect. Zhaoyang Lv
|
||||
// sR t * (1+v)I 0 * p = s(1+v)R t * p = s(1+v)Rp + t = sRp + vRp + t
|
||||
// 0001 000 1 1 000 1 1
|
||||
return s_ * q;
|
||||
}
|
||||
|
||||
Point3 Similarity3::operator*(const Point3& p) const {
|
||||
return transform_from(p);
|
||||
}
|
||||
|
||||
Matrix4 Similarity3::wedge(const Vector7& xi) {
|
||||
// http://www.ethaneade.org/latex2html/lie/node29.html
|
||||
const auto w = xi.head<3>();
|
||||
const auto u = xi.segment<3>(3);
|
||||
double lambda = xi[6];
|
||||
Matrix4 W;
|
||||
W << skewSymmetric(w), u, 0, 0, 0, -lambda;
|
||||
return W;
|
||||
}
|
||||
|
||||
Matrix7 Similarity3::AdjointMap() const {
|
||||
// ToDo: This adjoint might not be correct, it is based on delta = [u, w, lambda]
|
||||
// However, we use the convention delta = [w, u, lambda]
|
||||
// http://www.ethaneade.org/latex2html/lie/node30.html
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 t = t_.vector();
|
||||
Matrix3 A = s_ * skewSymmetric(t) * R;
|
||||
Matrix7 adj;
|
||||
adj << s_ * R, A, -s_ * t, // 3*7
|
||||
Z_3x3, R, Matrix31::Zero(), // 3*7
|
||||
Matrix16::Zero(), 1; // 1*7
|
||||
adj <<
|
||||
R, Z_3x3, Matrix31::Zero(), // 3*7
|
||||
A, s_ * R, -s_ * t, // 3*7
|
||||
Matrix16::Zero(), 1; // 1*7
|
||||
return adj;
|
||||
}
|
||||
|
||||
Matrix33 Similarity3::GetV(Vector3 w, double lambda){
|
||||
Matrix33 wx = skewSymmetric(w[0], w[1], w[2]);
|
||||
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);
|
||||
|
|
@ -122,13 +129,12 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){
|
|||
A = (1 - exp(-lambda)) / lambda;
|
||||
B = alpha * (beta - gama) + gama;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
}
|
||||
else if(thetasquared <= 1e-9 && lambdasquared > 1e-9) {
|
||||
} else if (thetasquared <= 1e-9 && lambdasquared > 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;
|
||||
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);
|
||||
|
|
@ -138,8 +144,7 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){
|
|||
A = (1 - exp(-lambda)) / lambda;
|
||||
B = alpha * (beta - gama) + gama;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
}
|
||||
else if(thetasquared > 1e-9 && lambdasquared <= 1e-9) {
|
||||
} else if (thetasquared > 1e-9 && lambdasquared <= 1e-9) {
|
||||
X = sin(theta) / theta;
|
||||
Y = (1 - cos(theta)) / thetasquared;
|
||||
Z = (1 - X) / thetasquared;
|
||||
|
|
@ -158,10 +163,9 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){
|
|||
}
|
||||
B = alpha * (beta - gama) + gama;
|
||||
C = alpha * (mu - upsilon) + upsilon;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
X = 1;
|
||||
Y = 0.5-thetasquared/24.0;
|
||||
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);
|
||||
|
|
@ -179,7 +183,7 @@ Matrix33 Similarity3::GetV(Vector3 w, double lambda){
|
|||
B = gama;
|
||||
C = upsilon;
|
||||
}
|
||||
return A * Matrix33::Identity() + B * wx + C * wx * wx;
|
||||
return A * I_3x3 + B * wx + C * wx * wx;
|
||||
}
|
||||
|
||||
Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
||||
|
|
@ -196,26 +200,27 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
|||
}
|
||||
|
||||
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
|
||||
Vector3 w(v.head<3>());
|
||||
const auto w = v.head<3>();
|
||||
const auto u = v.segment<3>(3);
|
||||
double lambda = v[6];
|
||||
if (Hm) {
|
||||
Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>());
|
||||
// Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>());
|
||||
// incomplete
|
||||
}
|
||||
return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda));
|
||||
const Matrix3 V = GetV(w, lambda);
|
||||
return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda));
|
||||
}
|
||||
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
|
||||
os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " <<
|
||||
p.scale() << "]\';";
|
||||
os << "[" << p.rotation().xyz().transpose() << " "
|
||||
<< p.translation().vector().transpose() << " " << p.scale() << "]\';";
|
||||
return os;
|
||||
}
|
||||
|
||||
const Matrix4 Similarity3::matrix() const {
|
||||
Matrix4 T;
|
||||
T.topRows<3>() << s_ * R_.matrix(), t_.vector();
|
||||
T.bottomRows<1>() << 0, 0, 0, 1;
|
||||
T.topRows<3>() << R_.matrix(), t_.vector();
|
||||
T.bottomRows<1>() << 0, 0, 0, 1.0/s_;
|
||||
return T;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ public:
|
|||
/// Compare with tolerance
|
||||
bool equals(const Similarity3& sim, double tol) const;
|
||||
|
||||
/// Compare with standard tolerance
|
||||
/// Exact equality
|
||||
bool operator==(const Similarity3& other) const;
|
||||
|
||||
/// Print with optional string
|
||||
|
|
@ -92,6 +92,7 @@ public:
|
|||
/// @name Group action on Point3
|
||||
/// @{
|
||||
|
||||
/// Action on a point p is s*(R*p+t)
|
||||
Point3 transform_from(const Point3& p, //
|
||||
OptionalJacobian<3, 7> H1 = boost::none, //
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
|
@ -124,11 +125,19 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
using LieGroup<Similarity3, 7>::inverse;
|
||||
|
||||
/**
|
||||
* wedge for Similarity3:
|
||||
* @param xi 7-dim twist (w,u,lambda) where
|
||||
* @return 4*4 element of Lie algebra that can be exponentiated
|
||||
* TODO(frank): rename to Hat, make part of traits
|
||||
*/
|
||||
static Matrix4 wedge(const Vector7& xi);
|
||||
|
||||
/// Project from one tangent space to another
|
||||
Matrix7 AdjointMap() const;
|
||||
|
||||
using LieGroup<Similarity3, 7>::inverse;
|
||||
|
||||
/// @}
|
||||
/// @name Standard interface
|
||||
/// @{
|
||||
|
|
@ -152,6 +161,7 @@ public:
|
|||
}
|
||||
|
||||
/// Convert to a rigid body pose (R, s*t)
|
||||
/// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
|
||||
operator Pose3() const;
|
||||
|
||||
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
|
||||
|
|
@ -170,7 +180,7 @@ public:
|
|||
|
||||
/// Calculate expmap and logmap coefficients.
|
||||
private:
|
||||
static Matrix33 GetV(Vector3 w, double lambda);
|
||||
static Matrix3 GetV(Vector3 w, double lambda);
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ 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 T_default(R, Point3(3.5, -8.2, 4.2), 1);
|
||||
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);
|
||||
|
|
@ -79,16 +79,13 @@ TEST(Similarity3, Getters) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, AdjointMap) {
|
||||
Similarity3 test(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
|
||||
Matrix7 result;
|
||||
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000,
|
||||
6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000,
|
||||
-2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000,
|
||||
0, 0, 0, -0.2248, -0.3502, -0.9093, 0,
|
||||
0, 0, 0, 0.9024, -0.4269, -0.0587, 0,
|
||||
0, 0, 0, -0.3676, -0.8337, 0.4120, 0,
|
||||
0, 0, 0, 0, 0, 0, 1.0000;
|
||||
EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
|
||||
const Matrix4 T = T2.matrix();
|
||||
// Check Ad with actual definition
|
||||
Vector7 delta;
|
||||
delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
|
||||
Matrix4 W = Similarity3::wedge(delta);
|
||||
Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta);
|
||||
EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -169,13 +166,13 @@ TEST( Similarity3, retract_first_order) {
|
|||
TEST(Similarity3, localCoordinates_first_order) {
|
||||
Vector d12 = repeat(7, 0.1);
|
||||
d12(6) = 1.0;
|
||||
Similarity3 t1 = T_default, t2 = t1.retract(d12);
|
||||
Similarity3 t1 = T1, t2 = t1.retract(d12);
|
||||
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, manifold_first_order) {
|
||||
Similarity3 t1 = T_default;
|
||||
Similarity3 t1 = T1;
|
||||
Similarity3 t2 = T3;
|
||||
Similarity3 origin;
|
||||
Vector d12 = t1.localCoordinates(t2);
|
||||
|
|
@ -188,10 +185,11 @@ TEST(Similarity3, manifold_first_order) {
|
|||
// Return as a 4*4 Matrix
|
||||
TEST(Similarity3, Matrix) {
|
||||
Matrix4 expected;
|
||||
expected << 2, 0, 0, 1,
|
||||
0, 2, 0, 1,
|
||||
0, 0, 2, 0,
|
||||
0, 0, 0, 1;
|
||||
expected <<
|
||||
1, 0, 0, 1,
|
||||
0, 1, 0, 1,
|
||||
0, 0, 1, 0,
|
||||
0, 0, 0, 0.5;
|
||||
Matrix4 actual = T6.matrix();
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
|
@ -226,55 +224,39 @@ TEST(Similarity3, ExpLogMap) {
|
|||
//******************************************************************************
|
||||
// Group action on Point3 (with simpler transform)
|
||||
TEST(Similarity3, GroupAction) {
|
||||
EXPECT(assert_equal(Point3(1, 1, 0), T6 * Point3(0, 0, 0)));
|
||||
EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0)));
|
||||
EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0)));
|
||||
|
||||
// Test actual group action on R^4
|
||||
// Test group action on R^4 via matrix representation
|
||||
Vector4 qh;
|
||||
qh << 1, 0, 0, 1;
|
||||
Vector4 ph;
|
||||
ph << 3, 1, 0, 1;
|
||||
ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0)
|
||||
EXPECT(assert_equal((Vector )ph, T6.matrix() * qh));
|
||||
|
||||
// Test some more...
|
||||
Point3 pa = Point3(1, 0, 0);
|
||||
Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0);
|
||||
Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0);
|
||||
|
||||
Point3 pa = Point3(1, 0, 0);
|
||||
Point3 pTa = Point3(2, 2, 3);
|
||||
Point3 pTb = Point3(3, 2, 3);
|
||||
|
||||
EXPECT(assert_equal(pTa, Ta.transform_from(pa)));
|
||||
EXPECT(assert_equal(pTb, Tb.transform_from(pa)));
|
||||
EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa)));
|
||||
EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa)));
|
||||
|
||||
Similarity3 Tc(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 1.0);
|
||||
Similarity3 Td(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 2.0);
|
||||
|
||||
Point3 pTc = Point3(1, 3, 3);
|
||||
Point3 pTd = Point3(1, 4, 3);
|
||||
|
||||
EXPECT(assert_equal(pTc, Tc.transform_from(pa)));
|
||||
EXPECT(assert_equal(pTd, Td.transform_from(pa)));
|
||||
EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa)));
|
||||
EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa)));
|
||||
|
||||
// Test derivative
|
||||
boost::function<Point3(Similarity3, Point3)> f = boost::bind(
|
||||
&Similarity3::transform_from, _1, _2, boost::none, boost::none);
|
||||
|
||||
{ // T default
|
||||
Point3 q(1, 2, 3);
|
||||
for (const auto T : { T1, T2, T3, T4, T5, T6 }) {
|
||||
Point3 q(1, 0, 0);
|
||||
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T_default, q);
|
||||
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T_default, q);
|
||||
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T1, q);
|
||||
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T1, q);
|
||||
Matrix actualH1, actualH2;
|
||||
T_default.transform_from(q, actualH1, actualH2);
|
||||
EXPECT(assert_equal(H1, actualH1));
|
||||
EXPECT(assert_equal(H2, actualH2));
|
||||
}
|
||||
{ // T4
|
||||
Point3 q(1, 0, 0);
|
||||
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T6, q);
|
||||
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T6, q);
|
||||
Matrix actualH1, actualH2;
|
||||
Point3 p = T6.transform_from(q, actualH1, actualH2);
|
||||
EXPECT(assert_equal(Point3(3, 1, 0), p));
|
||||
EXPECT(assert_equal(Point3(3, 1, 0), T6 * q));
|
||||
T1.transform_from(q, actualH1, actualH2);
|
||||
EXPECT(assert_equal(H1, actualH1));
|
||||
EXPECT(assert_equal(H2, actualH2));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue