diff --git a/sophus/common.hpp b/sophus/common.hpp index 9e87ee600..5634d35b0 100644 --- a/sophus/common.hpp +++ b/sophus/common.hpp @@ -18,6 +18,7 @@ #define SOPHUS_FMT_CSTR(description, ...) description #define SOPHUS_FMT_STR(description, ...) std::string(description) #define SOPHUS_FMT_PRINT(description, ...) std::printf("%s\n", description) +#define SOPHUS_FMT_ARG(arg) #else // !SOPHUS_USE_BASIC_LOGGING @@ -35,6 +36,12 @@ #include #include +#if FMT_VERSION >= 90000 +#define SOPHUS_FMT_ARG(arg) fmt::streamed(arg) +#else +#define SOPHUS_FMT_ARG(arg) arg +#endif + #ifdef SOPHUS_COMPILE_TIME_FMT // To keep compatibility with older libfmt versions, // disable the compile time check if FMT_STRING is not available. diff --git a/sophus/geometry.hpp b/sophus/geometry.hpp index e4f56a43c..e669ec130 100644 --- a/sophus/geometry.hpp +++ b/sophus/geometry.hpp @@ -27,7 +27,7 @@ Vector2 normalFromSO2(SO2 const& R_foo_line) { template SO2 SO2FromNormal(Vector2 normal_foo) { SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "{}", - normal_foo.transpose()); + SOPHUS_FMT_ARG(normal_foo.transpose())); normal_foo.normalize(); return SO2(normal_foo.y(), -normal_foo.x()); } @@ -61,18 +61,19 @@ Matrix3 rotationFromNormal(Vector3 const& normal_foo, T(0))) { SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), "xDirHint ({}) and yDirHint ({}) must be perpendicular.", - xDirHint_foo.transpose(), yDirHint_foo.transpose()); + SOPHUS_FMT_ARG(xDirHint_foo.transpose()), + SOPHUS_FMT_ARG(yDirHint_foo.transpose())); using std::abs; using std::sqrt; T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); T const normal_foo_sqr_length = normal_foo.squaredNorm(); SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "{}", - xDirHint_foo.transpose()); + SOPHUS_FMT_ARG(xDirHint_foo.transpose())); SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "{}", - yDirHint_foo.transpose()); + SOPHUS_FMT_ARG(yDirHint_foo.transpose())); SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "{}", - normal_foo.transpose()); + SOPHUS_FMT_ARG(normal_foo.transpose())); Matrix3 basis_foo; basis_foo.col(2) = normal_foo; @@ -101,8 +102,8 @@ Matrix3 rotationFromNormal(Vector3 const& normal_foo, T det = basis_foo.determinant(); // sanity check SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), - "Determinant of basis is not 1, but {}. Basis is \n{}\n", det, - basis_foo); + "Determinant of basis is not 1, but {}. Basis is \n{}\n", + SOPHUS_FMT_ARG(det), SOPHUS_FMT_ARG(basis_foo)); return basis_foo; } diff --git a/sophus/interpolate.hpp b/sophus/interpolate.hpp index c937dbd2a..ff509e4bf 100644 --- a/sophus/interpolate.hpp +++ b/sophus/interpolate.hpp @@ -28,7 +28,7 @@ enable_if_t::supported, G> interpolate( using Scalar = typename G::Scalar; Scalar inter_p(p); SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), - "p ({}) must in [0, 1].", inter_p); + "p ({}) must in [0, 1].", SOPHUS_FMT_ARG(inter_p)); return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); } diff --git a/sophus/rxso2.hpp b/sophus/rxso2.hpp index b1ae9d810..51a7f1661 100644 --- a/sophus/rxso2.hpp +++ b/sophus/rxso2.hpp @@ -399,7 +399,7 @@ class RxSO2Base { /// SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR), - "sR must be scaled orthogonal:\n {}", sR); + "sR must be scaled orthogonal:\n {}", SOPHUS_FMT_ARG(sR)); complex_nonconst() = sR.col(0); } @@ -498,14 +498,16 @@ class RxSO2 : public RxSO2Base> { SOPHUS_ENSURE(complex_.squaredNorm() >= Constants::epsilon() * Constants::epsilon(), "Scale factor must be greater-equal epsilon: {} vs {}", - complex_.squaredNorm(), - Constants::epsilon() * Constants::epsilon()); + SOPHUS_FMT_ARG(complex_.squaredNorm()), + SOPHUS_FMT_ARG(Constants::epsilon() * + Constants::epsilon())); SOPHUS_ENSURE( complex_.squaredNorm() <= Scalar(1.) / (Constants::epsilon() * Constants::epsilon()), - "Inverse scale factor must be greater-equal epsilon: % vs %", - Scalar(1.) / complex_.squaredNorm(), - Constants::epsilon() * Constants::epsilon()); + "Inverse scale factor must be greater-equal epsilon: {} vs {}", + SOPHUS_FMT_ARG(Scalar(1.) / complex_.squaredNorm()), + SOPHUS_FMT_ARG(Constants::epsilon() * + Constants::epsilon())); } /// Constructor from complex number. diff --git a/sophus/se2.hpp b/sophus/se2.hpp index a0343907f..e8dfb0740 100644 --- a/sophus/se2.hpp +++ b/sophus/se2.hpp @@ -344,9 +344,10 @@ class SE2Base { /// Precondition: ``R`` must be orthogonal and ``det(R)=1``. /// SOPHUS_FUNC void setRotationMatrix(Matrix const& R) { - SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", R); + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", + SOPHUS_FMT_ARG(R)); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}", - R.determinant()); + SOPHUS_FMT_ARG(R.determinant())); typename SO2Type::ComplexTemporaryType const complex( Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1))); so2().setComplex(complex); @@ -762,7 +763,7 @@ class SE2 : public SE2Base> { SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { SOPHUS_ENSURE( Omega.row(2).template lpNorm<1>() < Constants::epsilon(), - "Omega: \n{}", Omega); + "Omega: \n{}", SOPHUS_FMT_ARG(Omega)); Tangent upsilon_omega; upsilon_omega.template head<2>() = Omega.col(2).template head<2>(); upsilon_omega[2] = SO2::vee(Omega.template topLeftCorner<2, 2>()); diff --git a/sophus/se3.hpp b/sophus/se3.hpp index 732d17495..2f769dca8 100644 --- a/sophus/se3.hpp +++ b/sophus/se3.hpp @@ -397,9 +397,10 @@ class SE3Base { /// Precondition: ``R`` must be orthogonal and ``det(R)=1``. /// SOPHUS_FUNC void setRotationMatrix(Matrix3 const& R) { - SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", R); + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", + SOPHUS_FMT_ARG(R)); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}", - R.determinant()); + SOPHUS_FMT_ARG(R.determinant())); so3().setQuaternion(Eigen::Quaternion(R)); } @@ -517,7 +518,8 @@ class SE3 : public SE3Base> { SOPHUS_ENSURE((T.row(3) - Matrix(Scalar(0), Scalar(0), Scalar(0), Scalar(1))) .squaredNorm() < Constants::epsilon(), - "Last row is not (0,0,0,1), but ({}).", T.row(3)); + "Last row is not (0,0,0,1), but ({}).", + SOPHUS_FMT_ARG(T.row(3))); } /// This provides unsafe read/write access to internal data. SO(3) is diff --git a/sophus/so2.hpp b/sophus/so2.hpp index 8720a3019..dcdabd733 100644 --- a/sophus/so2.hpp +++ b/sophus/so2.hpp @@ -402,9 +402,10 @@ class SO2 : public SO2Base> { SOPHUS_FUNC explicit SO2(Transformation const& R) : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1))) { - SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", R); + SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", + SOPHUS_FMT_ARG(R)); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}", - R.determinant()); + SOPHUS_FMT_ARG(R.determinant())); } /// Constructor from pair of real and imaginary number. diff --git a/sophus/so3.hpp b/sophus/so3.hpp index ffb5a1200..2450860d0 100644 --- a/sophus/so3.hpp +++ b/sophus/so3.hpp @@ -284,7 +284,7 @@ class SO3Base { // w=0 should never happen here! SOPHUS_ENSURE(abs(w) >= Constants::epsilon(), "Quaternion ({}) should be normalized!", - unit_quaternion().coeffs().transpose()); + SOPHUS_FMT_ARG(unit_quaternion().coeffs().transpose())); Scalar squared_w = w * w; two_atan_nbyw_by_n = Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w); @@ -317,9 +317,10 @@ class SO3Base { /// SOPHUS_FUNC void normalize() { Scalar length = unit_quaternion_nonconst().norm(); - SOPHUS_ENSURE(length >= Constants::epsilon(), - "Quaternion ({}) should not be close to zero!", - unit_quaternion_nonconst().coeffs().transpose()); + SOPHUS_ENSURE( + length >= Constants::epsilon(), + "Quaternion ({}) should not be close to zero!", + SOPHUS_FMT_ARG(unit_quaternion_nonconst().coeffs().transpose())); unit_quaternion_nonconst().coeffs() /= length; } @@ -514,9 +515,9 @@ class SO3 : public SO3Base> { /// SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) { SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", - R * R.transpose()); + SOPHUS_FMT_ARG(R * R.transpose())); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}", - R.determinant()); + SOPHUS_FMT_ARG(R.determinant())); } /// Constructor from quaternion @@ -724,7 +725,8 @@ class SO3 : public SO3Base> { SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) < Sophus::Constants::epsilon(), "SO3::exp failed! omega: {}, real: {}, img: {}", - omega.transpose(), real_factor, imag_factor); + SOPHUS_FMT_ARG(omega.transpose()), + SOPHUS_FMT_ARG(real_factor), SOPHUS_FMT_ARG(imag_factor)); return q; } diff --git a/sophus/test_macros.hpp b/sophus/test_macros.hpp index e40d109ba..c48d43a43 100644 --- a/sophus/test_macros.hpp +++ b/sophus/test_macros.hpp @@ -122,7 +122,8 @@ void processTestResult(bool passed) { "{} (={}) is not approx {} (={}); {} is {}; nrm is {}\n", \ SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ - SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ + SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), \ + Sophus::details::pretty(nrm)); \ msg += SOPHUS_FMT_STR(descr, ##__VA_ARGS__); \ Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ msg); \ @@ -140,7 +141,8 @@ void processTestResult(bool passed) { "is {}\n", \ SOPHUS_STRINGIFY(left), Sophus::details::pretty(left), \ SOPHUS_STRINGIFY(right), Sophus::details::pretty(right), \ - SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm); \ + SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), \ + Sophus::details::pretty(nrm)); \ msg += SOPHUS_FMT_STR(descr, ##__VA_ARGS__); \ Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \ msg); \ diff --git a/sophus/types.hpp b/sophus/types.hpp index 16529041e..da2f969f7 100644 --- a/sophus/types.hpp +++ b/sophus/types.hpp @@ -128,7 +128,7 @@ template class SetElementAt { public: static void impl(Scalar& s, Scalar value, int at) { - SOPHUS_ENSURE(at == 0, "is {}", at); + SOPHUS_ENSURE(at == 0, "is {}", SOPHUS_FMT_ARG(at)); s = value; } }; @@ -137,7 +137,7 @@ template class SetElementAt, Scalar> { public: static void impl(Vector& v, Scalar value, int at) { - SOPHUS_ENSURE(at >= 0 && at < N, "is {}", at); + SOPHUS_ENSURE(at >= 0 && at < N, "is {}", SOPHUS_FMT_ARG(at)); v[at] = value; } }; diff --git a/test/core/test_rxso2.cpp b/test/core/test_rxso2.cpp index e0eba8f65..161a78ef1 100644 --- a/test/core/test_rxso2.cpp +++ b/test/core/test_rxso2.cpp @@ -71,11 +71,13 @@ class Tests { Matrix2 R = makeRotationMatrix(M); Matrix2 sR = scale * R; SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), - "isScaledOrthogonalAndPositive(sR): {} *\n{}", scale, R); + "isScaledOrthogonalAndPositive(sR): {} *\n{}", + SOPHUS_FMT_ARG(scale), SOPHUS_FMT_ARG(R)); Matrix2 sR_cols_swapped; sR_cols_swapped << sR.col(1), sR.col(0); SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), - "isScaledOrthogonalAndPositive(-sR): {} *\n{}", scale, R); + "isScaledOrthogonalAndPositive(-sR): {} *\n{}", + SOPHUS_FMT_ARG(scale), SOPHUS_FMT_ARG(R)); } } return passed; diff --git a/test/core/test_rxso3.cpp b/test/core/test_rxso3.cpp index 6762cf965..942030000 100644 --- a/test/core/test_rxso3.cpp +++ b/test/core/test_rxso3.cpp @@ -281,11 +281,13 @@ class Tests { Matrix3 R = makeRotationMatrix(M); Matrix3 sR = scale * R; SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR), - "isScaledOrthogonalAndPositive(sR): {} *\n{}", scale, R); + "isScaledOrthogonalAndPositive(sR): {} *\n{}", + SOPHUS_FMT_ARG(scale), SOPHUS_FMT_ARG(R)); Matrix3 sR_cols_swapped; sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2); SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped), - "isScaledOrthogonalAndPositive(-sR): {} *\n{}", scale, R); + "isScaledOrthogonalAndPositive(-sR): {} *\n{}", + SOPHUS_FMT_ARG(scale), SOPHUS_FMT_ARG(R)); } } return passed; diff --git a/test/core/tests.hpp b/test/core/tests.hpp index 40d66e3ff..7adad3ba1 100644 --- a/test/core/tests.hpp +++ b/test/core/tests.hpp @@ -77,7 +77,7 @@ class LieGroupTests { Tangent ad2 = LieGroup::vee(T * LieGroup::hat(x) * group_vec_[i].inverse().matrix()); SOPHUS_TEST_APPROX(passed, ad1, ad2, Scalar(10) * kSmallEps, - "Adjoint case %, %", i, j); + "Adjoint case {}, {}", i, j); } } return passed; @@ -148,44 +148,48 @@ class LieGroupTests { for (LieGroup foo_T_bar : group_vec_) { LieGroup foo_T2_bar = foo_T_bar; SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T2_bar.matrix(), - kSmallEps, "Copy constructor: %\nvs\n %", - transpose(foo_T_bar.matrix()), - transpose(foo_T2_bar.matrix())); + kSmallEps, "Copy constructor: {}\nvs\n {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.matrix())), + SOPHUS_FMT_ARG(transpose(foo_T2_bar.matrix()))); LieGroup foo_T3_bar; foo_T3_bar = foo_T_bar; SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T3_bar.matrix(), - kSmallEps, "Copy assignment: %\nvs\n %", - transpose(foo_T_bar.matrix()), - transpose(foo_T3_bar.matrix())); + kSmallEps, "Copy assignment: {}\nvs\n {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.matrix())), + SOPHUS_FMT_ARG(transpose(foo_T3_bar.matrix()))); LieGroup foo_T4_bar(foo_T_bar.matrix()); - SOPHUS_TEST_APPROX( - passed, foo_T_bar.matrix(), foo_T4_bar.matrix(), kSmallEps, - "Constructor from homogeneous matrix: %\nvs\n %", - transpose(foo_T_bar.matrix()), transpose(foo_T4_bar.matrix())); + SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T4_bar.matrix(), + kSmallEps, + "Constructor from homogeneous matrix: {}\nvs\n {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.matrix())), + SOPHUS_FMT_ARG(transpose(foo_T4_bar.matrix()))); Eigen::Map foo_Tmap_bar(foo_T_bar.data()); LieGroup foo_T5_bar = foo_Tmap_bar; - SOPHUS_TEST_APPROX( - passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps, - "Assignment from Eigen::Map type: %\nvs\n %", - transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix())); + SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), + kSmallEps, + "Assignment from Eigen::Map type: {}\nvs\n {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.matrix())), + SOPHUS_FMT_ARG(transpose(foo_T5_bar.matrix()))); Eigen::Map foo_Tcmap_bar(foo_T_bar.data()); LieGroup foo_T6_bar; foo_T6_bar = foo_Tcmap_bar; - SOPHUS_TEST_APPROX( - passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps, - "Assignment from Eigen::Map type: %\nvs\n %", - transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix())); + SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), + kSmallEps, + "Assignment from Eigen::Map type: {}\nvs\n {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.matrix())), + SOPHUS_FMT_ARG(transpose(foo_T5_bar.matrix()))); LieGroup I; Eigen::Map foo_Tmap2_bar(I.data()); foo_Tmap2_bar = foo_T_bar; SOPHUS_TEST_APPROX(passed, foo_Tmap2_bar.matrix(), foo_T_bar.matrix(), - kSmallEps, "Assignment to Eigen::Map type: %\nvs\n %", - transpose(foo_Tmap2_bar.matrix()), - transpose(foo_T_bar.matrix())); + kSmallEps, + "Assignment to Eigen::Map type: {}\nvs\n {}", + SOPHUS_FMT_ARG(transpose(foo_Tmap2_bar.matrix())), + SOPHUS_FMT_ARG(transpose(foo_T_bar.matrix()))); } return passed; } @@ -205,7 +209,7 @@ class LieGroupTests { }, Scalar(0)); SOPHUS_TEST_APPROX(passed, Gi, Gi2, kSmallEpsSqrt, - "Dxi_exp_x_matrix_at_ case %", i); + "Dxi_exp_x_matrix_at_ case {}", i); } return passed; @@ -225,7 +229,7 @@ class LieGroupTests { a); SOPHUS_TEST_APPROX(passed, J, J_num, 3 * kSmallEpsSqrt, - "Dx_exp_x case: %", j); + "Dx_exp_x case: {}", j); } Tangent o; @@ -251,7 +255,7 @@ class LieGroupTests { o); SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt, - "Dx_this_mul_exp_x_at_0 case: %", i); + "Dx_this_mul_exp_x_at_0 case: {}", i); } for (size_t i = 0; i < group_vec_.size(); ++i) { @@ -263,7 +267,7 @@ class LieGroupTests { Eigen::Matrix::Identity(); SOPHUS_TEST_APPROX(passed, J, J_exp, kSmallEpsSqrt, - "Dy_log_this_inv_by_at_x case: %", i); + "Dy_log_this_inv_by_at_x case: {}", i); } return passed; } @@ -277,7 +281,7 @@ class LieGroupTests { LieGroup mult = T1 * T2; T1 *= T2; SOPHUS_TEST_APPROX(passed, T1.matrix(), mult.matrix(), kSmallEps, - "Product case: %", i); + "Product case: {}", i); } return passed; } @@ -288,7 +292,7 @@ class LieGroupTests { for (size_t i = 0; i < group_vec_.size(); ++i) { Transformation T1 = group_vec_[i].matrix(); Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix(); - SOPHUS_TEST_APPROX(passed, T1, T2, kSmallEps, "G - exp(log(G)) case: %", + SOPHUS_TEST_APPROX(passed, T1, T2, kSmallEps, "G - exp(log(G)) case: {}", i); } return passed; @@ -301,7 +305,7 @@ class LieGroupTests { Transformation exp_x = LieGroup::exp(omega).matrix(); Transformation expmap_hat_x = (LieGroup::hat(omega)).exp(); SOPHUS_TEST_APPROX(passed, exp_x, expmap_hat_x, Scalar(10) * kSmallEps, - "expmap(hat(x)) - exp(x) case: %", i); + "expmap(hat(x)) - exp(x) case: {}", i); } return passed; } @@ -324,11 +328,12 @@ class LieGroupTests { Point gt_point1 = map(T, p); SOPHUS_TEST_APPROX(passed, point1, gt_point1, kSmallEps, - "Transform point case: %", i); + "Transform point case: {}", i); SOPHUS_TEST_APPROX(passed, hpoint1.hnormalized().eval(), gt_point1, - kSmallEps, "Transform homogeneous point case: %", i); + kSmallEps, "Transform homogeneous point case: {}", + i); SOPHUS_TEST_APPROX(passed, pointmap1, gt_point1, kSmallEps, - "Transform map point case: %", i); + "Transform map point case: {}", i); } } return passed; @@ -348,13 +353,13 @@ class LieGroupTests { SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p1_t), static_cast(0), kSmallEps, - "Transform line case (1st point) : %", i); + "Transform line case (1st point) : {}", i); SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p2_t), static_cast(0), kSmallEps, - "Transform line case (2nd point) : %", i); + "Transform line case (2nd point) : {}", i); SOPHUS_TEST_APPROX(passed, l_t.direction().squaredNorm(), l.direction().squaredNorm(), kSmallEps, - "Transform line case (direction) : %", i); + "Transform line case (direction) : {}", i); } } return passed; @@ -378,11 +383,11 @@ class LieGroupTests { for (int k = 0; k < PointDim; ++k) { SOPHUS_TEST_APPROX(passed, plane_t.signedDistance(points_t[k]), static_cast(0.), kSmallEps, - "Transform plane case (point #%): %", k, i); + "Transform plane case (point #{}): {}", k, i); } SOPHUS_TEST_APPROX(passed, plane_t.normal().squaredNorm(), plane.normal().squaredNorm(), kSmallEps, - "Transform plane case (normal): %", i); + "Transform plane case (normal): {}", i); } } return passed; @@ -399,7 +404,7 @@ class LieGroupTests { Tangent tangent2 = LieGroup::vee(hati * hatj - hatj * hati); SOPHUS_TEST_APPROX(passed, tangent1, tangent2, kSmallEps, - "Lie Bracket case: %", i); + "Lie Bracket case: {}", i); } } return passed; @@ -410,7 +415,7 @@ class LieGroupTests { for (size_t i = 0; i < tangent_vec_.size(); ++i) { SOPHUS_TEST_APPROX(passed, Tangent(tangent_vec_[i]), LieGroup::vee(LieGroup::hat(tangent_vec_[i])), - kSmallEps, "Hat-vee case: %", i); + kSmallEps, "Hat-vee case: {}", i); } return passed; } @@ -516,27 +521,32 @@ class LieGroupTests { optional foo_T_average = average(std::array({{foo_T_bar, foo_T_baz}})); SOPHUS_TEST(passed, bool(foo_T_average), - "log(foo_T_bar): %\nlog(foo_T_baz): %", - transpose(foo_T_bar.log()), transpose(foo_T_baz.log()), ""); + "log(foo_T_bar): {}\nlog(foo_T_baz): {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.log())), + SOPHUS_FMT_ARG(transpose(foo_T_baz.log())), ""); if (foo_T_average) { SOPHUS_TEST_APPROX( passed, foo_T_quiz.matrix(), foo_T_average->matrix(), sqrt_eps, - "log(foo_T_bar): %\nlog(foo_T_baz): %\n" - "log(interp): %\nlog(average): %", - transpose(foo_T_bar.log()), transpose(foo_T_baz.log()), - transpose(foo_T_quiz.log()), transpose(foo_T_average->log()), ""); + "log(foo_T_bar): {}\nlog(foo_T_baz): {}\n" + "log(interp): {}\nlog(average): {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.log())), + SOPHUS_FMT_ARG(transpose(foo_T_baz.log())), + SOPHUS_FMT_ARG(transpose(foo_T_quiz.log())), + SOPHUS_FMT_ARG(transpose(foo_T_average->log())), ""); } SOPHUS_TEST(passed, bool(foo_T_iaverage), - "log(foo_T_bar): %\nlog(foo_T_baz): %\n" - "log(interp): %\nlog(iaverage): %", - transpose(foo_T_bar.log()), transpose(foo_T_baz.log()), - transpose(foo_T_quiz.log()), - transpose(foo_T_iaverage->log()), ""); + "log(foo_T_bar): {}\nlog(foo_T_baz): {}\n" + "log(interp): {}\nlog(iaverage): {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.log())), + SOPHUS_FMT_ARG(transpose(foo_T_baz.log())), + SOPHUS_FMT_ARG(transpose(foo_T_quiz.log())), + SOPHUS_FMT_ARG(transpose(foo_T_iaverage->log())), ""); if (foo_T_iaverage) { - SOPHUS_TEST_APPROX( - passed, foo_T_quiz.matrix(), foo_T_iaverage->matrix(), sqrt_eps, - "log(foo_T_bar): %\nlog(foo_T_baz): %", - transpose(foo_T_bar.log()), transpose(foo_T_baz.log()), ""); + SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), + foo_T_iaverage->matrix(), sqrt_eps, + "log(foo_T_bar): {}\nlog(foo_T_baz): {}", + SOPHUS_FMT_ARG(transpose(foo_T_bar.log())), + SOPHUS_FMT_ARG(transpose(foo_T_baz.log())), ""); } } }