diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 1a63691bf..929b7ac22 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -137,7 +137,7 @@ public: } /// print with optional string - void print(const std::string& s = "") const { std::cout << s << *this; } + void print(const std::string& s = "") const { std::cout << s << *this << std::endl; } /** equality up to tolerance */ inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index a37d74b63..a03dd92f8 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -122,17 +122,22 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr(new This(*this))); } +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { + os << " preintegrated measurements:\n" << f._PIM_; + ; + // Print standard deviations on covariance only + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; +} + //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - Base::print(""); - _PIM_.print(" preintegrated measurements:"); - // Print standard deviations on covariance only - cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() - << endl; + cout << *this << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d94789d4a..b0b37d73b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -201,14 +201,13 @@ public: /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; - /** implement functions needed for Testable */ - - /// print + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&); virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + /// @} /** Access the preintegrated measurements. */ diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index 17a497158..d22c93dd9 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -37,7 +37,7 @@ void exportImuFactor() { .def(init()) .def(repr(self)); - class_ >( + class_>( "PreintegrationParams", init()) .def_readwrite("gyroscopeCovariance", &PreintegrationParams::gyroscopeCovariance) @@ -67,5 +67,8 @@ void exportImuFactor() { .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov); // NOTE(frank): Abstract classes need boost::noncopyable - class_("ImuFactor", no_init); + class_, boost::shared_ptr>( + "ImuFactor") + .def(init()) + .def(repr(self)); }