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

Some reformatting, doxygen niceties...

release/4.3a0
dellaert 2014-12-21 13:52:36 +01:00
parent 3d3d574325
commit a34dae558a
1 changed files with 63 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
gtsam/geometry/Quaternion.h
View File

@ -18,87 +18,90 @@
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#define QUATERNION_TEMPLATE typename _Scalar, int _Options
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
namespace gtsam { namespace gtsam {
// Define group traits // Define traits
template<QUATERNION_TEMPLATE> template<typename _Scalar, int _Options>
struct traits_x<QUATERNION_TYPE> { struct traits_x<QUATERNION_TYPE> {
typedef QUATERNION_TYPE ManifoldType; typedef QUATERNION_TYPE ManifoldType;
typedef QUATERNION_TYPE Q; typedef QUATERNION_TYPE Q;
typedef lie_group_tag structure_category; typedef lie_group_tag structure_category;
typedef multiplicative_group_tag group_flavor; typedef multiplicative_group_tag group_flavor;
enum { dimension = 3 }; /// @name Basic Manifold traits
/// @{
static Q Identity() { return Q::Identity(); } enum {
dimension = 3
// Define manifold traits };
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
typedef OptionalJacobian<3, 3> ChartJacobian; typedef OptionalJacobian<3, 3> ChartJacobian;
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=boost::none, ChartJacobian Hh=boost::none) { /// @}
if (Hg) { /// @name Lie group traits
//TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? ) /// @{
*Hg = h.toRotationMatrix().transpose(); static Q Identity() {
} return Q::Identity();
if (Hh) { }
//TODO : check Jacobian consistent with chart ( I(3)? )
*Hh = I_3x3; 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() ? )
if (Hh)
*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=boost::none, 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; Q d = g.inverse() * h;
if (Hg) { if (Hg)
//TODO : check Jacobian consistent with chart *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
*Hg = -d.toRotationMatrix().transpose(); if (Hh)
} *Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
if (Hh) { return d;
//TODO : check Jacobian consistent with chart (my guess I(3) )
*Hh = I_3x3;
}
return d;
} }
static Q Inverse(const Q &g, ChartJacobian H=boost::none) {
if (H) { static Q Inverse(const Q &g, ChartJacobian H = boost::none) {
//TODO : check Jacobian consistent with chart if (H)
*H = -g.toRotationMatrix(); *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
} return g.inverse();
return g.inverse();
} }
/// Exponential map, simply be converting omega to axis/angle representation /// Exponential map, simply be converting omega to axis/angle representation
// TODO: implement Jacobian 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));
} }
if (H)
throw std::runtime_error("TODO: implement Jacobian");
} }
/// We use our own Logmap, as there is a slight bug in Eigen /// We use our own Logmap, as there is a slight bug in Eigen
// TODO: implement Jacobian static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
static TangentVector Logmap(const Q& q, ChartJacobian H=boost::none) {
using std::acos; using std::acos;
using std::sqrt; using std::sqrt;
static const double twoPi = 2.0 * M_PI,
// define these compile time constants to avoid std::abs: // define these compile time constants to avoid std::abs:
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
const double qw = q.w(); const double qw = q.w();
if (qw > NearlyOne) { if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1 // Taylor expansion of (angle / s) at 1
//return (2 + 2 * (1-qw) / 3) * q.vec(); //return (2 + 2 * (1-qw) / 3) * q.vec();
return (8./3. - 2./3. * qw) * q.vec(); return (8. / 3. - 2. / 3. * qw) * q.vec();
} else if (qw < NearlyNegativeOne) { } else if (qw < NearlyNegativeOne) {
// Taylor expansion of (angle / s) at -1 // Taylor expansion of (angle / s) at -1
//return (-2 - 2 * (1 + qw) / 3) * q.vec(); //return (-2 - 2 * (1 + qw) / 3) * q.vec();
return (-8./3 + 2./3 * qw) * q.vec(); return (-8. / 3 + 2. / 3 * qw) * q.vec();
} else { } else {
// Normal, away from zero case // Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
@ -109,29 +112,39 @@ struct traits_x<QUATERNION_TYPE> {
angle += twoPi; angle += twoPi;
return (angle / s) * q.vec(); return (angle / s) * q.vec();
} }
if (H)
throw std::runtime_error("TODO: implement Jacobian");
} }
static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { /// @}
return Logmap(Between(origin,other,Horigin,Hother)); /// @name Manifold traits
/// @{
static TangentVector Local(const Q& origin, const Q& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return Logmap(Between(origin, other, Horigin, Hother));
// TODO: incorporate Jacobian of Logmap // TODO: incorporate Jacobian of Logmap
} }
static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { static Q Retract(const Q& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return Compose(origin, Expmap(v), Horigin, Hv); return Compose(origin, Expmap(v), Horigin, Hv);
// TODO : incorporate Jacobian of Expmap // TODO : incorporate Jacobian of Expmap
} }
/// @}
/// @name Testable
/// @{
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) {
return Between(q1,q2).vec().array().abs().maxCoeff() < tol; return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
} }
/// @}
}; };
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion; typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;