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,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected,
NavState PreintegrationBase::predict(const NavState& state_i,
const Rot3& deltaRij_biascorrected,
const Vector3& deltaPij_biascorrected,
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 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?
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(
const Pose3& pose_i, const Vector3& vel_i,
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i) const {
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
return predict(
pose_i, vel_i, bias_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
return predict(state_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
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,
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 Vector3 pos_j = pose_j.translation().vector();
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
/// Compute bias-corrected quantities
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_biascorrected_biasIncr;
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(
biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0);
const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(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);
// Evaluate residual error, according to [3]
// 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
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
@ -293,6 +294,6 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3&
q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q;
return predict(pose_i, vel_i, bias_i);
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
} /// namespace gtsam

43
gtsam/navigation/PreintegrationBase.h
View File

@ -25,21 +25,44 @@
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/ProductLieGroup.h>
#include <gtsam/base/Vector.h>
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 {
Pose3 pose;
Vector3 velocity;
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) {}
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
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const Rot3& deltaRij_biascorrected,
const Vector3& deltaPij_biascorrected,
const Vector3& deltaVij_biascorrected) const;
NavState predict(const NavState& navState, const Rot3& deltaRij_biascorrected,
const Vector3& deltaPij_biascorrected,
const Vector3& deltaVij_biascorrected) const;
/// Predict state at time j
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i) const;
NavState predict(const NavState& state_i,
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
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,