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

Renamed all dexpL/dexpInvL, merged Luca/Duy versions in Rot3

release/4.3a0
dellaert 2014-12-24 12:25:53 +01:00
parent 2ffa9dc6d2
commit e0a767e7fd
13 changed files with 108 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/LieScalar.h
View File

@ -107,12 +107,12 @@ namespace gtsam {
static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); }
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {
static Matrix ExpmapDerivative(const Vector& v) {
return eye(1);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector& v) {
static Matrix LogmapDerivative(const Vector& v) {
return eye(1);
}

8
gtsam/geometry/Point2.h
View File

@ -174,14 +174,10 @@ public:
static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); }
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector2& v) {
return eye(2);
}
static Matrix ExpmapDerivative(const Vector2& v) {return I_2x2;}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector2& v) {
return eye(2);
}
static Matrix LogmapDerivative(const Vector2& v) { return I_2x2;}
/// @}
/// @name Vector Space

4
gtsam/geometry/Point3.h
View File

@ -142,12 +142,12 @@ namespace gtsam {
static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector& v) {
static Matrix3 ExpmapDerivative(const Vector& v) {
return I_3x3;
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector& v) {
static Matrix3 LogmapDerivative(const Vector& v) {
return I_3x3;
}

6
gtsam/geometry/Pose2.cpp
View File

@ -135,7 +135,7 @@ Matrix3 Pose2::adjointMap(const Vector& v) {
}
/* ************************************************************************* */
Matrix3 Pose2::dexpL(const Vector3& v) {
Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
double alpha = v[2];
if (fabs(alpha) > 1e-5) {
// Chirikjian11book2, pg. 36
@ -145,7 +145,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) {
* \dot{g} g^{-1} = dexpR_{q}\dot{q}
* where q = A, and g = exp(A)
* and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26.
* Hence, to compute dexpL, we have to use the formula of J_r Chirikjian11book2, pg.36
* Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36
*/
double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
@ -164,7 +164,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) {
}
/* ************************************************************************* */
Matrix3 Pose2::dexpInvL(const Vector3& v) {
Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
double alpha = v[2];
if (fabs(alpha) > 1e-5) {
double alphaInv = 1/alpha;

4
gtsam/geometry/Pose2.h
View File

@ -182,10 +182,10 @@ public:
}
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector3& v);
static Matrix3 ExpmapDerivative(const Vector3& v);
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v);
static Matrix3 LogmapDerivative(const Vector3& v);
/// @}

4
gtsam/geometry/Rot2.h
View File

@ -176,12 +176,12 @@ namespace gtsam {
}
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {
static Matrix ExpmapDerivative(const Vector& v) {
return ones(1);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector& v) {
static Matrix LogmapDerivative(const Vector& v) {
return ones(1);
}

97
gtsam/geometry/Rot3.cpp
View File

@ -107,32 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
return q;
}
/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix3 x = skewSymmetric(v);
Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2;
return res;
}
/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix3 x = skewSymmetric(v);
Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix3 res = I_3x3 + 0.5*x - s2*x2;
return res;
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
if(index == 3)
@ -176,35 +150,56 @@ Vector Rot3::quaternion() const {
/* ************************************************************************* */
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;
if (normx < 10e-8){
Jr = I_3x3;
}
else{
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;
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::LogmapDerivative(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = I_3x3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = I_3x3 +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
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
}
/* ************************************************************************* */

42
gtsam/geometry/Rot3.h
View File

@ -216,7 +216,7 @@ namespace gtsam {
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const;
Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
@ -288,45 +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, 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);
static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) {
if(H) *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
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const Rot3& R, boost::optional<Matrix3&> H = boost::none);
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector3& v);
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v);
/**
* 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))
*/
/// Derivative of Expmap
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)
*/
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& x);
/// @}

9
gtsam/geometry/Rot3M.cpp
View File

@ -158,7 +158,7 @@ Matrix3 Rot3::transpose() const {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose()));
}
@ -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, boost::optional<Matrix3&> H) {
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
static const double PI = boost::math::constants::pi<double>();
@ -223,10 +223,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
rot(1,0)-rot(0,1));
}
if(H){
H->resize(3,3);
*H = Rot3::LogmapDerivative(thetaR);
}
if(H) *H = Rot3::LogmapDerivative(thetaR);
return thetaR;
}

9
gtsam/geometry/Rot3Q.cpp
View File

@ -101,7 +101,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix3&> H1) const {
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse());
}
@ -133,7 +133,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
@ -160,10 +160,7 @@ namespace gtsam {
thetaR = (angle / s) * q.vec();
}
if(H){
H->resize(3,3);
*H = Rot3::LogmapDerivative(thetaR);
}
if(H) *H = Rot3::LogmapDerivative(thetaR);
return thetaR;
}

10
gtsam/geometry/tests/testPose2.cpp
View File

@ -204,22 +204,22 @@ Vector3 testDexpL(const Vector3 w, const Vector3& dw) {
return y;
}
TEST( Pose2, dexpL) {
Matrix actualDexpL = Pose2::dexpL(w);
TEST( Pose2, ExpmapDerivative) {
Matrix actualDexpL = Pose2::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Pose2::dexpInvL(w);
Matrix actualDexpInvL = Pose2::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
// test case where alpha = 0
Matrix actualDexpL0 = Pose2::dexpL(w0);
Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0);
Matrix expectedDexpL0 = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w0, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5));
Matrix actualDexpInvL0 = Pose2::dexpInvL(w0);
Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
}

6
gtsam/geometry/tests/testRot2.cpp
View File

@ -166,14 +166,14 @@ Vector1 testDexpL(const Vector& dw) {
return y;
}
TEST( Rot2, dexpL) {
Matrix actualDexpL = Rot2::dexpL(w);
TEST( Rot2, ExpmapDerivative) {
Matrix actualDexpL = Rot2::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector, Vector1>(
boost::function<Vector(const Vector&)>(
boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot2::dexpInvL(w);
Matrix actualDexpInvL = Rot2::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
}

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

@ -214,14 +214,39 @@ TEST(Rot3, log)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
}
/* ************************************************************************* */
Vector w = Vector3(0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3& dw) {
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
}
TEST( Rot3, ExpmapDerivative) {
Matrix actualDexpL = Rot3::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
}
/* ************************************************************************* */
Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, ExpmapDerivative )
TEST( Rot3, ExpmapDerivative2)
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual2));
}
/* ************************************************************************* */
@ -412,27 +437,6 @@ TEST( Rot3, between )
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
Vector w = Vector3(0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3& dw) {
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
}
TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
Matrix actualDexpInvL = Rot3::dexpInvL(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
}
/* ************************************************************************* */
TEST( Rot3, xyz )
{