Moved all tests to their own test files
Frank Dellaert
Fan Jiang
parent
095071cf36
commit
e93149babb
|
|
@ -15,14 +15,23 @@
|
|||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/geometry/SOt.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Identity) {
|
||||
const SO3 R;
|
||||
EXPECT_LONGS_EQUAL(3, R.rows());
|
||||
EXPECT_LONGS_EQUAL(3, SO3::dimension);
|
||||
EXPECT_LONGS_EQUAL(3, SO3::Dim());
|
||||
EXPECT_LONGS_EQUAL(3, R.dim());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
|
||||
|
|
@ -50,7 +59,21 @@ TEST(SO3, Local) {
|
|||
TEST(SO3, Retract) {
|
||||
Vector3 v(0, 0, 0.1);
|
||||
SO3 actual = traits<SO3>::Retract(R1, v);
|
||||
EXPECT(actual.isApprox(R2));
|
||||
EXPECT(assert_equal(R2, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Logmap) {
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = SO3::Logmap(R1.between(R2));
|
||||
EXPECT(assert_equal((Vector)expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Expmap) {
|
||||
Vector3 v(0, 0, 0.1);
|
||||
SO3 actual = R1 * SO3::Expmap(v);
|
||||
EXPECT(assert_equal(R2, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -231,17 +254,17 @@ TEST(SO3, JacobianLogmap) {
|
|||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ApplyDexp) {
|
||||
Matrix aH1, aH2;
|
||||
for (bool nearZeroApprox : {true, false}) {
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
|
||||
return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
|
||||
};
|
||||
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
|
||||
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
|
||||
so3::DexpFunctor local(omega, nearZeroApprox);
|
||||
sot::DexpFunctor local(omega, nearZeroApprox);
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
||||
Vector3(0.4, 0.3, 0.2)}) {
|
||||
EXPECT(assert_equal(Vector3(local.dexp() * v),
|
||||
|
|
@ -254,17 +277,17 @@ TEST(SO3, ApplyDexp) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ApplyInvDexp) {
|
||||
Matrix aH1, aH2;
|
||||
for (bool nearZeroApprox : {true, false}) {
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
|
||||
return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
|
||||
};
|
||||
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
|
||||
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
|
||||
so3::DexpFunctor local(omega, nearZeroApprox);
|
||||
sot::DexpFunctor local(omega, nearZeroApprox);
|
||||
Matrix invDexp = local.dexp().inverse();
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
||||
Vector3(0.4, 0.3, 0.2)}) {
|
||||
|
|
@ -278,15 +301,13 @@ TEST(SO3, ApplyInvDexp) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, vec) {
|
||||
const Vector9 expected = Eigen::Map<Vector9>(R2.data());
|
||||
const Vector9 expected = Eigen::Map<const Vector9>(R2.matrix().data());
|
||||
Matrix actualH;
|
||||
const Vector9 actual = R2.vec(actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) {
|
||||
return Q.vec();
|
||||
};
|
||||
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
|
||||
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
|
||||
CHECK(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,10 +19,9 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
#include <gtsam/geometry/SOt.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <iostream>
|
||||
|
|
@ -30,6 +29,16 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
|
||||
TEST(SO4, Identity) {
|
||||
const SO4 R;
|
||||
EXPECT_LONGS_EQUAL(4, R.rows());
|
||||
EXPECT_LONGS_EQUAL(6, SO4::dimension);
|
||||
EXPECT_LONGS_EQUAL(6, SO4::Dim());
|
||||
EXPECT_LONGS_EQUAL(6, R.dim());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
|
||||
|
|
@ -39,9 +48,9 @@ TEST(SO4, Concept) {
|
|||
|
||||
//******************************************************************************
|
||||
SO4 id;
|
||||
Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
|
||||
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
|
||||
SO4 Q1 = SO4::Expmap(v1);
|
||||
Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished();
|
||||
Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
|
||||
SO4 Q2 = SO4::Expmap(v2);
|
||||
Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
|
||||
SO4 Q3 = SO4::Expmap(v3);
|
||||
|
|
@ -55,12 +64,12 @@ TEST(SO4, Random) {
|
|||
//******************************************************************************
|
||||
TEST(SO4, Expmap) {
|
||||
// If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
|
||||
auto R1 = SO3::Expmap(v1.head<3>());
|
||||
EXPECT((Q1.topLeft().isApprox(R1)));
|
||||
auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
|
||||
EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
|
||||
|
||||
// Same here
|
||||
auto R2 = SO3::Expmap(v2.head<3>());
|
||||
EXPECT((Q2.topLeft().isApprox(R2)));
|
||||
auto R2 = SO3::Expmap(v2.tail<3>()).matrix();
|
||||
EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2)));
|
||||
|
||||
// Check commutative subgroups
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
|
|
@ -69,7 +78,7 @@ TEST(SO4, Expmap) {
|
|||
SO4 Q1 = SO4::Expmap(xi);
|
||||
xi[i] = 3;
|
||||
SO4 Q2 = SO4::Expmap(xi);
|
||||
EXPECT(((Q1 * Q2).isApprox(Q2 * Q1)));
|
||||
EXPECT(assert_equal(Q1 * Q2, Q2 * Q1));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -83,7 +92,7 @@ TEST(SO4, Cayley) {
|
|||
TEST(SO4, Retract) {
|
||||
auto v = Vector6::Zero();
|
||||
SO4 actual = traits<SO4>::Retract(id, v);
|
||||
EXPECT(actual.isApprox(id));
|
||||
EXPECT(assert_equal(id, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -108,7 +117,7 @@ TEST(SO4, Invariants) {
|
|||
EXPECT(check_manifold_invariants(Q1, Q2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO4, compose) {
|
||||
SO4 expected = Q1 * Q2;
|
||||
Matrix actualH1, actualH2;
|
||||
|
|
@ -124,10 +133,10 @@ TEST(SO4, compose) {
|
|||
CHECK(assert_equal(numericalH2, actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO4, vec) {
|
||||
using Vector16 = SO4::Vector16;
|
||||
const Vector16 expected = Eigen::Map<Vector16>(Q2.data());
|
||||
using Vector16 = SO4::VectorN2;
|
||||
const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
|
||||
Matrix actualH;
|
||||
const Vector16 actual = Q2.vec(actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
|
@ -138,31 +147,31 @@ TEST(SO4, vec) {
|
|||
CHECK(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO4, topLeft) {
|
||||
const Matrix3 expected = Q3.topLeftCorner<3, 3>();
|
||||
Matrix actualH;
|
||||
const Matrix3 actual = Q3.topLeft(actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
|
||||
return Q3.topLeft();
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
CHECK(assert_equal(numericalH, actualH));
|
||||
}
|
||||
// /* *************************************************************************
|
||||
// */ TEST(SO4, topLeft) {
|
||||
// const Matrix3 expected = Q3.topLeftCorner<3, 3>();
|
||||
// Matrix actualH;
|
||||
// const Matrix3 actual = Q3.topLeft(actualH);
|
||||
// CHECK(assert_equal(expected, actual));
|
||||
// boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
|
||||
// return Q3.topLeft();
|
||||
// };
|
||||
// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
// CHECK(assert_equal(numericalH, actualH));
|
||||
// }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO4, stiefel) {
|
||||
const Matrix43 expected = Q3.leftCols<3>();
|
||||
Matrix actualH;
|
||||
const Matrix43 actual = Q3.stiefel(actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
|
||||
return Q3.stiefel();
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
CHECK(assert_equal(numericalH, actualH));
|
||||
}
|
||||
// /* *************************************************************************
|
||||
// */ TEST(SO4, stiefel) {
|
||||
// const Matrix43 expected = Q3.leftCols<3>();
|
||||
// Matrix actualH;
|
||||
// const Matrix43 actual = Q3.stiefel(actualH);
|
||||
// CHECK(assert_equal(expected, actual));
|
||||
// boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
|
||||
// return Q3.stiefel();
|
||||
// };
|
||||
// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
// CHECK(assert_equal(numericalH, actualH));
|
||||
// }
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue