Tying up loose ends, ostream, get rid of cov_

gtsam/navigation/PreintegrationBase.cpp

@@ -32,7 +32,6 @@ namespace gtsam {
 PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& 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 << s << *this << 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");
 }
 
 //------------------------------------------------------------------------------
@@ -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,

gtsam/navigation/PreintegrationBase.h

@@ -26,6 +26,8 @@
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/linear/NoiseModel.h>
 
+#include <iosfwd>
+
 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,

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<PreintegratedImuMeasurements::Params> 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<PreintegratedImuMeasurements::Params> 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));
 

python/handwritten/navigation/Scenario.cpp

@@ -70,13 +70,14 @@ void exportScenario() {
   class_<PreintegratedImuMeasurements>(
       "PreintegratedImuMeasurements",
       init<const boost::shared_ptr<PreintegrationParams>&,
-           const imuBias::ConstantBias&>());
+           const imuBias::ConstantBias&>()).def(repr(self));
 
   class_<NavState>("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_<imuBias::ConstantBias>("ConstantBias", init<>());