diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h index cf15fb37a5d8c1130211126835f675ca0afa599b..2be43b54d15773093796950d7bb89a9e1581d43d 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;