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(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)); 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 // Do a comprehensive test of Lie Group Chart derivatives
template<typename G> template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_, void testChartDerivatives(TestResult& result_, const std::string& name_,
@ -61,7 +62,7 @@ void testChartDerivatives(TestResult& result_, const std::string& name_,
Matrix H1, H2; Matrix H1, H2;
typedef traits<G> T; typedef traits<G> T;
typedef typename G::TangentVector V; typedef typename T::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> OJ; typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Retract // 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)); EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
// Local // 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(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)); 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(); 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 /// @name Basic manifold traits
/// @{ /// @{
@ -65,28 +52,29 @@ struct traits<QUATERNION_TYPE> {
/// @} /// @}
/// @name Lie group traits /// @name Lie group traits
/// @{ /// @{
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = static Q Compose(const Q &g, const Q & h,
boost::none) { ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
if (Hg) 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) 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; return g * h;
} }
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = static Q Between(const Q &g, const Q & h,
boost::none) { ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
Q d = g.inverse() * h; Q d = g.inverse() * h;
if (Hg) if (Hg)
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
if (Hh) 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; return d;
} }
static Q Inverse(const Q &g, ChartJacobian H) { static Q Inverse(const Q &g,
ChartJacobian H = boost::none) {
if (H) if (H)
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
return g.inverse(); return g.inverse();
} }
@ -94,7 +82,7 @@ struct traits<QUATERNION_TYPE> {
static Q Expmap(const Eigen::Ref<const TangentVector>& omega, static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) { ChartJacobian H = boost::none) {
if (omega.isZero()) if (omega.isZero())
return Q::Identity(); return Q::Identity();
else { else {
_Scalar angle = omega.norm(); _Scalar angle = omega.norm();
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); 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: // define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, 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; Vector3 omega;
@ -127,9 +115,9 @@ struct traits<QUATERNION_TYPE> {
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous // Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI) if (angle > M_PI)
angle -= twoPi; angle -= twoPi;
else if (angle < -M_PI) else if (angle < -M_PI)
angle += twoPi; angle += twoPi;
omega = (angle / s) * q.vec(); omega = (angle / s) * q.vec();
} }
@ -156,9 +144,9 @@ struct traits<QUATERNION_TYPE> {
/// @{ /// @{
static void Print(const Q& q, const std::string& str = "") { static void Print(const Q& q, const std::string& str = "") {
if (str.size() == 0) if (str.size() == 0)
std::cout << "Eigen::Quaternion: "; std::cout << "Eigen::Quaternion: ";
else else
std::cout << str << " "; std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl; std::cout << q.vec().transpose() << std::endl;
} }
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { 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/geometry/Quaternion.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
@ -37,14 +39,6 @@ TEST(Quaternion , Constructor) {
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); 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) { TEST(Quaternion , Local) {
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1);
@ -74,47 +68,62 @@ TEST(Quaternion , Compose) {
Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1 * q2; Q expected = q1 * q2;
Matrix actualH1, actualH2; Q actual = traits<Q>::Compose(q1, q2);
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual)); 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) { TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis)); Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1.inverse() * q2; Q expected = q1.inverse() * q2;
Matrix actualH1, actualH2; Q actual = traits<Q>::Between(q1, q2);
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual)); 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) { TEST(Quaternion , Inverse) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.1, z_axis)); Q q1(Eigen::AngleAxisd(0.1, z_axis));
Q expected(Eigen::AngleAxisd(-0.1, z_axis)); Q expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH; Q actual = traits<Q>::Inverse(q1);
Q actual = traits<Q>::Inverse(q1, actualH);
EXPECT(traits<Q>::Equals(expected,actual)); 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);
} }
//****************************************************************************** //******************************************************************************