Skip to content

Commit

Permalink
clang tidy fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@henrygerardmoore
henrygerardmoore committed Oct 17, 2024
1 parent 7365249 commit 94322b3
Show file tree
Hide file tree
Showing 2 changed files with 92 additions and 94 deletions.
102 changes: 47 additions & 55 deletions fuse_core/include/fuse_core/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,10 +65,8 @@ static inline T getPitch(const T w, const T x, const T y, const T z)
{
return (sin_pitch >= T(0.0) ? T(1.0) : T(-1.0)) * T(M_PI / 2.0);
}
else
{
return ceres::asin(sin_pitch);
}

return ceres::asin(sin_pitch);
}

/**
Expand Down Expand Up @@ -117,11 +115,11 @@ template <typename T>
void wrapAngle2D(T& angle)
{
// Define some necessary variations of PI with the correct type (double or Jet)
static const T PI = T(M_PI);
static const T TAU = T(2 * M_PI);
static const T pi = T(M_PI);
static const T tau = T(2 * M_PI);
// Handle the 1*Tau roll-over (https://tauday.com/tau-manifesto)
// Use ceres::floor because it is specialized for double and Jet types.
angle -= TAU * ceres::floor((angle + PI) / TAU);
angle -= tau * ceres::floor((angle + pi) / tau);
}

/**
Expand Down Expand Up @@ -167,7 +165,7 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian
rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]);
rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]);

if (jacobian)
if (jacobian != nullptr)
{
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_map(jacobian);
const double qw = q[0];
Expand All @@ -185,60 +183,54 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian
jacobian_map(2, 1) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0));
return;
}
else if (discr < -gl_limit)
if (discr < -gl_limit)
{
// pitch = -90 deg
jacobian_map.setZero();
jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0));
jacobian_map(2, 1) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0));
return;
}
else
{
// Non-degenerate case:
jacobian_map(0, 0) =
-(2.0 * qx) /
((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) +
1.0) *
(2.0 * qx * qx + 2.0 * qy * qy - 1.0));
jacobian_map(0, 1) =
-((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) -
(4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
(std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
jacobian_map(0, 2) =
-((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) -
(4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
(std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
jacobian_map(0, 3) =
-(2.0 * qy) /
((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) +
1.0) *
(2.0 * qx * qx + 2.0 * qy * qy - 1.0));

jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
// Non-degenerate case:
jacobian_map(0, 0) =
-(2.0 * qx) /
((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) *
(2.0 * qx * qx + 2.0 * qy * qy - 1.0));
jacobian_map(0, 1) =
-((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) -
(4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
(std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
jacobian_map(0, 2) =
-((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) -
(4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
(std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
jacobian_map(0, 3) =
-(2.0 * qy) /
((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) *
(2.0 * qx * qx + 2.0 * qy * qy - 1.0));

jacobian_map(2, 0) =
-(2.0 * qz) /
((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) +
1.0) *
(2.0 * qy * qy + 2.0 * qz * qz - 1.0));
jacobian_map(2, 1) =
-(2.0 * qy) /
((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) +
1.0) *
(2.0 * qy * qy + 2.0 * qz * qz - 1.0));
jacobian_map(2, 2) =
-((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) -
(4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
(std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
jacobian_map(2, 3) =
-((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) -
(4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
(std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
}
jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));

jacobian_map(2, 0) =
-(2.0 * qz) /
((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0) *
(2.0 * qy * qy + 2.0 * qz * qz - 1.0));
jacobian_map(2, 1) =
-(2.0 * qy) /
((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0) *
(2.0 * qy * qy + 2.0 * qz * qz - 1.0));
jacobian_map(2, 2) =
-((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) -
(4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
(std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
jacobian_map(2, 3) =
-((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) -
(4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
(std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
}
}

Expand All @@ -256,7 +248,7 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian
static inline void quaternionProduct(const double* z, const double* w, double* zw, double* jacobian = nullptr)
{
ceres::QuaternionProduct(z, w, zw);
if (jacobian)
if (jacobian != nullptr)
{
Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>> jacobian_map(jacobian);
jacobian_map << z[0], -z[1], -z[2], -z[3], z[1], z[0], -z[3], z[2], z[2], z[3], z[0], -z[1], z[3], -z[2], z[1],
Expand All @@ -274,7 +266,7 @@ static inline void quaternionProduct(const double* z, const double* w, double* z
static inline void quaternionToAngleAxis(const double* q, double* angle_axis, double* jacobian = nullptr)
{
ceres::QuaternionToAngleAxis(q, angle_axis);
if (jacobian)
if (jacobian != nullptr)
{
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_map(jacobian);
const double& q0 = q[0];
Expand Down
84 changes: 45 additions & 39 deletions fuse_core/test/test_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
#include <fuse_core/util.hpp>
#include <fuse_core/eigen.hpp>

// NOLINTBEGIN(*c-arrays*,*array-to-pointer-decay*)
struct Quat2RPY
{
template <typename T>
Expand All @@ -51,9 +52,9 @@ struct Quat2RPY
return true;
}

static ceres::CostFunction* Create()
static std::unique_ptr<ceres::CostFunction> create()
{
return (new ceres::AutoDiffCostFunction<Quat2RPY, 3, 4>(new Quat2RPY()));
return std::unique_ptr<ceres::CostFunction>(new ceres::AutoDiffCostFunction<Quat2RPY, 3, 4>(new Quat2RPY()));
}
};

Expand All @@ -66,9 +67,9 @@ struct QuatProd
return true;
}

static ceres::CostFunction* Create()
static std::unique_ptr<ceres::CostFunction> create()
{
return (new ceres::AutoDiffCostFunction<QuatProd, 4, 4, 4>(new QuatProd()));
return std::unique_ptr<ceres::CostFunction>(new ceres::AutoDiffCostFunction<QuatProd, 4, 4, 4>(new QuatProd()));
}
};

Expand All @@ -81,9 +82,10 @@ struct Quat2AngleAxis
return true;
}

static ceres::CostFunction* Create()
static std::unique_ptr<ceres::CostFunction> create()
{
return (new ceres::AutoDiffCostFunction<Quat2AngleAxis, 3, 4>(new Quat2AngleAxis()));
return std::unique_ptr<ceres::CostFunction>(
new ceres::AutoDiffCostFunction<Quat2AngleAxis, 3, 4>(new Quat2AngleAxis()));
}
};

Expand Down Expand Up @@ -153,25 +155,26 @@ TEST(Util, quaternion2rpy)

// Test correct quaternion to roll-pitch-yaw function jacobian
const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom();
double J_analytic[12], J_autodiff[12];
double j_analytic[12];
double j_autodiff[12];
q[0] = q_eigen.w();
q[1] = q_eigen.x();
q[2] = q_eigen.y();
q[3] = q_eigen.z();

fuse_core::quaternion2rpy(q, rpy, J_analytic);
fuse_core::quaternion2rpy(q, rpy, j_analytic);

double* jacobians[1] = { J_autodiff };
double* jacobians[1] = { j_autodiff };
const double* parameters[1] = { q };

ceres::CostFunction* quat2rpy_cf = Quat2RPY::Create();
auto quat2rpy_cf = Quat2RPY::create();
double rpy_autodiff[3];
quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians);

Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_autodiff_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_analytic_map(J_analytic);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_autodiff_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_analytic_map(j_analytic);

EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map));
EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map));
}

TEST(Util, quaternionProduct)
Expand All @@ -187,30 +190,31 @@ TEST(Util, quaternionProduct)
// Atm only the jacobian wrt the second quaternion is implemented. If the computation will be
// extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1.
// double J_analytic_q1[16]; // Analytical Jacobians wrt first quaternion
double J_analytic_q2[16]; // Analytical Jacobian wrt second quaternion
double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion
double j_analytic_q2[16]; // Analytical Jacobian wrt second quaternion
double j_autodiff_q1[16];
double j_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion

fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2);
fuse_core::quaternionProduct(q1, q2, q_out, j_analytic_q2);

double* jacobians[2];
jacobians[0] = J_autodiff_q1;
jacobians[1] = J_autodiff_q2;
jacobians[0] = j_autodiff_q1;
jacobians[1] = j_autodiff_q2;

const double* parameters[2];
parameters[0] = q1;
parameters[1] = q2;

ceres::CostFunction* quat_prod_cf = QuatProd::Create();
auto quat_prod_cf = QuatProd::create();
double q_out_autodiff[4];
quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians);

Eigen::Map<fuse_core::Matrix<double, 4, 4>> J_autodiff_q1_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 4, 4>> J_autodiff_q2_map(jacobians[1]);
Eigen::Map<fuse_core::Matrix<double, 4, 4>> j_autodiff_q1_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 4, 4>> j_autodiff_q2_map(jacobians[1]);

// Eigen::Map<fuse_core::Matrix<double, 4, 4>> J_analytic_q1_map(J_analytic_q1);
Eigen::Map<fuse_core::Matrix<double, 4, 4>> J_analytic_q2_map(J_analytic_q2);
Eigen::Map<fuse_core::Matrix<double, 4, 4>> j_analytic_q2_map(j_analytic_q2);

EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map));
EXPECT_TRUE(j_analytic_q2_map.isApprox(j_autodiff_q2_map));
}

TEST(Util, quaternionToAngleAxis)
Expand All @@ -221,44 +225,46 @@ TEST(Util, quaternionToAngleAxis)
double angle_axis[3];
double q[4]{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() };

double J_analytic[12];
double J_autodiff[12];
double j_analytic[12];
double j_autodiff[12];

fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic);
fuse_core::quaternionToAngleAxis(q, angle_axis, j_analytic);

double* jacobians[1] = { J_autodiff };
double* jacobians[1] = { j_autodiff };
const double* parameters[1] = { q };

ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create();
auto quat2angle_axis_cf = Quat2AngleAxis::create();
double angle_axis_autodiff[3];
quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians);

Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_autodiff_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_analytic_map(J_analytic);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_autodiff_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_analytic_map(j_analytic);

EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map));
EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map));
}

// Test correct quaternion to angle-axis function jacobian, for quaternions representing zero rotation
{
double angle_axis[3];
double q[4]{ 1.0, 0.0, 0.0, 0.0 };

double J_analytic[12];
double J_autodiff[12];
double j_analytic[12];
double j_autodiff[12];

fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic);
fuse_core::quaternionToAngleAxis(q, angle_axis, j_analytic);

double* jacobians[1] = { J_autodiff };
double* jacobians[1] = { j_autodiff };
const double* parameters[1] = { q };

ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create();
auto quat2angle_axis_cf = Quat2AngleAxis::create();
double angle_axis_autodiff[3];
quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians);

Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_autodiff_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_analytic_map(J_analytic);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_autodiff_map(jacobians[0]);
Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_analytic_map(j_analytic);

EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map));
EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map));
}
}

// NOLINTEND(*c-arrays*,*array-to-pointer-decay*)

0 comments on commit 94322b3

Please sign in to comment.