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

Merged in Luca's rename to ExpmapDerivative and LogMapDerivative from 'origin/feature/imuFixed'

release/4.3a0
dellaert 2014-12-24 11:07:52 +01:00
parent 30afbc5f1d d3c8d348c5
commit 0a7e099eec
5 changed files with 79 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Rot3.cpp
View File

@ -175,7 +175,7 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
@ -183,15 +183,15 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
Jr = I_3x3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx
Jr = I_3x3 - ((1-cos(normx))/(normx)) * X +
(1 -sin(normx)/normx) * X * X; // right Jacobian
}
return Jr;
}
/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) {
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;

27
gtsam/geometry/Rot3.h
View File

@ -16,6 +16,7 @@
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
* @author Luca Carlone
*/
// \callgraph
@ -287,15 +288,21 @@ namespace gtsam {
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
if(H){
H->resize(3,3);
*H = Rot3::ExpmapDerivative(v);
}
if(zero(v))
return Rot3();
else
return rodriguez(v);
}
/**
* Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const Rot3& R);
static Vector3 Logmap(const Rot3& R, boost::optional<Matrix3&> H = boost::none);
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector3& v);
@ -306,13 +313,21 @@ namespace gtsam {
/**
* 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))
*/
static Matrix3 rightJacobianExpMapSO3(const Vector3& x);
static Matrix3 ExpmapDerivative(const Vector3& x);
/** 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)
*/
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x);
static Matrix3 LogmapDerivative(const Vector3& x);
/// @}
/// @name Group Action on Point3

14
gtsam/geometry/Rot3M.cpp
View File

@ -184,7 +184,7 @@ Point3 Rot3::rotate(const Point3& p,
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) {
Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
static const double PI = boost::math::constants::pi<double>();
@ -192,6 +192,8 @@ Vector3 Rot3::Logmap(const Rot3& R) {
// Get trace(R)
double tr = rot.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) {
@ -202,7 +204,7 @@ Vector3 Rot3::Logmap(const Rot3& R) {
return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (PI / sqrt(2.0+2.0*rot(0,0))) *
thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else {
double magnitude;
@ -215,11 +217,17 @@ Vector3 Rot3::Logmap(const Rot3& R) {
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0;
}
return magnitude*Vector3(
thetaR = magnitude*Vector3(
rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1));
}
if(H){
H->resize(3,3);
*H = Rot3::LogmapDerivative(thetaR);
}
return thetaR;
}
/* ************************************************************************* */

15
gtsam/geometry/Rot3Q.cpp
View File

@ -133,21 +133,22 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R) {
Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
// define these compile time constants to avoid std::abs:
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
Vector3 thetaR;
const Quaternion& q = R.quaternion_;
const double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
return (2 - 2 * (qw - 1) / 3) * q.vec();
thetaR = (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector
return Vector3::Zero();
thetaR = Vector3::Zero();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
@ -156,8 +157,14 @@ namespace gtsam {
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
thetaR = (angle / s) * q.vec();
}
if(H){
H->resize(3,3);
*H = Rot3::LogmapDerivative(thetaR);
}
return thetaR;
}
/* ************************************************************************* */

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

@ -214,33 +214,45 @@ TEST(Rot3, log)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
/* ************************************************************************* */
Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, ExpmapDerivative )
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3 )
TEST( Rot3, jacobianExpmap )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
CHECK(assert_equal(expectedJacobian, actualJacobian));
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
&Rot3::Expmap, _1, boost::none), thetahat);
Matrix3 Jactual;
const Rot3 R = Rot3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3inverse )
TEST( Rot3, LogmapDerivative )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 deltatheta; deltatheta << 0, 0, 0;
Rot3 R = Rot3::Expmap(thetahat); // some rotation
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
&Rot3::Logmap, _1, boost::none), R);
Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
/* ************************************************************************* */
TEST( Rot3, jacobianLogmap )
{
Rot3 R = Rot3::Expmap(thetahat); // some rotation
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
&Rot3::Logmap, _1, boost::none), R);
Matrix3 Jactual;
Rot3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */