Commit f5a77736 authored by Nikolaus Demmel's avatar Nikolaus Demmel

add script and ci-job to clang-format all files; fix formatting;

parent 29e3fa59
Pipeline #58063383 passed with stage
in 12 minutes and 4 seconds
......@@ -2,5 +2,6 @@
Language: Cpp
BasedOnStyle: Google
IndentWidth: 2
IncludeBlocks: Preserve
...
......@@ -79,3 +79,17 @@ xenial-release-compile:
elcapitan-release-compile:
<<: *compile_and_test_definition
tags: [macos, "10.11"]
# check if clang-format would make any changes
clang-format:
# TODO: add clang-format version >= 8 to "vladyslavusenko/b_image"
image: nikolausdemmel/ubuntu-dev-dbatk:16.04
tags:
- docker
stage: build
variables:
GIT_SUBMODULE_STRATEGY: none
script:
- ./scripts/clang-format-all.sh
# check if any files are now modified and error if yes
- (if git diff --name-only --diff-filter=M | grep '\..pp$'; then echo $'\n Some files are not properly formatted. You can use "./scripts/clang-format-all.sh".\n'; git diff --diff-filter=M; false; fi)
#!/usr/bin/env bash
# Format all source files in the project.
# Optionally take folder as argument; default is full inlude and src dirs.
set -e
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
FOLDER="${1:-$SCRIPT_DIR/../include $SCRIPT_DIR/../test/src $SCRIPT_DIR/../test/include}"
CLANG_FORMAT_COMMANDS="clang-format-9 clang-format-8 clang-format"
# find the first available command:
for CMD in $CLANG_FORMAT_COMMANDS; do
if hash $CMD 2>/dev/null; then
CLANG_FORMAT_CMD=$CMD
break
fi
done
if [ -z $CLANG_FORMAT_CMD ]; then
echo "clang-format not installed..."
exit 1
fi
# clang format check version
MAJOR_VERSION_NEEDED=8
MAJOR_VERSION_DETECTED=`$CLANG_FORMAT_CMD -version | sed -n -E 's/.*version ([0-9]+).*/\1/p'`
if [ -z $MAJOR_VERSION_DETECTED ]; then
echo "Failed to parse major version (`$CLANG_FORMAT_CMD -version`)"
exit 1
fi
echo "clang-format version $MAJOR_VERSION_DETECTED (`$CLANG_FORMAT_CMD -version`)"
if [ $MAJOR_VERSION_DETECTED -lt $MAJOR_VERSION_NEEDED ]; then
echo "Looks like your clang format is too old; need at least version $MAJOR_VERSION_NEEDED"
exit 1
fi
find $FOLDER -iname "*.?pp" -or -iname "*.h" | xargs $CLANG_FORMAT_CMD -verbose -i
......@@ -58,25 +58,27 @@ void test_project_jacobian() {
Vec2 res1;
cam.project(p, res1, &Jp, &Jparam);
test_jacobian("d_r_d_p", Jp,
[&](const Vec4 &x) {
Vec2 res;
cam.project(p + x, res);
return res;
},
Vec4::Zero());
test_jacobian("d_r_d_param", Jparam,
[&](const VecN &x) {
Vec2 res;
CamT cam1 = cam;
cam1 += x;
cam1.project(p, res);
return res;
},
VecN::Zero());
test_jacobian(
"d_r_d_p", Jp,
[&](const Vec4 &x) {
Vec2 res;
cam.project(p + x, res);
return res;
},
Vec4::Zero());
test_jacobian(
"d_r_d_param", Jparam,
[&](const VecN &x) {
Vec2 res;
CamT cam1 = cam;
cam1 += x;
cam1.project(p, res);
return res;
},
VecN::Zero());
}
}
}
......@@ -134,24 +136,26 @@ void test_unproject_jacobians() {
Vec4 res1;
cam.unproject(p, res1, &Jp, &Jparam);
test_jacobian("d_r_d_p", Jp,
[&](const Vec2 &x) {
Vec4 res;
cam.unproject(p + x, res);
return res;
},
Vec2::Zero());
test_jacobian("d_r_d_param", Jparam,
[&](const VecN &x) {
Vec4 res;
CamT cam1 = cam;
cam1 += x;
cam1.unproject(p, res);
return res;
},
VecN::Zero());
test_jacobian(
"d_r_d_p", Jp,
[&](const Vec2 &x) {
Vec4 res;
cam.unproject(p + x, res);
return res;
},
Vec2::Zero());
test_jacobian(
"d_r_d_param", Jparam,
[&](const VecN &x) {
Vec4 res;
CamT cam1 = cam;
cam1 += x;
cam1.unproject(p, res);
return res;
},
VecN::Zero());
}
}
}
......@@ -268,9 +272,9 @@ TEST(CameraTestCase, StereographicParamProjectJacobians) {
ASSERT_TRUE(res1.isApprox(res2))
<< "res1 " << res1.transpose() << " res2 " << res2.transpose();
test_jacobian("d_r_d_p", Jp,
[&](const Vec4 &x) { return CamT::project(p + x); },
Vec4::Zero());
test_jacobian(
"d_r_d_p", Jp, [&](const Vec4 &x) { return CamT::project(p + x); },
Vec4::Zero());
}
}
}
......@@ -318,9 +322,9 @@ TEST(CameraTestCase, StereographicParamUnprojectJacobians) {
ASSERT_TRUE(res1.isApprox(res2))
<< "res1 " << res1.transpose() << " res2 " << res2.transpose();
test_jacobian("d_r_d_p", Jp,
[&](const Vec2 &x) { return CamT::unproject(p + x); },
Vec2::Zero());
test_jacobian(
"d_r_d_p", Jp, [&](const Vec2 &x) { return CamT::unproject(p + x); },
Vec2::Zero());
}
}
}
This diff is collapsed.
......@@ -64,11 +64,12 @@ TEST(SophusUtilsCase, RightJacobianInvSO3) {
Eigen::Vector3d x0;
x0.setZero();
test_jacobian("rightJacobianInvSO3", Ja,
[&](const Eigen::Vector3d &x) {
return (Sophus::SO3d::exp(phi) * Sophus::SO3d::exp(x)).log();
},
x0);
test_jacobian(
"rightJacobianInvSO3", Ja,
[&](const Eigen::Vector3d &x) {
return (Sophus::SO3d::exp(phi) * Sophus::SO3d::exp(x)).log();
},
x0);
}
TEST(SophusUtilsCase, LeftJacobianSO3) {
......@@ -100,11 +101,12 @@ TEST(SophusUtilsCase, LeftJacobianInvSO3) {
Eigen::Vector3d x0;
x0.setZero();
test_jacobian("leftJacobianInvSO3", Ja,
[&](const Eigen::Vector3d &x) {
return (Sophus::SO3d::exp(x) * Sophus::SO3d::exp(phi)).log();
},
x0);
test_jacobian(
"leftJacobianInvSO3", Ja,
[&](const Eigen::Vector3d &x) {
return (Sophus::SO3d::exp(x) * Sophus::SO3d::exp(phi)).log();
},
x0);
}
TEST(SophusUtilsCase, RightJacobianSE3Decoupled) {
......@@ -117,12 +119,13 @@ TEST(SophusUtilsCase, RightJacobianSE3Decoupled) {
Sophus::Vector6d x0;
x0.setZero();
test_jacobian("rightJacobianSE3Decoupled", Ja,
[&](const Sophus::Vector6d &x) {
return Sophus::logd(Sophus::expd(phi).inverse() *
Sophus::expd(phi + x));
},
x0);
test_jacobian(
"rightJacobianSE3Decoupled", Ja,
[&](const Sophus::Vector6d &x) {
return Sophus::logd(Sophus::expd(phi).inverse() *
Sophus::expd(phi + x));
},
x0);
}
TEST(SophusUtilsCase, RightJacobianInvSE3Decoupled) {
......@@ -135,11 +138,12 @@ TEST(SophusUtilsCase, RightJacobianInvSE3Decoupled) {
Sophus::Vector6d x0;
x0.setZero();
test_jacobian("rightJacobianInvSE3Decoupled", Ja,
[&](const Sophus::Vector6d &x) {
return Sophus::logd(Sophus::expd(phi) * Sophus::expd(x));
},
x0);
test_jacobian(
"rightJacobianInvSE3Decoupled", Ja,
[&](const Sophus::Vector6d &x) {
return Sophus::logd(Sophus::expd(phi) * Sophus::expd(x));
},
x0);
}
TEST(SophusUtilsCase, Adjoint) {
......@@ -153,11 +157,12 @@ TEST(SophusUtilsCase, Adjoint) {
Sophus::Vector6d x0;
x0.setZero();
test_jacobian("Adj", Ja,
[&](const Sophus::Vector6d &x) {
return Sophus::logd(pose.inverse() * Sophus::expd(x) * pose);
},
x0);
test_jacobian(
"Adj", Ja,
[&](const Sophus::Vector6d &x) {
return Sophus::logd(pose.inverse() * Sophus::expd(x) * pose);
},
x0);
}
TEST(SophusUtilsCase, RotTestSO3) {
......@@ -175,11 +180,12 @@ TEST(SophusUtilsCase, RotTestSO3) {
Eigen::Vector3d x0;
x0.setZero();
test_jacobian("Rot Test", J,
[&](const Eigen::Vector3d &x) {
return Sophus::SO3d::exp(k * (t1 + x)) * t2;
},
x0);
test_jacobian(
"Rot Test", J,
[&](const Eigen::Vector3d &x) {
return Sophus::SO3d::exp(k * (t1 + x)) * t2;
},
x0);
}
TEST(SophusUtilsCase, incTest) {
......@@ -198,15 +204,16 @@ TEST(SophusUtilsCase, incTest) {
Sophus::Vector6d x0;
x0.setZero();
test_jacobian("inc test", Ja,
[&](const Sophus::Vector6d &x) {
Sophus::SE3d pose1;
pose1.so3() = Sophus::SO3d::exp(x.tail<3>()) * pose.so3();
pose1.translation() = pose.translation() + x.head<3>();
test_jacobian(
"inc test", Ja,
[&](const Sophus::Vector6d &x) {
Sophus::SE3d pose1;
pose1.so3() = Sophus::SO3d::exp(x.tail<3>()) * pose.so3();
pose1.translation() = pose.translation() + x.head<3>();
return Sophus::logd(pose.inverse() * pose1);
},
x0);
return Sophus::logd(pose.inverse() * pose1);
},
x0);
}
TEST(SophusUtilsCase, SO2Test) {
......@@ -222,11 +229,12 @@ TEST(SophusUtilsCase, SO2Test) {
Eigen::Matrix<double, 1, 1> x0;
x0.setZero();
test_jacobian("inc test", Ja,
[&](const Eigen::Matrix<double, 1, 1> &x) {
return Sophus::SO2d::exp(x[0]) * phi;
},
x0);
test_jacobian(
"inc test", Ja,
[&](const Eigen::Matrix<double, 1, 1> &x) {
return Sophus::SO2d::exp(x[0]) * phi;
},
x0);
}
TEST(SophusUtilsCase, Se3Test) {
......@@ -243,11 +251,12 @@ TEST(SophusUtilsCase, Se3Test) {
Eigen::Matrix<double, 3, 1> x0;
x0.setZero();
test_jacobian("inc test", Ja,
[&](const Eigen::Matrix<double, 3, 1> &x) {
return Sophus::SE2d::exp(x) * phi;
},
x0);
test_jacobian(
"inc test", Ja,
[&](const Eigen::Matrix<double, 3, 1> &x) {
return Sophus::SE2d::exp(x) * phi;
},
x0);
}
TEST(SophusUtilsCase, Sim2Test) {
......@@ -266,11 +275,12 @@ TEST(SophusUtilsCase, Sim2Test) {
Eigen::Matrix<double, 4, 1> x0;
x0.setZero();
test_jacobian("inc test", Ja,
[&](const Eigen::Matrix<double, 4, 1> &x) {
return Sophus::Sim2d::exp(x) * phi;
},
x0);
test_jacobian(
"inc test", Ja,
[&](const Eigen::Matrix<double, 4, 1> &x) {
return Sophus::Sim2d::exp(x) * phi;
},
x0);
}
TEST(SophusUtilsCase, RxSO2Test) {
......@@ -288,9 +298,10 @@ TEST(SophusUtilsCase, RxSO2Test) {
Eigen::Matrix<double, 2, 1> x0;
x0.setZero();
test_jacobian("inc test", Ja,
[&](const Eigen::Matrix<double, 2, 1> &x) {
return Sophus::RxSO2d::exp(x) * phi;
},
x0);
test_jacobian(
"inc test", Ja,
[&](const Eigen::Matrix<double, 2, 1> &x) {
return Sophus::RxSO2d::exp(x) * phi;
},
x0);
}
......@@ -62,14 +62,15 @@ void test_evaluate(const basalt::RdSpline<DIM, N> &spline, int64_t t_ns) {
Ja.diagonal().setConstant(J.d_val_d_knot[i - J.start_idx]);
}
test_jacobian(ss.str(), Ja,
[&](const VectorD &x) {
basalt::RdSpline<DIM, N> spline1 = spline;
spline1.getKnot(i) += x;
return spline1.template evaluate<DERIV>(t_ns);
},
x0);
test_jacobian(
ss.str(), Ja,
[&](const VectorD &x) {
basalt::RdSpline<DIM, N> spline1 = spline;
spline1.getKnot(i) += x;
return spline1.template evaluate<DERIV>(t_ns);
},
x0);
}
}
......@@ -82,12 +83,13 @@ void test_time_deriv(const basalt::RdSpline<DIM, N> &spline, int64_t t_ns) {
Eigen::Matrix<double, 1, 1> x0;
x0.setZero();
test_jacobian("d_val_d_t", d_val_d_t,
[&](const Eigen::Matrix<double, 1, 1> &x) {
int64_t inc = x[0] * 1e9;
return spline.template evaluate<DERIV>(t_ns + inc);
},
x0);
test_jacobian(
"d_val_d_t", d_val_d_t,
[&](const Eigen::Matrix<double, 1, 1> &x) {
int64_t inc = x[0] * 1e9;
return spline.template evaluate<DERIV>(t_ns + inc);
},
x0);
}
template <int N>
......@@ -115,16 +117,17 @@ void test_evaluate_so3(const basalt::So3Spline<N> &spline, int64_t t_ns) {
Ja = J.d_val_d_knot[i - J.start_idx];
}
test_jacobian(ss.str(), Ja,
[&](const VectorD &x) {
basalt::So3Spline<N> spline1 = spline;
spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
test_jacobian(
ss.str(), Ja,
[&](const VectorD &x) {
basalt::So3Spline<N> spline1 = spline;
spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
SO3 res1 = spline1.evaluate(t_ns);
SO3 res1 = spline1.evaluate(t_ns);
return (res1 * res.inverse()).log();
},
x0);
return (res1 * res.inverse()).log();
},
x0);
}
}
......@@ -145,12 +148,13 @@ void test_time_deriv_so3(const basalt::So3Spline<N> &spline, int64_t t_ns) {
Eigen::Matrix<double, 1, 1> x0;
x0.setZero();
test_jacobian("d_val_d_t", d_res_d_t,
[&](const Eigen::Matrix<double, 1, 1> &x) {
int64_t inc = x[0] * 1e9;
return (res.inverse() * spline.evaluate(t_ns + inc)).log();
},
x0);
test_jacobian(
"d_val_d_t", d_res_d_t,
[&](const Eigen::Matrix<double, 1, 1> &x) {
int64_t inc = x[0] * 1e9;
return (res.inverse() * spline.evaluate(t_ns + inc)).log();
},
x0);
}
template <int N>
......@@ -181,14 +185,15 @@ void test_evaluate_so3_vel(const basalt::So3Spline<N> &spline, int64_t t_ns) {
Ja = J.d_val_d_knot[i - J.start_idx];
}
test_jacobian(ss.str(), Ja,
[&](const VectorD &x) {
basalt::So3Spline<N> spline1 = spline;
spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
test_jacobian(
ss.str(), Ja,
[&](const VectorD &x) {
basalt::So3Spline<N> spline1 = spline;
spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
return spline1.velocityBody(t_ns);
},
x0);
return spline1.velocityBody(t_ns);
},
x0);
}
}
......
......@@ -66,14 +66,15 @@ void test_gyro_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
J.setZero();
}
test_jacobian(ss.str(), J,
[&](const Sophus::Vector3d &x) {
basalt::Se3Spline<N> s1 = s;
s1.getKnotSO3(i) = Sophus::SO3d::exp(x) * s.getKnotSO3(i);
return s1.gyroResidual(t_ns, measurement, bias);
},
x0);
test_jacobian(
ss.str(), J,
[&](const Sophus::Vector3d &x) {
basalt::Se3Spline<N> s1 = s;
s1.getKnotSO3(i) = Sophus::SO3d::exp(x) * s.getKnotSO3(i);
return s1.gyroResidual(t_ns, measurement, bias);
},
x0);
}
{
......@@ -83,13 +84,14 @@ void test_gyro_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
std::stringstream ss;
ss << "Spline order " << N << " d_gyro_res_d_bias";
test_jacobian(ss.str(), J_bias,
[&](const Eigen::Matrix<double, 12, 1> &x) {
auto b1 = bias;
b1 += x;
return s.gyroResidual(t_ns, measurement, b1);
},
x0);
test_jacobian(
ss.str(), J_bias,
[&](const Eigen::Matrix<double, 12, 1> &x) {
auto b1 = bias;
b1 += x;
return s.gyroResidual(t_ns, measurement, b1);
},
x0);
}
}
......@@ -124,14 +126,15 @@ void test_accel_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
J.setZero();
}
test_jacobian(ss.str(), J,
[&](const Sophus::Vector6d &x) {
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
test_jacobian(
ss.str(), J,
[&](const Sophus::Vector6d &x) {
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
return s1.accelResidual(t_ns, measurement, bias, g);
},
x0);
return s1.accelResidual(t_ns, measurement, bias, g);
},
x0);
}
{
......@@ -141,13 +144,14 @@ void test_accel_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
std::stringstream ss;
ss << "Spline order " << N << " d_accel_res_d_bias";
test_jacobian(ss.str(), J_bias,
[&](const Eigen::Matrix<double, 9, 1> &x) {
auto b1 = bias;
b1 += x;
return s.accelResidual(t_ns, measurement, b1, g);
},
x0);
test_jacobian(
ss.str(), J_bias,
[&](const Eigen::Matrix<double, 9, 1> &x) {
auto b1 = bias;
b1 += x;
return s.accelResidual(t_ns, measurement, b1, g);
},
x0);
}
{
......@@ -157,11 +161,12 @@ void test_accel_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
std::stringstream ss;
ss << "Spline order " << N << " d_accel_res_d_g";
test_jacobian(ss.str(), J_g,
[&](const Sophus::Vector3d &x) {
return s.accelResidual(t_ns, measurement, bias, g + x);
},
x0);
test_jacobian(
ss.str(), J_g,
[&](const Sophus::Vector3d &x) {
return s.accelResidual(t_ns, measurement, bias, g + x);
},
x0);
}
}
......@@ -189,18 +194,19 @@ void test_orientation_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
Sophus::Vector3d x0;
x0.setZero();
test_jacobian(ss.str(), J,
[&](const Sophus::Vector3d &x_rot) {
Sophus::Vector6d x;
x.setZero();
x.tail<3>() = x_rot;
test_jacobian(
ss.str(), J,
[&](const Sophus::Vector3d &x_rot) {
Sophus::Vector6d x;
x.setZero();
x.tail<3>() = x_rot;
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
return s1.orientationResidual(t_ns, measurement);
},
x0);
return s1.orientationResidual(t_ns, measurement);
},
x0);
}
}
......@@ -228,18 +234,19 @@ void test_position_res(const basalt::Se3Spline<N> &s, int64_t t_ns) {
Sophus::Vector3d x0;
x0.setZero();
test_jacobian(ss.str(), J,
[&](const Sophus::Vector3d &x_rot) {
Sophus::Vector6d x;
x.setZero();
x.head<3>() = x_rot;
test_jacobian(
ss.str(), J,
[&](const Sophus::Vector3d &x_rot) {
Sophus::Vector6d x;
x.setZero();
x.head<3>() = x_rot;
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
return s1.positionResidual(t_ns, measurement);
},
x0);
return s1.positionResidual(t_ns, measurement);
},
x0);
}
}
......@@ -264,14 +271,15 @@ void test_pose(const basalt::Se3Spline<N> &s, int64_t t_ns) {
J.setZero();
}
test_jacobian(ss.str(), J,
[&](const Sophus::Vector6d &x) {
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
test_jacobian(
ss.str(), J,
[&](const Sophus::Vector6d &x) {
basalt::Se3Spline<N> s1 = s;
s1.applyInc(i, x);
return Sophus::logd(res.inverse() * s1.pose(t_ns));
},
x0);
return Sophus::logd(res.inverse() * s1.pose(t_ns));
},
x0);
}
{
......@@ -285,14 +293,15 @@ void test_pose(const basalt::Se3Spline<N> &s, int64_t t_ns) {
s.d_pose_d_t(t_ns, J);
test_jacobian("J_pose_time", J,
[&](const Eigen::Matrix<double, 1, 1> &x) {
int64_t t_ns_new = t_ns;
t_ns_new += x[0] * 1e9;
test_jacobian(
"J_pose_time", J,
[&](const Eigen::Matrix<double, 1, 1> &x) {
int64_t t_ns_new = t_ns;
t_ns_new += x[0] * 1e9;
return Sophus::logd(res.inverse() * s.pose(t_ns_new));
},
x0);
return Sophus::logd(res.inverse() * s.pose(t_ns_new));
},
x0);
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment