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

renamed right jacobian of expmap and logmap (removed "right", according to Frank's suggestion :-)

release/4.3a0
Luca 2014-12-09 17:10:04 -05:00
parent 1e8402231c
commit e82b815a48
9 changed files with 22 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Rot3.cpp
View File

@ -195,7 +195,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 // x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle double normx = norm_2(x); // rotation angle
Matrix3 Jr; Matrix3 Jr;
@ -211,7 +211,7 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { Matrix3 Rot3::logmapDerivative(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation // x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle double normx = norm_2(x); // rotation angle
Matrix3 Jrinv; Matrix3 Jrinv;

11
gtsam/geometry/Rot3.h
View File

@ -16,6 +16,7 @@
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
* @author Luca Carlone
*/ */
// \callgraph // \callgraph
@ -297,7 +298,7 @@ namespace gtsam {
static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) { static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
if(H){ if(H){
H->resize(3,3); H->resize(3,3);
*H = Rot3::rightJacobianExpMapSO3(v); *H = Rot3::expmapDerivative(v);
} }
if(zero(v)) if(zero(v))
return Rot3(); return Rot3();
@ -320,20 +321,20 @@ namespace gtsam {
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in * 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. * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = rightJacobianExpMapSO3(thetahat); * where Jr = expmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to * This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega)) * 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 /** 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. * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = rightJacobianExpMapSO3inverse(omega); * where Jrinv = logmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega)) * This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * 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 /// @name Group Action on Point3

2
gtsam/geometry/Rot3M.cpp
View File

@ -241,7 +241,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
if(H){ if(H){
H->resize(3,3); H->resize(3,3);
*H = Rot3::rightJacobianExpMapSO3inverse(thetaR); *H = Rot3::logmapDerivative(thetaR);
} }
return thetaR; return thetaR;
} }

2
gtsam/geometry/Rot3Q.cpp
View File

@ -175,7 +175,7 @@ namespace gtsam {
if(H){ if(H){
H->resize(3,3); H->resize(3,3);
*H = Rot3::rightJacobianExpMapSO3inverse(thetaR); *H = Rot3::logmapDerivative(thetaR);
} }
return thetaR; return thetaR;
} }

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

@ -217,11 +217,11 @@ TEST(Rot3, log)
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 thetahat(0.1, 0, 0.1); Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, rightJacobianExpMapSO3 ) TEST( Rot3, expmapDerivative )
{ {
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>( Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix Jactual = Rot3::rightJacobianExpMapSO3(thetahat); Matrix Jactual = Rot3::expmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual)); CHECK(assert_equal(Jexpected, Jactual));
} }
@ -236,12 +236,12 @@ TEST( Rot3, jacobianExpmap )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3inverse ) TEST( Rot3, logmapDerivative )
{ {
Rot3 R = Rot3::Expmap(thetahat); // some rotation Rot3 R = Rot3::Expmap(thetahat); // some rotation
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind( Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
&Rot3::Logmap, _1, boost::none), R); &Rot3::Logmap, _1, boost::none), R);
Matrix3 Jactual = Rot3::rightJacobianExpMapSO3inverse(thetahat); Matrix3 Jactual = Rot3::logmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual)); EXPECT(assert_equal(Jexpected, Jactual));
} }

4
gtsam/navigation/AHRSFactor.cpp
View File

@ -188,8 +188,8 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
Vector3 fR = Rot3::Logmap(fRrot); Vector3 fR = Rot3::Logmap(fRrot);
// Terms common to derivatives // Terms common to derivatives
const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega); const Matrix3 D_cDeltaRij_cOmega = Rot3::expmapDerivative(correctedOmega);
const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR); const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(fR);
if (H1) { if (H1) {
// dfR/dRi // dfR/dRi

2
gtsam/navigation/PreintegratedRotation.h
View File

@ -106,7 +106,7 @@ public:
// This was done via an expmap, now we go *back* to so<3> // This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = // const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); Rot3::expmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return biascorrectedOmega; return biascorrectedOmega;
} }

4
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( const Matrix3 Jr = Rot3::expmapDerivative(
(measuredOmega - biasOmega) * deltaT); (measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
Vector3 deltabiasOmega; Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha; deltabiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( const Matrix3 Jr = Rot3::expmapDerivative(
(measuredOmega - biasOmega) * deltaT); (measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign

6
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -340,7 +340,7 @@ TEST( ImuFactor, PartialDerivativeExpmap )
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::expmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
@ -361,7 +361,7 @@ TEST( ImuFactor, PartialDerivativeLogmap )
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind( Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); &evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
Matrix3 actualDelFdeltheta = Rot3::rightJacobianExpMapSO3inverse(thetahat); Matrix3 actualDelFdeltheta = Rot3::logmapDerivative(thetahat);
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
@ -399,7 +399,7 @@ TEST( ImuFactor, fistOrderExponential )
double alpha = 0.0; double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::expmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign