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

Back to compile flags

release/4.3a0
dellaert 2014-12-26 00:15:57 +01:00
parent 11858da42b
commit 9955610ea4
8 changed files with 81 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/base/testLie.h
View File

@ -29,9 +29,8 @@ 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) {
void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2, bool advanced) {
G id;
Matrix H1, H2;
@ -76,6 +75,8 @@ void testLieGroupDerivatives(TestResult& result_,
EXPECT(assert_equal(numericalDerivative21(T::Between, id, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Between, id, t2), H2));
if (!advanced) return;
// Retract
typename G::TangentVector z = T::Local(id, id);
@ -117,9 +118,9 @@ void testLieGroupDerivatives(TestResult& result_,
EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2));
}
} // namespace gtsam
} // 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) \
{ gtsam::testLieGroupDerivatives(result_, name_, t1,t2); }
#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2,flag) \
{ gtsam::testLieGroupDerivatives(result_, name_, t1, t2, flag); }

2
gtsam/geometry/Pose2.h
View File

@ -24,8 +24,6 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#define SLOW_BUT_CORRECT_EXPMAP
namespace gtsam {
/**

20
gtsam/geometry/Pose3.cpp
View File

@ -23,8 +23,6 @@
#include <iostream>
#include <cmath>
#define GTSAM_POSE3_EXPMAP
using namespace std;
namespace gtsam {
@ -160,8 +158,13 @@ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
#ifdef GTSAM_POSE3_EXPMAP
return Expmap(xi, H);
#else
if (H) CONCEPT_NOT_IMPLEMENTED;
return Pose3(Rot3::Retract(xi.head<3>()), Point3(xi.tail<3>()));
Matrix3 DR;
Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0);
if (H) {
*H = I_6x6;
H->topLeftCorner<3,3>() = DR;
}
return Pose3(R, Point3(xi.tail<3>()));
#endif
}
@ -170,9 +173,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
#ifdef GTSAM_POSE3_EXPMAP
return Logmap(T, H);
#else
if (H) CONCEPT_NOT_IMPLEMENTED;
Matrix3 DR;
Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0);
if (H) {
*H = I_6x6;
H->topLeftCorner<3,3>() = DR;
}
Vector6 xi;
xi << Rot3::Logmap(T.rotation()), T.translation().vector();
xi << omega, T.translation().vector();
return xi;
#endif
}

6
gtsam/geometry/Rot3.h
View File

@ -247,10 +247,10 @@ namespace gtsam {
#ifndef GTSAM_USE_QUATERNIONS
// Cayley chart around origin, no derivatives
// Cayley chart around origin
struct CayleyChart {
static Rot3 Retract(const Vector3& v);
static Vector3 Local(const Rot3& r);
static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
};
/// Retraction from R^3 to Rot3 manifold using the Cayley transform

28
gtsam/geometry/Rot3M.cpp
View File

@ -206,7 +206,8 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
}
/* ************************************************************************* */
Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) {
Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative");
const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x * x, y2 = y * y, z2 = z * z;
const double xy = x * y, xz = x * z, yz = y * z;
@ -217,7 +218,8 @@ Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) {
}
/* ************************************************************************* */
Vector3 Rot3::CayleyChart::Local(const Rot3& R) {
Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
// Create a fixed-size matrix
Matrix3 A = R.matrix();
// Mathematica closed form optimization (procrastination?) gone wild:
@ -237,28 +239,16 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R) {
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
if (H) CONCEPT_NOT_IMPLEMENTED;
if(mode == Rot3::CAYLEY) {
return CayleyChart::Retract(omega);
} else {
assert(false);
exit(1);
}
if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H);
else throw std::runtime_error("Rot3::Retract: unknown mode");
}
/* ************************************************************************* */
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Logmap(R,H);
if (H) CONCEPT_NOT_IMPLEMENTED;
if(mode == Rot3::CAYLEY) {
return CayleyChart::Local(R);
} else {
assert(false);
exit(1);
}
if (mode == Rot3::EXPMAP) return Logmap(R, H);
if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H);
else throw std::runtime_error("Rot3::Local: unknown mode");
}
/* ************************************************************************* */

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

@ -773,7 +773,7 @@ TEST(Pose2 , Traits) {
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);
CHECK_LIE_GROUP_DERIVATIVES(t1,t2,true);
}
/* ************************************************************************* */

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

@ -28,8 +28,6 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
#define GTSAM_POSE3_EXPMAP
GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3)
@ -463,16 +461,24 @@ TEST( Pose3, transformPose_to)
}
/* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST(Pose3, localCoordinates_first_order)
TEST(Pose3, Retract_LocalCoordinates)
{
Vector d12 = repeat(6,0.1);
Vector6 d;
d << 1,2,3,4,5,6; d/=10;
R = Rot3::Retract(d.head<3>());
Pose3 t = Pose3::Retract(d);
EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
}
/* ************************************************************************* */
TEST(Pose3, retract_localCoordinates)
{
Vector6 d12;
d12 << 1,2,3,4,5,6; d12/=10;
Pose3 t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
#endif
/* ************************************************************************* */
TEST(Pose3, localCoordinates_expmap)
TEST(Pose3, expmap_logmap)
{
Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.expmap(d12);
@ -480,8 +486,7 @@ TEST(Pose3, localCoordinates_expmap)
}
/* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST(Pose3, manifold_first_order)
TEST(Pose3, retract_localCoordinates2)
{
Pose3 t1 = T;
Pose3 t2 = T3;
@ -491,7 +496,6 @@ TEST(Pose3, manifold_first_order)
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
}
#endif
/* ************************************************************************* */
TEST(Pose3, manifold_expmap)
{
@ -678,9 +682,9 @@ TEST( Pose3, ExpmapDerivative1) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix6 expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
@ -689,9 +693,9 @@ TEST( Pose3, LogmapDerivative1) {
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix6 expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none);
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
@ -745,7 +749,7 @@ TEST( Pose3, stream)
TEST(Pose3 , Traits) {
check_group_invariants(T2,T3);
check_manifold_invariants(T2,T3);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3,false);
}
/* ************************************************************************* */

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

@ -18,11 +18,10 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/testLie.h>
//#include <gtsam/base/chartTesting.h>
#include <boost/math/constants/constants.hpp>
@ -220,6 +219,30 @@ TEST(Rot3, log)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
}
/* ************************************************************************* */
TEST(Rot3, retract_localCoordinates)
{
Vector3 d12 = repeat(3,0.1);
Rot3 R2 = R.retract(d12);
EXPECT(assert_equal(d12, R.localCoordinates(R2)));
}
/* ************************************************************************* */
TEST(Rot3, expmap_logmap)
{
Vector3 d12 = repeat(3,0.1);
Rot3 R2 = R.expmap(d12);
EXPECT(assert_equal(d12, R.logmap(R2)));
}
/* ************************************************************************* */
TEST(Rot3, retract_localCoordinates2)
{
Rot3 t1 = R, t2 = R*R, origin;
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
}
/* ************************************************************************* */
Vector w = Vector3(0.1, 0.27, -0.2);
@ -634,39 +657,7 @@ TEST(Rot3 , Traits) {
Rot3 R2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
check_group_invariants(R1, R2);
check_manifold_invariants(R1, R2);
Rot3 expected, actual;
Matrix actualH1, actualH2;
Matrix numericalH1, numericalH2;
expected = R1 * R2;
actual = traits_x<Rot3>::Compose(R1, R2, actualH1, actualH2);
EXPECT(assert_equal(expected,actual));
numericalH1 = numericalDerivative21(traits_x<Rot3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
numericalH2 = numericalDerivative22(traits_x<Rot3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
expected = R1.inverse() * R2;
actual = traits_x<Rot3>::Between(R1, R2, actualH1, actualH2);
EXPECT(assert_equal(expected,actual));
numericalH1 = numericalDerivative21(traits_x<Rot3>::Between, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
numericalH2 = numericalDerivative22(traits_x<Rot3>::Between, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
expected = R1.inverse();
actual = traits_x<Rot3>::Inverse(R1, actualH1);
EXPECT(assert_equal(expected,actual));
numericalH1 = numericalDerivative11(traits_x<Rot3>::Inverse, R1);
EXPECT(assert_equal(numericalH1,actualH1));
CHECK_LIE_GROUP_DERIVATIVES(R1,R2);
CHECK_LIE_GROUP_DERIVATIVES(R1,R2,false);
}
/* ************************************************************************* */