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

Moved ExpmapDerivative and LogmapDerivative to SO3

release/4.3a0
dellaert 2015-02-10 20:14:59 +01:00
parent 5a41f50f8c
commit 982ddaeecb
3 changed files with 145 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
gtsam/geometry/Rot3.cpp
View File

@ -19,6 +19,7 @@
*/
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
@ -149,57 +150,13 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
double theta = x.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = x^, normalized by normx
const Matrix3 Y = skewSymmetric(x) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
return SO3::ExpmapDerivative(x);
}
/* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
double theta = x.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
return SO3::LogmapDerivative(x);
}
/* ************************************************************************* */

92
gtsam/geometry/SO3.cpp
View File

@ -20,9 +20,12 @@
#include <gtsam/base/concepts.h>
#include <cmath>
using namespace std;
namespace gtsam {
SO3 Rodrigues(const double& theta, const Vector3& axis) {
/* ************************************************************************* */
SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
using std::cos;
using std::sin;
@ -46,27 +49,25 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega,
SO3 SO3::Expmap(const Vector3& omega,
ChartJacobian H) {
if (H)
CONCEPT_NOT_IMPLEMENTED;
*H = ExpmapDerivative(omega);
if (omega.isZero())
return SO3::Identity();
return Identity();
else {
double angle = omega.norm();
return Rodrigues(angle, omega / angle);
return Rodrigues(omega / angle, angle);
}
}
/* ************************************************************************* */
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt;
using std::sin;
if (H)
CONCEPT_NOT_IMPLEMENTED;
// note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
@ -75,16 +76,18 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// Get trace(R)
double tr = R.trace();
Vector3 thetaR;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
thetaR = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
thetaR = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
thetaR = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
double tr_3 = tr - 3.0; // always negative
@ -96,9 +99,74 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
}
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
thetaR = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}
if(H) *H = LogmapDerivative(thetaR);
return thetaR;
}
/* ************************************************************************* */
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = omega^, normalized by normx
const Matrix3 Y = skewSymmetric(omega) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
}
/* ************************************************************************* */
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
if(zero(omega)) return I_3x3;
double theta = omega.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
}
/* ************************************************************************* */
} // end namespace gtsam

74
gtsam/geometry/SO3.h
View File

@ -30,15 +30,21 @@ namespace gtsam {
* We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it...
*/
class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3,3> {
class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3, 3> {
protected:
public:
enum { dimension=3 };
enum {
dimension = 3
};
/// @name Constructors
/// @{
/// Constructor from AngleAxisd
SO3() : Matrix3(I_3x3) {
SO3() :
Matrix3(I_3x3) {
}
/// Constructor from Eigen Matrix
@ -52,6 +58,13 @@ public:
Matrix3(angleAxis) {
}
/// Static, named constructor TODO think about relation with above
static SO3 Rodrigues(const Vector3& axis, double theta);
/// @}
/// @name Testable
/// @{
void print(const std::string& s) const {
std::cout << s << *this << std::endl;
}
@ -60,32 +73,67 @@ public:
return equal_with_abs_tol(*this, R, tol);
}
static SO3 identity() { return I_3x3; }
SO3 inverse() const { return this->Matrix3::inverse(); }
/// @}
/// @name Group
/// @{
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega, ChartJacobian H = boost::none);
/// identity rotation for group operation
static SO3 identity() {
return I_3x3;
}
/// inverse of a rotation = transpose
SO3 inverse() const {
return this->Matrix3::inverse();
}
/// @}
/// @name Lie Group
/// @{
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
*/
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
Matrix3 AdjointMap() const { return *this; }
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& omega);
Matrix3 AdjointMap() const {
return *this;
}
// Chart at origin
struct ChartAtOrigin {
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) {
return Expmap(v,H);
static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
return Expmap(omega, H);
}
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
return Logmap(R,H);
return Logmap(R, H);
}
};
using LieGroup<SO3,3>::inverse;
using LieGroup<SO3, 3>::inverse;
/// @}
};
template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
struct traits<SO3> : public internal::LieGroupTraits<SO3> {
};
template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {};
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
};
} // end namespace gtsam