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

Split up derivative tests

release/4.3a0
dellaert 2014-12-26 16:10:47 +01:00
parent aaf4588f20
commit 27156ec8c1
5 changed files with 160 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/base/testLie.h
View File

@ -30,17 +30,12 @@ namespace gtsam {
// Do a comprehensive test of Lie Group derivatives
template<typename G>
void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2, bool advanced) {
const G& t1, const G& t2) {
G id;
Matrix H1, H2;
typedef traits_x<G> T;
// Inverse
EXPECT(assert_equal(id,T::Inverse(id, H1)));
EXPECT(assert_equal(numericalDerivative11(T::Inverse, id),H1));
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1));
@ -48,79 +43,38 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1));
// Compose
EXPECT(assert_equal(t1, T::Compose(id, t1, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t1), H1));
EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t1), H2));
EXPECT(assert_equal(t2, T::Compose(id, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t2), H2));
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2));
// Between
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2));
}
// Do a comprehensive test of Lie Group Chart derivatives
template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2) {
EXPECT(assert_equal(id.inverse() * t1,T::Between(id, t1, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Between, id, t1), H1));
EXPECT(assert_equal(numericalDerivative22(T::Between, id, t1), H2));
EXPECT(assert_equal(id.inverse() * t2,T::Between(id, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Between, id, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Between, id, t2), H2));
if (!advanced) return;
Matrix H1, H2;
typedef traits_x<G> T;
// Retract
typename G::TangentVector z = T::Local(id, id);
EXPECT(assert_equal(id, T::Retract(id,z, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, z), H1));
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, z), H2));
typename G::TangentVector w1 = T::Local(id, t1);
EXPECT(assert_equal(t1, T::Retract(id,w1, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w1), H1));
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w1), H2));
typename G::TangentVector w2 = T::Local(id, t2);
EXPECT(assert_equal(t2, T::Retract(id,w2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w2), H2));
typename G::TangentVector w12 = T::Local(t1, t2);
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1));
EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2));
// Local
EXPECT(assert_equal(z, id.localCoordinates(id, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Local, id, id), H1));
EXPECT(assert_equal(numericalDerivative22(T::Local, id, id), H2));
EXPECT(assert_equal(w1, id.localCoordinates(t1, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Local, id, t1), H1));
EXPECT(assert_equal(numericalDerivative22(T::Local, id, t1), H2));
EXPECT(assert_equal(w2, id.localCoordinates(t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Local, id, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Local, id, t2), H2));
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2));
}
} // namespace gtsam
/// \brief Perform a concept check on the default chart for a type.
/// \param value An instantiation of the type to be tested.
#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2,flag) \
{ gtsam::testLieGroupDerivatives(result_, name_, t1, t2, flag); }
#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
{ gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
#define CHECK_CHART_DERIVATIVES(t1,t2) \
{ gtsam::testChartDerivatives(result_, name_, t1, t2); }

46
gtsam/geometry/tests/testPose2.cpp
View File

@ -153,7 +153,7 @@ namespace screw {
Pose2 expected(expectedR, expectedT);
}
TEST(Pose3, expmap_c)
TEST(Pose2, expmap_c)
{
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
@ -768,12 +768,44 @@ TEST(Pose2, align_4) {
}
//******************************************************************************
TEST(Pose2 , Traits) {
Pose2 t1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 t2(M_PI / 2.0, Point2(0.0, 2.0));
check_group_invariants(t1, t2);
check_manifold_invariants(t1, t2);
CHECK_LIE_GROUP_DERIVATIVES(t1,t2,true);
Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
//******************************************************************************
TEST(Pose2 , Invariants) {
Pose2 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
}
//******************************************************************************
TEST(Pose2 , LieGroupDerivatives) {
Pose2 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Pose2 , ChartDerivatives) {
Pose2 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
}
/* ************************************************************************* */

34
gtsam/geometry/tests/testPose3.cpp
View File

@ -746,10 +746,40 @@ TEST( Pose3, stream)
}
//******************************************************************************
TEST(Pose3 , Traits) {
TEST(Pose3 , Invariants) {
Pose3 id;
check_group_invariants(id,id);
check_group_invariants(id,T3);
check_group_invariants(T2,id);
check_group_invariants(T2,T3);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T3);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T3);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3,false);
}
//******************************************************************************
TEST(Pose3 , LieGroupDerivatives) {
Pose3 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);
}
//******************************************************************************
TEST(Pose3 , ChartDerivatives) {
Pose3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3);
}
/* ************************************************************************* */

57
gtsam/geometry/tests/testRot2.cpp
View File

@ -15,10 +15,10 @@
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
@ -155,26 +155,45 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */
Vector w = (Vector(1) << 0.27).finished();
//******************************************************************************
Rot2 T1(0.1);
Rot2 T2(0.2);
//******************************************************************************
TEST(Rot2 , Invariants) {
Rot2 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector1 testDexpL(const Vector& dw) {
Vector1 y = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw));
return y;
}
TEST( Rot2, ExpmapDerivative) {
Matrix actualDexpL = Rot2::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector, Vector1>(
boost::function<Vector(const Vector&)>(
boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
//******************************************************************************
TEST(Rot2 , LieGroupDerivatives) {
Rot2 id;
Matrix actualDexpInvL = Rot2::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Rot2 , ChartDerivatives) {
Rot2 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
}
/* ************************************************************************* */

44
gtsam/geometry/tests/testRot3.cpp
View File

@ -652,12 +652,44 @@ TEST( Rot3, slerp)
}
//******************************************************************************
TEST(Rot3 , Traits) {
Rot3 R1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
Rot3 R2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
check_group_invariants(R1, R2);
check_manifold_invariants(R1, R2);
CHECK_LIE_GROUP_DERIVATIVES(R1,R2,false);
Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
//******************************************************************************
TEST(Rot3 , Invariants) {
Rot3 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
}
//******************************************************************************
TEST(Rot3 , LieGroupDerivatives) {
Rot3 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Rot3 , ChartDerivatives) {
Rot3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
}
/* ************************************************************************* */