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

Params::print, and comments

release/4.3a0
dellaert 2015-08-09 11:23:34 -07:00
parent 887c0d8f59
commit a3032fe367
2 changed files with 32 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/navigation/PreintegrationBase.cpp
View File

@ -26,7 +26,21 @@ using namespace std;
namespace gtsam {
/// Re-initialize PreintegratedMeasurements
//------------------------------------------------------------------------------
void PreintegrationBase::Params::print(const string& s) const {
PreintegratedRotation::Params::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
if (omegaCoriolis && use2ndOrderCoriolis)
cout << "Using 2nd-order Coriolis" << endl;
if (body_P_sensor)
body_P_sensor->print(" ");
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
}
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
@ -37,7 +51,7 @@ void PreintegrationBase::resetIntegration() {
delVdelBiasOmega_ = Z_3x3;
}
/// Needed for testable
//------------------------------------------------------------------------------
void PreintegrationBase::print(const string& s) const {
cout << s << endl;
cout << " deltaTij [" << deltaTij_ << "]" << endl;
@ -47,7 +61,7 @@ void PreintegrationBase::print(const string& s) const {
biasHat_.print(" biasHat");
}
/// Needed for testable
//------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return fabs(deltaTij_ - other.deltaTij_) < tol
@ -61,7 +75,7 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
}
//------------------------------------------------------------------------------
std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const {
// Correct for bias in the sensor frame
@ -106,8 +120,8 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
}
//------------------------------------------------------------------------------
void PreintegrationBase::update(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, const double dt,
void PreintegrationBase::update(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
// Save current rotation for updating Jacobians
@ -130,7 +144,8 @@ void PreintegrationBase::update(
const Vector3 integratedOmega = j_correctedOmega * dt;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
@ -227,7 +242,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
if (H1)
*H1 << D_error_predict * D_predict_state_i.leftCols<6>();
if (H2)
*H2 << D_error_predict * D_predict_state_i.rightCols<3>() * state_i.R().transpose();
*H2
<< D_error_predict * D_predict_state_i.rightCols<3>()
* state_i.R().transpose();
if (H3)
*H3 << D_error_state_j.leftCols<6>();
if (H4)
@ -251,4 +268,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
//------------------------------------------------------------------------------
}/// namespace gtsam

6
gtsam/navigation/PreintegrationBase.h
View File

@ -60,10 +60,8 @@ public:
struct Params: PreintegratedRotation::Params {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
/// (to compensate errors in Euler integration)
/// (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity constant in body frame
Vector3 n_gravity; ///< Gravity vector in nav frame
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
@ -82,6 +80,8 @@ public:
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
void print(const std::string& s) const;
protected:
/// Default constructor for serialization only: uninitialized!
Params();