diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..46e553e
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,8 @@
+# VS Code stuff 
+/.vscode/**
+**/__pycache__/
+
+# ROS 2 build files
+build/
+install/
+log/
diff --git a/navmap_core/include/navmap_core/Geometry.hpp b/navmap_core/include/navmap_core/Geometry.hpp
index 8b63286..c251d03 100644
--- a/navmap_core/include/navmap_core/Geometry.hpp
+++ b/navmap_core/include/navmap_core/Geometry.hpp
@@ -34,7 +34,6 @@
 #include <limits>
 #include <cmath>
 #include <algorithm>
-#include <array>
 
 namespace navmap
 {
diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp
index 6ad0a41..b380c72 100644
--- a/navmap_core/include/navmap_core/NavMap.hpp
+++ b/navmap_core/include/navmap_core/NavMap.hpp
@@ -45,10 +45,8 @@
 #include <string>
 #include <array>
 #include <limits>
-#include <stack>
 #include <cmath>
 #include <cstdint>
-#include <span>
 #include <type_traits>
 #include <deque>
 #include <algorithm>
diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp
index 0eb0380..3131485 100644
--- a/navmap_core/src/navmap_core/NavMap.cpp
+++ b/navmap_core/src/navmap_core/NavMap.cpp
@@ -15,6 +15,7 @@
 
 
 #include "navmap_core/NavMap.hpp"
+#include <stack>
 #include <algorithm>
 #include <functional>
 #include <cmath>
diff --git a/navmap_core/tests/test_geometry.cpp b/navmap_core/tests/test_geometry.cpp
index 2e63771..492a435 100644
--- a/navmap_core/tests/test_geometry.cpp
+++ b/navmap_core/tests/test_geometry.cpp
@@ -15,7 +15,6 @@
 
 #include <gtest/gtest.h>
 #include <Eigen/Core>
-#include <limits>
 #include "navmap_core/Geometry.hpp"
 
 using namespace navmap;
diff --git a/navmap_core/tests/test_navmap_uniform_and_closest.cpp b/navmap_core/tests/test_navmap_uniform_and_closest.cpp
index 446a3cc..9e49c29 100644
--- a/navmap_core/tests/test_navmap_uniform_and_closest.cpp
+++ b/navmap_core/tests/test_navmap_uniform_and_closest.cpp
@@ -15,7 +15,6 @@
 
 #include <gtest/gtest.h>
 #include <Eigen/Core>
-#include <limits>
 #include "navmap_core/NavMap.hpp"
 
 using namespace navmap;
diff --git a/navmap_examples/src/01_flat_plane.cpp b/navmap_examples/src/01_flat_plane.cpp
index 130bba3..326d8a0 100644
--- a/navmap_examples/src/01_flat_plane.cpp
+++ b/navmap_examples/src/01_flat_plane.cpp
@@ -17,16 +17,12 @@
 #include <iostream>
 #include <vector>
 #include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
 using navmap::LayerType;
 using Eigen::Vector3f;
 using std::cout; using std::cerr; using std::endl;
diff --git a/navmap_examples/src/02_two_floors.cpp b/navmap_examples/src/02_two_floors.cpp
index e3ccd1b..ac89f44 100644
--- a/navmap_examples/src/02_two_floors.cpp
+++ b/navmap_examples/src/02_two_floors.cpp
@@ -16,20 +16,14 @@
 
 #include <iostream>
 #include <vector>
-#include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
+using std::cout; using std::endl;
 
 // 02_two_floors: two stacked floors, locate & closest_navcel
 static void make_two_floors(NavMap & nm, float z0, float z1)
diff --git a/navmap_examples/src/03_slope_surface.cpp b/navmap_examples/src/03_slope_surface.cpp
index c4cbcb6..f479cb9 100644
--- a/navmap_examples/src/03_slope_surface.cpp
+++ b/navmap_examples/src/03_slope_surface.cpp
@@ -16,20 +16,14 @@
 
 #include <iostream>
 #include <vector>
-#include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
+using std::cout; using std::endl;
 
 // 03_slope_surface: sloped z, sample_layer_at
 int main()
diff --git a/navmap_examples/src/04_layers.cpp b/navmap_examples/src/04_layers.cpp
index 5edfffe..efb2617 100644
--- a/navmap_examples/src/04_layers.cpp
+++ b/navmap_examples/src/04_layers.cpp
@@ -15,21 +15,15 @@
 
 
 #include <iostream>
-#include <vector>
 #include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
+using std::cout; using std::endl;
 
 // 04_layers: add/list/set/get
 int main()
diff --git a/navmap_examples/src/05_neighbors_and_centroids.cpp b/navmap_examples/src/05_neighbors_and_centroids.cpp
index 7f11065..954c595 100644
--- a/navmap_examples/src/05_neighbors_and_centroids.cpp
+++ b/navmap_examples/src/05_neighbors_and_centroids.cpp
@@ -16,20 +16,14 @@
 
 #include <iostream>
 #include <vector>
-#include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
+using std::cout; using std::endl;
 
 // 05_neighbors_and_centroids
 static void make_flat_square(NavMap & nm)
diff --git a/navmap_examples/src/06_area_marking.cpp b/navmap_examples/src/06_area_marking.cpp
index fdda86c..603bc95 100644
--- a/navmap_examples/src/06_area_marking.cpp
+++ b/navmap_examples/src/06_area_marking.cpp
@@ -15,21 +15,15 @@
 
 
 #include <iostream>
-#include <vector>
 #include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
+using std::cout; using std::endl;
 
 // 06_area_marking: set_area CIRCULAR y RECTANGULAR sobre una malla 1x1 de 2 tris
 #include <cmath>
diff --git a/navmap_examples/src/07_raycast.cpp b/navmap_examples/src/07_raycast.cpp
index ce97653..4ff7d8d 100644
--- a/navmap_examples/src/07_raycast.cpp
+++ b/navmap_examples/src/07_raycast.cpp
@@ -16,20 +16,13 @@
 
 #include <iostream>
 #include <vector>
-#include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
 
 // 07_raycast: simple y batch (raycast_many)
 int main()
diff --git a/navmap_examples/src/08_copy_and_assign.cpp b/navmap_examples/src/08_copy_and_assign.cpp
index 0b84de4..a11affb 100644
--- a/navmap_examples/src/08_copy_and_assign.cpp
+++ b/navmap_examples/src/08_copy_and_assign.cpp
@@ -17,19 +17,13 @@
 #include <iostream>
 #include <vector>
 #include <cstdint>
-#include <limits>
-#include <algorithm>
 #include <Eigen/Core>
 
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavMap;
 using navmap::NavCelId;
-using navmap::Surface;
-using navmap::LayerView;
-using navmap::LayerType;
 using Eigen::Vector3f;
-using std::cout; using std::cerr; using std::endl;
 
 // 08_copy_and_assign: muestra operator= optimizado (igual geometría) y completo (distinta)
 static void fill_one_tri_map(navmap::NavMap & m)
diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp
index a7e9f49..1d5ef88 100644
--- a/navmap_ros/include/navmap_ros/conversions.hpp
+++ b/navmap_ros/include/navmap_ros/conversions.hpp
@@ -37,8 +37,6 @@
  */
 
 #include <string>
-#include <memory>
-#include <vector>
 #include <Eigen/Core>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
diff --git a/navmap_ros/include/navmap_ros/navmap_io.hpp b/navmap_ros/include/navmap_ros/navmap_io.hpp
index a2b8d60..1500b28 100644
--- a/navmap_ros/include/navmap_ros/navmap_io.hpp
+++ b/navmap_ros/include/navmap_ros/navmap_io.hpp
@@ -41,7 +41,6 @@
 #include <system_error>
 
 #include "navmap_core/NavMap.hpp"
-#include "navmap_ros/conversions.hpp"
 
 #include "navmap_ros_interfaces/msg/nav_map.hpp"
 
diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp
index 65a3eb1..276e1dd 100644
--- a/navmap_ros/src/navmap_ros/conversions.cpp
+++ b/navmap_ros/src/navmap_ros/conversions.cpp
@@ -25,21 +25,17 @@
 #include <numeric>
 
 #include "geometry_msgs/msg/pose.hpp"
-#include <std_msgs/msg/header.hpp>
+#include "std_msgs/msg/header.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "navmap_ros_interfaces/msg/nav_map.hpp"
 #include "navmap_ros_interfaces/msg/nav_map_layer.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 
-#include "navmap_core/Geometry.hpp"
-
 #include "pcl_conversions/pcl_conversions.h"
-#include "pcl/point_types_conversion.h"
+#include "pcl/common/point_tests.h"
 
-#include "pcl/common/transforms.h"
 #include "pcl/point_cloud.h"
 #include "pcl/point_types.h"
-#include "pcl/PointIndices.h"
 #include "pcl/kdtree/kdtree_flann.h"
 
 namespace navmap_ros
diff --git a/navmap_ros/tests/test_conversions.cpp b/navmap_ros/tests/test_conversions.cpp
index 54bc3f0..8d0d021 100644
--- a/navmap_ros/tests/test_conversions.cpp
+++ b/navmap_ros/tests/test_conversions.cpp
@@ -17,7 +17,6 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
 #include "navmap_ros_interfaces/msg/nav_map_layer.hpp"
-#include "navmap_ros_interfaces/msg/nav_map.hpp"
 
 #include "navmap_ros/conversions.hpp"
 #include "navmap_core/NavMap.hpp"
diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp
index 07a6209..c6674cb 100644
--- a/navmap_ros/tests/test_navmap_io.cpp
+++ b/navmap_ros/tests/test_navmap_io.cpp
@@ -18,7 +18,6 @@
 #include <rclcpp/serialized_message.hpp>
 
 #include <filesystem>
-#include <fstream>
 #include <vector>
 #include <string>
 #include <unistd.h>
@@ -92,7 +91,7 @@ static void ExpectNavMapMsgEqualSemantic(
   const navmap_ros_interfaces::msg::NavMap & A,
   const navmap_ros_interfaces::msg::NavMap & B)
 {
-  // Header: frame must match; stamp puede variar → lo ignoramos
+  // Header: frame must match; stamp may change -> we ignore it
   EXPECT_EQ(A.header.frame_id, B.header.frame_id);
 
   // Geometry
@@ -306,7 +305,7 @@ ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.messag
 auto msg_from_core = navmap_ros::to_msg(core);
 auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded);
 
-// Comparación semántica (tolerante a orden y FP)
+// Semantic comparison (order and FP tolerant)
 ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded);
 
 std::filesystem::remove(path);
diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp
index 4ef3f86..1e62d8f 100644
--- a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp
+++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp
@@ -18,10 +18,8 @@
 #define NAVMAP_RVIZ_PLUGIN__NAVMAP_DISPLAY_HPP_
 
 #include <cstdint>
-#include <memory>
 #include <string>
 #include <unordered_map>
-#include <vector>
 
 #include <QObject>
 
diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp
index 7c00499..7a4562d 100644
--- a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp
+++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp
@@ -20,8 +20,7 @@
 #include <QObject>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 #include "navmap_rviz_plugin/navmap_pose_tool.hpp"
 #include "rviz_default_plugins/visibility_control.hpp"
diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp
index 9b20898..0cdae6a 100644
--- a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp
+++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp
@@ -32,9 +32,6 @@
 #include "rviz_rendering/viewport_projection_finder.hpp"
 #include "rviz_default_plugins/visibility_control.hpp"
 
-#include "navmap_rviz_plugin/NavMapDisplay.hpp"
-
-
 namespace rviz_rendering
 {
 class Arrow;
diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp
index a64d61f..ad64a76 100644
--- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp
+++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp
@@ -14,39 +14,22 @@
 // limitations under the License.
 
 
-#include "navmap_rviz_plugin/NavMapDisplay.hpp"
+#include <cmath>
 
 #include <OgreSceneManager.h>
 #include <OgreSceneNode.h>
-#include <OgreManualObject.h>
 #include <OgreEntity.h>
 #include <OgreMesh.h>
 #include <OgreMeshManager.h>
-#include <OgreHardwareVertexBuffer.h>
-#include <OgreHardwareBufferManager.h>
 #include <OgreMaterialManager.h>
 #include <OgreSubMesh.h>
-#include <OgreMaterial.h>
 #include <OgreTechnique.h>
 #include <OgrePass.h>
-#include <OgreVertexIndexData.h>
-#include <OgreRoot.h>
-
-#include <rclcpp/exceptions.hpp>
-#include <rviz_common/display_context.hpp>
-#include <rviz_common/frame_manager_iface.hpp>
-#include <rviz_common/logging.hpp>
-#include <rviz_common/uniform_string_stream.hpp>
-#include <rviz_common/validate_floats.hpp>
 
 #include "navmap_core/NavMap.hpp"
 #include "navmap_ros/conversions.hpp"
 
-
-#include <QCoreApplication>
-#include <cmath>
-#include <sstream>
-
+#include "navmap_rviz_plugin/NavMapDisplay.hpp"
 namespace
 {
 
diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp
index 887a433..1d1aa85 100644
--- a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp
+++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp
@@ -21,7 +21,6 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 #include "rviz_common/display_context.hpp"
-#include "rviz_common/logging.hpp"
 #include "rviz_common/properties/string_property.hpp"
 #include "rviz_common/properties/qos_profile_property.hpp"
 
diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp
index 4897f89..398f5bd 100644
--- a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp
+++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp
@@ -33,11 +33,15 @@
 #include "rviz_rendering/render_window.hpp"
 
 #include "rviz_common/logging.hpp"
+#include "rviz_common/display_context.hpp"
 #include "rviz_common/render_panel.hpp"
 #include "rviz_common/viewport_mouse_event.hpp"
 #include "rviz_common/view_manager.hpp"
 #include "rviz_common/view_controller.hpp"
 
+#include "navmap_core/NavMap.hpp"
+#include "navmap_rviz_plugin/NavMapDisplay.hpp"
+
 namespace navmap_rviz_plugin
 {