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

.clang-format

@@ -1,6 +1,7 @@
 ---
-Language:        Cpp
+Language:       Cpp
-BasedOnStyle:  Google
+BasedOnStyle:   Google
-IndentWidth:     2
+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,8 +255,9 @@ 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,
-                                    J.d_val_d_knot[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>(
             start_bias, start_i,
@@ -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/keypoints.h

@@ -53,7 +53,7 @@ namespace basalt {
 typedef std::bitset<256> Descriptor;
 
 void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
-                      KeypointsData& kd, int num_features);
+                            KeypointsData& kd, int num_features);
 
 void detectKeypoints(const basalt::Image<const uint16_t>& img_raw,
                      KeypointsData& kd, int PATCH_SIZE = 32,

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.cpp

@@ -323,7 +323,7 @@ int main(int argc, char** argv) {
       std::this_thread::sleep_for(std::chrono::milliseconds(50));
     }
   } else {
-    //optimize();
+    // optimize();
     detect();
     match();
     tracks();

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>
@@ -120,7 +119,7 @@ double alignButton();
 void setup_points();
 
 constexpr int UI_WIDTH = 200;
-//constexpr int NUM_FRAMES = 500;
+// constexpr int NUM_FRAMES = 500;
 
 basalt::Calibration<double> calib;
 

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/utils/keypoints.cpp

@@ -128,7 +128,7 @@ const static char pattern_31_y_b[256] = {
     -11};
 
 void detectKeypointsMapping(const basalt::Image<const uint16_t>& img_raw,
-                      KeypointsData& kd, int num_features) {
+                            KeypointsData& kd, int num_features) {
   cv::Mat image(img_raw.h, img_raw.w, CV_8U);
 
   uint8_t* dst = image.ptr();

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,9 +594,9 @@ 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"));
+                             pangolin::PixelFormatFromString("GRAY8"));
 
     images.back().Fill(200);
   }

test/src/test_image.cpp

@@ -27,14 +27,15 @@ TEST(Pattern, ImageInterp) {
 
   // std::cerr << "vg\n" << vg << std::endl;
 
-  test_jacobian("d_val_d_p", J,
+  test_jacobian(
-                [&](const Eigen::Vector2d &x) {
+      "d_val_d_p", J,
-                  Eigen::Matrix<double, 1, 1> res;
+      [&](const Eigen::Vector2d &x) {
-                  Eigen::Vector2d p1 = offset + x;
+        Eigen::Matrix<double, 1, 1> res;
-                  res[0] = img.interpGrad(p1)[0];
+        Eigen::Vector2d p1 = offset + x;
-                  return res;
+        res[0] = img.interpGrad(p1)[0];
-                },
+        return res;
-                Eigen::Vector2d::Zero(), 1.0);
+      },
+      Eigen::Vector2d::Zero(), 1.0);
 }
 
 TEST(Image, ImageInterpolate) {

test/src/test_nfr.cpp

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

test/src/test_vio.cpp

@@ -372,37 +372,37 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_xi", d_res_d_xi,
+    test_jacobian(
-                  [&](const Sophus::Vector6d& x) {
+        "d_res_d_xi", d_res_d_xi,
-                    Eigen::Matrix4d T_t_h_new =
+        [&](const Sophus::Vector6d& x) {
-                        (Sophus::expd(x) * T_t_h_sophus).matrix();
+          Eigen::Matrix4d T_t_h_new = (Sophus::expd(x) * T_t_h_sophus).matrix();
 
-                    Eigen::Vector2d res;
+          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;
+          return res;
-                  },
+        },
-                  x0);
+        x0);
   }
 
   {
     Eigen::Vector3d x0;
     x0.setZero();
-    test_jacobian("d_res_d_p", d_res_d_p,
+    test_jacobian(
-                  [&](const Eigen::Vector3d& x) {
+        "d_res_d_p", d_res_d_p,
-                    basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new =
+        [&](const Eigen::Vector3d& x) {
-                        kpt_pos;
+          basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos;
 
-                    kpt_pos_new.dir += x.head<2>();
+          kpt_pos_new.dir += x.head<2>();
-                    kpt_pos_new.id += x[2];
+          kpt_pos_new.id += x[2];
 
-                    Eigen::Vector2d res;
+          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;
+          return res;
-                  },
+        },
-                  x0);
+        x0);
   }
 }