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

Renamed old Rot3 methods

release/4.3a0
dellaert 2014-12-24 11:36:06 +01:00
parent 2f31500170
commit 2ffa9dc6d2
5 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/navigation/AHRSFactor.cpp
View File

@ -99,7 +99,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Matrix3 incrRt = incrR.transpose(); const Matrix3 incrRt = incrR.transpose();
// Right Jacobian computed at theta_incr // 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 // Update Jacobians
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
@ -108,11 +108,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// Update preintegrated measurements covariance // Update preintegrated measurements covariance
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) 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; Rot3 Rot_j = deltaRij_ * incrR;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) 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 // 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 // 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); const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) { if (H) {
const Matrix3 Jrinv_theta_bc = // const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); Rot3::LogmapDerivative(theta_biascorrected);
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 theta_biascorrected; return theta_biascorrected;
@ -238,8 +238,8 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
Vector3 fR = Rot3::Logmap(fRhat); Vector3 fR = Rot3::Logmap(fRhat);
// Terms common to derivatives // Terms common to derivatives
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR);
if (H1) { if (H1) {
// dfR/dRi // 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 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 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 // 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 // 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 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; Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) 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 // Single Jacobians to propagate covariance
Matrix3 H_pos_pos = I_3x3; 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 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 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) { if(H1) {
H1->resize(15,6); H1->resize(15,6);
@ -382,8 +382,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
} }
if(H5) { if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(15,6); 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 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 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 // 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 // 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 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; Rot3 Rot_j = deltaRij_ * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) 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_pos = I_3x3;
Matrix H_pos_vel = I_3x3 * deltaT; 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 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 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) { if(H1) {
H1->resize(9,6); H1->resize(9,6);
@ -348,8 +348,8 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
} }
if(H5) { if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(9,6); H5->resize(9,6);

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

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

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