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

Made Chart and Lie derivatives compile for Quaternion

release/4.3a0
dellaert 2015-02-10 23:04:02 +01:00
parent 1494b43448
commit 7fef3618bd
3 changed files with 59 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/base/testLie.h
View File

@ -54,6 +54,7 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
}
// Do a comprehensive test of Lie Group Chart derivatives
template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_,
@ -61,7 +62,7 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
Matrix H1, H2;
typedef traits<G> T;
typedef typename G::TangentVector V;
typedef typename T::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Retract
@ -72,7 +73,7 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
// Local
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
}

46
gtsam/geometry/Quaternion.h
View File

@ -40,19 +40,6 @@ struct traits<QUATERNION_TYPE> {
return Q::Identity();
}
static Q Compose(const Q &g, const Q & h) {
return g * h;
}
static Q Between(const Q &g, const Q & h) {
Q d = g.inverse() * h;
return d;
}
static Q Inverse(const Q &g) {
return g.inverse();
}
/// @}
/// @name Basic manifold traits
/// @{
@ -65,28 +52,29 @@ struct traits<QUATERNION_TYPE> {
/// @}
/// @name Lie group traits
/// @{
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
static Q Compose(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
if (Hg)
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? )
*Hh = I_3x3;// TODO : check Jacobian consistent with chart ( I(3)? )
return g * h;
}
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
static Q Between(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
Q d = g.inverse() * h;
if (Hg)
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
*Hh = I_3x3;// TODO : check Jacobian consistent with chart (my guess I(3) )
return d;
}
static Q Inverse(const Q &g, ChartJacobian H) {
static Q Inverse(const Q &g,
ChartJacobian H = boost::none) {
if (H)
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
return g.inverse();
}
@ -94,7 +82,7 @@ struct traits<QUATERNION_TYPE> {
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
if (omega.isZero())
return Q::Identity();
return Q::Identity();
else {
_Scalar angle = omega.norm();
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
@ -109,7 +97,7 @@ struct traits<QUATERNION_TYPE> {
// define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
NearlyNegativeOne = -1.0 + 1e-10;
Vector3 omega;
@ -127,9 +115,9 @@ struct traits<QUATERNION_TYPE> {
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
angle += twoPi;
omega = (angle / s) * q.vec();
}
@ -156,9 +144,9 @@ struct traits<QUATERNION_TYPE> {
/// @{
static void Print(const Q& q, const std::string& str = "") {
if (str.size() == 0)
std::cout << "Eigen::Quaternion: ";
std::cout << "Eigen::Quaternion: ";
else
std::cout << str << " ";
std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl;
}
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {

69
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -17,6 +17,8 @@
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -37,14 +39,6 @@ TEST(Quaternion , Constructor) {
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
}
//******************************************************************************
TEST(Quaternion , Invariants) {
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
check_group_invariants(q1, q2);
check_manifold_invariants(q1, q2);
}
//******************************************************************************
TEST(Quaternion , Local) {
Vector3 z_axis(0, 0, 1);
@ -74,47 +68,62 @@ TEST(Quaternion , Compose) {
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1 * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
Q actual = traits<Q>::Compose(q1, q2);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
Q id;
Vector3 z_axis(0, 0, 1);
Q R1(Eigen::AngleAxisd(1, z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
//******************************************************************************
TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1.inverse() * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
Q actual = traits<Q>::Between(q1, q2);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(Quaternion , Inverse) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.1, z_axis));
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH;
Q actual = traits<Q>::Inverse(q1, actualH);
Q actual = traits<Q>::Inverse(q1);
EXPECT(traits<Q>::Equals(expected,actual));
}
Matrix numericalH = numericalDerivative11(traits<Q>::Inverse, q1);
EXPECT(assert_equal(numericalH,actualH));
//******************************************************************************
TEST(Quaternion , Invariants) {
check_group_invariants(id,id);
check_group_invariants(id,R1);
check_group_invariants(R2,id);
check_group_invariants(R2,R1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1);
}
//******************************************************************************
TEST(Quaternion , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
}
//******************************************************************************
TEST(Quaternion , ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,R2);
CHECK_CHART_DERIVATIVES(R2,id);
CHECK_CHART_DERIVATIVES(R2,R1);
}
//******************************************************************************