Fix curly bracket formatting to follow project coding style (#9)

* Initial plan

* Fix curly bracket formatting in all C++ files

Co-authored-by: MatthijsBurgh <18014833+MatthijsBurgh@users.noreply.github.com>

* Clean up build artifacts and update .gitignore

Co-authored-by: MatthijsBurgh <18014833+MatthijsBurgh@users.noreply.github.com>

---------

Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com>
Co-authored-by: MatthijsBurgh <18014833+MatthijsBurgh@users.noreply.github.com>

Author: Copilot

.gitignore

@@ -40,8 +40,9 @@
 # debug information files
 *.dwo
 
-# build directory
+# build directories
 build/
+devel/
 
 # vscode settings
 .vscode/

include/bmm/bayesian_mixture_model.hpp

@@ -7,14 +7,16 @@
 #include <ros/console.h>
 #include <geolib/datatypes.h>
 
-struct GMMParams {
+struct GMMParams
+{
     float alpha;
     float kappa0;
     float psi0;
     float nu0;
 };
 
-class MAPGMM {
+class MAPGMM
+{
 public:
     MAPGMM(int n_components = 2,
            const std::vector<geo::Vec3>& points = std::vector<geo::Vec3>(), const GMMParams& params = GMMParams());

src/bayesian_mixture_model.cpp

@@ -2,7 +2,8 @@
 
 #include <console_bridge/console.h>
 
-MAPGMM::MAPGMM(int n_components, const std::vector<geo::Vec3>& points, const GMMParams & params) : K_(n_components), inlier_component_(0), params_(params) {
+MAPGMM::MAPGMM(int n_components, const std::vector<geo::Vec3>& points, const GMMParams & params) : K_(n_components), inlier_component_(0), params_(params)
+{
     // Initialize all containers to proper sizes
     means_.resize(K_);
     covs_.resize(K_);
@@ -16,7 +17,8 @@ MAPGMM::MAPGMM(int n_components, const std::vector<geo::Vec3>& points, const GMM
     nu0_.resize(K_);
 
     // Initialize with identity matrices to avoid uninitialized memory
-    for (int k = 0; k < K_; k++) {
+    for (int k = 0; k < K_; k++)
+    {
         means_[k] = Eigen::Vector3d::Zero();
         covs_[k] = Eigen::Matrix3d::Identity();
         mu0_[k] = Eigen::Vector3d::Zero();
@@ -27,13 +29,15 @@ MAPGMM::MAPGMM(int n_components, const std::vector<geo::Vec3>& points, const GMM
     setupPriors(points);
 }
 
-void MAPGMM::fit(const std::vector<geo::Vec3>& points, const geo::Pose3D& sensor_pose) {
+void MAPGMM::fit(const std::vector<geo::Vec3>& points, const geo::Pose3D& sensor_pose)
+{
     // Convert points to Eigen matrix
     int N = points.size();
     Eigen::MatrixXd data(N, 3);
 
     // Transform points to sensor frame
-    for (int i = 0; i < N; i++) {
+    for (int i = 0; i < N; i++)
+    {
         geo::Vec3 p_map = sensor_pose * points[i];
         data(i, 0) = p_map.x;
         data(i, 1) = p_map.y;
@@ -49,7 +53,8 @@ void MAPGMM::fit(const std::vector<geo::Vec3>& points, const geo::Pose3D& sensor
     covs_.resize(K_);
 
     // Simple initialization: random points as centers for means and small covariances
-    for (int k = 0; k < K_; k++) {
+    for (int k = 0; k < K_; k++)
+    {
         int idx = rand() % N;
         means_[k] = data.row(idx).transpose();
         covs_[k] = Eigen::Matrix3d::Identity() * 0.01;  // Covariance here is not the prior but the initial guess of the cluster.
@@ -66,13 +71,15 @@ void MAPGMM::fit(const std::vector<geo::Vec3>& points, const geo::Pose3D& sensor
 
     resp_ = Eigen::MatrixXd::Zero(N, K_);
 
-    for (int iter = 0; iter < max_iter; iter++) {
+    for (int iter = 0; iter < max_iter; iter++)
+    {
         // E-step: Calculate responsibilities
         double log_likelihood = eStep(data, resp_);
 
         // Check convergence
         double change = std::abs((log_likelihood - prev_log_likelihood) / (std::abs(prev_log_likelihood) + 1e-10));
-        if (iter > 0 && change < likelihood_change) {
+        if (iter > 0 && change < likelihood_change)
+        {
             CONSOLE_BRIDGE_logInform("MAP-GMM converged after %d iterations", iter);
             break;
         }
@@ -84,7 +91,8 @@ void MAPGMM::fit(const std::vector<geo::Vec3>& points, const geo::Pose3D& sensor
 
     // Assign labels based on highest responsibility
     labels_.resize(N);
-    for (int i = 0; i < N; i++) {
+    for (int i = 0; i < N; i++)
+    {
         Eigen::VectorXd r = resp_.row(i);
         int max_idx = 0;
         r.maxCoeff(&max_idx);
@@ -95,31 +103,37 @@ void MAPGMM::fit(const std::vector<geo::Vec3>& points, const geo::Pose3D& sensor
     determineInlierComponent();
 }
 
-std::vector<int> MAPGMM::get_labels() const {
+std::vector<int> MAPGMM::get_labels() const
+{
     return labels_;
 }
-int MAPGMM::get_inlier_component() const {
+int MAPGMM::get_inlier_component() const
+{
     return inlier_component_;
 }
 
 
 
-void MAPGMM::setupPriors(const std::vector<geo::Vec3>& points) {
+void MAPGMM::setupPriors(const std::vector<geo::Vec3>& points)
+{
     if (mu0_.size() != static_cast<size_t>(K_) ||
         kappa0_.size() != static_cast<size_t>(K_) ||
         Psi0_.size() != static_cast<size_t>(K_) ||
-        nu0_.size() != static_cast<size_t>(K_)) {
+        nu0_.size() != static_cast<size_t>(K_))
+    {
         CONSOLE_BRIDGE_logError("Prior vectors not properly initialized in MAPGMM constructor");
         return;
     }
     // Dirichlet prior for weights (alpha>1 favors more uniform weights)
     alpha_ = params_.alpha;
     Eigen::Vector3d means = Eigen::Vector3d::Zero();
-    for (const auto& p : points) {
+    for (const auto& p : points)
+    {
         means += Eigen::Vector3d(p.x, p.y, p.z);
     }
     means /= points.size();
-    for (int k = 0; k < K_; k++) {
+    for (int k = 0; k < K_; k++)
+    {
         // Same prior for all components initially
         mu0_[k] = means;
 
@@ -132,35 +146,42 @@ void MAPGMM::setupPriors(const std::vector<geo::Vec3>& points) {
     }
 }
 
-void MAPGMM::computeBoundingVolume(const Eigen::MatrixXd& data) {
+void MAPGMM::computeBoundingVolume(const Eigen::MatrixXd& data)
+{
     Eigen::Vector3d min_vals = data.colwise().minCoeff();
     Eigen::Vector3d max_vals = data.colwise().maxCoeff();
     volume_ = (max_vals - min_vals).prod();  // volume = (xmax - xmin) * (ymax - ymin) * (zmax - zmin)
-    if (volume_ <= 1e-12) {
+    if (volume_ <= 1e-12)
+    {
         volume_ = 1e-6;  // avoid division by zero in uniform component
         CONSOLE_BRIDGE_logWarn("MAP-GMM: bounding volume too small, clamping to %g", volume_);
     }
 }
 
-double MAPGMM::eStep(const Eigen::MatrixXd& data, Eigen::MatrixXd& resp_) {
+double MAPGMM::eStep(const Eigen::MatrixXd& data, Eigen::MatrixXd& resp_)
+{
     // Number of data points
     int N = data.rows();
     double log_likelihood = 0.0;
 
     // Calculate log probabilities for each data point and component (number of components is K_ which is uniform outlier + actual clusters)
     Eigen::MatrixXd log_probs(N, K_);
 
-    for (int k = 0; k < K_; k++) {
+    for (int k = 0; k < K_; k++)
+    {
         // Compute log probability of each point under component k
-        for (int i = 0; i < N; i++) {
+        for (int i = 0; i < N; i++)
+        {
             Eigen::Vector3d x = data.row(i).transpose();
-            if (k == 0) {
+            if (k == 0)
+            {
                 // For the outlier component, use a uniform distribution over the bounding volume
                 double uniform_prob = 1.0 / volume_;
                 log_probs(i, k) = std::log(weights_[k]) + std::log(uniform_prob);
                 continue;
             }
-            else{
+            else
+            {
                 Eigen::Vector3d diff = x - means_[k];
 
                 // Calculate log probability of x under Gaussian component k (Math is here)
@@ -173,7 +194,8 @@ double MAPGMM::eStep(const Eigen::MatrixXd& data, Eigen::MatrixXd& resp_) {
                 // Regularize covariance for numerical stability
                 Eigen::Matrix3d Sigma = covs_[k] + Eigen::Matrix3d::Identity() * 1e-9;
                 double det = Sigma.determinant();
-                if (det <= 1e-18 || !std::isfinite(det)) {
+                if (det <= 1e-18 || !std::isfinite(det))
+                {
                     Sigma += Eigen::Matrix3d::Identity() * 1e-6;
                     det = Sigma.determinant();
                 }
@@ -193,7 +215,8 @@ double MAPGMM::eStep(const Eigen::MatrixXd& data, Eigen::MatrixXd& resp_) {
     }
 
     // Calculate responsibilities (and normalize)
-    for (int i = 0; i < N; i++) {
+    for (int i = 0; i < N; i++)
+    {
         // Numerical stability: subtract max value
         double max_log_prob = log_probs.row(i).maxCoeff();
         Eigen::VectorXd exp_probs = (log_probs.row(i).array() - max_log_prob).exp();
@@ -209,11 +232,13 @@ double MAPGMM::eStep(const Eigen::MatrixXd& data, Eigen::MatrixXd& resp_) {
     return log_likelihood;
 }
 
-void MAPGMM::mStep(const Eigen::MatrixXd& data, const Eigen::MatrixXd& resp_) {
+void MAPGMM::mStep(const Eigen::MatrixXd& data, const Eigen::MatrixXd& resp_)
+{
     int N = data.rows();
 
     // For each component
-    for (int k = 0; k < K_; k++) {
+    for (int k = 0; k < K_; k++)
+    {
         // Component responsibility sum
         double Nk = resp_.col(k).sum();
 
@@ -224,7 +249,8 @@ void MAPGMM::mStep(const Eigen::MatrixXd& data, const Eigen::MatrixXd& resp_) {
 
         // Calculate mean of data
         Eigen::Vector3d mean_data = Eigen::Vector3d::Zero();
-        for (int i = 0; i < N; i++) {
+        for (int i = 0; i < N; i++)
+        {
             mean_data += resp_(i, k) * data.row(i).transpose();
         }
         mean_data /= Nk;
@@ -234,7 +260,8 @@ void MAPGMM::mStep(const Eigen::MatrixXd& data, const Eigen::MatrixXd& resp_) {
 
         // Calculate weighted covariance of data
         Eigen::Matrix3d cov_data = Eigen::Matrix3d::Zero();
-        for (int i = 0; i < N; i++) {
+        for (int i = 0; i < N; i++)
+        {
             Eigen::Vector3d diff = data.row(i).transpose() - mean_data;
             cov_data += resp_(i, k) * diff * diff.transpose();
         }
@@ -256,22 +283,27 @@ void MAPGMM::mStep(const Eigen::MatrixXd& data, const Eigen::MatrixXd& resp_) {
     weights_ /= weights_.sum();
 }
 
-void MAPGMM::determineInlierComponent() {
+void MAPGMM::determineInlierComponent()
+{
      // Compute N_k: total responsibility mass for each component
     std::vector<double> Nk(K_, 0.0);
     int N = labels_.size();  // or you can use resp.rows()
 
-    for (int i = 0; i < N; i++) {
-        for (int k = 0; k < K_; k++) {
+    for (int i = 0; i < N; i++)
+    {
+        for (int k = 0; k < K_; k++)
+        {
             Nk[k] += resp_(i, k);  // You'll need to save `resp_` as a class member
         }
     }
 
     // Find the component with max Nk
     // Skip the outlier uniform distribution component (k=0)
     inlier_component_ = 1;
-    for (int k = 1; k < K_; k++) {
-        if (Nk[k] > Nk[inlier_component_]) {
+    for (int k = 1; k < K_; k++)
+    {
+        if (Nk[k] > Nk[inlier_component_])
+        {
             inlier_component_ = k;
         }
     }

src/main.cpp

@@ -13,7 +13,8 @@
 static void populateSynthetic(std::vector<geo::Vec3>& cluster,
                               std::vector<int>& gt_labels,  // 1 = cluster, 0 = noise
                               int n_cluster = 10000,
-                              int n_noise = 1500) {
+                              int n_noise = 1500)
+{
   cluster.clear();
   gt_labels.clear();
 
@@ -34,7 +35,8 @@ static void populateSynthetic(std::vector<geo::Vec3>& cluster,
   gt_labels.reserve(static_cast<size_t>(n_cluster + n_noise));
 
   // Cluster points: Gaussian ball
-  for (int i = 0; i < n_cluster; ++i) {
+  for (int i = 0; i < n_cluster; ++i)
+  {
     geo::Vec3 v; v.x = ndx(rng); v.y = ndy(rng); v.z = ndz(rng);
     cluster.push_back(v);
     gt_labels.push_back(1);
@@ -44,15 +46,17 @@ static void populateSynthetic(std::vector<geo::Vec3>& cluster,
   int added = 0;
   int tries = 0;
   const int max_tries = n_noise * 50;
-  while (added < n_noise && tries++ < max_tries) {
+  while (added < n_noise && tries++ < max_tries)
+  {
     geo::Vec3 v; v.x = cx + ud(rng); v.y = cy + ud(rng); v.z = cz + ud(rng);
     double dx = v.x - cx, dy = v.y - cy, dz = v.z - cz;
     if (dx*dx + dy*dy + dz*dz <= Re2) continue;  // reject inside sphere
     cluster.push_back(v);
     gt_labels.push_back(0);
     ++added;
   }
-  if (added < n_noise) {
+  if (added < n_noise)
+  {
     CONSOLE_BRIDGE_logWarn("populateSynthetic: produced %d/%d noise points (tight box/exclusion)", added, n_noise);
   }
 
@@ -63,15 +67,17 @@ static void populateSynthetic(std::vector<geo::Vec3>& cluster,
 
   std::vector<geo::Vec3> pts_shuf; pts_shuf.reserve(cluster.size());
   std::vector<int>      lab_shuf; lab_shuf.reserve(gt_labels.size());
-  for (size_t i = 0; i < idx.size(); ++i) {
+  for (size_t i = 0; i < idx.size(); ++i)
+  {
     pts_shuf.push_back(cluster[idx[i]]);
     lab_shuf.push_back(gt_labels[idx[i]]);
   }
   cluster.swap(pts_shuf);
   gt_labels.swap(lab_shuf);
 }
 
-int main() {
+int main()
+{
   // Synthetic input
   GMMParams params; // defaults from header
   params.alpha = 1.0;   // Dirichlet prior (1.0 = uniform)
@@ -97,7 +103,8 @@ int main() {
   if (gt_labels.size() != N) gt_labels.resize(N, 0);
 
   int TP=0, TN=0, FP=0, FN=0;
-  for (size_t i = 0; i < N; ++i) {
+  for (size_t i = 0; i < N; ++i)
+  {
     bool pred_in = (labels[i] == inlier_component);
     bool gt_in   = (gt_labels[i] == 1);
     if (pred_in && gt_in) ++TP;
@@ -117,7 +124,8 @@ int main() {
   // Visualization: all points colored by prediction (green=inlier, red=outlier)
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
   cloud->reserve(N);
-  for (size_t i = 0; i < N; ++i) {
+  for (size_t i = 0; i < N; ++i)
+  {
     pcl::PointXYZRGB p;
     p.x = cluster[i].x; p.y = cluster[i].y; p.z = cluster[i].z;
     bool pred_in = (labels[i] == inlier_component);
@@ -127,7 +135,8 @@ int main() {
     cloud->push_back(p);
   }
   cloud->width = cloud->size(); cloud->height = 1; cloud->is_dense = false;
-  if (pcl::io::savePCDFileBinary("clusters_pred.pcd", *cloud) == 0) {
+  if (pcl::io::savePCDFileBinary("clusters_pred.pcd", *cloud) == 0)
+  {
     CONSOLE_BRIDGE_logInform("Wrote clusters_pred.pcd (green=inlier, red=outlier). View with: pcl_viewer clusters_pred.pcd");
   } else {
     CONSOLE_BRIDGE_logWarn("Failed to save clusters_pred.pcd");
@@ -137,7 +146,8 @@ int main() {
   {
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr inliers(new pcl::PointCloud<pcl::PointXYZRGB>());
     inliers->reserve(N);
-    for (size_t i = 0; i < N; ++i) {
+    for (size_t i = 0; i < N; ++i)
+    {
       if (labels[i] != inlier_component) continue;
       pcl::PointXYZRGB p; p.x = cluster[i].x; p.y = cluster[i].y; p.z = cluster[i].z;
       p.r = p.g = p.b = 255;