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

New tests

release/4.3a0
Frank Dellaert 2016-01-16 18:33:56 -08:00
parent ac544eab89
commit 3975d0a642
2 changed files with 28 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/tests/testPoint3.cpp
View File

@ -133,6 +133,17 @@ TEST (Point3, distance) {
EXPECT(assert_equal(numH2, H2, 1e-8));
}
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
omega.cross(theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -270,19 +270,27 @@ TEST( Rot3, ExpmapDerivative) {
/* ************************************************************************* */
TEST( Rot3, ExpmapDerivative2)
{
const Vector3 thetahat(0.1, 0, 0.1);
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
const Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
const Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual2));
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
TEST(Rot3, ExpmapDerivative3) {
TEST( Rot3, ExpmapDerivative3)
{
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
TEST(Rot3, ExpmapDerivative4) {
// Iserles05an (Lie-group Methods) says:
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@ -309,7 +317,7 @@ TEST(Rot3, ExpmapDerivative3) {
}
/* ************************************************************************* */
TEST(Rot3, ExpmapDerivative4) {
TEST(Rot3, ExpmapDerivative5) {
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };