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
double normx = norm_2(x); // rotation angle
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
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;

11
gtsam/geometry/Rot3.h
View File

@ -16,6 +16,7 @@
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
* @author Luca Carlone
*/
// \callgraph
@ -297,7 +298,7 @@ namespace gtsam {
static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
if(H){
H->resize(3,3);
*H = Rot3::rightJacobianExpMapSO3(v);
*H = Rot3::expmapDerivative(v);
}
if(zero(v))
return Rot3();
@ -320,20 +321,20 @@ 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 = rightJacobianExpMapSO3(thetahat);
* 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 = rightJacobianExpMapSO3inverse(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

2
gtsam/geometry/Rot3M.cpp
View File

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

2
gtsam/geometry/Rot3Q.cpp
View File

@ -175,7 +175,7 @@ namespace gtsam {
if(H){
H->resize(3,3);
*H = Rot3::rightJacobianExpMapSO3inverse(thetaR);
*H = Rot3::logmapDerivative(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);
TEST( Rot3, rightJacobianExpMapSO3 )
TEST( Rot3, expmapDerivative )
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix Jactual = Rot3::rightJacobianExpMapSO3(thetahat);
Matrix Jactual = Rot3::expmapDerivative(thetahat);
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
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
&Rot3::Logmap, _1, boost::none), R);
Matrix3 Jactual = Rot3::rightJacobianExpMapSO3inverse(thetahat);
Matrix3 Jactual = Rot3::logmapDerivative(thetahat);
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);
// Terms common to derivatives
const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega);
const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR);
const Matrix3 D_cDeltaRij_cOmega = Rot3::expmapDerivative(correctedOmega);
const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(fR);
if (H1) {
// 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>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr);
Rot3::expmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return biascorrectedOmega;
}

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

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