Cosmetic fixes (#13)

* Removed redundant comments * Made general interface for base metrics * Small interface fixes in tests * Replaced ulong with unsigned long * Add optional predef into metrics_utils.h * Added reduce header * [styleguide] Replaced unsigned long with int * [styleguide] All functions now have CamelCase names Co-authored-by: Arthur_Ch <56088401+ArthurChains@users.noreply.github.com>
MobileRoboticsSkoltech · Dec 7, 2021 · a9eb3c5 · a9eb3c5
1 parent 0645b52
commit a9eb3c5
Show file tree

Hide file tree

Showing 10 changed files with 213 additions and 180 deletions.
diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(SOURCES metrics.cpp metrics.h)
+set(SOURCES metrics.cpp metrics.h metrics_utils.cpp metrics_utils.h)
 
 add_library(map_metrics SHARED ${SOURCES})
 

diff --git a/cpp/metrics.cpp b/cpp/metrics.cpp
@@ -1,122 +1,56 @@
 //
-// Created by achains on 18.11.2021.
+// Created by achains on 07.12.2021.
 //
 
+#include "metrics_utils.h"
 #include "metrics.h"
 
-#define _USE_MATH_DEFINES
-#include <cmath>
-
-#include <algorithm>
 #include <numeric>
 
-#include "Eigen/Core"
-#include "Eigen/Dense"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-#ifndef M_E
-#define M_E 2.718281828459045235360
-#endif
-
-namespace map_metrics{
-
-    Eigen::MatrixX3d cov(Eigen::MatrixX3d const & M){
-        Eigen::MatrixX3d centered = M.rowwise() - M.colwise().mean();
-        return (centered.adjoint() * centered) / (static_cast<double>(M.rows()) - 1.0);
-    }
-
-    Eigen::MatrixX3d points_idx_to_matrix(cilantro::VectorSet3d const & points, std::vector<unsigned long> const & idx){
-        Eigen::MatrixX3d matrix(idx.size(), 3);
-        Eigen::Index row_idx = 0;
-        for (const auto & i : idx){
-            matrix.row(row_idx++) = points.col(static_cast<Eigen::Index>(i));
-        }
-        return matrix;
-    }
-
-    PointCloud aggregate_map(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts){
-        assert (pcs.size() == ts.size());
-
-        std::vector<Eigen::Matrix4d> inv_ts;
-        Eigen::Matrix4d inv_elem = ts[0].inverse();
-        for (Eigen::Matrix4d const & mx : ts){
-            inv_ts.emplace_back(mx * inv_elem);
-        }
-
-        cilantro::VectorSet3d pc_map_points(3, 0);
-        for (size_t i = 0; i < pcs.size(); ++i){
-            cilantro::RigidTransform3d transform_mx(inv_ts[i]);
-            cilantro::VectorSet3d transformed_points = pcs[i].transformed(transform_mx).points;
-
-            Eigen::Index old_size = pc_map_points.cols();
-            pc_map_points.conservativeResize(3, old_size + transformed_points.cols());
-            for (Eigen::Index col_idx = 0; col_idx < transformed_points.cols(); ++col_idx){
-                pc_map_points.col(old_size + col_idx) = transformed_points.col(col_idx);
-            }
-        }
-
-        return PointCloud{pc_map_points};
-    }
-
-    std::vector<unsigned long> get_radius_search_indices(KDTree const & tree,
-                                                       Eigen::Vector3d const & query, double radius){
-        cilantro::NeighborSet<double> nn = tree.radiusSearch(query, radius);
-        std::vector<unsigned long> indices(nn.size());
-        for (size_t i = 0; i < nn.size(); ++i){
-            indices[i] = nn[i].index;
+namespace metrics {
+    double ComputeBaseMetrics(
+            std::vector<cilantro::VectorSet3d> const &pcs_points,
+            std::vector<Eigen::Matrix4d> const &ts,
+            int min_knn,
+            double knn_radius,
+            std::optional<double> (*algorithm)
+                    (cilantro::VectorSet3d const &points,
+                     std::vector<int> const &indices)) {
+
+        std::vector<cilantro::PointCloud3d> pcs(pcs_points.size());
+        for (size_t i = 0; i < pcs_points.size(); ++i){
+            pcs[i] = cilantro::PointCloud3d(pcs_points[i]);
         }
 
-        return indices;
-    }
-
-    double mpv(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts){
-        const int min_knn = 5;
-        const double knn_rad = 1.0;
+        metrics_utils::PointCloud pc_map = metrics_utils::AggregateMap(pcs, ts);
 
-        PointCloud pc_map = aggregate_map(pcs, ts);
-
-        KDTree map_tree(pc_map.points);
+        metrics_utils::KDTree map_tree(pc_map.points);
         cilantro::VectorSet3d points = pc_map.points;
 
         std::vector<double> metric;
-        for (Eigen::Index i = 0; i < points.cols(); ++i){
-            std::vector<unsigned long> indices = get_radius_search_indices(map_tree,
-                                                                        points.col(i), knn_rad);
-            if (indices.size() > min_knn){
-                Eigen::MatrixX3d tmp = points_idx_to_matrix(points, indices);
-                Eigen::MatrixX3d cov_matrix = cov(tmp);
-                Eigen::VectorXd eigenvalues = cov_matrix.eigenvalues().real();
-                metric.push_back(eigenvalues.minCoeff());
+        for (Eigen::Index i = 0; i < points.cols(); ++i) {
+            std::vector<int> indices = metrics_utils::GetRadiusSearchIndices(map_tree,
+                                                                           points.col(i), knn_radius);
+            if (indices.size() > min_knn) {
+                std::optional<double> result = algorithm(points, indices);
+                if (result.has_value())
+                    metric.push_back(result.value());
             }
         }
-        return (metric.empty() ? 0 : std::reduce(metric.begin(), metric.end()) / static_cast<double>(metric.size()));
-    }
 
-    double mme(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts){
-        const int min_knn = 5;
-        const double knn_rad = 1.0;
-        cilantro::
-        PointCloud pc_map = aggregate_map(pcs, ts);
-
-        KDTree map_tree(pc_map.points);
-        cilantro::VectorSet3d points = pc_map.points;
+        return (metric.empty() ? 0 :
+        std::reduce(metric.begin(), metric.end()) / static_cast<double>(metric.size()));
+    }
 
-        std::vector<double> metric;
-        for (Eigen::Index i = 0; i < points.cols(); ++i){
-            std::vector<unsigned long> indices = get_radius_search_indices(map_tree,
-                                                                           points.col(i), knn_rad);
-            if (indices.size() > min_knn){
-                Eigen::MatrixXd tmp = points_idx_to_matrix(points, indices);
-                Eigen::MatrixXd cov_matrix = cov(tmp);
-                double det =  (2. * M_PI * M_E * cov_matrix).determinant();
-                if (det > 0)
-                    metric.push_back(0.5 * std::log(det));
-            }
-        }
-        return (metric.empty() ? 0 : std::reduce(metric.begin(), metric.end()) / static_cast<double>(metric.size()));
+    double GetMPV(std::vector<cilantro::VectorSet3d> const & pcs_points, std::vector<Eigen::Matrix4d> const & ts,
+               int min_knn, double knn_rad){
+        return ComputeBaseMetrics(pcs_points, ts, min_knn, knn_rad,
+                                   &metrics_utils::metrics_algorithm::ComputeEigenvalues);
     }
 
-}
+    double GetMME(std::vector<cilantro::VectorSet3d> const & pcs_points, std::vector<Eigen::Matrix4d> const & ts,
+               int min_knn, double knn_rad){
+        return ComputeBaseMetrics(pcs_points, ts, min_knn, knn_rad,
+                                   &metrics_utils::metrics_algorithm::ComputeEntropy);
+    }
+} // namespace metrics
diff --git a/cpp/metrics.h b/cpp/metrics.h
@@ -1,35 +1,34 @@
 //
-// Created by achains on 18.11.2021.
+// Created by achains on 07.12.2021.
 //
 
 #ifndef MAP_METRICS_METRICS_H
 #define MAP_METRICS_METRICS_H
 
-#include <cilantro/core/kd_tree.hpp>
-#include <cilantro/utilities/point_cloud.hpp>
-
-namespace map_metrics {
-
-    using PointCloud = cilantro::PointCloud3d;
-    using KDTree = cilantro::KDTree3d<>;
-
-    Eigen::MatrixX3d cov(Eigen::MatrixX3d const & M);
-
-    // Points are represented like
-    //    x1   ...   xn
-    //  ( y1 ) ... ( yn )
-    //    z1   ...   zn
-    Eigen::MatrixX3d points_idx_to_matrix(cilantro::VectorSet3d const & points, std::vector<unsigned long> const & idx);
-
-    PointCloud aggregate_map(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts);
-
-    std::vector<unsigned long> get_radius_search_indices(KDTree const & tree,
-                                                       Eigen::Vector3d const & query, double radius);
-
-    double mpv(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts);
-
-    double mme(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts);
-
-}
+#include "metrics_utils.h"
+
+namespace metrics{
+
+    // Available Base metrics are:
+    // MPV -- Mean Plane Variance
+    // MME -- Mean Map Entropy
+    double ComputeBaseMetrics(
+            std::vector<cilantro::VectorSet3d> const & pcs_points,
+            std::vector<Eigen::Matrix4d> const & ts,
+            int min_knn = 5,
+            double knn_radius = 1.0,
+            std::optional<double> (*algorithm)
+            (cilantro::VectorSet3d const & points,
+             std::vector<int> const & indices) = metrics_utils::metrics_algorithm::ComputeEigenvalues
+            );
+
+    // MPV. Mean Plane Variance
+    double GetMPV(std::vector<cilantro::VectorSet3d> const & pcs_points, std::vector<Eigen::Matrix4d> const & ts,
+               int min_knn = 5, double knn_rad = 1.0);
+
+    // MME. Mean Map Entropy
+    double GetMME(std::vector<cilantro::VectorSet3d> const & pcs_points, std::vector<Eigen::Matrix4d> const & ts,
+               int min_knn = 5, double knn_rad = 1.0);
+} // namespace metrics
 
 #endif //MAP_METRICS_METRICS_H
diff --git a/cpp/metrics_utils.cpp b/cpp/metrics_utils.cpp
@@ -0,0 +1,91 @@
+//
+// Created by achains on 18.11.2021.
+//
+
+#include "metrics_utils.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "Eigen/Core"
+#include "Eigen/Dense"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#ifndef M_E
+#define M_E 2.718281828459045235360
+#endif
+
+namespace metrics_utils{
+
+    Eigen::MatrixX3d FindCov(Eigen::MatrixX3d const & M){
+        Eigen::MatrixX3d centered = M.rowwise() - M.colwise().mean();
+        return (centered.adjoint() * centered) / (static_cast<double>(M.rows()) - 1.0);
+    }
+
+    Eigen::MatrixX3d TransformPointIdxToMatrix(cilantro::VectorSet3d const & points, std::vector<int> const & idx){
+        Eigen::MatrixX3d matrix(idx.size(), 3);
+        Eigen::Index row_idx = 0;
+        for (const auto & i : idx){
+            matrix.row(row_idx++) = points.col(static_cast<Eigen::Index>(i));
+        }
+        return matrix;
+    }
+
+    PointCloud AggregateMap(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts){
+        assert (pcs.size() == ts.size());
+
+        std::vector<Eigen::Matrix4d> inv_ts;
+        Eigen::Matrix4d inv_elem = ts[0].inverse();
+        for (Eigen::Matrix4d const & mx : ts){
+            inv_ts.emplace_back(mx * inv_elem);
+        }
+
+        cilantro::VectorSet3d pc_map_points(3, 0);
+        for (size_t i = 0; i < pcs.size(); ++i){
+            cilantro::RigidTransform3d transform_mx(inv_ts[i]);
+            cilantro::VectorSet3d transformed_points = pcs[i].transformed(transform_mx).points;
+
+            Eigen::Index old_size = pc_map_points.cols();
+            pc_map_points.conservativeResize(3, old_size + transformed_points.cols());
+            for (Eigen::Index col_idx = 0; col_idx < transformed_points.cols(); ++col_idx){
+                pc_map_points.col(old_size + col_idx) = transformed_points.col(col_idx);
+            }
+        }
+
+        return PointCloud{pc_map_points};
+    }
+
+    std::vector<int> GetRadiusSearchIndices(KDTree const & tree,
+                                                       Eigen::Vector3d const & query, double radius){
+        cilantro::NeighborSet<double> nn = tree.radiusSearch(query, radius);
+        std::vector<int> indices(nn.size());
+        for (size_t i = 0; i < nn.size(); ++i){
+            indices[i] = static_cast<int>(nn[i].index);
+        }
+
+        return indices;
+    }
+
+    namespace metrics_algorithm{
+        std::optional<double> ComputeEigenvalues(cilantro::VectorSet3d const & points,
+                                   std::vector<int> const & indices){
+            Eigen::MatrixX3d tmp = TransformPointIdxToMatrix(points, indices);
+            Eigen::MatrixX3d cov_matrix = FindCov(tmp);
+            Eigen::VectorXd eigenvalues = cov_matrix.eigenvalues().real();
+            return eigenvalues.minCoeff();
+        }
+
+        std::optional<double> ComputeEntropy(cilantro::VectorSet3d const & points,
+                                              std::vector<int> const & indices){
+            Eigen::MatrixXd tmp = TransformPointIdxToMatrix(points, indices);
+            Eigen::MatrixXd cov_matrix = FindCov(tmp);
+            double det =  (2. * M_PI * M_E * cov_matrix).determinant();
+            if (det > 0)
+                return 0.5 * std::log(det);
+            return {};
+        }
+    } // namespace metrics_algorithm
+} // namespace metrics_utils
diff --git a/cpp/metrics_utils.h b/cpp/metrics_utils.h
@@ -0,0 +1,40 @@
+//
+// Created by achains on 18.11.2021.
+//
+
+#ifndef MAP_METRICS_METRICS_UTILS_H
+#define MAP_METRICS_METRICS_UTILS_H
+
+#include <cilantro/core/kd_tree.hpp>
+#include <cilantro/utilities/point_cloud.hpp>
+
+#include <optional>
+
+namespace metrics_utils {
+
+    using PointCloud = cilantro::PointCloud3d;
+    using KDTree = cilantro::KDTree3d<>;
+
+    Eigen::MatrixX3d FindCov(Eigen::MatrixX3d const & M);
+
+    // Points are represented like
+    //    x1   ...   xn
+    //  ( y1 ) ... ( yn )
+    //    z1   ...   zn
+    Eigen::MatrixX3d TransformPointIdxToMatrix(cilantro::VectorSet3d const & points, std::vector<int> const & idx);
+
+    PointCloud AggregateMap(std::vector<PointCloud> const & pcs, std::vector<Eigen::Matrix4d> const & ts);
+
+    std::vector<int> GetRadiusSearchIndices(KDTree const & tree,
+                                                       Eigen::Vector3d const & query, double radius);
+
+    namespace metrics_algorithm{
+        std::optional<double> ComputeEigenvalues(cilantro::VectorSet3d const & points,
+                                                  std::vector<int> const & indices);
+
+        std::optional<double> ComputeEntropy(cilantro::VectorSet3d const & points,
+                                              std::vector<int> const & indices);
+    } // namespace metrics_algorithm
+} // namespace metrics_utils
+
+#endif //MAP_METRICS_METRICS_UTILS_H
diff --git a/cpp/pybind/metrics.cpp b/cpp/pybind/metrics.cpp
@@ -11,32 +11,15 @@
 #include <metrics.h>
 
 // Python: arg0: List[numpy.ndarray[numpy.float64[3, n]]], arg1: List[numpy.ndarray[numpy.float64[4, 4]]]) -> float
-double py_mpv(std::vector<cilantro::VectorSet3d> const & pcs_points, std::vector<Eigen::Matrix4d> const & ts){
-    std::vector<cilantro::PointCloud3d> pcs(pcs_points.size());
-    for (size_t i = 0; i < pcs_points.size(); ++i){
-        pcs[i] = cilantro::PointCloud3d(pcs_points[i]);
-    }
-
-    return map_metrics::mpv(pcs, ts);
-}
-
-double py_mme(std::vector<cilantro::VectorSet3d> const & pcs_points, std::vector<Eigen::Matrix4d> const & ts){
-    std::vector<cilantro::PointCloud3d> pcs(pcs_points.size());
-    for (size_t i = 0; i < pcs_points.size(); ++i){
-        pcs[i] = cilantro::PointCloud3d(pcs_points[i]);
-    }
-
-    return map_metrics::mme(pcs, ts);
-}
 
 namespace py = pybind11;
 
 PYBIND11_MODULE(map_metrics_py, m){
     m.doc() = "Baseline of MPV and MME metrics";
     m.def("mpv", py::overload_cast<
           const std::vector<cilantro::VectorSet3d> &,
-          const std::vector<Eigen::Matrix4d> &>(&py_mpv));
+          const std::vector<Eigen::Matrix4d> &, int, double>(&metrics::GetMPV));
     m.def("mme", py::overload_cast<
           const std::vector<cilantro::VectorSet3d> &,
-          const std::vector<Eigen::Matrix4d> &>(&py_mme));
+          const std::vector<Eigen::Matrix4d> &, int, double>(&metrics::GetMME));
 }