From 7e15063983df1a90d32de41fc9a6f740a04c5ab9 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sat, 20 Jun 2020 01:16:29 +0200
Subject: [PATCH] debugging couts
---
include/imu/factor/factor_imu.h | 25 +++++++++++++++++++++++++
1 file changed, 25 insertions(+)
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index cf15fb37a..2be43b54d 100644
--- a/include/imu/factor/factor_imu.h
+++ b/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;
--
GitLab