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

capitalized ExpmapDerivative and LogmapDerivative

release/4.3a0
Luca 2014-12-10 16:16:29 -05:00
parent 57d83be48a
commit b3f0f3877c
9 changed files with 21 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

@ -175,7 +175,7 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::expmapDerivative(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;
@ -191,7 +191,7 @@ Matrix3 Rot3::expmapDerivative(const Vector3& x) {
}
/* ************************************************************************* */
Matrix3 Rot3::logmapDerivative(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;

10
gtsam/geometry/Rot3.h
View File

@ -291,7 +291,7 @@ namespace gtsam {
static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
if(H){
H->resize(3,3);
*H = Rot3::expmapDerivative(v);
*H = Rot3::ExpmapDerivative(v);
}
if(zero(v))
return Rot3();
@ -314,20 +314,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 = expmapDerivative(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 expmapDerivative(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);
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
static Matrix3 logmapDerivative(const Vector3& x);
static Matrix3 LogmapDerivative(const Vector3& x);
/// @}
/// @name Group Action on Point3

2
gtsam/geometry/Rot3M.cpp
View File

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

2
gtsam/geometry/Rot3Q.cpp
View File

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

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

@ -216,11 +216,11 @@ TEST(Rot3, log)
/* ************************************************************************* */
Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, expmapDerivative )
TEST( Rot3, ExpmapDerivative )
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix Jactual = Rot3::expmapDerivative(thetahat);
Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
}
@ -235,12 +235,12 @@ TEST( Rot3, jacobianExpmap )
}
/* ************************************************************************* */
TEST( Rot3, logmapDerivative )
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::logmapDerivative(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::expmapDerivative(correctedOmega);
const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(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::expmapDerivative(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::expmapDerivative(
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::expmapDerivative(
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::expmapDerivative((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::logmapDerivative(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::expmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign