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

Merge ExmapeDerivative/LogmapDerivative changes from 'origin/develop' into feature/tighteningTraits 

Conflicts:
	gtsam/base/LieScalar.h
	gtsam/geometry/Point2.h
	gtsam/geometry/Point3.h
	gtsam/geometry/Rot3.h
	gtsam/geometry/Rot3M.cpp
	gtsam/geometry/Rot3Q.cpp
	gtsam/geometry/tests/testRot3.cpp
release/4.3a0
dellaert 2014-12-24 13:55:15 +01:00
parent f22c922600 7dfbcc04e9
commit 78386ad144
16 changed files with 164 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -154,10 +154,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);
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {

4
gtsam/geometry/Rot2.h
View File

@ -118,12 +118,12 @@ namespace gtsam {
Matrix1 AdjointMap() const { return I_1x1; }
/// 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);
}

99
gtsam/geometry/Rot3.cpp
View File

@ -121,32 +121,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)
@ -189,36 +163,57 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3(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); // 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
}
return Jr;
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
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::rightJacobianExpMapSO3inverse(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{
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
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^
Jrinv = I_3x3 +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
}
/* ************************************************************************* */

21
gtsam/geometry/Rot3.h
View File

@ -16,6 +16,7 @@
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
* @author Luca Carlone
*/
// \callgraph
@ -282,22 +283,22 @@ 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, OptionalJacobian<3, 3> H = boost::none) {
if (H) CONCEPT_NOT_IMPLEMENTED;
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, OptionalJacobian<3, 3> 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);
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& x);
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& x);
Rot3 retract(const Vector& omega, OptionalJacobian<3, 3> Hthis,
OptionalJacobian<3, 3> Hv = boost::none, Rot3::CoordinatesMode mode =

13
gtsam/geometry/Rot3M.cpp
View File

@ -184,9 +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, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
static const double PI = boost::math::constants::pi<double>();
@ -194,6 +192,8 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
// 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) {
@ -204,7 +204,7 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
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;
@ -217,11 +217,14 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
// 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 = Rot3::LogmapDerivative(thetaR);
return thetaR;
}
/* ************************************************************************* */

4
gtsam/geometry/Rot3Q.cpp
View File

@ -102,7 +102,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());
}
@ -135,7 +135,7 @@ namespace gtsam {
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
if(H) *H = Rot3::LogmapDerivative(thetaR);
return QuaternionChart::Logmap(R.quaternion_);
}

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

@ -211,22 +211,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));
}

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

@ -220,33 +220,70 @@ 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) ) );
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3 )
Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, ExpmapDerivative2)
{
// Linearization point
Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
CHECK(assert_equal(expectedJacobian, actualJacobian));
Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual2));
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3inverse )
TEST( Rot3, jacobianExpmap )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 deltatheta; deltatheta << 0, 0, 0;
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));
}
Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
/* ************************************************************************* */
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);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
@ -406,27 +443,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 )
{

14
gtsam/navigation/AHRSFactor.cpp
View File

@ -99,7 +99,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Matrix3 incrRt = incrR.transpose();
// Right Jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr);
// Update Jacobians
// ---------------------------------------------------------------------------
@ -108,11 +108,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// Update preintegrated measurements covariance
// ---------------------------------------------------------------------------
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i);
Rot3 Rot_j = deltaRij_ * incrR;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
// Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF framework
@ -145,9 +145,9 @@ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) {
const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr);
Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
}
return theta_biascorrected;
@ -238,8 +238,8 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
Vector3 fR = Rot3::Logmap(fRhat);
// Terms common to derivatives
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR);
const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected);
const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR);
if (H1) {
// dfR/dRi

14
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -116,7 +116,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
@ -138,11 +138,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
// Single Jacobians to propagate covariance
Matrix3 H_pos_pos = I_3x3;
@ -293,11 +293,11 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(15,6);
@ -382,8 +382,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
}
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(15,6);

14
gtsam/navigation/ImuFactor.cpp
View File

@ -113,7 +113,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
@ -133,11 +133,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
/* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j);
Matrix H_pos_pos = I_3x3;
Matrix H_pos_vel = I_3x3 * deltaT;
@ -275,11 +275,11 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(9,6);
@ -348,8 +348,8 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
}
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(9,6);

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

@ -242,7 +242,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
@ -294,7 +294,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

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

@ -307,7 +307,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
@ -355,7 +355,7 @@ TEST( ImuFactor, fistOrderExponential )
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

2
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -45,7 +45,7 @@ static boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3,
static double rankTol = 1.0;
static double linThreshold = -1.0;
static bool manageDegeneracy = true;
// static bool manageDegeneracy = true;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(3));