debugging couts

include/imu/factor/factor_imu.h

@@ -398,6 +398,31 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
     //////////////////////////////////////////////////////////////////////////////////////////////
     /////////////////////////////////       PRINT VALUES       ///////////////////////////////////
 #if 0
+    if (sizeof(T) == sizeof(double))
+    {
+        std::cout << "Frames " << getFrame()->id() << " - " << getFrameOther()->id() << std::endl;
+        std::cout << "\tpreint: " << acc_bias_preint_.transpose()
+                                  << " " << gyro_bias_preint_.transpose() << std::endl;
+        std::cout << "\tb1:     " << _ab1(0) << " "
+                                  << _ab1(1) << " "
+                                  << _ab1(2) << " "
+                                  << _wb1(0) << " "
+                                  << _wb1(1) << " "
+                                  << _wb1(2) << "\n";
+        std::cout << "\tb2:     " << _ab2(0) << " "
+                                  << _ab2(1) << " "
+                                  << _ab2(2) << " "
+                                  << _wb2(0) << " "
+                                  << _wb2(1) << " "
+                                  << _wb2(2) << "\n";
+        std::cout << "\tb2-b1   " << _ab2(0)-_ab1(0) << " "
+                                  << _ab2(1)-_ab1(1) << " "
+                                  << _ab2(2)-_ab1(2) << " "
+                                  << _wb2(0)-_wb1(0) << " "
+                                  << _wb2(1)-_wb1(1) << " "
+                                  << _wb2(2)-_wb1(2) << "\n";
+    }
+
     // print values -----------------------
     Matrix<T, 10, 1> x1; x1 << _p1 , _q1.coeffs(), _v1;
     Matrix<T, 10, 1> x2; x2 << _p2 , _q2.coeffs(), _v2;