add ./scripts/clang-format-all.sh and fix formatting

.clang-format

@@ -2,5 +2,6 @@
 Language:       Cpp
 BasedOnStyle:   Google
 IndentWidth:    2
+IncludeBlocks:  Preserve
 ...
 

include/basalt/calibration/vignette.h

@@ -84,4 +84,4 @@ class VignetteEstimator {
   std::vector<double> irradiance;
   std::vector<basalt::RdSpline<1, SPLINE_N>> vign_param;
 };
-}
+}  // namespace basalt

include/basalt/io/dataset_io.h

@@ -179,8 +179,9 @@ template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
 inline
     typename std::enable_if<_Rows == Eigen::Dynamic || _Cols == Eigen::Dynamic,
                             void>::type
-    load(Archive &ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows,
+    load(Archive &ar,
-                                    _MaxCols> &matrix) {
+         Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
+             &matrix) {
   std::int32_t rows;
   std::int32_t cols;
   ar(rows);

include/basalt/optimization/spline_linearize.h

@@ -135,9 +135,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
       // std::cout << "time " << time << std::endl;
       // std::cout << "sline.minTime() " << spline.minTime() << std::endl;
 
-      BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "time " << time_ns << " spline.minTimeNs() "
+          time_ns >= spline->minTimeNs(),
-                                   << spline->minTimeNs());
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
 
       // Residual from current value of spline
       Vector3 residual_pos =
@@ -207,12 +207,12 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
       //      std::cout << "time " << t << std::endl;
       //      std::cout << "sline.minTime() " << spline.minTime() << std::endl;
 
-      BASALT_ASSERT_STREAM(t >= spline->minTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "t " << t << " spline.minTime() "
+          t >= spline->minTimeNs(),
-                                << spline->minTimeNs());
+          "t " << t << " spline.minTime() " << spline->minTimeNs());
-      BASALT_ASSERT_STREAM(t <= spline->maxTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "t " << t << " spline.maxTime() "
+          t <= spline->maxTimeNs(),
-                                << spline->maxTimeNs());
+          "t " << t << " spline.maxTime() " << spline->maxTimeNs());
 
       Vector3 residual = spline->accelResidual(
           t, pm.data, this->common_data.calibration->calib_accel_bias,
@@ -255,7 +255,8 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
           BASALT_ASSERT(start_j < opt_size);
 
           accum.template addH<POSE_SIZE, POSE_SIZE>(
-              start_i, start_j, accel_var_inv * J.d_val_d_knot[i].transpose() *
+              start_i, start_j,
+              accel_var_inv * J.d_val_d_knot[i].transpose() *
                   J.d_val_d_knot[j]);
         }
         accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
@@ -497,9 +498,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
       if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
         continue;
 
-      BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "time " << time_ns << " spline.minTimeNs() "
+          time_ns >= spline->minTimeNs(),
-                                   << spline->minTimeNs());
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
 
       const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w;
       const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
@@ -563,9 +564,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
                          d_res_d_T_w_i.transpose() * residual);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
-            start_T_moc_w, start_i, mocap_var_inv *
+            start_T_moc_w, start_i,
-                                        d_res_d_T_moc_w.transpose() *
+            mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_w_i *
-                                        d_res_d_T_w_i * J_pose.d_val_d_knot[i]);
+                J_pose.d_val_d_knot[i]);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
             start_T_i_mark, start_i,

include/basalt/utils/common_types.h

@@ -238,7 +238,7 @@ using ImageProjections = std::map<TimeCamId, ImageProjection>;
 /// inlier projections indexed per track
 using TrackProjections =
     std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>;
-}
+}  // namespace basalt
 
 namespace cereal {
 
@@ -251,4 +251,4 @@ template <class Archive>
 void serialize(Archive& ar, basalt::MatchData& c) {
   ar(c.T_i_j, c.matches, c.inliers);
 }
-}
+}  // namespace cereal

include/basalt/utils/nfr.h

@@ -117,4 +117,4 @@ inline Sophus::Vector2d rollPitchError(
 
   return res.head<2>();
 }
-}
+}  // namespace basalt

include/basalt/utils/sim_utils.h

@@ -43,13 +43,11 @@ struct SimObservations {
   std::vector<int> id;
 };
 
-}
+}  // namespace basalt
 
 namespace cereal {
 template <class Archive>
 void serialize(Archive& ar, basalt::SimObservations& c) {
   ar(c.pos, c.id);
 }
-}
+}  // namespace cereal
-
-

include/basalt/utils/tracks.h

@@ -47,11 +47,8 @@
 #include <basalt/utils/common_types.h>
 #include <basalt/utils/union_find.h>
 
-
 namespace basalt {
 
-
-
 /// TrackBuild class creates feature tracks from matches
 struct TrackBuilder {
   std::map<ImageFeaturePair, TrackId> map_node_to_index;
@@ -222,4 +219,4 @@ bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks,
   return !track_ids.empty();
 }
 
-}
+}  // namespace basalt

scripts/clang-format-all.sh

@@ -0,0 +1,43 @@
+#!/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/../src $SCRIPT_DIR/../test/src}"
+
+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

src/calibrate.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <basalt/calibration/cam_calib.h>
 
 #include <tbb/tbb.h>

src/calibration/vignette.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <basalt/calibration/vignette.h>
 
 namespace basalt {

src/io/dataset_io.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <basalt/io/dataset_io.h>
 #include <basalt/io/dataset_io_euroc.h>
 #include <basalt/io/dataset_io_rosbag.h>

src/mapper_sim.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <algorithm>
 #include <chrono>
 #include <iostream>

src/mapper_sim_naive.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <algorithm>
 #include <chrono>
 #include <iostream>
@@ -492,8 +491,8 @@ void load_data(const std::string& calib_path) {
   if (os.is_open()) {
     cereal::JSONInputArchive archive(os);
     archive(calib);
-    std::cout << "Loaded camera with " << calib.intrinsics.size()
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
-              << " cameras" << std::endl;
+              << std::endl;
 
   } else {
     std::cerr << "could not load camera calibration " << calib_path

src/opt_flow.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <algorithm>
 #include <chrono>
 #include <iostream>
@@ -321,8 +320,8 @@ void load_data(const std::string& calib_path) {
   if (os.is_open()) {
     cereal::JSONInputArchive archive(os);
     archive(calib);
-    std::cout << "Loaded camera with " << calib.intrinsics.size()
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
-              << " cameras" << std::endl;
+              << std::endl;
 
   } else {
     std::cerr << "could not load camera calibration " << calib_path

src/optical_flow/optical_flow.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <basalt/optical_flow/optical_flow.h>
 
 #include <basalt/optical_flow/frame_to_frame_optical_flow.h>

src/vi_estimator/ba_base.cpp

@@ -33,8 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
-
 #include <basalt/vi_estimator/ba_base.h>
 
 #include <tbb/parallel_for.h>
@@ -354,7 +352,6 @@ void BundleAdjustmentBase::linearizeHelper(
             }
           }
         }
-
       });
 
   for (const auto& rld : rld_vec) error += rld.error;
@@ -522,4 +519,4 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
   abs_H.resize(0, 0);
   abs_b.resize(0);
 }
-}
+}  // namespace basalt

src/vio_sim.cpp

@@ -33,7 +33,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #include <algorithm>
 #include <chrono>
 #include <iostream>
@@ -541,8 +540,8 @@ void load_data(const std::string& calib_path) {
   if (os.is_open()) {
     cereal::JSONInputArchive archive(os);
     archive(calib);
-    std::cout << "Loaded camera with " << calib.intrinsics.size()
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
-              << " cameras" << std::endl;
+              << std::endl;
 
   } else {
     std::cerr << "could not load camera calibration " << calib_path
@@ -595,8 +594,8 @@ void compute_projections() {
 void gen_data() {
   for (size_t i = 0; i < calib.intrinsics.size(); i++) {
     images.emplace_back();
-    images.back() = pangolin::TypedImage(
+    images.back() =
-        calib.resolution[i][0], calib.resolution[i][1],
+        pangolin::TypedImage(calib.resolution[i][0], calib.resolution[i][1],
                              pangolin::PixelFormatFromString("GRAY8"));
 
     images.back().Fill(200);

test/src/test_image.cpp

@@ -27,7 +27,8 @@ TEST(Pattern, ImageInterp) {
 
   // std::cerr << "vg\n" << vg << std::endl;
 
-  test_jacobian("d_val_d_p", J,
+  test_jacobian(
+      "d_val_d_p", J,
       [&](const Eigen::Vector2d &x) {
         Eigen::Matrix<double, 1, 1> res;
         Eigen::Vector2d p1 = offset + x;

test/src/test_nfr.cpp

@@ -34,13 +34,13 @@ TEST(PreIntegrationTestSuite, RelPoseTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
 
           return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j);
-
         },
         x0);
   }
@@ -48,13 +48,13 @@ TEST(PreIntegrationTestSuite, RelPoseTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_j", d_res_d_T_w_j,
+    test_jacobian(
+        "d_res_d_T_w_j", d_res_d_T_w_j,
         [&](const Sophus::Vector6d& x) {
           auto T_w_j_new = T_w_j;
           basalt::PoseState::incPose(x, T_w_j_new);
 
           return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new);
-
         },
         x0);
   }
@@ -71,13 +71,13 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
 
           return basalt::absPositionError(T_w_i_new, pos);
-
         },
         x0);
   }
@@ -97,7 +97,8 @@ TEST(PreIntegrationTestSuite, YawTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
@@ -105,7 +106,6 @@ TEST(PreIntegrationTestSuite, YawTest) {
           double res = basalt::yawError(T_w_i_new, yaw_dir_body);
 
           return Eigen::Matrix<double, 1, 1>(res);
-
         },
         x0);
   }
@@ -124,13 +124,13 @@ TEST(PreIntegrationTestSuite, RollPitchTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
 
           return basalt::rollPitchError(T_w_i_new, R_w_i);
-
         },
         x0);
   }

test/src/test_vio.cpp

@@ -372,14 +372,14 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_xi", d_res_d_xi,
+    test_jacobian(
+        "d_res_d_xi", d_res_d_xi,
         [&](const Sophus::Vector6d& x) {
-                    Eigen::Matrix4d T_t_h_new =
+          Eigen::Matrix4d T_t_h_new = (Sophus::expd(x) * T_t_h_sophus).matrix();
-                        (Sophus::expd(x) * T_t_h_sophus).matrix();
 
           Eigen::Vector2d res;
-                    basalt::KeypointVioEstimator::linearizePoint(
+          basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos,
-                        kpt_obs, kpt_pos, T_t_h_new, cam, res);
+                                                       T_t_h_new, cam, res);
 
           return res;
         },
@@ -389,17 +389,17 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) {
   {
     Eigen::Vector3d x0;
     x0.setZero();
-    test_jacobian("d_res_d_p", d_res_d_p,
+    test_jacobian(
+        "d_res_d_p", d_res_d_p,
         [&](const Eigen::Vector3d& x) {
-                    basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new =
+          basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos;
-                        kpt_pos;
 
           kpt_pos_new.dir += x.head<2>();
           kpt_pos_new.id += x[2];
 
           Eigen::Vector2d res;
-                    basalt::KeypointVioEstimator::linearizePoint(
+          basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos_new,
-                        kpt_obs, kpt_pos_new, T_t_h, cam, res);
+                                                       T_t_h, cam, res);
 
           return res;
         },