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

Moved all tests to their own test files

release/4.3a0
Frank Dellaert 2019-05-06 15:37:54 -04:00 committed by Fan Jiang
parent 095071cf36
commit e93149babb
2 changed files with 83 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
gtsam/geometry/tests/testSO3.cpp
View File

@ -15,14 +15,23 @@
* @author Frank Dellaert * @author Frank Dellaert
**/ **/
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.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 std;
using namespace gtsam; 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) { TEST(SO3, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3>)); BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
@ -50,7 +59,21 @@ TEST(SO3, Local) {
TEST(SO3, Retract) { TEST(SO3, Retract) {
Vector3 v(0, 0, 0.1); Vector3 v(0, 0, 0.1);
SO3 actual = traits<SO3>::Retract(R1, v); 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)); EXPECT(assert_equal(Jexpected, Jactual));
} }
/* ************************************************************************* */ //******************************************************************************
TEST(SO3, ApplyDexp) { TEST(SO3, ApplyDexp) {
Matrix aH1, aH2; Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) { for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) { [=](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), 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)}) { 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), for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) { Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(local.dexp() * v), EXPECT(assert_equal(Vector3(local.dexp() * v),
@ -254,17 +277,17 @@ TEST(SO3, ApplyDexp) {
} }
} }
/* ************************************************************************* */ //******************************************************************************
TEST(SO3, ApplyInvDexp) { TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2; Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) { for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f = boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) { [=](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), 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)}) { 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(); Matrix invDexp = local.dexp().inverse();
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) { Vector3(0.4, 0.3, 0.2)}) {
@ -278,15 +301,13 @@ TEST(SO3, ApplyInvDexp) {
} }
} }
/* ************************************************************************* */ //******************************************************************************
TEST(SO3, vec) { TEST(SO3, vec) {
const Vector9 expected = Eigen::Map<Vector9>(R2.data()); const Vector9 expected = Eigen::Map<const Vector9>(R2.matrix().data());
Matrix actualH; Matrix actualH;
const Vector9 actual = R2.vec(actualH); const Vector9 actual = R2.vec(actualH);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
return Q.vec();
};
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
CHECK(assert_equal(numericalH, actualH)); CHECK(assert_equal(numericalH, actualH));
} }

85
gtsam/geometry/tests/testSO4.cpp
View File

@ -19,10 +19,9 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SOt.h>
#include <gtsam/geometry/SO4.h> #include <gtsam/geometry/SO4.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream> #include <iostream>
@ -30,6 +29,16 @@
using namespace std; using namespace std;
using namespace gtsam; 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) { TEST(SO4, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO4>)); BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
@ -39,9 +48,9 @@ TEST(SO4, Concept) {
//****************************************************************************** //******************************************************************************
SO4 id; 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); 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); SO4 Q2 = SO4::Expmap(v2);
Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
SO4 Q3 = SO4::Expmap(v3); SO4 Q3 = SO4::Expmap(v3);
@ -55,12 +64,12 @@ TEST(SO4, Random) {
//****************************************************************************** //******************************************************************************
TEST(SO4, Expmap) { TEST(SO4, Expmap) {
// If we do exponential map in SO(3) subgroup, topleft should be equal to R1. // If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
auto R1 = SO3::Expmap(v1.head<3>()); auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
EXPECT((Q1.topLeft().isApprox(R1))); EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
// Same here // Same here
auto R2 = SO3::Expmap(v2.head<3>()); auto R2 = SO3::Expmap(v2.tail<3>()).matrix();
EXPECT((Q2.topLeft().isApprox(R2))); EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2)));
// Check commutative subgroups // Check commutative subgroups
for (size_t i = 0; i < 6; i++) { for (size_t i = 0; i < 6; i++) {
@ -69,7 +78,7 @@ TEST(SO4, Expmap) {
SO4 Q1 = SO4::Expmap(xi); SO4 Q1 = SO4::Expmap(xi);
xi[i] = 3; xi[i] = 3;
SO4 Q2 = SO4::Expmap(xi); 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) { TEST(SO4, Retract) {
auto v = Vector6::Zero(); auto v = Vector6::Zero();
SO4 actual = traits<SO4>::Retract(id, v); 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)); EXPECT(check_manifold_invariants(Q1, Q2));
} }
/* ************************************************************************* */ //******************************************************************************
TEST(SO4, compose) { TEST(SO4, compose) {
SO4 expected = Q1 * Q2; SO4 expected = Q1 * Q2;
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
@ -124,10 +133,10 @@ TEST(SO4, compose) {
CHECK(assert_equal(numericalH2, actualH2)); CHECK(assert_equal(numericalH2, actualH2));
} }
/* ************************************************************************* */ //******************************************************************************
TEST(SO4, vec) { TEST(SO4, vec) {
using Vector16 = SO4::Vector16; using Vector16 = SO4::VectorN2;
const Vector16 expected = Eigen::Map<Vector16>(Q2.data()); const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
Matrix actualH; Matrix actualH;
const Vector16 actual = Q2.vec(actualH); const Vector16 actual = Q2.vec(actualH);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
@ -138,31 +147,31 @@ TEST(SO4, vec) {
CHECK(assert_equal(numericalH, actualH)); CHECK(assert_equal(numericalH, actualH));
} }
/* ************************************************************************* */ // /* *************************************************************************
TEST(SO4, topLeft) { // */ TEST(SO4, topLeft) {
const Matrix3 expected = Q3.topLeftCorner<3, 3>(); // const Matrix3 expected = Q3.topLeftCorner<3, 3>();
Matrix actualH; // Matrix actualH;
const Matrix3 actual = Q3.topLeft(actualH); // const Matrix3 actual = Q3.topLeft(actualH);
CHECK(assert_equal(expected, actual)); // CHECK(assert_equal(expected, actual));
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) { // boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
return Q3.topLeft(); // return Q3.topLeft();
}; // };
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); // const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
CHECK(assert_equal(numericalH, actualH)); // CHECK(assert_equal(numericalH, actualH));
} // }
/* ************************************************************************* */ // /* *************************************************************************
TEST(SO4, stiefel) { // */ TEST(SO4, stiefel) {
const Matrix43 expected = Q3.leftCols<3>(); // const Matrix43 expected = Q3.leftCols<3>();
Matrix actualH; // Matrix actualH;
const Matrix43 actual = Q3.stiefel(actualH); // const Matrix43 actual = Q3.stiefel(actualH);
CHECK(assert_equal(expected, actual)); // CHECK(assert_equal(expected, actual));
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) { // boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
return Q3.stiefel(); // return Q3.stiefel();
}; // };
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); // const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
CHECK(assert_equal(numericalH, actualH)); // CHECK(assert_equal(numericalH, actualH));
} // }
//****************************************************************************** //******************************************************************************
int main() { int main() {