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

Introduce and use NavState for predict

release/4.3a0
dellaert 2015-07-19 01:52:08 -07:00
parent 3c59168406
commit 042d874f08
2 changed files with 52 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/navigation/PreintegrationBase.cpp
View File

@ -129,13 +129,15 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
} }
} }
/// Predict state at time j
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, NavState PreintegrationBase::predict(const NavState& state_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Rot3& deltaRij_biascorrected,
const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected, const Vector3& deltaPij_biascorrected,
const Vector3& deltaVij_biascorrected) const { const Vector3& deltaVij_biascorrected) const {
const Pose3& pose_i = state_i.pose();
const Vector3& vel_i = state_i.velocity();
const double dt = deltaTij(), dt2 = dt * dt; const double dt = deltaTij(), dt2 = dt * dt;
const Matrix3 Ri = pose_i.rotation().matrix(); const Matrix3 Ri = pose_i.rotation().matrix();
@ -158,21 +160,17 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
// TODO(frank): pose update below is separate expmap for R,t. Is that kosher? // TODO(frank): pose update below is separate expmap for R,t. Is that kosher?
const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR), pose_i.translation() + Point3(dP)); const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR), pose_i.translation() + Point3(dP));
return PoseVelocityBias(pose_j, vel_i + dV, bias_i); // bias is predicted as a constant return NavState(pose_j, vel_i + dV);
} }
/// Predict state at time j
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict( NavState PreintegrationBase::predict(const NavState& state_i,
const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i) const { const imuBias::ConstantBias& bias_i) const {
const imuBias::ConstantBias biasIncr = bias_i - biasHat_; const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
return predict( return predict(state_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
pose_i, vel_i, bias_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr)); biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr));
} }
/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j, const Pose3& pose_j, const Vector3& vel_j,
@ -190,25 +188,28 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
const Rot3& rot_j = pose_j.rotation(); const Rot3& rot_j = pose_j.rotation();
const Vector3 pos_j = pose_j.translation().vector(); const Vector3 pos_j = pose_j.translation().vector();
// Evaluate residual error, according to [3] /// Compute bias-corrected quantities
/* ---------------------------------------------------------------------------------------------------- */
const imuBias::ConstantBias biasIncr = bias_i - biasHat_; const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_biascorrected_biasIncr; Matrix3 D_biascorrected_biasIncr;
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij( const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(
biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0); biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0);
const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr); const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr); const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
PoseVelocityBias predictedState_j =
predict(pose_i, vel_i, bias_i, deltaRij_biascorrected, /// Predict state at time j
NavState predictedState_j =
predict(NavState(pose_i, vel_i), deltaRij_biascorrected,
deltaPij_biascorrected, deltaVij_biascorrected); deltaPij_biascorrected, deltaVij_biascorrected);
// Evaluate residual error, according to [3]
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose.translation().vector()); const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose().translation().vector());
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity); const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity());
// fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) // fR will be computed later.
// Note: it is the same as: fR = predictedState_j.pose.rotation().between(Rot_j)
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
// Get Get so<3> version of bias corrected rotation // Get Get so<3> version of bias corrected rotation
@ -293,6 +294,6 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3&
q->omegaCoriolis = omegaCoriolis; q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis; q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q; p_ = q;
return predict(pose_i, vel_i, bias_i); return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
} }
} /// namespace gtsam } /// namespace gtsam

43
gtsam/navigation/PreintegrationBase.h
View File

@ -25,21 +25,44 @@
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/ProductLieGroup.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
namespace gtsam { namespace gtsam {
/// Velocity in 3D is just a Vector3
typedef Vector3 Velocity3;
/** /**
* Struct to hold all state variables of returned by Predict function * Navigation state: Pose (rotation, translation) + velocity
*/ */
class NavState: private ProductLieGroup<Pose3, Velocity3> {
protected:
typedef ProductLieGroup<Pose3, Velocity3> Base;
typedef OptionalJacobian<9, 9> ChartJacobian;
public:
// constructors
NavState() {}
NavState(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {}
// access
const Pose3& pose() const { return first; }
const Point3& translation() const { return pose().translation(); }
const Rot3& rotation() const { return pose().rotation(); }
const Velocity3& velocity() const { return second; }
};
/// @deprecated
struct PoseVelocityBias { struct PoseVelocityBias {
Pose3 pose; Pose3 pose;
Vector3 velocity; Vector3 velocity;
imuBias::ConstantBias bias; imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias)
: pose(_pose), velocity(_velocity), bias(_bias) {} : pose(_pose), velocity(_velocity), bias(_bias) {}
PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias)
: pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {}
NavState navState() const { return NavState(pose,velocity);}
}; };
/** /**
@ -161,15 +184,13 @@ class PreintegrationBase : public PreintegratedRotation {
} }
/// Predict state at time j, with bias-corrected quantities given /// Predict state at time j, with bias-corrected quantities given
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, NavState predict(const NavState& navState, const Rot3& deltaRij_biascorrected,
const imuBias::ConstantBias& bias_i, const Vector3& deltaPij_biascorrected,
const Rot3& deltaRij_biascorrected, const Vector3& deltaVij_biascorrected) const;
const Vector3& deltaPij_biascorrected,
const Vector3& deltaVij_biascorrected) const;
/// Predict state at time j /// Predict state at time j
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, NavState predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i) const; const imuBias::ConstantBias& bias_i) const;
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,