diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 7664c7fd5..1a63691bf 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -124,17 +124,21 @@ public: //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } -/// @} -/// @name Testable -/// @{ + /// @} + /// @name Testable + /// @{ -/// print with optional string - void print(const std::string& s = "") const { - // explicit printing for now. - std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl; + /// ostream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const ConstantBias& bias) { + os << "acc = " << Point3(bias.accelerometer()); + os << " gyro = " << Point3(bias.gyroscope()); + return os; } + /// print with optional string + void print(const std::string& s = "") const { std::cout << s << *this; } + /** equality up to tolerance */ inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)