diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index ae88f6605..639ceb574 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -32,7 +32,6 @@ namespace gtsam { PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, const Bias& biasHat) : p_(p), biasHat_(biasHat), deltaTij_(0.0) { - cov_.setZero(); resetIntegration(); } @@ -47,14 +46,20 @@ void PreintegrationBase::resetIntegration() { delVdelBiasOmega_ = Z_3x3; } +//------------------------------------------------------------------------------ +ostream& operator<<(ostream& os, const PreintegrationBase& pim) { + os << " deltaTij " << pim.deltaTij_ << endl; + os << " deltaRij " << Point3(pim.theta()) << endl; + os << " deltaPij " << Point3(pim.deltaPij()) << endl; + os << " deltaVij " << Point3(pim.deltaVij()) << endl; + os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; + os << " acc_bias " << Point3(pim.biasHat_.accelerometer()) << endl; + return os; +} + //------------------------------------------------------------------------------ void PreintegrationBase::print(const string& s) const { - cout << s << endl; - cout << " deltaTij [" << deltaTij_ << "]" << endl; - cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl; - cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl; - biasHat_.print(" biasHat"); + cout << s << *this << endl; } //------------------------------------------------------------------------------ @@ -297,27 +302,6 @@ NavState PreintegrationBase::predict(const NavState& state_i, return state_j; } -//------------------------------------------------------------------------------ -SharedGaussian PreintegrationBase::noiseModel() const { - // Correct for application of retract, by calculating the retract derivative H - // From NavState::retract: - Matrix3 D_R_theta; - const Matrix3 iRj = Rot3::Expmap(theta(), D_R_theta).matrix(); - Matrix9 H; - H << D_R_theta, Z_3x3, Z_3x3, // - Z_3x3, iRj.transpose(), Z_3x3, // - Z_3x3, Z_3x3, iRj.transpose(); - - // TODO(frank): theta() itself is noisy, so should we not correct for that? - Matrix9 HcH = H * cov_ * H.transpose(); - return noiseModel::Gaussian::Covariance(HcH, false); -} - -//------------------------------------------------------------------------------ -Matrix9 PreintegrationBase::preintMeasCov() const { - return noiseModel()->covariance(); -} - //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 18264cc97..6e67f8bcf 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -26,6 +26,8 @@ #include #include +#include + namespace gtsam { #define ALLOW_DEPRECATED_IN_GTSAM4 @@ -119,7 +121,6 @@ class PreintegrationBase { */ /// Current estimate of deltaXij_k TangentVector deltaXij_; - Matrix9 cov_; Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias @@ -194,7 +195,6 @@ public: const double& deltaTij() const { return deltaTij_; } const Vector9& zeta() const { return deltaXij_.vector(); } - const Matrix9& zetaCov() const { return cov_; } Vector3 theta() const { return deltaXij_.theta(); } Vector3 deltaPij() const { return deltaXij_.position(); } @@ -215,6 +215,7 @@ public: /// @name Testable /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); void print(const std::string& s) const; bool equals(const PreintegrationBase& other, double tol) const; /// @} @@ -264,12 +265,6 @@ public: OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; - /// Return Gaussian noise model on prediction - SharedGaussian noiseModel() const; - - /// @deprecated: Explicitly calculate covariance - Matrix9 preintMeasCov() 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, const Vector3& vel_j, diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index 3397b4456..c2fdb963d 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -33,9 +33,9 @@ static const double kAccelSigma = 0.1 / 60.0; static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); -// Create default parameters with Z-down and above noise parameters +// Create default parameters with Z-up and above noise parameters static boost::shared_ptr defaultParams() { - auto p = PreintegrationParams::MakeSharedD(10); + auto p = PreintegrationParams::MakeSharedU(10); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->integrationCovariance = 0.0000001 * I_3x3; @@ -46,7 +46,7 @@ static boost::shared_ptr defaultParams() { /* ************************************************************************* */ TEST(ScenarioRunner, Spin) { - // angular velocity 6 kDegree/sec + // angular velocity 6 degree/sec const double w = 6 * kDegree; const Vector3 W(0, 0, w), V(0, 0, 0); const ConstantTwistScenario scenario(W, V); @@ -104,7 +104,7 @@ TEST(ScenarioRunner, ForwardWithBias) { /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { - // Forward velocity 2m/s, angular velocity 6 kDegree/sec + // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); @@ -122,7 +122,7 @@ TEST(ScenarioRunner, Circle) { /* ************************************************************************* */ TEST(ScenarioRunner, Loop) { // Forward velocity 2m/s - // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp index 1855417f2..05d2edf4d 100644 --- a/python/handwritten/navigation/Scenario.cpp +++ b/python/handwritten/navigation/Scenario.cpp @@ -70,13 +70,14 @@ void exportScenario() { class_( "PreintegratedImuMeasurements", init&, - const imuBias::ConstantBias&>()); + const imuBias::ConstantBias&>()).def(repr(self)); class_("NavState", init<>()) // TODO(frank): overload with jacobians // .def("attitude", &NavState::attitude) // .def("position", &NavState::position) // .def("velocity", &NavState::velocity) + .def(repr(self)) .def("pose", &NavState::pose); class_("ConstantBias", init<>());