From 8eb5fadc9fe180a8b6b54cac26c39df106ac693b Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Mon, 30 Mar 2026 19:11:11 +0200 Subject: [PATCH 1/2] Add missing qualifiers and move GL includes to cpp --- ..._data_and_drop_here-precision_forestry.cpp | 2 - .../lidar_odometry_gui.cpp | 2 - .../lidar_odometry_utils_optimizers.cpp | 3 +- .../mandeye_mission_recorder_calibration.cpp | 2 - .../multi_session_registration.cpp | 2 - core/include/Core/control_points.h | 23 +---- core/include/Core/ground_control_points.h | 16 +-- core/include/{ => Core}/imu_preintegration.h | 0 .../Core/manual_pose_graph_loop_closure.h | 5 - core/include/Core/observation_picking.h | 95 +----------------- core/include/Core/point_cloud.h | 7 -- .../include/Core/registration_plane_feature.h | 24 +++-- core/include/Core/structures.h | 61 +++--------- core/src/control_points.cpp | 5 +- core/src/ground_control_points.cpp | 4 +- core/src/imu_preintegration.cpp | 2 +- core/src/manual_pose_graph_loop_closure.cpp | 3 + core/src/observation_picking.cpp | 98 ++++++++++++++++++- core/src/point_cloud.cpp | 4 + core/src/point_clouds.cpp | 4 + .../CoreHDMapping/single_trajectory_viewer.h | 2 +- core_hd_mapping/src/project_settings.cpp | 31 ------ core_hd_mapping/src/roi_exporter.cpp | 86 +++++++--------- .../src/single_trajectory_viewer.cpp | 26 +++-- 24 files changed, 201 insertions(+), 306 deletions(-) rename core/include/{ => Core}/imu_preintegration.h (100%) 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 5b92a803..52b7605c 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 @@ -5,8 +5,6 @@ #include -// #define GLEW_STATIC -// #include #include #include diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 961978dc..010ab289 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -5,8 +5,6 @@ #include -// #define GLEW_STATIC -// #include #include #include 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 38132122..a4965ae1 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1,6 +1,5 @@ #include "lidar_odometry_utils.h" #include -#include #include #include #include @@ -9,6 +8,8 @@ #include #include +#include + const double DEG_TO_RAD = M_PI / 180.0f; const double RAD_TO_DEG = 180.0f / M_PI; 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 e9e05e6f..7fd7085a 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -7,8 +7,6 @@ #include -// #define GLEW_STATIC -// #include #include #include diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index ace798f7..56493b6f 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -8,8 +8,6 @@ #include -// #define GLEW_STATIC -// #include #include #include #include diff --git a/core/include/Core/control_points.h b/core/include/Core/control_points.h index c674344c..8713be6f 100644 --- a/core/include/Core/control_points.h +++ b/core/include/Core/control_points.h @@ -4,9 +4,6 @@ #include #include -#if WITH_GUI == 1 -#include -#endif struct ControlPoint { @@ -27,32 +24,22 @@ struct ControlPoint class ControlPoints { public: - ControlPoints() - { - ; - }; - ~ControlPoints() - { - ; - }; + ControlPoints() = default; + ~ControlPoints() = default; bool is_imgui = false; std::vector cps; + #if WITH_GUI == 1 bool picking_mode = false; - // int picking_mode_index_to_node_inner = -1; - // int picking_mode_index_to_node_outer = -1; bool draw_uncertainty = false; int index_picked_point = -1; bool track_pose_with_camera = true; int index_pose = 0; - // bool found_picked = false; - // ControlPoint picked_control_point; - - void imgui(PointClouds& point_clouds_container, Eigen::Vector3f& rotation_center); + void imgui(PointClouds& point_clouds_container, const Eigen::Vector3f& rotation_center); void render(const PointClouds& point_clouds_container, bool show_pc); - void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd = 1); + void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd = 1); #endif }; diff --git a/core/include/Core/ground_control_points.h b/core/include/Core/ground_control_points.h index ed23ef71..4eb73a0a 100644 --- a/core/include/Core/ground_control_points.h +++ b/core/include/Core/ground_control_points.h @@ -5,10 +5,6 @@ #include #include -#if WITH_GUI == 1 -#include -#endif - struct GroundControlPoint { char name[64]; @@ -26,14 +22,8 @@ struct GroundControlPoint class GroundControlPoints { public: - GroundControlPoints() - { - ; - }; - ~GroundControlPoints() - { - ; - }; + GroundControlPoints() = default; + ~GroundControlPoints() = default; std::vector gpcs; double default_lidar_height_above_ground = 0.15; @@ -46,6 +36,6 @@ class GroundControlPoints void imgui(PointClouds& point_clouds_container); void render(const PointClouds& point_clouds_container); - void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd = 1); + void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd = 1); #endif }; diff --git a/core/include/imu_preintegration.h b/core/include/Core/imu_preintegration.h similarity index 100% rename from core/include/imu_preintegration.h rename to core/include/Core/imu_preintegration.h diff --git a/core/include/Core/manual_pose_graph_loop_closure.h b/core/include/Core/manual_pose_graph_loop_closure.h index 295360fc..000b93a8 100644 --- a/core/include/Core/manual_pose_graph_loop_closure.h +++ b/core/include/Core/manual_pose_graph_loop_closure.h @@ -1,11 +1,6 @@ #pragma once #if WITH_GUI == 1 - -#include - -#include - #include #include #include diff --git a/core/include/Core/observation_picking.h b/core/include/Core/observation_picking.h index cefb7b09..0f9f45b4 100644 --- a/core/include/Core/observation_picking.h +++ b/core/include/Core/observation_picking.h @@ -4,8 +4,6 @@ #include #include -#include -#include #include #include @@ -31,96 +29,7 @@ class Intersection float width_length_height[3]; std::vector points; - void render() - { - TaitBryanPose pose; - pose.px = translation[0]; - pose.py = translation[1]; - pose.pz = translation[2]; - pose.om = rotation[0]; - pose.fi = rotation[1]; - pose.ka = rotation[2]; - Eigen::Affine3d m_pose = affine_matrix_from_pose_tait_bryan(pose); - - float& x_length = width_length_height[0]; - float& y_width = width_length_height[1]; - float& z_height = width_length_height[2]; - - glColor3f(color[0], color[1], color[2]); - - Eigen::Vector3d v000(-x_length * 0.5, -y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v100(x_length * 0.5, -y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v110(x_length * 0.5, y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v010(-x_length * 0.5, y_width * 0.5, -z_height * 0.5); - - Eigen::Vector3d v001(-x_length * 0.5, -y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v101(x_length * 0.5, -y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v111(x_length * 0.5, y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v011(-x_length * 0.5, y_width * 0.5, z_height * 0.5); - - Eigen::Vector3d v000t = m_pose * v000; - Eigen::Vector3d v100t = m_pose * v100; - Eigen::Vector3d v110t = m_pose * v110; - Eigen::Vector3d v010t = m_pose * v010; - - Eigen::Vector3d v001t = m_pose * v001; - Eigen::Vector3d v101t = m_pose * v101; - Eigen::Vector3d v111t = m_pose * v111; - Eigen::Vector3d v011t = m_pose * v011; - - glBegin(GL_LINES); - - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v100t.x(), v100t.y(), v100t.z()); - - glVertex3d(v100t.x(), v100t.y(), v100t.z()); - glVertex3d(v110t.x(), v110t.y(), v110t.z()); - - glVertex3d(v110t.x(), v110t.y(), v110t.z()); - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - - glVertex3d(v001t.x(), v001t.y(), v001t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); - - glVertex3d(v101t.x(), v101t.y(), v101t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); - - glVertex3d(v111t.x(), v111t.y(), v111t.z()); - glVertex3d(v011t.x(), v011t.y(), v011t.z()); - - glVertex3d(v011t.x(), v011t.y(), v011t.z()); - glVertex3d(v001t.x(), v001t.y(), v001t.z()); - - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v001t.x(), v001t.y(), v001t.z()); - - glVertex3d(v100t.x(), v100t.y(), v100t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); - - glVertex3d(v110t.x(), v110t.y(), v110t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); - - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v011t.x(), v011t.y(), v011t.z()); - - // - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v110t.x(), v110t.y(), v110t.z()); - - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v100t.x(), v100t.y(), v100t.z()); - - glVertex3d(v001t.x(), v001t.y(), v001t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); - - glVertex3d(v011t.x(), v011t.y(), v011t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); - - glEnd(); - } + void render(); }; class ObservationPicking @@ -144,7 +53,7 @@ class ObservationPicking void accept_current_observation(const std::vector& m_poses); void import_observations(const std::string& filename); void export_observation(const std::string& filename); - void add_intersection(Eigen::Vector3d translation); + void add_intersection(const Eigen::Vector3d& translation); std::map current_observation; std::vector> observations; diff --git a/core/include/Core/point_cloud.h b/core/include/Core/point_cloud.h index c76f5adc..e3e9846c 100644 --- a/core/include/Core/point_cloud.h +++ b/core/include/Core/point_cloud.h @@ -9,7 +9,6 @@ #if WITH_GUI == 1 #include -#include #endif class PointCloud @@ -148,12 +147,6 @@ class PointCloud void decimate(double bucket_x, double bucket_y, double bucket_z); void shift_to_center(); #if WITH_GUI == 1 - // void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, - // bool xz_intersection, bool yz_intersection, bool xy_intersection, - // bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, - // bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, - // bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, - // double intersection_width); void render( bool show_with_initial_pose, const ObservationPicking& observation_picking, diff --git a/core/include/Core/registration_plane_feature.h b/core/include/Core/registration_plane_feature.h index f76ffa60..b5aa80e4 100644 --- a/core/include/Core/registration_plane_feature.h +++ b/core/include/Core/registration_plane_feature.h @@ -24,21 +24,25 @@ class RegistrationPlaneFeature struct Plane { + double a = {}; + double b = {}; + double c = {}; + double d = {}; + Plane() + : a(0.0) + , b(0.0) + , c(0.0) + , d(0.0) { - a = b = c = d = 0.0; } - double a; - double b; - double c; - double d; - Plane(Eigen::Vector3d p, Eigen::Vector3d nv) + Plane(const Eigen::Vector3d& p, const Eigen::Vector3d& nv) + : a(nv.x()) + , b(nv.y()) + , c(nv.z()) + , d(-a * p.x() - b * p.y() - c * p.z()) { - a = nv.x(); - b = nv.y(); - c = nv.z(); - d = -a * p.x() - b * p.y() - c * p.z(); } }; diff --git a/core/include/Core/structures.h b/core/include/Core/structures.h index af764cc4..d7a3388c 100644 --- a/core/include/Core/structures.h +++ b/core/include/Core/structures.h @@ -172,43 +172,6 @@ bool save_vector_data(const std::string& file_name, const std::vector& out) } } -/*template -inline bool load_vector_data(const std::string& file_name, std::vector& vector_data) { - std::basic_ifstream vd_str(file_name, std::ios::binary); - if (!vd_str.good()) { - return false; - } - std::vector data((std::istreambuf_iterator(vd_str)), std::istreambuf_iterator()); - std::vector v(data.size() / sizeof(T)); - memcpy(v.data(), data.data(), data.size()); - vector_data = v; - return true; -}*/ - -/*template -bool load_vector_data(const std::string& file_name, std::vector& out) -{ - //static_assert(std::is_trivially_copyable_v, - // "T must be trivially copyable"); - - std::ifstream file(file_name, std::ios::binary | std::ios::ate); - if (!file) - return false; - - const std::streamsize size = file.tellg(); - if (size < 0 || size % sizeof(T) != 0) - return false; - - const size_t count = size / sizeof(T); - out.resize(count); - - file.seekg(0, std::ios::beg); - if (!file.read(reinterpret_cast(out.data()), size)) - return false; - - return true; -}*/ - template bool load_vector_data(const std::string& file_name, std::vector& out) { @@ -264,21 +227,25 @@ struct LaserBeam struct Plane { + double a = {}; + double b = {}; + double c = {}; + double d = {}; + Plane() + : a(0.0) + , b(0.0) + , c(0.0) + , d(0.0) { - a = b = c = d = 0.0; } - double a; - double b; - double c; - double d; - Plane(Eigen::Vector3d p, Eigen::Vector3d nv) + Plane(const Eigen::Vector3d& p, const Eigen::Vector3d& nv) + : a(nv.x()) + , b(nv.y()) + , c(nv.z()) + , d(-a * p.x() - b * p.y() - c * p.z()) { - a = nv.x(); - b = nv.y(); - c = nv.z(); - d = -a * p.x() - b * p.y() - c * p.z(); } }; diff --git a/core/src/control_points.cpp b/core/src/control_points.cpp index eef1a18c..77715c2a 100644 --- a/core/src/control_points.cpp +++ b/core/src/control_points.cpp @@ -5,6 +5,7 @@ #include #if WITH_GUI == 1 +#include #include #include #include @@ -15,7 +16,7 @@ static constexpr const char* xText = "Longitudinal (forward/backward)"; static constexpr const char* yText = "Lateral (left/right)"; static constexpr const char* zText = "Vertical (up/down)"; -void ControlPoints::imgui(PointClouds& point_clouds_container, Eigen::Vector3f& rotation_center) +void ControlPoints::imgui(PointClouds& point_clouds_container, const Eigen::Vector3f& rotation_center) { if (ImGui::Begin("Control Point", &is_imgui)) { @@ -463,7 +464,7 @@ void ControlPoints::render(const PointClouds& point_clouds_container, bool show_ return; } -void ControlPoints::draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd) +void ControlPoints::draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd) { Eigen::LLT> cholSolver(covar); Eigen::Matrix3d transform = cholSolver.matrixL(); diff --git a/core/src/ground_control_points.cpp b/core/src/ground_control_points.cpp index 2e530c4a..face93d4 100644 --- a/core/src/ground_control_points.cpp +++ b/core/src/ground_control_points.cpp @@ -5,6 +5,8 @@ #include #if WITH_GUI == 1 +#include + #include #include #include @@ -348,7 +350,7 @@ void GroundControlPoints::render(const PointClouds& point_clouds_container) return; } -void GroundControlPoints::draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd) +void GroundControlPoints::draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd) { Eigen::LLT> cholSolver(covar); Eigen::Matrix3d transform = cholSolver.matrixL(); diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index 60f6f697..24901071 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/core/src/manual_pose_graph_loop_closure.cpp b/core/src/manual_pose_graph_loop_closure.cpp index e22174be..9c8555ef 100644 --- a/core/src/manual_pose_graph_loop_closure.cpp +++ b/core/src/manual_pose_graph_loop_closure.cpp @@ -1,6 +1,9 @@ #include #if WITH_GUI == 1 +#include +#include + #include #include #include diff --git a/core/src/observation_picking.cpp b/core/src/observation_picking.cpp index d59550b0..8fce74fa 100644 --- a/core/src/observation_picking.cpp +++ b/core/src/observation_picking.cpp @@ -3,12 +3,100 @@ #include #include +#include + #include -// void renderBitmapString(float x, float y, float z, char* string) { -// glRasterPos3f(x, y, z); -// glutBitmapCharacter(font, GLUT_BITMAP_TIMES_ROMAN_24); -// } +void Intersection::render() +{ + TaitBryanPose pose; + pose.px = translation[0]; + pose.py = translation[1]; + pose.pz = translation[2]; + pose.om = rotation[0]; + pose.fi = rotation[1]; + pose.ka = rotation[2]; + Eigen::Affine3d m_pose = affine_matrix_from_pose_tait_bryan(pose); + + float& x_length = width_length_height[0]; + float& y_width = width_length_height[1]; + float& z_height = width_length_height[2]; + + glColor3f(color[0], color[1], color[2]); + + Eigen::Vector3d v000(-x_length * 0.5, -y_width * 0.5, -z_height * 0.5); + Eigen::Vector3d v100(x_length * 0.5, -y_width * 0.5, -z_height * 0.5); + Eigen::Vector3d v110(x_length * 0.5, y_width * 0.5, -z_height * 0.5); + Eigen::Vector3d v010(-x_length * 0.5, y_width * 0.5, -z_height * 0.5); + + Eigen::Vector3d v001(-x_length * 0.5, -y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v101(x_length * 0.5, -y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v111(x_length * 0.5, y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v011(-x_length * 0.5, y_width * 0.5, z_height * 0.5); + + Eigen::Vector3d v000t = m_pose * v000; + Eigen::Vector3d v100t = m_pose * v100; + Eigen::Vector3d v110t = m_pose * v110; + Eigen::Vector3d v010t = m_pose * v010; + + Eigen::Vector3d v001t = m_pose * v001; + Eigen::Vector3d v101t = m_pose * v101; + Eigen::Vector3d v111t = m_pose * v111; + Eigen::Vector3d v011t = m_pose * v011; + + glBegin(GL_LINES); + + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v100t.x(), v100t.y(), v100t.z()); + + glVertex3d(v100t.x(), v100t.y(), v100t.z()); + glVertex3d(v110t.x(), v110t.y(), v110t.z()); + + glVertex3d(v110t.x(), v110t.y(), v110t.z()); + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + + glVertex3d(v001t.x(), v001t.y(), v001t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); + + glVertex3d(v101t.x(), v101t.y(), v101t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); + + glVertex3d(v111t.x(), v111t.y(), v111t.z()); + glVertex3d(v011t.x(), v011t.y(), v011t.z()); + + glVertex3d(v011t.x(), v011t.y(), v011t.z()); + glVertex3d(v001t.x(), v001t.y(), v001t.z()); + + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v001t.x(), v001t.y(), v001t.z()); + + glVertex3d(v100t.x(), v100t.y(), v100t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); + + glVertex3d(v110t.x(), v110t.y(), v110t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); + + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v011t.x(), v011t.y(), v011t.z()); + + // + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v110t.x(), v110t.y(), v110t.z()); + + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v100t.x(), v100t.y(), v100t.z()); + + glVertex3d(v001t.x(), v001t.y(), v001t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); + + glVertex3d(v011t.x(), v011t.y(), v011t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); + + glEnd(); +} void ObservationPicking::render() { @@ -199,7 +287,7 @@ void ObservationPicking::export_observation(/*std::vector #include +#if WITH_GUI == 1 +#include +#endif + #include #include diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index f68754f1..eac47cb8 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -3,6 +3,10 @@ #include #include +#if WITH_GUI == 1 +#include +#endif + #include #include diff --git a/core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h b/core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h index c3684472..a36cc058 100644 --- a/core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h +++ b/core_hd_mapping/include/CoreHDMapping/single_trajectory_viewer.h @@ -28,7 +28,7 @@ class SingleTrajectoryViewer std::vector point_clouds; std::string trajectory_filename; - std::vector get_point_cloud_for_roi(Eigen::Vector3d roi, float roi_size); + std::vector get_point_cloud_for_roi(const Eigen::Vector3d& roi, float roi_size); bool load_fused_trajectory(const std::string& file_name); bool load_fused_trajectory(); diff --git a/core_hd_mapping/src/project_settings.cpp b/core_hd_mapping/src/project_settings.cpp index b4b3d94d..aec7783f 100644 --- a/core_hd_mapping/src/project_settings.cpp +++ b/core_hd_mapping/src/project_settings.cpp @@ -95,27 +95,6 @@ void ProjectSettings::imgui( odo_with_gnss_fusion.update_shift(common_data.shift_x, common_data.shift_y, common_data); } - /*if (ImGui::Button("set project main folder")) { - static std::shared_ptr folder; - std::string folder_name = ""; - ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)folder); - const auto t = [&]() { - auto sel = pfd::select_folder("Choose folder", "C:\\").result(); - folder_name = sel; - std::cout << "project_main_folder: '" << folder_name << "'" << std::endl; - }; - std::thread t1(t); - t1.join(); - - if (folder_name.size() > 0) { - project_main_folder = folder_name; - } - } - if (project_main_folder.size() > 0) { - ImGui::SameLine(); - ImGui::Text(std::string("project_main_folder: " + project_main_folder).c_str() ); - }*/ - if (ImGui::Button("add trajectory")) { std::string input_file_name; @@ -128,8 +107,6 @@ void ProjectSettings::imgui( if (input_file_name.size() > 0) { - // create motion model trajectory - add_trajectory( input_file_name, input_file_name_mm, float(rand() % 255) / 256.0, float(rand() % 255) / 256.0, float(rand() % 255) / 256.0); } @@ -429,13 +406,6 @@ void ProjectSettings::render(const std::vector& rois_with_co void ProjectSettings::pose_graph_slam(std::vector& rois_with_constraints) { - /*for (auto& trj : trajectories) { - for (auto& node : trj.fused_trajectory) { - node.m_pose(2, 3) += 10; - std::cout << node.index_to_gnss << " "; - } - }*/ - std::vector constraints; for (const auto& rwcs : rois_with_constraints) @@ -479,7 +449,6 @@ std::vector ProjectSettings::find_between_nodes(std::vector& { std::vector between_nodes; double dist_along = 0.0f; - // double dist_along_gnss = 0.0f; BetweenNode node_outer; node_outer.node_outer.index_to_lidar_odometry_odo = 0; diff --git a/core_hd_mapping/src/roi_exporter.cpp b/core_hd_mapping/src/roi_exporter.cpp index fd89a1da..b7c18220 100644 --- a/core_hd_mapping/src/roi_exporter.cpp +++ b/core_hd_mapping/src/roi_exporter.cpp @@ -113,11 +113,6 @@ void RoiExporter::imgui(CommonData& common_data, const ProjectSettings& project_ ImGui::Begin("ROI exporter"); ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - // if (ImGui::Button("reset view (angles)")) { - // common_data.rotate_x = 0.0; - // common_data.rotate_y = 0.0; - //} - // ImGui::SameLine(); ImGui::Checkbox("ROI::is_ortho", &common_data.is_ortho); if (common_data.is_ortho) @@ -225,15 +220,6 @@ void RoiExporter::imgui(CommonData& common_data, const ProjectSettings& project_ roi_point_clouds.insert(roi_point_clouds.end(), roi_point_clouds_per_job[i].begin(), roi_point_clouds_per_job[i].end()); } } - - /*for (auto t : project_setings.trajectories) { - SingleTrajectoryViewer tv; - tv.load_fused_trajectory(t.trajectory_file); - auto point_clouds = tv.get_point_cloud_for_roi(common_data.roi, common_data.roi_size); - for (const auto& pp : point_clouds) { - roi_point_clouds.push_back(pp); - } - }*/ } ImGui::SameLine(); ImGui::SliderInt("roi threads", &num_threads, 1, 128); @@ -293,56 +279,60 @@ void RoiExporter::imgui(CommonData& common_data, const ProjectSettings& project_ void RoiExporter::render(const CommonData& common_data) { + const auto& r = common_data.roi; + + const auto s = common_data.roi_size; + + const auto x0 = r.x() - s; + const auto x1 = r.x() + s; + const auto y0 = r.y() - s; + const auto y1 = r.y() + s; + const auto z = r.z(); + glColor3f(0, 0, 0); glBegin(GL_LINE_STRIP); - glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() + common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() + common_data.roi_size, common_data.roi.y() + common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() + common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); + glVertex3f(x0, y0, z); + glVertex3f(x1, y0, z); + glVertex3f(x1, y1, z); + glVertex3f(x0, y1, z); + glVertex3f(x0, y0, z); glEnd(); for (const auto& pp : roi_point_clouds) { - if (pp.visible) + if (!pp.visible) { - glPointSize(pp.point_size); - glBegin(GL_POINTS); - for (size_t i = 0; i < pp.points_global.size(); i += decimation) - { - const auto& p = pp.points_global[i]; - glColor3f((float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0); - glVertex3f(p.x, p.y, p.z); - } - glPointSize(1); - glEnd(); + continue; + } + + glPointSize(pp.point_size); + glBegin(GL_POINTS); + + for (size_t i = 0; i < pp.points_global.size(); i += decimation) + { + const auto& p = pp.points_global[i]; + + const auto intensity_scaled = (float(p.intensity) + 100.0f) / 256.0f; + + glColor3f(intensity_scaled, intensity_scaled, intensity_scaled); + glVertex3f(p.x, p.y, p.z); } + + glEnd(); + glPointSize(1); } - for (size_t i = 0; i < rois_with_constraints.size(); i++) + for (const auto& roi : rois_with_constraints) { - for (size_t j = 0; j < rois_with_constraints[i].constraints.size(); j++) + for (const auto& c : roi.constraints) { - // glBegin(GL_LINES); - Eigen::Affine3d& m = rois_with_constraints[i].constraints[j].m_pose; - /*float scale = 10; - glColor3f(1, 0, 0); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0,0) * scale, m(1, 3) + m(1,0) * scale, m(2, 3) + m(2,0) * scale); - - glColor3f(0, 1, 0); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1) * scale, m(1, 3) + m(1, 1) * scale, m(2, 3) + m(2, 1) * scale); - - glColor3f(0, 0, 1); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2) * scale, m(1, 3) + m(1, 2) * scale, m(2, 3) + m(2, 2) * scale); - glEnd();*/ + const auto& m = c.m_pose; + const auto& t = m.translation(); glColor3f(0, 0, 0); glPointSize(10); glBegin(GL_POINTS); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(t.x(), t.y(), t.z()); glEnd(); glPointSize(1); } diff --git a/core_hd_mapping/src/single_trajectory_viewer.cpp b/core_hd_mapping/src/single_trajectory_viewer.cpp index d716ea91..b9040586 100644 --- a/core_hd_mapping/src/single_trajectory_viewer.cpp +++ b/core_hd_mapping/src/single_trajectory_viewer.cpp @@ -159,20 +159,21 @@ void SingleTrajectoryViewer::imgui(const CommonData& common_data) void SingleTrajectoryViewer::render() { glColor3d(0.4, 0, 1); + glBegin(GL_POINTS); - for (size_t i = 0; i < trajectory_container.fused_trajectory.size(); i++) + for (const auto& pose_entry : trajectory_container.fused_trajectory) { - glVertex3f( - trajectory_container.fused_trajectory[i].m_pose(0, 3), - trajectory_container.fused_trajectory[i].m_pose(1, 3), - trajectory_container.fused_trajectory[i].m_pose(2, 3)); + const auto& t = pose_entry.m_pose.translation(); + glVertex3f(t.x(), t.y(), t.z()); } glEnd(); glBegin(GL_POINTS); for (const auto& p : points) { - glColor3f((float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0); + const auto intensity_scaled = (float(p.intensity) + 100.0f) / 256.0f; + + glColor3f(intensity_scaled, intensity_scaled, intensity_scaled); glVertex3f(p.x, p.y, p.z); } glEnd(); @@ -205,17 +206,12 @@ void SingleTrajectoryViewer::render() bool inside_roi(const Eigen::Affine3d& m_pose, const Eigen::Vector3d& roi, const float& roi_size) { - if (m_pose.translation().x() >= (roi.x() - roi_size) && m_pose.translation().x() <= (roi.x() + roi_size)) - { - if (m_pose.translation().y() >= (roi.y() - roi_size) && m_pose.translation().y() <= (roi.y() + roi_size)) - { - return true; - } - } - return false; + const auto& t = m_pose.translation(); + + return (t.x() >= roi.x() - roi_size && t.x() <= roi.x() + roi_size) && (t.y() >= roi.y() - roi_size && t.y() <= roi.y() + roi_size); } -std::vector SingleTrajectoryViewer::get_point_cloud_for_roi(Eigen::Vector3d roi, float roi_size) +std::vector SingleTrajectoryViewer::get_point_cloud_for_roi(const Eigen::Vector3d& roi, float roi_size) { std::vector pcs; From 4caf95d6f1278c7bf2012c94c319797f43d23d98 Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Mon, 30 Mar 2026 19:45:29 +0200 Subject: [PATCH 2/2] clang-format --- apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp | 1 - core/include/Core/control_points.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) 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 a4965ae1..72ca82a5 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -10,7 +10,6 @@ #include #include - const double DEG_TO_RAD = M_PI / 180.0f; const double RAD_TO_DEG = 180.0f / M_PI; diff --git a/core/include/Core/control_points.h b/core/include/Core/control_points.h index 8713be6f..c6385bf3 100644 --- a/core/include/Core/control_points.h +++ b/core/include/Core/control_points.h @@ -29,7 +29,7 @@ class ControlPoints bool is_imgui = false; std::vector cps; - + #if WITH_GUI == 1 bool picking_mode = false; bool draw_uncertainty = false;