From 478dcdae5a260a1f00447ea80cea3934a5dce3e3 Mon Sep 17 00:00:00 2001
From: mwlasiuk <michalwlasiuk@wp.pl>
Date: Sun, 1 Mar 2026 09:55:25 +0100
Subject: [PATCH 1/5] Work

---
 apps/precision_forestry_tools/CMakeLists.txt    |  5 ++---
 .../local_shape_features.cpp                    |  7 ++++---
 .../local_shape_features.h                      |  0
 .../precision_forestry_tools.cpp                |  5 +++--
 .../precision_forestry_tools}/surface.cpp       |  4 +---
 .../precision_forestry_tools}/surface.h         | 17 ++---------------
 core/CMakeLists.txt                             |  1 +
 core/src/point_cloud.cpp                        |  1 +
 core_hd_mapping/src/odo_with_gnss_fusion.cpp    |  3 +--
 9 files changed, 15 insertions(+), 28 deletions(-)
 rename {core/src => apps/precision_forestry_tools}/local_shape_features.cpp (99%)
 rename {core/include => apps/precision_forestry_tools}/local_shape_features.h (100%)
 rename {core/src => apps/precision_forestry_tools}/surface.cpp (99%)
 rename {core/include => apps/precision_forestry_tools}/surface.h (86%)

diff --git a/apps/precision_forestry_tools/CMakeLists.txt b/apps/precision_forestry_tools/CMakeLists.txt
index a2ede28f..afc596e2 100644
--- a/apps/precision_forestry_tools/CMakeLists.txt
+++ b/apps/precision_forestry_tools/CMakeLists.txt
@@ -4,9 +4,8 @@ project(precision_forestry_tools)
 
 add_executable(precision_forestry_tools 
   precision_forestry_tools.cpp 
-  ${REPOSITORY_DIRECTORY}/core/src/local_shape_features.cpp
-  ${REPOSITORY_DIRECTORY}/core/src/surface.cpp
-  ${REPOSITORY_DIRECTORY}/core/src/hash_utils.cpp)
+  surface.cpp
+  local_shape_features.cpp)
 
 target_compile_definitions(precision_forestry_tools PRIVATE -DWITH_GUI=1)
 
diff --git a/core/src/local_shape_features.cpp b/apps/precision_forestry_tools/local_shape_features.cpp
similarity index 99%
rename from core/src/local_shape_features.cpp
rename to apps/precision_forestry_tools/local_shape_features.cpp
index b325a6c2..516d6c22 100644
--- a/core/src/local_shape_features.cpp
+++ b/apps/precision_forestry_tools/local_shape_features.cpp
@@ -1,7 +1,8 @@
-#include <pch/pch.h>
-
 #include <hash_utils.h>
-#include <local_shape_features.h>
+
+#include "local_shape_features.h"
+
+#include <execution>
 
 bool LocalShapeFeatures::calculate_local_shape_features(std::vector<PointWithLocalShapeFeatures>& points, const Params& params)
 {
diff --git a/core/include/local_shape_features.h b/apps/precision_forestry_tools/local_shape_features.h
similarity index 100%
rename from core/include/local_shape_features.h
rename to apps/precision_forestry_tools/local_shape_features.h
diff --git a/apps/precision_forestry_tools/precision_forestry_tools.cpp b/apps/precision_forestry_tools/precision_forestry_tools.cpp
index b4f80450..28ff302b 100644
--- a/apps/precision_forestry_tools/precision_forestry_tools.cpp
+++ b/apps/precision_forestry_tools/precision_forestry_tools.cpp
@@ -18,15 +18,16 @@
 #include "pfd_wrapper.hpp"
 
 #include <HDMapping/Version.hpp>
-#include <local_shape_features.h>
 #include <session.h>
-#include <surface.h>
 #include <transformations.h>
 
 #include <filesystem>
 #include <fstream>
 #include <iostream>
 
+#include "local_shape_features.h"
+#include "surface.h"
+
 const uint32_t window_width = 800;
 const uint32_t window_height = 600;
 double camera_ortho_xy_view_zoom = 10;
diff --git a/core/src/surface.cpp b/apps/precision_forestry_tools/surface.cpp
similarity index 99%
rename from core/src/surface.cpp
rename to apps/precision_forestry_tools/surface.cpp
index 3a12ee02..30549ead 100644
--- a/core/src/surface.cpp
+++ b/apps/precision_forestry_tools/surface.cpp
@@ -1,6 +1,4 @@
-#include <pch/pch.h>
-
-#include <surface.h>
+#include "surface.h"
 
 #include <GL/freeglut.h>
 
diff --git a/core/include/surface.h b/apps/precision_forestry_tools/surface.h
similarity index 86%
rename from core/include/surface.h
rename to apps/precision_forestry_tools/surface.h
index 1a5065cd..e5399992 100644
--- a/core/include/surface.h
+++ b/apps/precision_forestry_tools/surface.h
@@ -8,21 +8,8 @@
 class Surface
 {
 public:
-    // struct TripletIndexes
-    //{
-    //     int index_before;
-    //     int index_curr;
-    //     int index_after;
-    // };
-
-    Surface()
-    {
-        ;
-    };
-    ~Surface()
-    {
-        ;
-    };
+    Surface() = default;
+    ~Surface() = default;
 
     std::vector<Eigen::Affine3d> vertices;
     std::vector<Eigen::Affine3d> vertices_odo;
diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
index 19969006..d6109a6e 100644
--- a/core/CMakeLists.txt
+++ b/core/CMakeLists.txt
@@ -23,6 +23,7 @@ set(CORE_BASE_SOURCES
     src/pose_graph_slam.cpp
     src/registration_plane_feature.cpp
     src/session.cpp
+    # src/utils.cpp # TODO(mwlasiuk) : broken AF ...
     ${EXTERNAL_LIBRARIES_DIRECTORY}/src/wgs84_do_puwg92.cc
     ${EXTERNAL_LIBRARIES_DIRECTORY}/src/plycpp.cpp
 )
diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp
index da5be209..351a2161 100644
--- a/core/src/point_cloud.cpp
+++ b/core/src/point_cloud.cpp
@@ -317,6 +317,7 @@ bool PointCloud::build_rgd()
 
     std::vector<std::thread> threads;
 
+    // TODO(mwlasiuk) : uint64 -> uint32 conversion warning
     for (size_t i = 0; i < jobs.size(); i++)
     {
         threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets));
diff --git a/core_hd_mapping/src/odo_with_gnss_fusion.cpp b/core_hd_mapping/src/odo_with_gnss_fusion.cpp
index 127c7c4b..260a279d 100644
--- a/core_hd_mapping/src/odo_with_gnss_fusion.cpp
+++ b/core_hd_mapping/src/odo_with_gnss_fusion.cpp
@@ -22,8 +22,7 @@ void OdoWithGnssFusion::imgui(CommonData& common_data)
 
     if (ImGui::Button("load GNSS trajectory"))
     {
-        std::string input_file_name = "";
-        input_file_name = mandeye::fd::OpenFileDialogOneFile("Load GNSS trajectory", {});
+        std::string input_file_name = mandeye::fd::OpenFileDialogOneFile("Load GNSS trajectory", {});
 
         if (input_file_name.size() > 0)
         {

From ca13a06e6db2a1df5575983b59d71639302ead88 Mon Sep 17 00:00:00 2001
From: mwlasiuk <michalwlasiuk@wp.pl>
Date: Sun, 1 Mar 2026 10:54:34 +0100
Subject: [PATCH 2/5] Restruecture coire libraries and add todos to fix...

---
 .../mandeye_compare_trajectories.cpp          |  2 +-
 .../concatenate_multi_livox.cpp               |  2 +-
 apps/console_tools/matrix_mul.cpp             |  5 +--
 ...ply_timestamps_session_point_cloud_laz.cpp |  9 +++---
 ...iply_timestamps_session_trajectory_csv.cpp |  6 ++--
 ..._data_and_drop_here-precision_forestry.cpp | 11 +++----
 apps/lidar_odometry_step_1/lidar_odometry.cpp |  3 +-
 apps/lidar_odometry_step_1/lidar_odometry.h   |  5 +--
 .../lidar_odometry_gui.cpp                    | 11 +++----
 .../lidar_odometry_utils.cpp                  |  3 +-
 .../lidar_odometry_utils.h                    |  7 +++--
 .../lidar_odometry_utils_optimizers.cpp       |  4 +--
 .../livox_mid_360_intrinsic_calibration.cpp   | 14 +++------
 .../mandeye_mission_recorder_calibration.cpp  | 12 +++----
 .../mandeye_raw_data_viewer.cpp               | 15 ++++-----
 .../mandeye_single_session_viewer.cpp         | 11 +++----
 apps/manual_color/manual_color.cpp            | 19 ++++++------
 .../multi_session_factor_graph.cpp            |  4 +--
 .../multi_session_factor_graph.h              |  2 +-
 .../multi_session_registration.cpp            | 24 ++++++--------
 .../multi_view_tls_registration.cpp           |  2 +-
 .../multi_view_tls_registration.h             | 15 ++++-----
 .../multi_view_tls_registration_gui.cpp       | 31 +++++++++----------
 .../perform_experiment.cpp                    | 27 +++++++---------
 .../local_shape_features.cpp                  |  2 +-
 .../precision_forestry_tools.cpp              |  8 ++---
 apps/precision_forestry_tools/surface.cpp     |  5 +--
 apps/precision_forestry_tools/surface.h       |  2 +-
 .../single_session_manual_coloring.cpp        | 20 ++++++------
 apps/split_multi_livox/split_multi_livox.cpp  |  7 +++--
 core/include/{ => Core}/GL_assert.h           |  0
 core/include/{ => Core}/color_las_loader.h    |  0
 core/include/{ => Core}/control_points.h      |  3 +-
 core/include/{ => Core}/exportPcd.h           |  0
 core/include/{ => Core}/export_laz.h          |  3 +-
 core/include/{ => Core}/fmt_filesystem.hpp    |  0
 core/include/{ => Core}/gnss.h                |  2 +-
 .../{ => Core}/ground_control_points.h        |  2 +-
 core/include/{ => Core}/hash_utils.h          |  0
 core/include/{ => Core}/icp.h                 |  3 +-
 core/include/{ => Core}/m_estimators.h        |  0
 .../manual_pose_graph_loop_closure.h          | 10 +++---
 core/include/{ => Core}/ndt.h                 |  5 +--
 core/include/{ => Core}/nmea.h                |  0
 core/include/{ => Core}/observation_picking.h |  5 +--
 .../pair_wise_iterative_closest_point.h       |  0
 core/include/{ => Core}/pfd_wrapper.hpp       |  0
 core/include/{ => Core}/point_cloud.h         |  6 ++--
 core/include/{ => Core}/point_clouds.h        |  7 +++--
 .../{ => Core}/pose_graph_loop_closure.h      |  8 ++---
 core/include/{ => Core}/pose_graph_slam.h     |  4 +--
 .../{ => Core}/registration_plane_feature.h   |  2 +-
 core/include/{ => Core}/session.h             | 11 ++++---
 core/include/{ => Core}/structures.h          |  0
 core/include/{ => Core}/system_info.hpp       |  0
 core/include/{ => Core}/transformations.h     |  0
 core/include/{ => Core}/utils.hpp             | 10 +++---
 core/src/color_las_loader.cpp                 |  2 +-
 core/src/control_points.cpp                   |  2 +-
 core/src/gnss.cpp                             |  5 +--
 core/src/ground_control_points.cpp            |  4 ++-
 core/src/hash_utils.cpp                       |  2 +-
 core/src/icp.cpp                              |  7 ++---
 core/src/manual_pose_graph_loop_closure.cpp   | 22 ++++++-------
 core/src/ndt.cpp                              | 10 +++---
 core/src/nmea.cpp                             |  2 +-
 core/src/observation_picking.cpp              |  4 +--
 ...zation_point_to_point_source_to_target.cpp | 17 +++++-----
 ...stance_point_to_plane_source_to_target.cpp | 16 +++++-----
 ...timize_plane_to_plane_source_to_target.cpp | 19 +++++-------
 ...timize_point_to_plane_source_to_target.cpp | 16 +++++-----
 ...projection_onto_plane_source_to_target.cpp | 22 +++++--------
 .../src/pair_wise_iterative_closest_point.cpp |  9 +++---
 core/src/pfd_wrapper.cpp                      |  3 +-
 core/src/point_cloud.cpp                      |  4 +--
 core/src/point_clouds.cpp                     |  6 ++--
 core/src/pose_graph_loop_closure.cpp          | 15 ++++-----
 core/src/pose_graph_slam.cpp                  | 14 ++++-----
 core/src/registration_plane_feature.cpp       |  9 +++---
 core/src/session.cpp                          |  6 ++--
 core/src/utils.cpp                            |  2 +-
 .../include/{ => CoreHDMapping}/laz_wrapper.h |  6 ++--
 .../odo_with_gnss_fusion.h                    |  3 +-
 .../{ => CoreHDMapping}/project_settings.h    |  6 ++--
 .../{ => CoreHDMapping}/roi_exporter.h        |  6 ++--
 .../single_trajectory_viewer.h                |  5 +--
 core_hd_mapping/src/laz_wrapper.cpp           |  4 +--
 core_hd_mapping/src/odo_with_gnss_fusion.cpp  |  9 +++---
 core_hd_mapping/src/project_settings.cpp      |  8 ++---
 core_hd_mapping/src/roi_exporter.cpp          | 10 +++---
 .../src/single_trajectory_viewer.cpp          |  8 ++---
 91 files changed, 318 insertions(+), 334 deletions(-)
 rename core/include/{ => Core}/GL_assert.h (100%)
 rename core/include/{ => Core}/color_las_loader.h (100%)
 rename core/include/{ => Core}/control_points.h (97%)
 rename core/include/{ => Core}/exportPcd.h (100%)
 rename core/include/{ => Core}/export_laz.h (99%)
 rename core/include/{ => Core}/fmt_filesystem.hpp (100%)
 rename core/include/{ => Core}/gnss.h (99%)
 rename core/include/{ => Core}/ground_control_points.h (97%)
 rename core/include/{ => Core}/hash_utils.h (100%)
 rename core/include/{ => Core}/icp.h (99%)
 rename core/include/{ => Core}/m_estimators.h (100%)
 rename core/include/{ => Core}/manual_pose_graph_loop_closure.h (87%)
 rename core/include/{ => Core}/ndt.h (99%)
 rename core/include/{ => Core}/nmea.h (100%)
 rename core/include/{ => Core}/observation_picking.h (98%)
 rename core/include/{ => Core}/pair_wise_iterative_closest_point.h (100%)
 rename core/include/{ => Core}/pfd_wrapper.hpp (100%)
 rename core/include/{ => Core}/point_cloud.h (98%)
 rename core/include/{ => Core}/point_clouds.h (97%)
 rename core/include/{ => Core}/pose_graph_loop_closure.h (94%)
 rename core/include/{ => Core}/pose_graph_slam.h (98%)
 rename core/include/{ => Core}/registration_plane_feature.h (99%)
 rename core/include/{ => Core}/session.h (85%)
 rename core/include/{ => Core}/structures.h (100%)
 rename core/include/{ => Core}/system_info.hpp (100%)
 rename core/include/{ => Core}/transformations.h (100%)
 rename core/include/{ => Core}/utils.hpp (97%)
 rename core_hd_mapping/include/{ => CoreHDMapping}/laz_wrapper.h (85%)
 rename core_hd_mapping/include/{ => CoreHDMapping}/odo_with_gnss_fusion.h (95%)
 rename core_hd_mapping/include/{ => CoreHDMapping}/project_settings.h (93%)
 rename core_hd_mapping/include/{ => CoreHDMapping}/roi_exporter.h (88%)
 rename core_hd_mapping/include/{ => CoreHDMapping}/single_trajectory_viewer.h (92%)

diff --git a/apps/compare_trajectories/mandeye_compare_trajectories.cpp b/apps/compare_trajectories/mandeye_compare_trajectories.cpp
index 83d43314..4f9c6fa6 100644
--- a/apps/compare_trajectories/mandeye_compare_trajectories.cpp
+++ b/apps/compare_trajectories/mandeye_compare_trajectories.cpp
@@ -15,7 +15,7 @@
 
 #include <Eigen/Eigen>
 
-#include <transformations.h>
+#include <Core/transformations.h>
 
 #include <portable-file-dialogs.h>
 
diff --git a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp
index 2e094887..7022a5ef 100644
--- a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp
+++ b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp
@@ -1,5 +1,5 @@
 #include "../lidar_odometry_step_1/lidar_odometry_utils.h"
-#include "export_laz.h"
+#include <Core/export_laz.h>
 #include <filesystem>
 #include <iostream>
 
diff --git a/apps/console_tools/matrix_mul.cpp b/apps/console_tools/matrix_mul.cpp
index 02c7109c..9f453ed9 100644
--- a/apps/console_tools/matrix_mul.cpp
+++ b/apps/console_tools/matrix_mul.cpp
@@ -1,8 +1,9 @@
 #include <Eigen/Eigen>
 #include <fstream>
 #include <iostream>
-#include <structures.h>
-#include <transformations.h>
+
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
 int main(int argc, char* argv[])
 {
diff --git a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp
index e37de472..73217f0d 100644
--- a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp
+++ b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp
@@ -1,15 +1,14 @@
 #include <Eigen/Eigen>
 #include <fstream>
 #include <iostream>
-#include <structures.h>
-#include <transformations.h>
 
-#include <point_clouds.h>
+#include <Core/export_laz.h>
+#include <Core/point_clouds.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
 #include <laszip/laszip_api.h>
 
-#include <export_laz.h>
-
 #include <spdlog/spdlog.h>
 
 bool load_pc(PointCloud& pc, const std::string& input_file_name)
diff --git a/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp b/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp
index e5b938d6..e734ea48 100644
--- a/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp
+++ b/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp
@@ -1,10 +1,10 @@
 #include <Eigen/Eigen>
 #include <fstream>
 #include <iostream>
-#include <structures.h>
-#include <transformations.h>
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
 inline void split(std::string& str, char delim, std::vector<std::string>& out)
 {
diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
index cde46174..0e2608bb 100644
--- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
+++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp
@@ -12,19 +12,18 @@
 
 #include "lidar_odometry.h"
 #include "lidar_odometry_utils.h"
-#include <export_laz.h>
-#include <registration_plane_feature.h>
 
-#include <utils.hpp>
+#include <Core/export_laz.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/utils.hpp>
 
 #include "toml_io.h"
 #include <HDMapping/Version.hpp>
 #include <chrono>
 #include <ctime>
-#include <export_laz.h>
 #include <mutex>
-#include <pfd_wrapper.hpp>
-#include <session.h>
 #include <spdlog/cfg/env.h>
 #include <spdlog/spdlog.h>
 
diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp
index e5b65745..acc02811 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp
@@ -2,7 +2,8 @@
 #include "tbb/tbb.h"
 #include <UTL/profiler.hpp>
 #include <mutex>
-#include <system_info.hpp>
+
+#include <Core/system_info.hpp>
 
 namespace fs = std::filesystem;
 
diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h
index c93944a5..e080ecb8 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry.h
+++ b/apps/lidar_odometry_step_1/lidar_odometry.h
@@ -6,10 +6,11 @@
 #include "lidar_odometry_utils.h"
 #include "toml_io.h"
 #include <HDMapping/Version.hpp>
-#include <export_laz.h>
 #include <laszip/laszip_api.h>
 #include <nlohmann/json.hpp>
-#include <session.h>
+
+#include <Core/export_laz.h>
+#include <Core/session.h>
 
 // #define SAMPLE_PERIOD (1.0 / 200.0)
 
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
index 5ffb9a98..363735ab 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
@@ -12,19 +12,18 @@
 
 #include "lidar_odometry.h"
 #include "lidar_odometry_utils.h"
-#include <export_laz.h>
-#include <registration_plane_feature.h>
 
-#include <utils.hpp>
+#include <Core/export_laz.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/utils.hpp>
 
 #include "toml_io.h"
 #include <HDMapping/Version.hpp>
 #include <chrono>
 #include <ctime>
-#include <export_laz.h>
 #include <mutex>
-#include <pfd_wrapper.hpp>
-#include <session.h>
 #include <spdlog/cfg/env.h>
 #include <spdlog/spdlog.h>
 
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
index 44512477..b95cc2ba 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
@@ -2,11 +2,12 @@
 #include "csv.hpp"
 #include <algorithm>
 #include <filesystem>
-#include <hash_utils.h>
 #include <regex>
 #include <tbb/parallel_for_each.h>
 #include <tbb/parallel_invoke.h>
 
+#include <Core/hash_utils.h>
+
 #include <spdlog/spdlog.h>
 
 namespace fs = std::filesystem;
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h
index 8491497d..34a3ae72 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h
+++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h
@@ -14,10 +14,11 @@
 #include <Fusion.h>
 #include <common/include/cauchy.h>
 #include <laszip/laszip_api.h>
-#include <ndt.h>
 #include <nlohmann/json.hpp>
-#include <structures.h>
-#include <transformations.h>
+
+#include <Core/ndt.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/constraint_fixed_parameter_jacobian.h>
 #include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
index 5d33b71e..bf76e85a 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
@@ -1,13 +1,13 @@
 #include "lidar_odometry_utils.h"
 #include <UTL/profiler.hpp>
-#include <hash_utils.h>
 #include <spdlog/spdlog.h>
 #include <spdlog/stopwatch.h>
 #include <tbb/combinable.h>
 #include <tbb/parallel_for.h>
 #include <thread>
 
-#include <export_laz.h>
+#include <Core/export_laz.h>
+#include <Core/hash_utils.h>
 
 const double DEG_TO_RAD = M_PI / 180.0f;
 const double RAD_TO_DEG = 180.0f / M_PI;
diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
index 2a9f677d..eb86db29 100644
--- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
+++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
@@ -15,20 +15,16 @@
 
 #include <Eigen/Eigen>
 
-#include <transformations.h>
-
-#include "pfd_wrapper.hpp"
+#include <Core/export_laz.h>
+#include <Core/hash_utils.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
 
 #include "../lidar_odometry_step_1/lidar_odometry_utils.h"
 
 #include <HDMapping/Version.hpp>
 
-#include <pair_wise_iterative_closest_point.h>
-
-#include <export_laz.h>
-
-#include <hash_utils.h>
-
 #include <filesystem>
 #include <mutex>
 
diff --git a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp
index c3880bb9..8d5869c7 100644
--- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp
+++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp
@@ -13,14 +13,9 @@
 #include <glm/gtc/matrix_transform.hpp>
 
 #include <spdlog/spdlog.h>
-#include <utils.hpp>
 
 #include <Eigen/Eigen>
 
-#include <transformations.h>
-
-#include "pfd_wrapper.hpp"
-
 #include "../lidar_odometry_step_1/lidar_odometry_utils.h"
 #include <filesystem>
 
@@ -28,7 +23,10 @@
 
 #include <mutex>
 
-#include <pair_wise_iterative_closest_point.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
+#include <Core/utils.hpp>
 
 #ifdef _WIN32
 #include "resource.h"
@@ -36,8 +34,6 @@
 #include <windows.h>
 #endif
 
-#include <spdlog/spdlog.h>
-
 ///////////////////////////////////////////////////////////////////////////////////
 
 std::string winTitle = std::string("MR calibration ") + HDMAPPING_VERSION_STRING;
diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp
index 2d20aa42..66be38b4 100644
--- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp
+++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp
@@ -13,13 +13,14 @@
 #include <glm/gtc/matrix_transform.hpp>
 
 #include <spdlog/spdlog.h>
-#include <utils.hpp>
 
-#include <Eigen/Eigen>
-
-#include <transformations.h>
+#include <Core/export_laz.h>
+#include <Core/hash_utils.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
+#include <Core/utils.hpp>
 
-#include "pfd_wrapper.hpp"
+#include <Eigen/Eigen>
 
 #include "../lidar_odometry_step_1/lidar_odometry.h"
 #include "../lidar_odometry_step_1/lidar_odometry_utils.h"
@@ -30,12 +31,8 @@
 #include "tbb/tbb.h"
 #include <mutex>
 
-#include <export_laz.h>
-
 #include <opencv2/opencv.hpp>
 
-#include <hash_utils.h>
-
 #ifdef _WIN32
 #include "resource.h"
 #include <shellapi.h> // <-- Required for ShellExecuteA
diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp
index 86da4ec6..fa339884 100644
--- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp
+++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp
@@ -14,13 +14,13 @@
 #include <glm/gtc/matrix_transform.hpp>
 
 #include <spdlog/spdlog.h>
-#include <utils.hpp>
 
-#include <Eigen/Eigen>
-
-#include <transformations.h>
+#include <Core/GL_assert.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
+#include <Core/utils.hpp>
 
-#include "pfd_wrapper.hpp"
+#include <Eigen/Eigen>
 
 #include "../lidar_odometry_step_1/lidar_odometry_utils.h"
 #include <filesystem>
@@ -34,7 +34,6 @@
 #include <shellapi.h> // <-- Required for ShellExecuteA
 #include <windows.h>
 #endif
-#include <GL_assert.h>
 
 ///////////////////////////////////////////////////////////////////////////////////
 
diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp
index 0cc51e54..3333f016 100644
--- a/apps/manual_color/manual_color.cpp
+++ b/apps/manual_color/manual_color.cpp
@@ -8,25 +8,26 @@
 
 #define STB_IMAGE_IMPLEMENTATION
 #include "stb_image.h"
-#include <mutex>
-#include <vector>
 
 #include <array>
 #include <cstdint>
+#include <execution>
 #include <iostream>
-#include <nlohmann/json.hpp>
+#include <mutex>
 #include <sstream>
-
-#include "color_las_loader.h"
-#include "pfd_wrapper.hpp"
-#include <pch/pch.h>
+#include <vector>
 
 #include <Eigen/Eigen>
+#include <nlohmann/json.hpp>
+
+#include <Core/color_las_loader.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/structures.h>
+#include <Core/transformations.h>
+
 #include <observation_equations/codes/common/include/cauchy.h>
 #include <observation_equations/codes/python-scripts/camera-metrics/equirectangular_camera_colinearity_tait_bryan_wc_jacobian.h>
 #include <observation_equations/codes/python-scripts/camera-metrics/fisheye_camera_calibRT_tait_bryan_wc_jacobian.h>
-#include <structures.h>
-#include <transformations.h>
 
 #include <HDMapping/Version.hpp>
 
diff --git a/apps/multi_session_registration/multi_session_factor_graph.cpp b/apps/multi_session_registration/multi_session_factor_graph.cpp
index 6b0ceb27..780c17d7 100644
--- a/apps/multi_session_registration/multi_session_factor_graph.cpp
+++ b/apps/multi_session_registration/multi_session_factor_graph.cpp
@@ -1,7 +1,7 @@
 #include "multi_session_factor_graph.h"
-#include <utils.hpp>
 
-#include <m_estimators.h>
+#include <Core/m_estimators.h>
+#include <Core/utils.hpp>
 
 #include <python-scripts/constraints/relative_pose_tait_bryan_cw_jacobian.h>
 #include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
diff --git a/apps/multi_session_registration/multi_session_factor_graph.h b/apps/multi_session_registration/multi_session_factor_graph.h
index a911ffd1..3d301add 100644
--- a/apps/multi_session_registration/multi_session_factor_graph.h
+++ b/apps/multi_session_registration/multi_session_factor_graph.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <session.h>
+#include <Core/session.h>
 
 struct Edge
 {
diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp
index 417791e6..fd56b92f 100644
--- a/apps/multi_session_registration/multi_session_registration.cpp
+++ b/apps/multi_session_registration/multi_session_registration.cpp
@@ -17,26 +17,20 @@
 
 #include <Eigen/Eigen>
 
-#include <session.h>
+#include <Core/export_laz.h>
+#include <Core/icp.h>
+#include <Core/ndt.h>
+#include <Core/observation_picking.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/utils.hpp>
 
-#include <pfd_wrapper.hpp>
 #include <portable-file-dialogs.h>
 
-#include <utils.hpp>
-
-#include <icp.h>
-
-#include <registration_plane_feature.h>
-
 #include <HDMapping/Version.hpp>
 
-#include <ndt.h>
-
-#include <observation_picking.h>
-#include <pair_wise_iterative_closest_point.h>
-
-#include <export_laz.h>
-
 #ifdef _WIN32
 #include "resource.h"
 #include <windows.h>
diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp
index eaa35911..a64b0325 100644
--- a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp
+++ b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp
@@ -1,5 +1,5 @@
 #include "multi_view_tls_registration.h"
-#include <export_laz.h>
+#include <Core/export_laz.h>
 #include <fstream>
 #include <laszip/laszip_api.h>
 
diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.h b/apps/multi_view_tls_registration/multi_view_tls_registration.h
index 2f858dfd..0553abb5 100644
--- a/apps/multi_view_tls_registration/multi_view_tls_registration.h
+++ b/apps/multi_view_tls_registration/multi_view_tls_registration.h
@@ -2,14 +2,15 @@
 #pragma once
 
 #include <filesystem>
-#include <gnss.h>
-#include <icp.h>
-#include <ndt.h>
-#include <pose_graph_slam.h>
 #include <regex>
-#include <registration_plane_feature.h>
-#include <session.h>
-#include <transformations.h>
+
+#include <Core/gnss.h>
+#include <Core/icp.h>
+#include <Core/ndt.h>
+#include <Core/pose_graph_slam.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/transformations.h>
 
 namespace fs = std::filesystem;
 
diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
index acae4371..d077b0f4 100644
--- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
+++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
@@ -9,21 +9,26 @@
 
 #include <ImGuizmo.h>
 
+#include <spdlog/spdlog.h>
+
 #include <Eigen/Eigen>
 
 #include <glm/glm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
 
-#include <icp.h>
-#include <ndt.h>
-#include <observation_picking.h>
-#include <pose_graph_slam.h>
-#include <registration_plane_feature.h>
-#include <transformations.h>
-
-#include <fmt_filesystem.hpp>
-#include <spdlog/spdlog.h>
-#include <utils.hpp>
+#include <Core/export_laz.h>
+#include <Core/fmt_filesystem.hpp>
+#include <Core/gnss.h>
+#include <Core/icp.h>
+#include <Core/manual_pose_graph_loop_closure.h>
+#include <Core/ndt.h>
+#include <Core/observation_picking.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/pose_graph_slam.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/transformations.h>
+#include <Core/utils.hpp>
 
 #include <portable-file-dialogs.h>
 
@@ -36,17 +41,11 @@
 
 #include "../lidar_odometry_step_1/lidar_odometry_utils.h"
 #include "multi_view_tls_registration.h"
-#include <manual_pose_graph_loop_closure.h>
-
-#include <gnss.h>
-#include <pfd_wrapper.hpp>
-#include <session.h>
 
 #include <HDMapping/Version.hpp>
 
 #include "WGS84toCartesian.hpp"
 #include "wgs84_do_puwg92.h"
-#include <export_laz.h>
 
 #include <proj.h>
 #ifdef _WIN32
diff --git a/apps/multi_view_tls_registration/perform_experiment.cpp b/apps/multi_view_tls_registration/perform_experiment.cpp
index f2b4601f..3d09d804 100644
--- a/apps/multi_view_tls_registration/perform_experiment.cpp
+++ b/apps/multi_view_tls_registration/perform_experiment.cpp
@@ -1,4 +1,7 @@
 #include <cmath>
+#include <filesystem>
+#include <fstream>
+#include <iostream>
 
 // clang-format off
 #include <GL/glew.h>
@@ -17,26 +20,20 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
 
-#include <icp.h>
-#include <ndt.h>
-#include <observation_picking.h>
-#include <pose_graph_slam.h>
-#include <registration_plane_feature.h>
-#include <transformations.h>
+#include <Core/gnss.h>
+#include <Core/icp.h>
+#include <Core/manual_pose_graph_loop_closure.h>
+#include <Core/ndt.h>
+#include <Core/observation_picking.h>
+#include <Core/pose_graph_slam.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/transformations.h>
 
 #include <portable-file-dialogs.h>
 
 #include <laszip/laszip_api.h>
 
-#include <filesystem>
-#include <fstream>
-#include <iostream>
-
-#include <manual_pose_graph_loop_closure.h>
-
-#include <gnss.h>
-#include <session.h>
-
 #include <spdlog/spdlog.h>
 
 namespace fs = std::filesystem;
diff --git a/apps/precision_forestry_tools/local_shape_features.cpp b/apps/precision_forestry_tools/local_shape_features.cpp
index 516d6c22..9befe0b4 100644
--- a/apps/precision_forestry_tools/local_shape_features.cpp
+++ b/apps/precision_forestry_tools/local_shape_features.cpp
@@ -1,4 +1,4 @@
-#include <hash_utils.h>
+#include <Core/hash_utils.h>
 
 #include "local_shape_features.h"
 
diff --git a/apps/precision_forestry_tools/precision_forestry_tools.cpp b/apps/precision_forestry_tools/precision_forestry_tools.cpp
index 28ff302b..61cd0cf0 100644
--- a/apps/precision_forestry_tools/precision_forestry_tools.cpp
+++ b/apps/precision_forestry_tools/precision_forestry_tools.cpp
@@ -15,11 +15,11 @@
 
 #include <Eigen/Eigen>
 
-#include "pfd_wrapper.hpp"
-
 #include <HDMapping/Version.hpp>
-#include <session.h>
-#include <transformations.h>
+
+#include <Core/pfd_wrapper.hpp>
+#include <Core/session.h>
+#include <Core/transformations.h>
 
 #include <filesystem>
 #include <fstream>
diff --git a/apps/precision_forestry_tools/surface.cpp b/apps/precision_forestry_tools/surface.cpp
index 30549ead..1caebdab 100644
--- a/apps/precision_forestry_tools/surface.cpp
+++ b/apps/precision_forestry_tools/surface.cpp
@@ -2,9 +2,10 @@
 
 #include <GL/freeglut.h>
 
-#include <structures.h>
-#include <transformations.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
+//  TODO(mwlasiuk) : fix includes
 #include <../../3rdparty/observation_equations/codes/common/include/cauchy.h>
 #include <../../3rdparty/observation_equations/codes/python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
 #include <../../3rdparty/observation_equations/codes/python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
diff --git a/apps/precision_forestry_tools/surface.h b/apps/precision_forestry_tools/surface.h
index e5399992..b005cb7f 100644
--- a/apps/precision_forestry_tools/surface.h
+++ b/apps/precision_forestry_tools/surface.h
@@ -3,7 +3,7 @@
 #include <Eigen/Eigen>
 #include <vector>
 
-#include <hash_utils.h>
+#include <Core/hash_utils.h>
 
 class Surface
 {
diff --git a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp
index 3279d83d..f68302a2 100644
--- a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp
+++ b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp
@@ -7,32 +7,30 @@
 
 #define STB_IMAGE_IMPLEMENTATION
 #include "stb_image.h"
-#include <mutex>
-#include <vector>
 
-#include "color_las_loader.h"
-#include "pfd_wrapper.hpp"
-#include <pch/pch.h>
+#include <Core/color_las_loader.h>
+#include <Core/observation_picking.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/session.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
 #include <Eigen/Eigen>
+
 #include <observation_equations/codes/common/include/cauchy.h>
 #include <observation_equations/codes/python-scripts/camera-metrics/equirectangular_camera_colinearity_tait_bryan_wc_jacobian.h>
 #include <observation_equations/codes/python-scripts/camera-metrics/fisheye_camera_calibRT_tait_bryan_wc_jacobian.h>
-#include <structures.h>
-#include <transformations.h>
 
 #include <HDMapping/Version.hpp>
 
 #include <glm/glm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
 
-#include <session.h>
-
-#include <observation_picking.h>
-
 #include <execution>
 #include <filesystem>
 #include <iostream>
+#include <mutex>
+#include <vector>
 
 const uint32_t window_width = 800;
 const uint32_t window_height = 600;
diff --git a/apps/split_multi_livox/split_multi_livox.cpp b/apps/split_multi_livox/split_multi_livox.cpp
index 406b35e3..a82bdde0 100644
--- a/apps/split_multi_livox/split_multi_livox.cpp
+++ b/apps/split_multi_livox/split_multi_livox.cpp
@@ -1,12 +1,13 @@
-#include <Eigen/Dense>
+#include <Core/export_laz.h>
 #include <HDMapping/Version.hpp>
-#include <export_laz.h>
+
 #include <filesystem>
 #include <iostream>
-#include <laszip/laszip_api.h>
 #include <string>
 #include <vector>
 
+#include <Eigen/Dense>
+#include <laszip/laszip_api.h>
 #include <spdlog/spdlog.h>
 
 struct Point3Dis
diff --git a/core/include/GL_assert.h b/core/include/Core/GL_assert.h
similarity index 100%
rename from core/include/GL_assert.h
rename to core/include/Core/GL_assert.h
diff --git a/core/include/color_las_loader.h b/core/include/Core/color_las_loader.h
similarity index 100%
rename from core/include/color_las_loader.h
rename to core/include/Core/color_las_loader.h
diff --git a/core/include/control_points.h b/core/include/Core/control_points.h
similarity index 97%
rename from core/include/control_points.h
rename to core/include/Core/control_points.h
index f9eff83a..c674344c 100644
--- a/core/include/control_points.h
+++ b/core/include/Core/control_points.h
@@ -1,6 +1,7 @@
 #pragma once
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
+
 #include <string>
 #include <vector>
 #if WITH_GUI == 1
diff --git a/core/include/exportPcd.h b/core/include/Core/exportPcd.h
similarity index 100%
rename from core/include/exportPcd.h
rename to core/include/Core/exportPcd.h
diff --git a/core/include/export_laz.h b/core/include/Core/export_laz.h
similarity index 99%
rename from core/include/export_laz.h
rename to core/include/Core/export_laz.h
index f35f9478..9ace206f 100644
--- a/core/include/export_laz.h
+++ b/core/include/Core/export_laz.h
@@ -2,9 +2,10 @@
 
 // TODO(mwlasiuk) : do this at the end (print -> spdlog) as it is .h and not cpp + hpp and that breaks some subprojects ...
 
+#include <Core/session.h>
+
 #include <filesystem>
 #include <iostream>
-#include <session.h>
 #include <string>
 #include <vector>
 
diff --git a/core/include/fmt_filesystem.hpp b/core/include/Core/fmt_filesystem.hpp
similarity index 100%
rename from core/include/fmt_filesystem.hpp
rename to core/include/Core/fmt_filesystem.hpp
diff --git a/core/include/gnss.h b/core/include/Core/gnss.h
similarity index 99%
rename from core/include/gnss.h
rename to core/include/Core/gnss.h
index 2bae2916..19227211 100644
--- a/core/include/gnss.h
+++ b/core/include/Core/gnss.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
 
 #include <set>
 #include <string>
diff --git a/core/include/ground_control_points.h b/core/include/Core/ground_control_points.h
similarity index 97%
rename from core/include/ground_control_points.h
rename to core/include/Core/ground_control_points.h
index e0b0966d..ed23ef71 100644
--- a/core/include/ground_control_points.h
+++ b/core/include/Core/ground_control_points.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
 
 #include <string>
 #include <vector>
diff --git a/core/include/hash_utils.h b/core/include/Core/hash_utils.h
similarity index 100%
rename from core/include/hash_utils.h
rename to core/include/Core/hash_utils.h
diff --git a/core/include/icp.h b/core/include/Core/icp.h
similarity index 99%
rename from core/include/icp.h
rename to core/include/Core/icp.h
index 187dcea9..08cb9210 100644
--- a/core/include/icp.h
+++ b/core/include/Core/icp.h
@@ -1,6 +1,7 @@
 #pragma once
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
+
 #include <thread>
 
 class ICP
diff --git a/core/include/m_estimators.h b/core/include/Core/m_estimators.h
similarity index 100%
rename from core/include/m_estimators.h
rename to core/include/Core/m_estimators.h
diff --git a/core/include/manual_pose_graph_loop_closure.h b/core/include/Core/manual_pose_graph_loop_closure.h
similarity index 87%
rename from core/include/manual_pose_graph_loop_closure.h
rename to core/include/Core/manual_pose_graph_loop_closure.h
index adc217b5..295360fc 100644
--- a/core/include/manual_pose_graph_loop_closure.h
+++ b/core/include/Core/manual_pose_graph_loop_closure.h
@@ -3,11 +3,13 @@
 #if WITH_GUI == 1
 
 #include <GL/freeglut.h>
-#include <gnss.h>
+
 #include <imgui.h>
-#include <observation_picking.h>
-#include <point_clouds.h>
-#include <pose_graph_loop_closure.h>
+
+#include <Core/gnss.h>
+#include <Core/observation_picking.h>
+#include <Core/point_clouds.h>
+#include <Core/pose_graph_loop_closure.h>
 
 class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure
 {
diff --git a/core/include/ndt.h b/core/include/Core/ndt.h
similarity index 99%
rename from core/include/ndt.h
rename to core/include/Core/ndt.h
index 4a11b207..8ed338c4 100644
--- a/core/include/ndt.h
+++ b/core/include/Core/ndt.h
@@ -1,8 +1,9 @@
 #pragma once
 
+#include <Core/point_cloud.h>
+#include <Core/session.h>
+
 #include <Eigen/Eigen>
-#include <point_cloud.h>
-#include <session.h>
 #include <thread>
 
 class NDT
diff --git a/core/include/nmea.h b/core/include/Core/nmea.h
similarity index 100%
rename from core/include/nmea.h
rename to core/include/Core/nmea.h
diff --git a/core/include/observation_picking.h b/core/include/Core/observation_picking.h
similarity index 98%
rename from core/include/observation_picking.h
rename to core/include/Core/observation_picking.h
index fe44fa9a..cefb7b09 100644
--- a/core/include/observation_picking.h
+++ b/core/include/Core/observation_picking.h
@@ -1,12 +1,13 @@
 #pragma once
 
+#include <Core/structures.h>
+#include <Core/transformations.h>
+
 #include <Eigen/Eigen>
 #include <GL/freeglut.h>
 #include <imgui.h>
 #include <map>
 #include <nlohmann/json.hpp>
-#include <structures.h>
-#include <transformations.h>
 
 struct PointInsideIntersection
 {
diff --git a/core/include/pair_wise_iterative_closest_point.h b/core/include/Core/pair_wise_iterative_closest_point.h
similarity index 100%
rename from core/include/pair_wise_iterative_closest_point.h
rename to core/include/Core/pair_wise_iterative_closest_point.h
diff --git a/core/include/pfd_wrapper.hpp b/core/include/Core/pfd_wrapper.hpp
similarity index 100%
rename from core/include/pfd_wrapper.hpp
rename to core/include/Core/pfd_wrapper.hpp
diff --git a/core/include/point_cloud.h b/core/include/Core/point_cloud.h
similarity index 98%
rename from core/include/point_cloud.h
rename to core/include/Core/point_cloud.h
index a00bd53e..dce5de80 100644
--- a/core/include/point_cloud.h
+++ b/core/include/Core/point_cloud.h
@@ -1,13 +1,15 @@
 #pragma once
 
 #include <Eigen/Eigen>
+
 #include <string>
-#include <structures.h>
 #include <vector>
 
+#include <Core/structures.h>
+
 #if WITH_GUI == 1
+#include <Core/observation_picking.h>
 #include <GL/freeglut.h>
-#include <observation_picking.h>
 #endif
 
 class PointCloud
diff --git a/core/include/point_clouds.h b/core/include/Core/point_clouds.h
similarity index 97%
rename from core/include/point_clouds.h
rename to core/include/Core/point_clouds.h
index f8c1ab78..b9aa988d 100644
--- a/core/include/point_clouds.h
+++ b/core/include/Core/point_clouds.h
@@ -3,10 +3,11 @@
 #include <string>
 #include <vector>
 
-#include <point_cloud.h>
-#include <transformations.h>
+#include <Core/point_cloud.h>
+#include <Core/transformations.h>
+
 #if WITH_GUI == 1
-#include <observation_picking.h>
+#include <Core/observation_picking.h>
 #endif
 
 class PointClouds
diff --git a/core/include/pose_graph_loop_closure.h b/core/include/Core/pose_graph_loop_closure.h
similarity index 94%
rename from core/include/pose_graph_loop_closure.h
rename to core/include/Core/pose_graph_loop_closure.h
index 8adcd622..2d60089a 100644
--- a/core/include/pose_graph_loop_closure.h
+++ b/core/include/Core/pose_graph_loop_closure.h
@@ -1,9 +1,9 @@
 #pragma once
 
-#include <control_points.h>
-#include <gnss.h>
-#include <ground_control_points.h>
-#include <point_clouds.h>
+#include <Core/control_points.h>
+#include <Core/gnss.h>
+#include <Core/ground_control_points.h>
+#include <Core/point_clouds.h>
 
 class PoseGraphLoopClosure
 {
diff --git a/core/include/pose_graph_slam.h b/core/include/Core/pose_graph_slam.h
similarity index 98%
rename from core/include/pose_graph_slam.h
rename to core/include/Core/pose_graph_slam.h
index 5f9d5bed..ad4c58c4 100644
--- a/core/include/pose_graph_slam.h
+++ b/core/include/Core/pose_graph_slam.h
@@ -1,7 +1,7 @@
 #pragma once
 
-#include <icp.h>
-#include <point_clouds.h>
+#include <Core/icp.h>
+#include <Core/point_clouds.h>
 
 class PoseGraphSLAM
 {
diff --git a/core/include/registration_plane_feature.h b/core/include/Core/registration_plane_feature.h
similarity index 99%
rename from core/include/registration_plane_feature.h
rename to core/include/Core/registration_plane_feature.h
index e40c3a2e..f76ffa60 100644
--- a/core/include/registration_plane_feature.h
+++ b/core/include/Core/registration_plane_feature.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
 
 class RegistrationPlaneFeature
 {
diff --git a/core/include/session.h b/core/include/Core/session.h
similarity index 85%
rename from core/include/session.h
rename to core/include/Core/session.h
index 763d2c78..57f13717 100644
--- a/core/include/session.h
+++ b/core/include/Core/session.h
@@ -1,16 +1,17 @@
 #pragma once
 
-#include <point_clouds.h>
+#include <Core/point_clouds.h>
+
 #include <string>
 #include <vector>
 
 #if WITH_GUI == 1
-#include <control_points.h>
-#include <ground_control_points.h>
-#include <manual_pose_graph_loop_closure.h>
+#include <Core/control_points.h>
+#include <Core/ground_control_points.h>
+#include <Core/manual_pose_graph_loop_closure.h>
 
 #else
-#include <pose_graph_loop_closure.h>
+#include <Core/pose_graph_loop_closure.h>
 #endif
 
 class Session
diff --git a/core/include/structures.h b/core/include/Core/structures.h
similarity index 100%
rename from core/include/structures.h
rename to core/include/Core/structures.h
diff --git a/core/include/system_info.hpp b/core/include/Core/system_info.hpp
similarity index 100%
rename from core/include/system_info.hpp
rename to core/include/Core/system_info.hpp
diff --git a/core/include/transformations.h b/core/include/Core/transformations.h
similarity index 100%
rename from core/include/transformations.h
rename to core/include/Core/transformations.h
diff --git a/core/include/utils.hpp b/core/include/Core/utils.hpp
similarity index 97%
rename from core/include/utils.hpp
rename to core/include/Core/utils.hpp
index 853acf6a..0835a52b 100644
--- a/core/include/utils.hpp
+++ b/core/include/Core/utils.hpp
@@ -3,11 +3,13 @@
 
 #pragma once
 
-#include <Eigen/Geometry>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/structures.h>
+
 #include <imgui.h>
-#include <registration_plane_feature.h>
-#include <session.h>
-#include <structures.h>
+
+#include <Eigen/Geometry>
 
 ///////////////////////////////////////////////////////////////////////////////////
 
diff --git a/core/src/color_las_loader.cpp b/core/src/color_las_loader.cpp
index 3edcd8ba..d74fafae 100644
--- a/core/src/color_las_loader.cpp
+++ b/core/src/color_las_loader.cpp
@@ -1,6 +1,6 @@
 #include <pch/pch.h>
 
-#include "color_las_loader.h"
+#include "Core/color_las_loader.h"
 
 #include <spdlog/spdlog.h>
 
diff --git a/core/src/control_points.cpp b/core/src/control_points.cpp
index d46532dc..eef1a18c 100644
--- a/core/src/control_points.cpp
+++ b/core/src/control_points.cpp
@@ -1,6 +1,6 @@
 #include <pch/pch.h>
 
-#include <control_points.h>
+#include <Core/control_points.h>
 
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
 
diff --git a/core/src/gnss.cpp b/core/src/gnss.cpp
index 8e0d97ac..6b94f70e 100644
--- a/core/src/gnss.cpp
+++ b/core/src/gnss.cpp
@@ -1,8 +1,9 @@
 #include <pch/pch.h>
 
+#include <Core/gnss.h>
+#include <Core/nmea.h>
+
 #include <WGS84toCartesian.hpp>
-#include <gnss.h>
-#include <nmea.h>
 #include <wgs84_do_puwg92.h>
 
 #if WITH_GUI == 1
diff --git a/core/src/ground_control_points.cpp b/core/src/ground_control_points.cpp
index 02fc2519..2e530c4a 100644
--- a/core/src/ground_control_points.cpp
+++ b/core/src/ground_control_points.cpp
@@ -1,7 +1,9 @@
 #include <pch/pch.h>
 
-#include <ground_control_points.h>
+#include <Core/ground_control_points.h>
+
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
+
 #if WITH_GUI == 1
 #include <imgui.h>
 #include <imgui_impl_glut.h>
diff --git a/core/src/hash_utils.cpp b/core/src/hash_utils.cpp
index de550736..3ac5dfde 100644
--- a/core/src/hash_utils.cpp
+++ b/core/src/hash_utils.cpp
@@ -1,6 +1,6 @@
 #include <pch/pch.h>
 
-#include <hash_utils.h>
+#include <Core/hash_utils.h>
 
 uint64_t get_index_2d(const int16_t x, const int16_t y)
 {
diff --git a/core/src/icp.cpp b/core/src/icp.cpp
index ddd3509c..11c8ac34 100644
--- a/core/src/icp.cpp
+++ b/core/src/icp.cpp
@@ -1,13 +1,12 @@
 #include <pch/pch.h>
 
-#include <icp.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_point_tait_bryan_wc_jacobian.h>
 
-#include <m_estimators.h>
-#include <transformations.h>
-
 std::vector<ICP::Job> ICP::get_jobs(uint64_t size, int num_threads)
 {
     int hc = size / num_threads;
diff --git a/core/src/manual_pose_graph_loop_closure.cpp b/core/src/manual_pose_graph_loop_closure.cpp
index 912e77f5..c5c9f283 100644
--- a/core/src/manual_pose_graph_loop_closure.cpp
+++ b/core/src/manual_pose_graph_loop_closure.cpp
@@ -1,10 +1,15 @@
 #include <pch/pch.h>
 
 #if WITH_GUI == 1
-#include <icp.h>
-#include <manual_pose_graph_loop_closure.h>
-#include <pair_wise_iterative_closest_point.h>
-#include <transformations.h>
+#include <Core/export_laz.h>
+#include <Core/gnss.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/manual_pose_graph_loop_closure.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
+#include <Core/utils.hpp>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/constraints/relative_pose_quaternion_cw_jacobian.h>
@@ -15,17 +20,8 @@
 #include <python-scripts/point-to-feature-metrics/point_to_line_tait_bryan_wc_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
 
-#include <gnss.h>
-#include <m_estimators.h>
-
 #include <common/include/cauchy.h>
 
-#include <pfd_wrapper.hpp>
-
-#include "export_laz.h"
-
-#include <utils.hpp>
-
 void ManualPoseGraphLoopClosure::Gui(
     PointClouds& point_clouds_container,
     int& index_loop_closure_source,
diff --git a/core/src/ndt.cpp b/core/src/ndt.cpp
index 36d360a7..3cca056a 100644
--- a/core/src/ndt.cpp
+++ b/core/src/ndt.cpp
@@ -1,11 +1,9 @@
 #include <pch/pch.h>
 
-#include <ndt.h>
-#include <structures.h>
-
-#include <point_clouds.h>
-
-#include <transformations.h>
+#include <Core/ndt.h>
+#include <Core/point_clouds.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/elementary_error_theory_for_terrestrial_laser_scanner_jacobian.h>
diff --git a/core/src/nmea.cpp b/core/src/nmea.cpp
index 4f10ae3e..85b1df93 100644
--- a/core/src/nmea.cpp
+++ b/core/src/nmea.cpp
@@ -1,6 +1,6 @@
 #include <pch/pch.h>
 
-#include "nmea.h"
+#include <Core/nmea.h>
 
 namespace hd_mapping::nmea
 {
diff --git a/core/src/observation_picking.cpp b/core/src/observation_picking.cpp
index e4034f5f..d59550b0 100644
--- a/core/src/observation_picking.cpp
+++ b/core/src/observation_picking.cpp
@@ -1,8 +1,8 @@
 #include <pch/pch.h>
 
-#include <GL/freeglut.h>
-#include <observation_picking.h>
+#include <Core/observation_picking.h>
 
+#include <GL/freeglut.h>
 #include <nlohmann/json.hpp>
 
 // void renderBitmapString(float x, float y, float z, char* string) {
diff --git a/core/src/optimization_point_to_point_source_to_target.cpp b/core/src/optimization_point_to_point_source_to_target.cpp
index 4b536c9e..8aad8bb0 100644
--- a/core/src/optimization_point_to_point_source_to_target.cpp
+++ b/core/src/optimization_point_to_point_source_to_target.cpp
@@ -1,19 +1,16 @@
 #include <pch/pch.h>
 
-#include <icp.h>
-
-#include <m_estimators.h>
-#include <transformations.h>
-
-#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_cw_jacobian.h>
-#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
-
-#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_rodrigues_cw_jacobian.h>
-#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_rodrigues_wc_jacobian.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_quaternion_cw_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_quaternion_wc_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_rodrigues_cw_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_rodrigues_wc_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_cw_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
 
 Eigen::Matrix<double, 3, 1> get_delta_point_to_point_tait_bryan(
     ICP::PoseConvention pose_convention, Eigen::Affine3d m_pose_wc, Eigen::Vector3d p_s, Eigen::Vector3d p_t)
diff --git a/core/src/optimize_distance_point_to_plane_source_to_target.cpp b/core/src/optimize_distance_point_to_plane_source_to_target.cpp
index 4b8664fd..e22311c7 100644
--- a/core/src/optimize_distance_point_to_plane_source_to_target.cpp
+++ b/core/src/optimize_distance_point_to_plane_source_to_target.cpp
@@ -1,18 +1,16 @@
 #include <pch/pch.h>
 
-#include <m_estimators.h>
-#include <registration_plane_feature.h>
-#include <transformations.h>
-
-#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_tait_bryan_cw_jacobian.h>
-#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_tait_bryan_wc_jacobian.h>
-
-#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_rodrigues_cw_jacobian.h>
-#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_rodrigues_wc_jacobian.h>
+#include <Core/m_estimators.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/point-to-feature-metrics/distance_point_to_plane_quaternion_cw_jacobian.h>
 #include <python-scripts/point-to-feature-metrics/distance_point_to_plane_quaternion_wc_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_rodrigues_cw_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_rodrigues_wc_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_tait_bryan_cw_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/distance_point_to_plane_tait_bryan_wc_jacobian.h>
 
 Eigen::Matrix<double, 1, 1> get_delta_distance_point_to_plane_tait_bryan(
     RegistrationPlaneFeature::PoseConvention pose_convention,
diff --git a/core/src/optimize_plane_to_plane_source_to_target.cpp b/core/src/optimize_plane_to_plane_source_to_target.cpp
index f89b0bec..1d9ddd6f 100644
--- a/core/src/optimize_plane_to_plane_source_to_target.cpp
+++ b/core/src/optimize_plane_to_plane_source_to_target.cpp
@@ -1,20 +1,17 @@
 #include <pch/pch.h>
 
-#include <icp.h>
-#include <registration_plane_feature.h>
-
-#include <m_estimators.h>
-#include <transformations.h>
-
-#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_tait_bryan_cw_jacobian.h>
-#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_tait_bryan_wc_jacobian.h>
-
-#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_rodrigues_cw_jacobian.h>
-#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_rodrigues_wc_jacobian.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_quaternion_cw_jacobian.h>
 #include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_quaternion_wc_jacobian.h>
+#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_rodrigues_cw_jacobian.h>
+#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_rodrigues_wc_jacobian.h>
+#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_tait_bryan_cw_jacobian.h>
+#include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_tait_bryan_wc_jacobian.h>
 
 Eigen::Matrix<double, 4, 1> get_plane_to_plane_source_to_target_tait_bryan(
     RegistrationPlaneFeature::PoseConvention pose_convention,
diff --git a/core/src/optimize_point_to_plane_source_to_target.cpp b/core/src/optimize_point_to_plane_source_to_target.cpp
index 1232bf8d..237381df 100644
--- a/core/src/optimize_point_to_plane_source_to_target.cpp
+++ b/core/src/optimize_point_to_plane_source_to_target.cpp
@@ -1,18 +1,16 @@
 #include <pch/pch.h>
 
-#include <m_estimators.h>
-#include <registration_plane_feature.h>
-#include <transformations.h>
-
-#include <python-scripts/point-to-feature-metrics/point_to_plane_tait_bryan_cw_jacobian.h>
-#include <python-scripts/point-to-feature-metrics/point_to_plane_tait_bryan_wc_jacobian.h>
-
-#include <python-scripts/point-to-feature-metrics/point_to_plane_rodrigues_cw_jacobian.h>
-#include <python-scripts/point-to-feature-metrics/point_to_plane_rodrigues_wc_jacobian.h>
+#include <Core/m_estimators.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/point-to-feature-metrics/point_to_plane_quaternion_cw_jacobian.h>
 #include <python-scripts/point-to-feature-metrics/point_to_plane_quaternion_wc_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/point_to_plane_rodrigues_cw_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/point_to_plane_rodrigues_wc_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/point_to_plane_tait_bryan_cw_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/point_to_plane_tait_bryan_wc_jacobian.h>
 
 Eigen::Matrix<double, 1, 1> get_delta_point_to_plane_tait_bryan(
     RegistrationPlaneFeature::PoseConvention pose_convention,
diff --git a/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp b/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp
index 91933852..d233f3e7 100644
--- a/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp
+++ b/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp
@@ -1,23 +1,17 @@
 #include <pch/pch.h>
 
-#include <icp.h>
-#include <registration_plane_feature.h>
-
-#include <m_estimators.h>
-#include <transformations.h>
-// #include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_tait_bryan_wc_jacobian.h>
-#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_tait_bryan_cw_jacobian.h>
-#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_tait_bryan_wc_jacobian.h>
-
-#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_rodrigues_cw_jacobian.h>
-#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_rodrigues_wc_jacobian.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_quaternion_cw_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_quaternion_wc_jacobian.h>
-
-// #include <python-scripts/point-to-feature-metrics/point_to_plane_tait_bryan_wc_jacobian.h>
-// #include <python-scripts/point-to-feature-metrics/distance_point_to_plane_tait_bryan_wc_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_rodrigues_cw_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_rodrigues_wc_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_tait_bryan_cw_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_projection_onto_plane_tait_bryan_wc_jacobian.h>
 
 Eigen::Matrix<double, 3, 1> get_delta_point_to_projection_onto_plane_tait_bryan(
     RegistrationPlaneFeature::PoseConvention pose_convention,
diff --git a/core/src/pair_wise_iterative_closest_point.cpp b/core/src/pair_wise_iterative_closest_point.cpp
index f1494d66..a57ba808 100644
--- a/core/src/pair_wise_iterative_closest_point.cpp
+++ b/core/src/pair_wise_iterative_closest_point.cpp
@@ -1,10 +1,11 @@
 #include <pch/pch.h>
 
-#include <hash_utils.h>
-#include <pair_wise_iterative_closest_point.h>
+#include <Core/hash_utils.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/structures.h>
+#include <Core/transformations.h>
+
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian_simplified.h>
-#include <structures.h>
-#include <transformations.h>
 
 inline void point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_4(
     Eigen::Matrix<double, 6, 6, Eigen::RowMajor>& AtPA,
diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp
index 0b0da44c..c34357d0 100644
--- a/core/src/pfd_wrapper.cpp
+++ b/core/src/pfd_wrapper.cpp
@@ -1,6 +1,7 @@
 #include <pch/pch.h>
 
-#include <pfd_wrapper.hpp>
+#include <Core/pfd_wrapper.hpp>
+
 #include <portable-file-dialogs.h>
 
 namespace mandeye::fd
diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp
index 351a2161..cb7327c2 100644
--- a/core/src/point_cloud.cpp
+++ b/core/src/point_cloud.cpp
@@ -1,7 +1,7 @@
 #include <pch/pch.h>
 
-#include <point_cloud.h>
-#include <transformations.h>
+#include <Core/point_cloud.h>
+#include <Core/transformations.h>
 
 #include <plycpp.h>
 
diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp
index ab48009a..03d15299 100644
--- a/core/src/point_clouds.cpp
+++ b/core/src/point_clouds.cpp
@@ -1,9 +1,9 @@
 #include <pch/pch.h>
 
-#include <laszip/laszip_api.h>
-#include <point_clouds.h>
-#include <transformations.h>
+#include <Core/point_clouds.h>
+#include <Core/transformations.h>
 
+#include <laszip/laszip_api.h>
 #include <spdlog/spdlog.h>
 
 inline void split(std::string& str, char delim, std::vector<std::string>& out)
diff --git a/core/src/pose_graph_loop_closure.cpp b/core/src/pose_graph_loop_closure.cpp
index 1d80f7c6..ce902dd3 100644
--- a/core/src/pose_graph_loop_closure.cpp
+++ b/core/src/pose_graph_loop_closure.cpp
@@ -1,13 +1,14 @@
 #include <pch/pch.h>
 
 #include <common/include/cauchy.h>
-#include <control_points.h>
-#include <ground_control_points.h>
-#include <icp.h>
-#include <m_estimators.h>
-#include <pair_wise_iterative_closest_point.h>
-#include <pose_graph_loop_closure.h>
-#include <transformations.h>
+
+#include <Core/control_points.h>
+#include <Core/ground_control_points.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/pose_graph_loop_closure.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/relative_pose_tait_bryan_cw_jacobian.h>
 #include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
diff --git a/core/src/pose_graph_slam.cpp b/core/src/pose_graph_slam.cpp
index 12361548..baa533de 100644
--- a/core/src/pose_graph_slam.cpp
+++ b/core/src/pose_graph_slam.cpp
@@ -1,6 +1,11 @@
 #include <pch/pch.h>
 
-#include <pose_graph_slam.h>
+#include <Core/m_estimators.h>
+#include <Core/ndt.h>
+#include <Core/pose_graph_slam.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/transformations.h>
+
 #include <python-scripts/constraints/quaternion_constraint_jacobian.h>
 #include <python-scripts/constraints/relative_pose_quaternion_cw_jacobian.h>
 #include <python-scripts/constraints/relative_pose_quaternion_wc_jacobian.h>
@@ -8,11 +13,6 @@
 #include <python-scripts/constraints/relative_pose_tait_bryan_cw_jacobian.h>
 #include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
 
-#include <transformations.h>
-
-#include <ndt.h>
-#include <registration_plane_feature.h>
-
 #ifdef WITH_PCL
 #include <pcl/filters/approximate_voxel_grid.h>
 #include <pcl/io/pcd_io.h>
@@ -38,8 +38,6 @@
 #include <manif/SE3.h>
 #endif
 
-#include <m_estimators.h>
-
 std::random_device rd;
 std::mt19937 gen(rd());
 
diff --git a/core/src/registration_plane_feature.cpp b/core/src/registration_plane_feature.cpp
index 215a3bf7..d819c736 100644
--- a/core/src/registration_plane_feature.cpp
+++ b/core/src/registration_plane_feature.cpp
@@ -1,10 +1,9 @@
 #include <pch/pch.h>
 
-#include <icp.h>
-#include <registration_plane_feature.h>
-
-#include <m_estimators.h>
-#include <transformations.h>
+#include <Core/icp.h>
+#include <Core/m_estimators.h>
+#include <Core/registration_plane_feature.h>
+#include <Core/transformations.h>
 
 #include <python-scripts/feature-to-feature-metrics/plane_to_plane_source_to_target_tait_bryan_wc_jacobian.h>
 #include <python-scripts/point-to-feature-metrics/distance_point_to_plane_tait_bryan_wc_jacobian.h>
diff --git a/core/src/session.cpp b/core/src/session.cpp
index 025282af..bbc2fe9f 100644
--- a/core/src/session.cpp
+++ b/core/src/session.cpp
@@ -1,8 +1,10 @@
 #include <pch/pch.h>
 
-#include "../../shared/include/HDMapping/Version.hpp"
+#include <Core/session.h>
+
 #include <nlohmann/json.hpp>
-#include <session.h>
+
+#include <HDMapping/Version.hpp>
 
 namespace fs = std::filesystem;
 
diff --git a/core/src/utils.cpp b/core/src/utils.cpp
index bbc6aaf4..e1f03265 100644
--- a/core/src/utils.cpp
+++ b/core/src/utils.cpp
@@ -15,7 +15,7 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
 
-#include "utils.hpp"
+#include <Core/utils.hpp>
 
 #include <HDMapping/Version.hpp>
 
diff --git a/core_hd_mapping/include/laz_wrapper.h b/core_hd_mapping/include/CoreHDMapping/laz_wrapper.h
similarity index 85%
rename from core_hd_mapping/include/laz_wrapper.h
rename to core_hd_mapping/include/CoreHDMapping/laz_wrapper.h
index 0ec66154..dead6210 100644
--- a/core_hd_mapping/include/laz_wrapper.h
+++ b/core_hd_mapping/include/CoreHDMapping/laz_wrapper.h
@@ -7,8 +7,10 @@
 
 #include <ImGuizmo.h>
 
-#include <project_settings.h>
-#include <structures.h>
+#include <CoreHDMapping/project_settings.h>
+
+// TODO(mwlasiuk) : cross includes ???
+#include <Core/structures.h>
 
 class LazWrapper
 {
diff --git a/core_hd_mapping/include/odo_with_gnss_fusion.h b/core_hd_mapping/include/CoreHDMapping/odo_with_gnss_fusion.h
similarity index 95%
rename from core_hd_mapping/include/odo_with_gnss_fusion.h
rename to core_hd_mapping/include/CoreHDMapping/odo_with_gnss_fusion.h
index f01520e8..4c2ea0d9 100644
--- a/core_hd_mapping/include/odo_with_gnss_fusion.h
+++ b/core_hd_mapping/include/CoreHDMapping/odo_with_gnss_fusion.h
@@ -9,7 +9,8 @@
 
 #include <Eigen/Eigen>
 
-#include <structures.h>
+// TODO(mwlasiuk) : cross includes ???
+#include <Core/structures.h>
 
 class OdoWithGnssFusion
 {
diff --git a/core_hd_mapping/include/project_settings.h b/core_hd_mapping/include/CoreHDMapping/project_settings.h
similarity index 93%
rename from core_hd_mapping/include/project_settings.h
rename to core_hd_mapping/include/CoreHDMapping/project_settings.h
index 54e100e6..78d0c8ba 100644
--- a/core_hd_mapping/include/project_settings.h
+++ b/core_hd_mapping/include/CoreHDMapping/project_settings.h
@@ -7,8 +7,10 @@
 
 #include <ImGuizmo.h>
 
-#include <odo_with_gnss_fusion.h>
-#include <structures.h>
+#include <CoreHDMapping/odo_with_gnss_fusion.h>
+
+// TODO(mwlasiuk) : cross includes ???
+#include <Core/structures.h>
 
 class ProjectSettings
 {
diff --git a/core_hd_mapping/include/roi_exporter.h b/core_hd_mapping/include/CoreHDMapping/roi_exporter.h
similarity index 88%
rename from core_hd_mapping/include/roi_exporter.h
rename to core_hd_mapping/include/CoreHDMapping/roi_exporter.h
index 51080f73..059fce22 100644
--- a/core_hd_mapping/include/roi_exporter.h
+++ b/core_hd_mapping/include/CoreHDMapping/roi_exporter.h
@@ -7,8 +7,10 @@
 
 #include <ImGuizmo.h>
 
-#include <project_settings.h>
-#include <structures.h>
+#include <CoreHDMapping/project_settings.h>
+
+// TODO(mwlasiuk) : cross includes ???
+#include <Core/structures.h>
 
 class RoiExporter
 {
diff --git a/core_hd_mapping/include/single_trajectory_viewer.h b/core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h
similarity index 92%
rename from core_hd_mapping/include/single_trajectory_viewer.h
rename to core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h
index d216a46e..c3684472 100644
--- a/core_hd_mapping/include/single_trajectory_viewer.h
+++ b/core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h
@@ -7,8 +7,9 @@
 
 #include <ImGuizmo.h>
 
-#include <odo_with_gnss_fusion.h>
-#include <structures.h>
+#include <CoreHDMapping/odo_with_gnss_fusion.h>
+
+#include <Core/structures.h>
 
 class SingleTrajectoryViewer
 {
diff --git a/core_hd_mapping/src/laz_wrapper.cpp b/core_hd_mapping/src/laz_wrapper.cpp
index cf7d65cd..684b4568 100644
--- a/core_hd_mapping/src/laz_wrapper.cpp
+++ b/core_hd_mapping/src/laz_wrapper.cpp
@@ -1,6 +1,6 @@
 #include <pch/pch.h>
 
-#include <laz_wrapper.h>
+#include <CoreHDMapping/laz_wrapper.h>
 
 #include <GL/freeglut.h>
 #include <laszip/laszip_api.h>
@@ -10,7 +10,7 @@
 #include <iostream>
 #include <thread>
 
-#include <pfd_wrapper.hpp>
+#include <Core/pfd_wrapper.hpp>
 
 #include <spdlog/spdlog.h>
 
diff --git a/core_hd_mapping/src/odo_with_gnss_fusion.cpp b/core_hd_mapping/src/odo_with_gnss_fusion.cpp
index 260a279d..92273459 100644
--- a/core_hd_mapping/src/odo_with_gnss_fusion.cpp
+++ b/core_hd_mapping/src/odo_with_gnss_fusion.cpp
@@ -1,14 +1,13 @@
 #include <pch/pch.h>
 
-#include <odo_with_gnss_fusion.h>
+#include <CoreHDMapping/odo_with_gnss_fusion.h>
 
 #include <GL/freeglut.h>
 #include <portable-file-dialogs.h>
 
-#include <m_estimators.h>
-#include <transformations.h>
-
-#include <pfd_wrapper.hpp>
+#include <Core/m_estimators.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
diff --git a/core_hd_mapping/src/project_settings.cpp b/core_hd_mapping/src/project_settings.cpp
index 70121caf..b4b3d94d 100644
--- a/core_hd_mapping/src/project_settings.cpp
+++ b/core_hd_mapping/src/project_settings.cpp
@@ -1,14 +1,14 @@
 #include <pch/pch.h>
 
-#include <project_settings.h>
+#include <CoreHDMapping/project_settings.h>
 
 #include <GL/freeglut.h>
 #include <nlohmann/json.hpp>
 #include <portable-file-dialogs.h>
 
-#include <m_estimators.h>
-#include <pfd_wrapper.hpp>
-#include <transformations.h>
+#include <Core/m_estimators.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
 
 #include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
 #include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
diff --git a/core_hd_mapping/src/roi_exporter.cpp b/core_hd_mapping/src/roi_exporter.cpp
index e9c24019..fd89a1da 100644
--- a/core_hd_mapping/src/roi_exporter.cpp
+++ b/core_hd_mapping/src/roi_exporter.cpp
@@ -1,15 +1,15 @@
 #include <pch/pch.h>
 
-#include <roi_exporter.h>
+#include <Core/pfd_wrapper.hpp>
+
+#include <CoreHDMapping/laz_wrapper.h>
+#include <CoreHDMapping/roi_exporter.h>
+#include <CoreHDMapping/single_trajectory_viewer.h>
 
 #include <GL/freeglut.h>
 #include <plycpp.h>
 #include <portable-file-dialogs.h>
 
-#include <laz_wrapper.h>
-#include <pfd_wrapper.hpp>
-#include <single_trajectory_viewer.h>
-
 std::vector<Point> decimate(const std::vector<Point>& pc)
 {
     std::vector<Point> pc_out;
diff --git a/core_hd_mapping/src/single_trajectory_viewer.cpp b/core_hd_mapping/src/single_trajectory_viewer.cpp
index 9468e6b3..d716ea91 100644
--- a/core_hd_mapping/src/single_trajectory_viewer.cpp
+++ b/core_hd_mapping/src/single_trajectory_viewer.cpp
@@ -1,10 +1,10 @@
-#include <single_trajectory_viewer.h>
-
 #include <pch/pch.h>
 
+#include <Core/pfd_wrapper.hpp>
+#include <Core/transformations.h>
+#include <CoreHDMapping/single_trajectory_viewer.h>
+
 #include <GL/freeglut.h>
-#include <pfd_wrapper.hpp>
-#include <transformations.h>
 
 namespace fs = std::filesystem;
 

From af03f205fa6c8f248647aa964e8aea02f97b54c8 Mon Sep 17 00:00:00 2001
From: mwlasiuk <michalwlasiuk@wp.pl>
Date: Sun, 1 Mar 2026 11:46:31 +0100
Subject: [PATCH 3/5] Fix pybind

---
 apps/manual_color/manual_color.cpp  |  1 +
 pybind/core_binding.cpp             |  7 ++++---
 pybind/lidar_odometry_binding.cpp   | 12 +++++++-----
 pybind/tls_registration_binding.cpp | 11 ++++++-----
 4 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp
index 3333f016..cc53f2ac 100644
--- a/apps/manual_color/manual_color.cpp
+++ b/apps/manual_color/manual_color.cpp
@@ -16,6 +16,7 @@
 #include <mutex>
 #include <sstream>
 #include <vector>
+#include <thread>
 
 #include <Eigen/Eigen>
 #include <nlohmann/json.hpp>
diff --git a/pybind/core_binding.cpp b/pybind/core_binding.cpp
index 72a4acaa..dbe5fdc2 100644
--- a/pybind/core_binding.cpp
+++ b/pybind/core_binding.cpp
@@ -1,10 +1,11 @@
-#include <export_laz.h>
 #include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 #include <pybind11/stl/filesystem.h>
-#include <session.h>
-#include <structures.h>
+
+#include <Core/export_laz.h>
+#include <Core/session.h>
+#include <Core/structures.h>
 
 namespace py = pybind11;
 
diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp
index 932b4b7b..f02e9ef2 100644
--- a/pybind/lidar_odometry_binding.cpp
+++ b/pybind/lidar_odometry_binding.cpp
@@ -1,12 +1,14 @@
-#include <export_laz.h>
-#include <lidar_odometry.h>
-#include <lidar_odometry_utils.h>
 #include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 #include <pybind11/stl/filesystem.h>
-#include <session.h>
-#include <structures.h>
+
+#include <lidar_odometry_utils.h>
+#include <lidar_odometry.h>
+
+#include <Core/export_laz.h>
+#include <Core/session.h>
+#include <Core/structures.h>
 
 namespace py = pybind11;
 
diff --git a/pybind/tls_registration_binding.cpp b/pybind/tls_registration_binding.cpp
index 531a8cc1..82799066 100644
--- a/pybind/tls_registration_binding.cpp
+++ b/pybind/tls_registration_binding.cpp
@@ -2,12 +2,13 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include <gnss.h>
-#include <icp.h>
+#include <Core/gnss.h>
+#include <Core/icp.h>
+#include <Core/pose_graph_loop_closure.h>
+#include <Core/pose_graph_slam.h>
+#include <Core/registration_plane_feature.h>
+
 #include <multi_view_tls_registration.h>
-#include <pose_graph_loop_closure.h>
-#include <pose_graph_slam.h>
-#include <registration_plane_feature.h>
 
 namespace py = pybind11;
 

From e8552527d3c9c7c5e03b09f0607a268c5c5829c9 Mon Sep 17 00:00:00 2001
From: mwlasiuk <michalwlasiuk@wp.pl>
Date: Sun, 1 Mar 2026 12:29:07 +0100
Subject: [PATCH 4/5] clang-format

---
 apps/manual_color/manual_color.cpp | 2 +-
 pybind/lidar_odometry_binding.cpp  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp
index cc53f2ac..69fddc53 100644
--- a/apps/manual_color/manual_color.cpp
+++ b/apps/manual_color/manual_color.cpp
@@ -15,8 +15,8 @@
 #include <iostream>
 #include <mutex>
 #include <sstream>
-#include <vector>
 #include <thread>
+#include <vector>
 
 #include <Eigen/Eigen>
 #include <nlohmann/json.hpp>
diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp
index f02e9ef2..fc1f581c 100644
--- a/pybind/lidar_odometry_binding.cpp
+++ b/pybind/lidar_odometry_binding.cpp
@@ -3,8 +3,8 @@
 #include <pybind11/stl.h>
 #include <pybind11/stl/filesystem.h>
 
-#include <lidar_odometry_utils.h>
 #include <lidar_odometry.h>
+#include <lidar_odometry_utils.h>
 
 #include <Core/export_laz.h>
 #include <Core/session.h>

From 2286752fff3c635bcaf83acb93d58bcdd9342ff3 Mon Sep 17 00:00:00 2001
From: mwlasiuk <michalwlasiuk@wp.pl>
Date: Sun, 1 Mar 2026 12:48:01 +0100
Subject: [PATCH 5/5] Adjust for Apple + Clang

---
 CMakeLists.txt                                |  6 ++++
 .../lidar_odometry_utils_optimizers.cpp       | 34 ++++++++++++++++---
 .../livox_mid_360_intrinsic_calibration.cpp   |  8 ++++-
 apps/manual_color/manual_color.cpp            |  7 +++-
 .../local_shape_features.cpp                  |  8 ++++-
 apps/precision_forestry_tools/surface.cpp     |  8 ++++-
 apps/quick_start_demo/quick_start_demo.cpp    |  6 +++-
 .../single_session_manual_coloring.cpp        |  2 ++
 .../src/pair_wise_iterative_closest_point.cpp | 16 +++++++--
 core/src/point_clouds.cpp                     |  2 ++
 10 files changed, 86 insertions(+), 11 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 44c930be..a77873c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -34,6 +34,12 @@ set(EXTERNAL_LIBRARIES_DIRECTORY ${REPOSITORY_DIRECTORY}/external)
 set(HD_CPU_OPTIMIZATION "AUTO" CACHE STRING "CPU optimization target (AUTO, INTEL, AMD, ARM, GENERIC)")
 set_property(CACHE HD_CPU_OPTIMIZATION PROPERTY STRINGS AUTO INTEL AMD ARM GENERIC)
 
+if(APPLE AND CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+    add_compile_definitions(USE_EXECUTION_PAR_UNSEQ=0)
+else()
+    add_compile_definitions(USE_EXECUTION_PAR_UNSEQ=1)
+endif()
+
 if (MSVC)
     add_definitions(-D_HAS_STD_BYTE=0)
     
diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
index bf76e85a..bedb70e3 100644
--- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
+++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
@@ -598,7 +598,13 @@ void optimize_sf(
     {
         // spdlog::info("start adding lidar observations");
         if (multithread)
-            std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun);
+            std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+                std::execution::par_unseq,
+#endif
+                std::begin(points_local),
+                std::end(points_local),
+                hessian_fun);
         else
             std::for_each(std::begin(points_local), std::end(points_local), hessian_fun);
         // spdlog::info("adding lidar observations finished");
@@ -915,7 +921,14 @@ void optimize_sf2(
     std::vector<Blocks> AtPAndtBlocksToSum(intermediate_points.size());
 
     if (useMultithread)
-        std::transform(std::execution::par_unseq, std::begin(indexes), std::end(indexes), std::begin(AtPAndtBlocksToSum), hessian_fun);
+        std::transform(
+#if USE_EXECUTION_PAR_UNSEQ
+            std::execution::par_unseq,
+#endif
+            std::begin(indexes),
+            std::end(indexes),
+            std::begin(AtPAndtBlocksToSum),
+            hessian_fun);
     else
         std::transform(std::begin(indexes), std::end(indexes), std::begin(AtPAndtBlocksToSum), hessian_fun);
 
@@ -1303,7 +1316,14 @@ void optimize_rigid_sf(
         std::vector<Blocks> blocks(indexes.size());
 
         if (useMultithread) // ToDo fix for this case
-            std::transform(std::execution::par_unseq, std::begin(indexes), std::end(indexes), std::begin(blocks), hessian_fun);
+            std::transform(
+#if USE_EXECUTION_PAR_UNSEQ
+                std::execution::par_unseq,
+#endif
+                std::begin(indexes),
+                std::end(indexes),
+                std::begin(blocks),
+                hessian_fun);
         else
             std::transform(std::begin(indexes), std::end(indexes), std::begin(blocks), hessian_fun);
 
@@ -3306,7 +3326,13 @@ void Consistency(std::vector<WorkerData>& worker_data, const LidarOdometryParams
             };
 
             if (multithread)
-                std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun);
+                std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+                    std::execution::par_unseq,
+#endif
+                    std::begin(points_local),
+                    std::end(points_local),
+                    hessian_fun);
             else
                 std::for_each(std::begin(points_local), std::end(points_local), hessian_fun);
 
diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
index eb86db29..c4bb4b09 100644
--- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
+++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
@@ -1526,7 +1526,13 @@ void calibrate_intrinsics()
     std::cout << "start adding lidar observations" << std::endl;
     if (multithread)
     {
-        std::for_each(std::execution::par_unseq, std::begin(point_cloud), std::end(point_cloud), hessian_fun);
+        std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+            std::execution::par_unseq,
+#endif
+            std::begin(point_cloud),
+            std::end(point_cloud),
+            hessian_fun);
     }
     else
     {
diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp
index 69fddc53..2c187642 100644
--- a/apps/manual_color/manual_color.cpp
+++ b/apps/manual_color/manual_color.cpp
@@ -1,4 +1,3 @@
-
 #include <GL/freeglut.h>
 #include <iostream>
 
@@ -370,7 +369,9 @@ std::vector<mandeye::PointRGB> ApplyColorToPointcloud(
 {
     std::vector<mandeye::PointRGB> newCloud(pointsRGB.size());
     std::transform(
+#if USE_EXECUTION_PAR_UNSEQ
         std::execution::par_unseq,
+#endif
         pointsRGB.begin(),
         pointsRGB.end(),
         newCloud.begin(),
@@ -418,7 +419,9 @@ std::vector<mandeye::PointRGB> ApplyColorToPointcloudFishEye(
 {
     std::vector<mandeye::PointRGB> newCloud(pointsRGB.size());
     std::transform(
+#if USE_EXECUTION_PAR_UNSEQ
         std::execution::par_unseq,
+#endif
         pointsRGB.begin(),
         pointsRGB.end(),
         newCloud.begin(),
@@ -1359,7 +1362,9 @@ void mouse(int glut_button, int state, int x, int y)
                 std::pair<double, int> distanceIndexPair{ std::numeric_limits<double>::max(), -1 };
 
                 std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
                     std::execution::par_unseq,
+#endif
                     SystemData::points.begin(),
                     SystemData::points.end(),
                     [&](const mandeye::PointRGB& p)
diff --git a/apps/precision_forestry_tools/local_shape_features.cpp b/apps/precision_forestry_tools/local_shape_features.cpp
index 9befe0b4..4e430fd7 100644
--- a/apps/precision_forestry_tools/local_shape_features.cpp
+++ b/apps/precision_forestry_tools/local_shape_features.cpp
@@ -131,7 +131,13 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector<PointWithLoc
 
     if (params.multithread)
     {
-        std::for_each(std::execution::par_unseq, std::begin(indexes), std::end(indexes), hessian_fun);
+        std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+            std::execution::par_unseq,
+#endif
+            std::begin(indexes),
+            std::end(indexes),
+            hessian_fun);
     }
     else
     {
diff --git a/apps/precision_forestry_tools/surface.cpp b/apps/precision_forestry_tools/surface.cpp
index 1caebdab..a7ad54c8 100644
--- a/apps/precision_forestry_tools/surface.cpp
+++ b/apps/precision_forestry_tools/surface.cpp
@@ -536,7 +536,13 @@ std::vector<int> Surface::get_filtered_indexes(
 
     if (multithread)
     {
-        std::for_each(std::execution::par_unseq, std::begin(indexes), std::end(indexes), hessian_fun);
+        std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+            std::execution::par_unseq,
+#endif
+            std::begin(indexes),
+            std::end(indexes),
+            hessian_fun);
     }
     else
     {
diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp
index 0ab13b2f..0caa445d 100644
--- a/apps/quick_start_demo/quick_start_demo.cpp
+++ b/apps/quick_start_demo/quick_start_demo.cpp
@@ -853,7 +853,11 @@ int main(int argc, char *argv[])
         std::mutex mtx;
         std::cout << "start std::transform" << std::endl;
 
-        std::transform(std::execution::par_unseq, std::begin(laz_files), std::end(laz_files), std::begin(pointsPerFile), [&](const std::string &fn)
+        std::transform(
+            #if USE_EXECUTION_PAR_UNSEQ
+            std::execution::par_unseq, 
+            #endif
+            std::begin(laz_files), std::end(laz_files), std::begin(pointsPerFile), [&](const std::string &fn)
                        {
                            // Load mapping from id to sn
                            fs::path fnSn(fn);
diff --git a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp
index f68302a2..444317a9 100644
--- a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp
+++ b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp
@@ -174,7 +174,9 @@ std::vector<mandeye::PointRGB> ApplyColorToPointcloud(
 {
     std::vector<mandeye::PointRGB> newCloud(pointsRGB.size());
     std::transform(
+#if USE_EXECUTION_PAR_UNSEQ
         std::execution::par_unseq,
+#endif
         pointsRGB.begin(),
         pointsRGB.end(),
         newCloud.begin(),
diff --git a/core/src/pair_wise_iterative_closest_point.cpp b/core/src/pair_wise_iterative_closest_point.cpp
index a57ba808..9c1371da 100644
--- a/core/src/pair_wise_iterative_closest_point.cpp
+++ b/core/src/pair_wise_iterative_closest_point.cpp
@@ -296,7 +296,13 @@ bool PairWiseICP::compute(
 
         if (multithread)
         {
-            std::for_each(std::execution::par_unseq, std::begin(source), std::end(source), hessian_fun);
+            std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+                std::execution::par_unseq,
+#endif
+                std::begin(source),
+                std::end(source),
+                hessian_fun);
         }
         else
         {
@@ -503,7 +509,13 @@ bool PairWiseICP::compute_fast(
 
         if (multithread)
         {
-            std::for_each(std::execution::par_unseq, std::begin(source), std::end(source), hessian_fun);
+            std::for_each(
+#if USE_EXECUTION_PAR_UNSEQ
+                std::execution::par_unseq,
+#endif
+                std::begin(source),
+                std::end(source),
+                hessian_fun);
         }
         else
         {
diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp
index 03d15299..8841d6c6 100644
--- a/core/src/point_clouds.cpp
+++ b/core/src/point_clouds.cpp
@@ -1259,7 +1259,9 @@ bool PointClouds::load_whu_tls(
     point_clouds.resize(point_clouds_nodata.size());
 
     std::transform(
+#if USE_EXECUTION_PAR_UNSEQ
         std::execution::par_unseq,
+#endif
         std::begin(point_clouds_nodata),
         std::end(point_clouds_nodata),
         std::begin(point_clouds),