Skip to content

Commit

Permalink
fmt-v9 compatibility (#376)
Browse files Browse the repository at this point in the history
  • Loading branch information
@DmitriyKorchemkin
DmitriyKorchemkin committed Sep 13, 2022
1 parent 408f91b commit 8e19416
Show file tree
Hide file tree
Showing 13 changed files with 123 additions and 91 deletions.
7 changes: 7 additions & 0 deletions sophus/common.hpp
View file
Expand Up @@ -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

Expand All @@ -35,6 +36,12 @@
#include <fmt/format.h>
#include <fmt/ostream.h>

#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.
Expand Down
15 changes: 8 additions & 7 deletions sophus/geometry.hpp
View file
Expand Up @@ -27,7 +27,7 @@ Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
template <class T>
SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "{}",
normal_foo.transpose());
SOPHUS_FMT_ARG(normal_foo.transpose()));
normal_foo.normalize();
return SO2<T>(normal_foo.y(), -normal_foo.x());
}
Expand Down Expand Up @@ -61,18 +61,19 @@ Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
T(0))) {
SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::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<T>::epsilon(), "{}",
xDirHint_foo.transpose());
SOPHUS_FMT_ARG(xDirHint_foo.transpose()));
SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "{}",
yDirHint_foo.transpose());
SOPHUS_FMT_ARG(yDirHint_foo.transpose()));
SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "{}",
normal_foo.transpose());
SOPHUS_FMT_ARG(normal_foo.transpose()));

Matrix3<T> basis_foo;
basis_foo.col(2) = normal_foo;
Expand Down Expand Up @@ -101,8 +102,8 @@ Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
T det = basis_foo.determinant();
// sanity check
SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::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;
}

Expand Down
2 changes: 1 addition & 1 deletion sophus/interpolate.hpp
View file
Expand Up @@ -28,7 +28,7 @@ enable_if_t<interp_details::Traits<G>::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());
}

Expand Down
14 changes: 8 additions & 6 deletions sophus/rxso2.hpp
View file
Expand Up @@ -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);
}

Expand Down Expand Up @@ -498,14 +498,16 @@ class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
SOPHUS_ENSURE(complex_.squaredNorm() >= Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon: {} vs {}",
complex_.squaredNorm(),
Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon());
SOPHUS_FMT_ARG(complex_.squaredNorm()),
SOPHUS_FMT_ARG(Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()));
SOPHUS_ENSURE(
complex_.squaredNorm() <= Scalar(1.) / (Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()),
"Inverse scale factor must be greater-equal epsilon: % vs %",
Scalar(1.) / complex_.squaredNorm(),
Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon());
"Inverse scale factor must be greater-equal epsilon: {} vs {}",
SOPHUS_FMT_ARG(Scalar(1.) / complex_.squaredNorm()),
SOPHUS_FMT_ARG(Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()));
}

/// Constructor from complex number.
Expand Down
7 changes: 4 additions & 3 deletions sophus/se2.hpp
View file
Expand Up @@ -344,9 +344,10 @@ class SE2Base {
/// Precondition: ``R`` must be orthogonal and ``det(R)=1``.
///
SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> 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);
Expand Down Expand Up @@ -762,7 +763,7 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
SOPHUS_ENSURE(
Omega.row(2).template lpNorm<1>() < Constants<Scalar>::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<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
Expand Down
8 changes: 5 additions & 3 deletions sophus/se3.hpp
View file
Expand Up @@ -397,9 +397,10 @@ class SE3Base {
/// Precondition: ``R`` must be orthogonal and ``det(R)=1``.
///
SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> 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<Scalar>(R));
}

Expand Down Expand Up @@ -517,7 +518,8 @@ class SE3 : public SE3Base<SE3<Scalar_, Options>> {
SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0),
Scalar(0), Scalar(1)))
.squaredNorm() < Constants<Scalar>::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
Expand Down
5 changes: 3 additions & 2 deletions sophus/so2.hpp
View file
Expand Up @@ -402,9 +402,10 @@ class SO2 : public SO2Base<SO2<Scalar_, Options>> {
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.
Expand Down
16 changes: 9 additions & 7 deletions sophus/so3.hpp
View file
Expand Up @@ -284,7 +284,7 @@ class SO3Base {
// w=0 should never happen here!
SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::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);
Expand Down Expand Up @@ -317,9 +317,10 @@ class SO3Base {
///
SOPHUS_FUNC void normalize() {
Scalar length = unit_quaternion_nonconst().norm();
SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
"Quaternion ({}) should not be close to zero!",
unit_quaternion_nonconst().coeffs().transpose());
SOPHUS_ENSURE(
length >= Constants<Scalar>::epsilon(),
"Quaternion ({}) should not be close to zero!",
SOPHUS_FMT_ARG(unit_quaternion_nonconst().coeffs().transpose()));
unit_quaternion_nonconst().coeffs() /= length;
}

Expand Down Expand Up @@ -514,9 +515,9 @@ class SO3 : public SO3Base<SO3<Scalar_, Options>> {
///
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
Expand Down Expand Up @@ -724,7 +725,8 @@ class SO3 : public SO3Base<SO3<Scalar_, Options>> {
SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <
Sophus::Constants<Scalar>::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;
}

Expand Down
6 changes: 4 additions & 2 deletions sophus/test_macros.hpp
View file
Expand Up @@ -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); \
Expand All @@ -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); \
Expand Down
4 changes: 2 additions & 2 deletions sophus/types.hpp
View file
Expand Up @@ -128,7 +128,7 @@ template <class Scalar>
class SetElementAt<Scalar, Scalar> {
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;
}
};
Expand All @@ -137,7 +137,7 @@ template <class Scalar, int N>
class SetElementAt<Vector<Scalar, N>, Scalar> {
public:
static void impl(Vector<Scalar, N>& 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;
}
};
Expand Down
6 changes: 4 additions & 2 deletions test/core/test_rxso2.cpp
View file
Expand Up @@ -71,11 +71,13 @@ class Tests {
Matrix2<Scalar> R = makeRotationMatrix(M);
Matrix2<Scalar> 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<Scalar> 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;
Expand Down
6 changes: 4 additions & 2 deletions test/core/test_rxso3.cpp
View file
Expand Up @@ -281,11 +281,13 @@ class Tests {
Matrix3<Scalar> R = makeRotationMatrix(M);
Matrix3<Scalar> 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<Scalar> 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;
Expand Down

0 comments on commit 8e19416

Please sign in to comment.