Merge pull request #754 from borglab/feature/preint-params-print

Fix printing of preintegration params
borglab · Apr 24, 2021 · 32acaec · 32acaec
2 parents 301f7fe + 1cdc6c1
commit 32acaec
Showing 1 changed file with 6 additions and 5 deletions.
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
@@ -17,6 +17,7 @@
  *  @author Vadim Indelman
  *  @author David Jensen
  *  @author Frank Dellaert
+ *  @author Varun Agrawal
  **/
 
 #include "PreintegrationBase.h"
@@ -35,12 +36,12 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
 
 //------------------------------------------------------------------------------
 ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
-  os << "    deltaTij " << pim.deltaTij_ << endl;
+  os << "    deltaTij = " << pim.deltaTij_ << endl;
   os << "    deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << 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;
+  os << "    deltaPij = " << pim.deltaPij().transpose() << endl;
+  os << "    deltaVij = " << pim.deltaVij().transpose() << endl;
+  os << "    gyrobias = " << pim.biasHat_.gyroscope().transpose() << endl;
+  os << "    acc_bias = " << pim.biasHat_.accelerometer().transpose() << endl;
   return os;
 }