From 6d0e128d4d054b9b5fb1e317422093187f4433c3 Mon Sep 17 00:00:00 2001 From: yuckinus <40965122+yuckinus@users.noreply.github.com> Date: Sun, 7 Dec 2025 02:47:18 +0200 Subject: [PATCH] Fixed bugs/updates/refinements - replaced all "for(int x, x < vector.size()..." to "for (size_t..." - step2 / fixed bug Save session not working after latest PR - cleaned up code for utils.cpp\getClosestTrajectoriesPoint - step3 / session.ground_truth editing - step2 / remove simple gui selection and ground_truth - step2 / move show IMU to LIO diff to menu - all / change default camera view to isometric - moved offset display and printing to console from step2/settings to viewer/session properties window - step1 / change default output folder to lio_result instead of lidar_odometry_result - step2 / hide experiment buttons - fixed mixed separators in open file(s) path return (e.g.: c:\dir/file.ext) --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 3 +- .../lidar_odometry_gui.cpp | 47 ++- .../lidar_odometry_utils.cpp | 6 +- .../lidar_odometry_utils.h | 2 +- .../mandeye_mission_recorder_calibration.cpp | 12 +- .../mandeye_raw_data_viewer.cpp | 44 +-- .../mandeye_single_session_viewer.cpp | 17 +- .../multi_session_registration.cpp | 363 +++++++++--------- .../CMakeLists.txt | 6 +- .../multi_view_tls_registration_gui.cpp | 310 ++++++--------- core/include/export_laz.h | 4 +- core/include/utils.hpp | 6 +- core/src/pfd_wrapper.cpp | 5 + core/src/utils.cpp | 169 ++------ 14 files changed, 412 insertions(+), 582 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 9fdfb91d..28e366bf 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -999,8 +999,7 @@ void load_reference_point_clouds(std::vector input_file_names, Lida std::string save_results_automatic(LidarOdometryParams ¶ms, std::vector &worker_data, std::string working_directory, double elapsed_seconds) { - int result = get_next_result_id(working_directory); - fs::path outwd = working_directory / fs::path("lidar_odometry_result_" + std::to_string(result)); + fs::path outwd = get_next_result_path(working_directory); save_result(worker_data, params, outwd, elapsed_seconds); return outwd.string(); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 760ecac9..4ef9f673 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -277,7 +277,7 @@ std::vector> get_batches_of_points(std::string laz_file, i std::vector tmp_points = prev_points; int counter = tmp_points.size(); - for (int i = 0; i < points.size(); i++) + for (size_t i = 0; i < points.size(); i++) { counter++; tmp_points.push_back(points[i]); @@ -312,7 +312,7 @@ int get_index(set s, int k) void find_best_stretch(std::vector points, std::vector timestamps, std::vector poses, std::string fn1, std::string fn2) { - for (int i = 0; i < points.size(); i++) + for (size_t i = 0; i < points.size(); i++) { auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), points[i].timestamp); points[i].index_pose = std::distance(timestamps.begin(), lower); @@ -321,7 +321,7 @@ void find_best_stretch(std::vector points, std::vector timesta std::set indexes; - for (int i = 0; i < points.size(); i++) + for (size_t i = 0; i < points.size(); i++) { indexes.insert(points[i].index_pose); } @@ -339,7 +339,7 @@ void find_best_stretch(std::vector points, std::vector timesta // Sleep(2000); std::vector points_reindexed = points; - for (int i = 0; i < points_reindexed.size(); i++) + for (size_t i = 0; i < points_reindexed.size(); i++) { points_reindexed[i].index_pose = get_index(indexes, points[i].index_pose); } @@ -356,7 +356,7 @@ void find_best_stretch(std::vector points, std::vector timesta Eigen::Affine3d m = trajectory[0]; trajectory_stretched.push_back(m); - for (int i = 1; i < trajectory.size(); i++) + for (size_t i = 1; i < trajectory.size(); i++) { Eigen::Affine3d m_update = trajectory[i - 1].inverse() * trajectory[i] * (m_x_offset); m = m * m_update; @@ -396,7 +396,7 @@ void find_best_stretch(std::vector points, std::vector timesta } std::map trajectory_for_interpolation; - for (int i = 0; i < best_trajectory.size(); i++) + for (size_t i = 0; i < best_trajectory.size(); i++) { trajectory_for_interpolation[ts[i]] = best_trajectory[i].matrix(); } @@ -546,7 +546,7 @@ void alternative_approach() all_points.push_back(tmp_points[i]); } - for (int i = 1; i < laz_files.size(); i++) + for (size_t i = 1; i < laz_files.size(); i++) { prev_points = tmp_points[tmp_points.size() - 1]; tmp_points = get_batches_of_points(laz_files[i], point_count_threshold, prev_points); @@ -567,7 +567,7 @@ void alternative_approach() poses.push_back(m); } - for (int i = 0; i < all_points.size(); i++) + for (size_t i = 0; i < all_points.size(); i++) { std::cout << all_points[i].size() << std::endl; std::string fn1 = "C:/data/tmp/" + std::to_string(i) + "_best.laz"; @@ -665,8 +665,7 @@ void step2(const std::atomic &loPause) void save_results(bool info, double elapsed_seconds) { - int result = get_next_result_id(working_directory); - fs::path outwd = working_directory / fs::path("lidar_odometry_result_" + std::to_string(result)); + fs::path outwd = get_next_result_path(working_directory); save_result(worker_data, params, outwd, elapsed_seconds); if (info) { @@ -1185,7 +1184,7 @@ void project_gui() ImGui::Text("Selection: "); if (ImGui::Button("Select all")) { - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) { worker_data[k].show = true; } @@ -1193,7 +1192,7 @@ void project_gui() ImGui::SameLine(); if (ImGui::Button("Unselect")) { - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) { worker_data[k].show = false; } @@ -1203,7 +1202,7 @@ void project_gui() ImGui::SameLine(); if (ImGui::Button("")) { - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) worker_data[k].show = (k >= index_begin && k <= index_end); } @@ -1214,7 +1213,7 @@ void project_gui() index_begin--; index_end--; - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) worker_data[k].show = (k >= index_begin && k <= index_end); } } @@ -1226,7 +1225,7 @@ void project_gui() index_begin++; index_end++; - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) worker_data[k].show = (k >= index_begin && k <= index_end); } } @@ -1238,7 +1237,7 @@ void project_gui() index_begin -= 10; index_end -= 10; - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) worker_data[k].show = (k >= index_begin && k <= index_end); } } @@ -1250,7 +1249,7 @@ void project_gui() index_begin += 10; index_end += 10; - for (int k = 0; k < worker_data.size(); k++) + for (size_t k = 0; k < worker_data.size(); k++) worker_data[k].show = (k >= index_begin && k <= index_end); } } @@ -1371,10 +1370,10 @@ void project_gui() } std::vector> all_poses; - for (int i = 0; i < worker_data.size(); i++) + for (size_t i = 0; i < worker_data.size(); i++) { std::vector poses; - for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) + for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) { poses.push_back(worker_data[i].intermediate_trajectory[j]); } @@ -1387,7 +1386,7 @@ void project_gui() for (int i = index_begin; i < index_end; i++) { - for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) + for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(worker_data[i].intermediate_trajectory[j]); @@ -1403,9 +1402,9 @@ void project_gui() } } - for (int i = index_end; i < worker_data.size(); i++) + for (size_t i = index_end; i < worker_data.size(); i++) { - for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) + for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) { Eigen::Affine3d m_update = Eigen::Affine3d::Identity(); @@ -1674,7 +1673,7 @@ void display() } #if 0 // ToDo - for (int i = 0; i < worker_data.size(); i++) + for (size_t i = 0; i < worker_data.size(); i++) { if (worker_data[i].show) { @@ -1711,7 +1710,7 @@ void display() { glColor3f(1, 0, 0); glBegin(GL_POINTS); - for (int i = 0; i < params.reference_points.size(); i += dec_reference_points) + for (size_t i = 0; i < params.reference_points.size(); i += dec_reference_points) glVertex3f(params.reference_points[i].point.x(), params.reference_points[i].point.y(), params.reference_points[i].point.z()); glEnd(); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 2fe5053b..c06e9cc6 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -740,9 +740,9 @@ int MLvxCalib::GetImuIdToUse(const std::unordered_map &idToSn, return 0; } -int get_next_result_id(const std::string working_directory) +fs::path get_next_result_path(const std::string working_directory) { - std::regex pattern(R"(lidar_odometry_result_(\d+))"); + std::regex pattern(R"(lio_result_(\d+))"); int max_number = -1; for (const auto &entry : fs::directory_iterator(working_directory)) { @@ -758,7 +758,7 @@ int get_next_result_id(const std::string working_directory) } } } - return max_number + 1; + return (working_directory / fs::path("lio_result_" + std::to_string(max_number + 1))); } bool loadLaz(const std::string &filename, std::vector &points_out, std::vector index_poses_i, std::vector &intermediate_trajectory, const Eigen::Affine3d &m_pose) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index f4f26d69..6550a617 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -189,7 +189,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po bool save_poses(const std::string file_name, std::vector m_poses, std::vector filenames); -int get_next_result_id(const std::string working_directory); +fs::path get_next_result_path(const std::string working_directory); // this function performs main LiDAR odometry calculations void optimize_lidar_odometry(std::vector &intermediate_points, std::vector &intermediate_trajectory, 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 d47957ae..3318798e 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -339,7 +339,7 @@ void project_gui() input_file_names = mandeye::fd::OpenFileDialog("Point cloud files", mandeye::fd::LAS_LAZ_filter, true); if (input_file_names.size() > 0) - for (int i = 0; i < input_file_names.size(); i++) + for (size_t i = 0; i < input_file_names.size(); i++) load_pc(input_file_names[i].c_str(), point_cloud, true, filter_threshold_xy); // Initialize imu_lidar according to imuSnToUse @@ -390,7 +390,7 @@ void project_gui() ImGui::Separator(); ImGui::Text("Select LiDAR to calibrate:"); - for (int i = 0; i < calibrated_lidar.size(); i++) + for (size_t i = 0; i < calibrated_lidar.size(); i++) { std::string name = idToSn.at(i); ImGui::RadioButton(name.c_str(), &chosen_lidar, i); @@ -398,7 +398,7 @@ void project_gui() if (chosen_lidar != -1) { - for (int i = 0; i < calibrated_lidar.size(); i++) + for (size_t i = 0; i < calibrated_lidar.size(); i++) calibrated_lidar[i].check = (i == chosen_lidar); } } @@ -540,7 +540,7 @@ void project_gui() if (ImGui::IsItemHovered()) ImGui::SetTooltip(hintText); - for (int i = 0; i < imu_lidar.size(); i++) + for (size_t i = 0; i < imu_lidar.size(); i++) { std::string name = idToSn.at(i); ImGui::RadioButton(std::string(name + "##imu").c_str(), &chosen_imu, i); @@ -550,7 +550,7 @@ void project_gui() if (chosen_imu != -1) { - for (int i = 0; i < imu_lidar.size(); i++) + for (size_t i = 0; i < imu_lidar.size(); i++) imu_lidar[i].check = (i == chosen_imu); } @@ -727,7 +727,7 @@ void display() if (manual_calibration) { int index_calibrated_lidar = -1; - for (int i = 0; i < calibrated_lidar.size(); i++) + for (size_t i = 0; i < calibrated_lidar.size(); i++) { if (calibrated_lidar[i].check) index_calibrated_lidar = i; 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 4ffa5b05..d674fb0d 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -226,7 +226,7 @@ void optimize() auto tr = all_data[index_rendered_points_local].poses; auto trmm = all_data[index_rendered_points_local].poses; - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -265,7 +265,7 @@ void optimize() point_cloud_global; std::vector point_cloud_global_sc; - for (int i = 0; i < points_local.size(); i++) + for (size_t i = 0; i < points_local.size(); i++) { Point3Di pg = points_local[i]; pg.point = tr[points_local[i].index_pose] * pg.point; @@ -289,7 +289,7 @@ void optimize() std::vector> tripletListP; std::vector> tripletListB; - for (int i = 0; i < point_cloud_global.size(); i++) + for (size_t i = 0; i < point_cloud_global.size(); i++) { auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); @@ -600,7 +600,7 @@ void optimize() std::vector points_local_sf; std::vector points_local; - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -650,14 +650,14 @@ std::vector> get_nn() std::vector> nn; if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // for (size_t i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; // } auto worker_data = all_data[index_rendered_points_local]; // index data - for (int i = 0; i < worker_data.points_local.size(); i++) + for (size_t i = 0; i < worker_data.points_local.size(); i++) { auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -690,7 +690,7 @@ std::vector> get_nn() std::vector point_cloud_global_sc; std::vector points_local_sc; - for (int i = 0; i < worker_data.points_local.size(); i++) + for (size_t i = 0; i < worker_data.points_local.size(); i++) { double r_l = worker_data.points_local[i].point.norm(); if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) @@ -742,7 +742,7 @@ std::vector> get_nn() // std::vector> nn; Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); - for (int i = 0; i < point_cloud_global_sc.size(); i++) + for (size_t i = 0; i < point_cloud_global_sc.size(); i++) { auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); @@ -808,14 +808,14 @@ std::vector> get_mean_cov() std::vector> mc; if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // for (size_t i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; // } auto worker_data = all_data[index_rendered_points_local]; // index data - for (int i = 0; i < worker_data.points_local.size(); i++) + for (size_t i = 0; i < worker_data.points_local.size(); i++) { auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -844,7 +844,7 @@ std::vector> get_mean_cov() std::vector point_cloud_global_sc; std::vector points_local_sc; - for (int i = 0; i < worker_data.points_local.size(); i++) + for (size_t i = 0; i < worker_data.points_local.size(); i++) { double r_l = worker_data.points_local[i].point.norm(); if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) @@ -1198,7 +1198,7 @@ void loadData() double t; - for (int i = 0; i < pointsPerFile.size(); i++) + for (size_t i = 0; i < pointsPerFile.size(); i++) { std::cout << "indexed: " << i + 1 << " of " << pointsPerFile.size() << " files" << std::endl; for (const auto& pp : pointsPerFile[i]) @@ -1253,7 +1253,7 @@ void loadData() data.points_local = points_local; data.lidar_ids = lidar_ids; - for (int i = 0; i < timestamps.size(); i++) + for (size_t i = 0; i < timestamps.size(); i++) { if (timestamps[i].first >= points_local[0].timestamp && timestamps[i].first <= points_local[points_local.size() - 1].timestamp) { @@ -1272,7 +1272,7 @@ void loadData() //std::cout << "ts_step " << ts_step << std::endl; //std::cout << "ts_end " << data.timestamps[data.timestamps.size() - 1].first << std::endl; - for (int pp = 0; pp < data.points_local.size(); pp++) + for (size_t pp = 0; pp < data.points_local.size(); pp++) data.points_local[pp].timestamp = ts_begin + pp * ts_step; } @@ -1379,7 +1379,7 @@ void imu_data_gui() ofstream file; file.open(output_file_name); file << std::setprecision(20); - for (int i = 0; i < imu_data_plot.timestampLidar.size(); i++) + for (size_t i = 0; i < imu_data_plot.timestampLidar.size(); i++) { file << imu_data_plot.timestampLidar[i] << " " << imu_data_plot.angX[i] << " " << @@ -1446,7 +1446,7 @@ void project_gui() /*if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) { - for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + for (size_t i = 0; i < all_points_local[index_rendered_points_local].size(); i++) { pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); intensity.push_back(all_points_local[index_rendered_points_local][i].intensity); @@ -1487,7 +1487,7 @@ void project_gui() std::cout << std::setprecision(20); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -1508,7 +1508,7 @@ void project_gui() } std::cout << "max_diff " << max_diff << std::endl; std::cout << "----------------" << std::endl; - for (int k = 0; k < all_data[index_rendered_points_local].timestamps.size(); k++) + for (size_t k = 0; k < all_data[index_rendered_points_local].timestamps.size(); k++) std::cout << all_data[index_rendered_points_local].timestamps[k].first << std::endl; } } @@ -1595,7 +1595,7 @@ void display() glBegin(GL_POINTS); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) { - for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + for (size_t i = 0; i < all_points_local[index_rendered_points_local].size(); i++) { if (all_lidar_ids[index_rendered_points_local][i] == 0) { @@ -1617,7 +1617,7 @@ void display() { double max_diff = 0.0; - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -1633,7 +1633,7 @@ void display() } } - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool @@ -1668,7 +1668,7 @@ void display() if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool 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 2bcc6752..56c2899e 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -791,6 +791,19 @@ void session_gui() ImGui::Text("Number of clouds: %zu", session.point_clouds_container.point_clouds.size()); ImGui::Text("Number of points: %zu", session_total_number_of_points); + ImGui::Separator(); + + ImGui::Text("Offset [m]:"); + ImGui::Text("X: %.10f; Y: %.10f; Z: %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Click to copy to clipboard"); + if (ImGui::IsItemClicked()) + { + char tmp[64]; + snprintf(tmp, sizeof(tmp), "%.10f %.10f %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); + ImGui::SetClipboardText(tmp); + } + ImGui::Separator(); ImGui::Text("Dimensions:"); @@ -1078,7 +1091,7 @@ void display() pose(2, 3) -= cloud.m_pose(2, 3); } - for (int i = 0; i < iCloud.points_local.size(); i++) + for (size_t i = 0; i < iCloud.points_local.size(); i++) { Eigen::Vector3d p(iCloud.points_local[i].x(), iCloud.points_local[i].y(), @@ -1464,7 +1477,7 @@ void mouse(int glut_button, int state, int x, int y) const auto laser_beam = GetLaserBeam(x, y); double min_distance = std::numeric_limits::max(); - for (int j = 0; j < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size(); j++) + for (size_t j = 0; j < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size(); j++) { auto vp = session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[j]; diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 028d4aac..12f3c93e 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -169,7 +169,7 @@ int index_loop_closure_source = -1; int index_loop_closure_target = -1; int first_session_index = -1; int second_session_index = -1; -double search_radious = 0.3; +double search_radius = 0.3; bool loaded_sessions = false; bool optimized = false; bool gizmo_all_sessions = false; @@ -179,6 +179,8 @@ bool remove_gui = false; NDT ndt; int number_visible_sessions = 0; +int index_gt = -1; +int old_index_gt = -1; int index_gizmo = -1; int old_index_gizmo = -1; @@ -488,7 +490,7 @@ void loop_closure_gui() if (remove_edge_index != -1) { std::vector new_edges; - for (int i = 0; i < edges.size(); i++) + for (size_t i = 0; i < edges.size(); i++) { if (remove_edge_index != i) new_edges.push_back(edges[i]); @@ -613,7 +615,7 @@ void loop_closure_gui() std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - if (icp.compute(source, target, search_radious, number_of_iterations, m_pose)) + if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); /*PointClouds pcs; @@ -625,13 +627,13 @@ void loop_closure_gui() ICP icp; - icp.search_radious = (float)search_radious; + icp.search_radius = (float)search_radius; for (auto &pc : pcs.point_clouds) { - pc.rgd_params.resolution_X = icp.search_radious; - pc.rgd_params.resolution_Y = icp.search_radious; - pc.rgd_params.resolution_Z = icp.search_radious; + pc.rgd_params.resolution_X = icp.search_radius; + pc.rgd_params.resolution_Y = icp.search_radius; + pc.rgd_params.resolution_Z = icp.search_radius; pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); @@ -665,13 +667,13 @@ void loop_closure_gui() pcs.point_clouds[0].m_pose = Eigen::Affine3d::Identity(); pcs.point_clouds[1].m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); ICP icp; - icp.search_radious = (float)search_radious; + icp.search_radius = (float)search_radius; for (auto &pc : pcs.point_clouds) { - pc.rgd_params.resolution_X = icp.search_radious; - pc.rgd_params.resolution_Y = icp.search_radious; - pc.rgd_params.resolution_Z = icp.search_radious; + pc.rgd_params.resolution_X = icp.search_radius; + pc.rgd_params.resolution_Y = icp.search_radius; + pc.rgd_params.resolution_Z = icp.search_radius; pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); @@ -703,15 +705,15 @@ void loop_closure_gui() std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - if (icp.compute(source, target, search_radious, number_of_iterations, m_pose)) + if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); } } } ImGui::SameLine(); - ImGui::InputDouble("search_radious", &search_radious); - if (search_radious < 0.01) - search_radious = 0.01; + ImGui::InputDouble("search_radius", &search_radius); + if (search_radius < 0.01) + search_radius = 0.01; /*if (ImGui::Button("ICP")) { @@ -813,13 +815,13 @@ void loop_closure_gui() pcs.point_clouds[0].m_pose = Eigen::Affine3d::Identity(); pcs.point_clouds[1].m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); ICP icp; - icp.search_radious = (float)search_radious; + icp.search_radius = (float)search_radius; for (auto &pc : pcs.point_clouds) { - pc.rgd_params.resolution_X = icp.search_radious; - pc.rgd_params.resolution_Y = icp.search_radious; - pc.rgd_params.resolution_Z = icp.search_radious; + pc.rgd_params.resolution_X = icp.search_radius; + pc.rgd_params.resolution_Y = icp.search_radius; + pc.rgd_params.resolution_Z = icp.search_radius; pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); @@ -852,13 +854,13 @@ void loop_closure_gui() pcs.point_clouds[0].m_pose = Eigen::Affine3d::Identity(); pcs.point_clouds[1].m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); ICP icp; - icp.search_radious = (float)search_radious; + icp.search_radius = (float)search_radius; for (auto &pc : pcs.point_clouds) { - pc.rgd_params.resolution_X = icp.search_radious; - pc.rgd_params.resolution_Y = icp.search_radious; - pc.rgd_params.resolution_Z = icp.search_radious; + pc.rgd_params.resolution_X = icp.search_radius; + pc.rgd_params.resolution_Y = icp.search_radius; + pc.rgd_params.resolution_Z = icp.search_radius; pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); @@ -886,14 +888,14 @@ void loop_closure_gui() } } ImGui::SameLine(); - ImGui::InputDouble("search_radious", &search_radious); - if (search_radious < 0.01) + ImGui::InputDouble("search_radius", &search_radius); + if (search_radius < 0.01) { - search_radious = 0.01; + search_radius = 0.01; }*/ ///////////////////////////////// - if (ImGui::Button("ICP [search radious 2m]")) + if (ImGui::Button("ICP [search radius 2m]")) { float sr = 2.0; std::cout << "Iterative Closest Point" << std::endl; @@ -1001,7 +1003,7 @@ void loop_closure_gui() } ImGui::SameLine(); - if (ImGui::Button("ICP [search radious 1m]")) + if (ImGui::Button("ICP [search radius 1m]")) { float sr = 1.0; std::cout << "Iterative Closest Point" << std::endl; @@ -1108,7 +1110,7 @@ void loop_closure_gui() } } ImGui::SameLine(); - if (ImGui::Button("ICP [search radious 0.5m]")) + if (ImGui::Button("ICP [search radius 0.5m]")) { float sr = 0.5; std::cout << "Iterative Closest Point" << std::endl; @@ -1219,7 +1221,7 @@ void loop_closure_gui() } } ImGui::SameLine(); - if (ImGui::Button("ICP [search radious 0.25m]")) + if (ImGui::Button("ICP [search radius 0.25m]")) { float sr = 0.25; std::cout << "Iterative Closest Point" << std::endl; @@ -1330,7 +1332,7 @@ void loop_closure_gui() } } ImGui::SameLine(); - if (ImGui::Button("ICP [search radious 0.1m]")) + if (ImGui::Button("ICP [search radius 0.1m]")) { float sr = 0.1; std::cout << "Iterative Closest Point" << std::endl; @@ -1453,7 +1455,7 @@ void loop_closure_gui() std::vector intensity; std::vector timestamps; - for (int i = 0; i < source.size(); i++) + for (size_t i = 0; i < source.size(); i++) { pointcloud.push_back(source[i]); intensity.push_back(0); @@ -1505,7 +1507,7 @@ void save_trajectories_to_laz(const Session &session, std::string output_file_na { if (p.visible) { - for (int i = 0; i < p.local_trajectory.size(); i++) + for (size_t i = 0; i < p.local_trajectory.size(); i++) { const auto &pp = p.local_trajectory[i].m_pose.translation(); Eigen::Vector3d vp; @@ -1639,7 +1641,7 @@ void save_trajectories( { if (p.visible) { - for (int i = 0; i < p.local_trajectory.size(); i++) + for (size_t i = 0; i < p.local_trajectory.size(); i++) { const auto &m = p.local_trajectory[i].m_pose; Eigen::Affine3d pose = p.m_pose * m; @@ -1803,12 +1805,12 @@ bool optimize(std::vector &sessions) std::vector vfixed_fi; std::vector vfixed_ka; - for (int i = 0; i < sessions.size(); i++) + for (size_t i = 0; i < sessions.size(); i++) { sum += sessions[i].point_clouds_container.point_clouds.size(); sums.push_back(sum); - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) { vfixed_x.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_x); vfixed_y.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_y); @@ -1827,7 +1829,7 @@ bool optimize(std::vector &sessions) std::vector poses_motion_model; std::vector index_trajectory; - for (int j = 0; j < sessions.size(); j++) + for (size_t j = 0; j < sessions.size(); j++) { for (size_t i = 0; i < sessions[j].point_clouds_container.point_clouds.size(); i++) { @@ -1845,7 +1847,7 @@ bool optimize(std::vector &sessions) int iterations = 1; bool is_fix_first_node = true; std::vector indexes_ground_truth; - for (int j = 0; j < sessions.size(); j++) + for (size_t j = 0; j < sessions.size(); j++) { for (size_t i = 0; i < sessions[j].point_clouds_container.point_clouds.size(); i++) { @@ -1870,7 +1872,7 @@ bool optimize(std::vector &sessions) double angle = 0.1 * DEG_TO_RAD; double wangle = 1.0 / (angle * angle); - for (int i = 1; i < poses_motion_model.size(); i++) + for (size_t i = 1; i < poses_motion_model.size(); i++) { if (index_trajectory[i - 1] == index_trajectory[i]) { @@ -1891,9 +1893,9 @@ bool optimize(std::vector &sessions) } } - for (int i = 0; i < sessions.size(); i++) + for (size_t i = 0; i < sessions.size(); i++) { - for (int j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) + for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) { Edge edge; edge.index_from = sessions[i].pose_graph_loop_closure.edges[j].index_from + sums[i]; @@ -1905,7 +1907,7 @@ bool optimize(std::vector &sessions) } } - for (int i = 0; i < edges.size(); i++) + for (size_t i = 0; i < edges.size(); i++) { Edge edge; edge.index_from = edges[i].index_from + sums[edges[i].index_session_from]; @@ -2101,7 +2103,7 @@ bool optimize(std::vector &sessions) tripletListB.emplace_back(ir + 4, 0, 0); tripletListB.emplace_back(ir + 5, 0, 0); - for (int i = 1; i < index_trajectory.size(); i++) + for (size_t i = 1; i < index_trajectory.size(); i++) { if (index_trajectory[i - 1] != index_trajectory[i]) { @@ -2133,7 +2135,7 @@ bool optimize(std::vector &sessions) /*double angle = 1.0 * DEG_TO_RAD; double wangle = 1.0 / (angle * angle); - for (int index = 0; index < indexes_ground_truth.size(); index++) + for (size_t index = 0; index < indexes_ground_truth.size(); index++) { int ir = tripletListB.size(); int ic = indexes_ground_truth[index] * 6; @@ -2161,10 +2163,10 @@ bool optimize(std::vector &sessions) // gnss // for (const auto &pc : point_clouds_container.point_clouds) - /*for (int index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) + /*for (size_t index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) { const auto &pc = point_clouds_container.point_clouds[index_pose]; - for (int i = 0; i < gnss.gnss_poses.size(); i++) + for (size_t i = 0; i < gnss.gnss_poses.size(); i++) { double time_stamp = gnss.gnss_poses[i].timestamp; @@ -2228,9 +2230,9 @@ bool optimize(std::vector &sessions) }*/ // - for (int j = 0; j < sessions.size(); j++) + for (size_t j = 0; j < sessions.size(); j++) { - for (int jj = 0; jj < sessions[j].ground_control_points.gpcs.size(); jj++) + for (size_t jj = 0; jj < sessions[j].ground_control_points.gpcs.size(); jj++) { Eigen::Vector3d p_s = sessions[j].point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner].local_trajectory[sessions[j].ground_control_points.gpcs[jj].index_to_node_outer].m_pose.translation(); Eigen::Matrix jacobian; @@ -2273,9 +2275,9 @@ bool optimize(std::vector &sessions) } /////////////////////////////////////////////////////// - for (int i = 0; i < sessions.size(); i++) + for (size_t i = 0; i < sessions.size(); i++) { - for (int index_pose = 0; index_pose < sessions[i].point_clouds_container.point_clouds.size(); index_pose++) + for (size_t index_pose = 0; index_pose < sessions[i].point_clouds_container.point_clouds.size(); index_pose++) { if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_x) { @@ -2343,7 +2345,7 @@ bool optimize(std::vector &sessions) } ////////////////////////////////////////////////////// - // for (int i = 0; i < gcps.gpcs.size(); i++) + // for (size_t i = 0; i < gcps.gpcs.size(); i++) //{ Eigen::SparseMatrix matA(tripletListB.size(), poses.size() * 6); @@ -2639,6 +2641,8 @@ void loadSessions() Session session; session.load(fs::path(ps).string(), is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset); sessions.push_back(session); + if (session.is_ground_truth) + index_gt = sessions.size() - 1; } loaded_sessions = true; @@ -2649,7 +2653,7 @@ void loadSessions() std::map map_reorder; // project_settings.session_file_names. int new_index = 0; - for (int i = 0; i < sessions.size(); i++) + for (size_t i = 0; i < sessions.size(); i++) { if (sessions[i].is_ground_truth) { @@ -2658,7 +2662,7 @@ void loadSessions() map_reorder[i] = new_index++; } } - for (int i = 0; i < sessions.size(); i++) + for (size_t i = 0; i < sessions.size(); i++) { if (!sessions[i].is_ground_truth) { @@ -2701,7 +2705,7 @@ void loadSessions() void project_gui() { - if (ImGui::Begin("Multi session processing")) + if (ImGui::Begin("Settings")) { ImGui::Checkbox("Downsample during load", &is_decimate); ImGui::SameLine(); @@ -2743,9 +2747,9 @@ void project_gui() int index_point_clouds = -1; int index_local_trajectory = -1; bool found = false; - for (int a = 0; a < session.point_clouds_container.point_clouds.size(); a++) + for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++) { - for (int b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) + for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) { if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) { @@ -2767,146 +2771,149 @@ void project_gui() auto inv = (m1 * m2).inverse(); - for (int index = 0; index < session.point_clouds_container.point_clouds.size(); index++) + for (size_t index = 0; index < session.point_clouds_container.point_clouds.size(); index++) session.point_clouds_container.point_clouds[index].m_pose = inv * session.point_clouds_container.point_clouds[index].m_pose; } } } - ImGui::Separator(); - - ImGui::Text("Session file names:"); - - for (int i = 0; i < project_settings.session_file_names.size(); i++) + if (project_settings.session_file_names.size() > 0) { - ImGui::Text(truncPath(project_settings.session_file_names[i]).c_str()); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(project_settings.session_file_names[i].c_str()); + ImGui::Separator(); - if (project_settings.session_file_names.size() == sessions.size()) + ImGui::Text("Sessions:"); + + for (size_t i = 0; i < project_settings.session_file_names.size(); i++) { - if (!is_loop_closure_gui){ - ImGui::SameLine(); - ImGui::Checkbox(("Visible##" + std::to_string(i)).c_str(), &sessions[i].visible); - } + ImGui::Text(truncPath(project_settings.session_file_names[i]).c_str()); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(project_settings.session_file_names[i].c_str()); - ImGui::BeginDisabled(!sessions[i].visible); + if (project_settings.session_file_names.size() == sessions.size()) { - ImGui::BeginDisabled(sessions[i].is_ground_truth); + ImGui::BeginDisabled(is_loop_closure_gui); { ImGui::SameLine(); - if (ImGui::RadioButton(("Gizmo##" + std::to_string(i)).c_str(), &index_gizmo, i)) - { - if (old_index_gizmo == i) - index_gizmo = -1; // unselect - else - index_gizmo = i; - } + ImGui::Checkbox(("Visible##" + std::to_string(i)).c_str(), &sessions[i].visible); } - ImGui::EndDisabled(); + ImGui::EndDisabled(); ImGui::SameLine(); - ImGui::Checkbox(("Show RGB##" + std::to_string(i)).c_str(), &sessions[i].show_rgb); + if (ImGui::RadioButton(("Ground truth##" + std::to_string(i)).c_str(), &index_gt, i)) + if (old_index_gt == i) + index_gt = -1; // unselect + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select session as unmovable reference"); - if (!sessions[i].show_rgb) + ImGui::BeginDisabled(!sessions[i].visible); { + ImGui::BeginDisabled(sessions[i].is_ground_truth); + { + ImGui::SameLine(); + if (ImGui::RadioButton(("Gizmo##" + std::to_string(i)).c_str(), &index_gizmo, i)) + if (old_index_gizmo == i) + index_gizmo = -1; // unselect + } + ImGui::EndDisabled(); + ImGui::SameLine(); - ImGui::ColorEdit3(("Color##" + std::to_string(i)).c_str(), (float *)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); - for (auto &pc : sessions[i].point_clouds_container.point_clouds) + ImGui::Checkbox(("Show RGB##" + std::to_string(i)).c_str(), &sessions[i].show_rgb); + + if (!sessions[i].show_rgb) { - pc.render_color[0] = sessions[i].render_color[0]; - pc.render_color[1] = sessions[i].render_color[1]; - pc.render_color[2] = sessions[i].render_color[2]; - pc.show_color = sessions[i].show_rgb; + ImGui::SameLine(); + ImGui::ColorEdit3(("Color##" + std::to_string(i)).c_str(), (float *)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); + for (auto &pc : sessions[i].point_clouds_container.point_clouds) + { + pc.render_color[0] = sessions[i].render_color[0]; + pc.render_color[1] = sessions[i].render_color[1]; + pc.render_color[2] = sessions[i].render_color[2]; + pc.show_color = sessions[i].show_rgb; + } + } + else + { + for (auto &pc : sessions[i].point_clouds_container.point_clouds) + pc.show_color = sessions[i].show_rgb; } } - else - { - for (auto &pc : sessions[i].point_clouds_container.point_clouds) - pc.show_color = sessions[i].show_rgb; - } - } - ImGui::EndDisabled(); - - if (sessions[i].is_ground_truth) - { - ImGui::SameLine(); - ImGui::Text(" [ground_truth]"); - } + ImGui::EndDisabled(); - // - if (sessions[i].point_clouds_container.point_clouds.size() > 0) - { - if (sessions[i].point_clouds_container.point_clouds[0].local_trajectory.size() > 0) + // + if (sessions[i].point_clouds_container.point_clouds.size() > 0) { - if (sessions[i].point_clouds_container.point_clouds[sessions[i].point_clouds_container.point_clouds.size() - 1].local_trajectory.size() > 0) + if (sessions[i].point_clouds_container.point_clouds[0].local_trajectory.size() > 0) { - ImGui::SameLine(); + if (sessions[i].point_clouds_container.point_clouds[sessions[i].point_clouds_container.point_clouds.size() - 1].local_trajectory.size() > 0) + { + ImGui::SameLine(); - int index_last = sessions[i].point_clouds_container.point_clouds.size() - 1; - int index_last2 = sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory.size() - 1; + int index_last = sessions[i].point_clouds_container.point_clouds.size() - 1; + int index_last2 = sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory.size() - 1; - ImGui::Text("Timestamp range: <%.0f, %.0f>", - sessions[i].point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first, - sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory[index_last2].timestamps.first); + ImGui::Text("Timestamp range: <%.0f, %.0f>", + sessions[i].point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first, + sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory[index_last2].timestamps.first); + } } } } } - } - if (project_settings.session_file_names.size() == sessions.size()) - { - if (old_index_gizmo != index_gizmo) + if (project_settings.session_file_names.size() == sessions.size()) { - for (int i = 0; i < sessions.size(); i++) - sessions[i].is_gizmo = (i == index_gizmo); + if ((old_index_gt != index_gt) || (old_index_gizmo != index_gizmo)) + { + for (size_t i = 0; i < sessions.size(); i++) + { + sessions[i].is_ground_truth = (i == index_gt); + sessions[i].is_gizmo = (i == index_gizmo); + } - old_index_gizmo = index_gizmo; + old_index_gt = index_gt; + old_index_gizmo = index_gizmo; + } + + if (index_gizmo != -1 && index_gizmo < sessions.size()) + { + // sessions[index_gizmo].is_gizmo = true; + m_gizmo[0] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 0); + m_gizmo[1] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 0); + m_gizmo[2] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 0); + m_gizmo[3] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 0); + m_gizmo[4] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 1); + m_gizmo[5] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 1); + m_gizmo[6] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 1); + m_gizmo[7] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 1); + m_gizmo[8] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 2); + m_gizmo[9] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 2); + m_gizmo[10] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 2); + m_gizmo[11] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 2); + m_gizmo[12] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 3); + m_gizmo[13] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 3); + m_gizmo[14] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 3); + m_gizmo[15] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 3); + } } - if (index_gizmo != -1 && index_gizmo < sessions.size()) + ImGui::BeginDisabled((project_settings.session_file_names.size() < 2) || (index_gizmo == -1)); { - // sessions[index_gizmo].is_gizmo = true; - m_gizmo[0] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 0); - m_gizmo[1] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 0); - m_gizmo[2] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 0); - m_gizmo[3] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 0); - m_gizmo[4] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 1); - m_gizmo[5] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 1); - m_gizmo[6] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 1); - m_gizmo[7] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 1); - m_gizmo[8] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 2); - m_gizmo[9] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 2); - m_gizmo[10] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 2); - m_gizmo[11] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 2); - m_gizmo[12] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 3); - m_gizmo[13] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 3); - m_gizmo[14] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 3); - m_gizmo[15] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 3); + ImGui::Checkbox("Gizmo all sessions", &gizmo_all_sessions); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Gizmo will move all sessions except ground truth one"); } - } - - ImGui::BeginDisabled((project_settings.session_file_names.size() < 2) || (index_gizmo == -1)); - { - ImGui::Checkbox("Gizmo all sessions", &gizmo_all_sessions); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Gizmo will move all sessions"); - } - ImGui::EndDisabled(); + ImGui::EndDisabled(); - ImGui::Separator(); + ImGui::Separator(); - ImGui::NewLine(); + ImGui::NewLine(); - if (project_settings.session_file_names.size() > 0) - { if (project_settings.session_file_names.size() == sessions.size()) { number_visible_sessions = 0; bool first_session_index_found = false; - for (int index = 0; index < sessions.size(); index++) + for (size_t index = 0; index < sessions.size(); index++) { if (sessions[index].visible) { @@ -3141,9 +3148,9 @@ void display() i++; } - for (int i = 0; i < sessions.size(); i++) + for (size_t i = 0; i < sessions.size(); i++) { - for (int j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) + for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) { int index_src = sessions[i].pose_graph_loop_closure.edges[j].index_from; int index_trg = sessions[i].pose_graph_loop_closure.edges[j].index_to; @@ -3164,7 +3171,7 @@ void display() } } - for (int i = 0; i < edges.size(); i++) + for (size_t i = 0; i < edges.size(); i++) { int index_src = edges[i].index_from; int index_trg = edges[i].index_to; @@ -3204,9 +3211,9 @@ void display() int index_point_clouds = -1; int index_local_trajectory = -1; bool found = false; - for (int a = 0; a < session.point_clouds_container.point_clouds.size(); a++) + for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++) { - for (int b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) + for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) { if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) { @@ -3332,7 +3339,7 @@ void display() { prev_pose_manipulated = sessions[i].point_clouds_container.point_clouds[0].m_pose; std::vector all_m_poses; - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); // if (all_m_poses.size() > 1) @@ -3371,7 +3378,7 @@ void display() sessions[i].point_clouds_container.point_clouds[0].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; - for (int j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; @@ -3397,7 +3404,7 @@ void display() if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth) { std::vector all_m_poses; - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * sessions[i].point_clouds_container.point_clouds[0].m_pose; @@ -3416,7 +3423,7 @@ void display() sessions[i].point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; - for (int j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; @@ -3474,7 +3481,7 @@ void display() if (session.point_clouds_container.point_clouds[i].gizmo) { std::vector all_m_poses; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); ImGuiIO &io = ImGui::GetIO(); @@ -3525,7 +3532,7 @@ void display() if (!manipulate_only_marked_gizmo) { Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; - for (int j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; @@ -4325,7 +4332,7 @@ else ImGui::Text("Select session(s) to remove:"); ImGui::Separator(); - for (int i = 0; i < project_settings.session_file_names.size(); i++) + for (size_t i = 0; i < project_settings.session_file_names.size(); i++) { bool checked = session_marked_for_removal[i]; if (ImGui::Checkbox(project_settings.session_file_names[i].c_str(), &checked)) @@ -4336,7 +4343,7 @@ else if (ImGui::Button("Remove")) { - for (int i = project_settings.session_file_names.size() - 1; i >= 0; i--) + for (size_t i = project_settings.session_file_names.size() - 1; i >= 0; i--) { if (session_marked_for_removal[i]) { @@ -4430,32 +4437,14 @@ void mouse(int glut_button, int state, int x, int y) { if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) && !manipulate_active_edge) { - /*if (sessions.size() > 0){ - getClosestTrajectoriesPoint(sessions, x, y); - } - else{ - setNewRotationCenter(x, y); - }*/ - - if (io.KeyCtrl) + //if (s_loop_closure_gui) + if ((sessions.size() > 0) && (number_visible_sessions > 0)) { - /*if(number_visible_sessions == 1){ - std::cout << "number_visible_sessions == 1" << std::endl; - getClosestTrajectoriesPoint(sessions, x, y, 1, ); - } - if(number_visible_sessions == 2){ - std::cout << "number_visible_sessions == 2" << std::endl; - getClosestTrajectoriesPoint(sessions, x, y, 2); - } - else if (sessions.size() > 0) - { - getClosestTrajectoriesPoint(sessions, x, y, -1); - }*/ - getClosestTrajectoriesPoint(sessions, x, y, index_loop_closure_source, index_loop_closure_target, 0); + getClosestTrajectoriesPoint(sessions, x, y, first_session_index, second_session_index, number_visible_sessions, index_loop_closure_source, index_loop_closure_target, io.KeyShift); } - if (io.KeyShift) + else { - getClosestTrajectoriesPoint(sessions, x, y, index_loop_closure_source, index_loop_closure_target, 1); + setNewRotationCenter(x, y); } } diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 01afcb83..755f1fb4 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -6,6 +6,7 @@ project(multi_view_tls_registration_step_2) set(SOURCES multi_view_tls_registration.cpp perform_experiment.cpp multi_view_tls_registration_gui.cpp multi_view_tls_registration.h + ../lidar_odometry_step_1/lidar_odometry_utils.cpp "../../core/src/utils.cpp" ) @@ -26,6 +27,7 @@ target_include_directories( ${REPOSITORY_DIRECTORY}/core/include ${EXTERNAL_LIBRARIES_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} + ${EXTERNAL_LIBRARIES_DIRECTORY} ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo @@ -33,7 +35,9 @@ target_include_directories( ${FREEGLUT_INCLUDE_DIR} ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master - ${LASZIP_INCLUDE_DIR}/LASzip/include) + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) target_link_libraries( multi_view_tls_registration_step_2 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 343156cc..64527c62 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 @@ -38,6 +38,7 @@ #include #include "multi_view_tls_registration.h" +#include "../lidar_odometry_step_1/lidar_odometry_utils.h" #include #include @@ -180,7 +181,6 @@ static int PGSpwmtSelection = 0; std::string session_file_name = ""; int session_total_number_of_points = 0; -PointClouds::PointCloudDimensions session_dims; //bool dynamicSubsampling = true; //static double lastAdjustTime = 0.0; // last time we changed subsampling //const double cooldownSeconds = 1; // wait between auto adjustments @@ -195,12 +195,15 @@ bool is_manual_analisys = false; bool is_loop_closure_gui = false; bool is_lio_segments_gui = false; -bool new_loop_closure_index = false; +bool fillInSession = true; TLSRegistration tls_registration; ObservationPicking observation_picking; std::vector picked_points; +bool new_loop_closure_index = false; +int num_edge_extended_before = 0; +int num_edge_extended_after = 0; int index_loop_closure_source = 0; int index_loop_closure_target = 0; int index_begin = 0; @@ -224,16 +227,10 @@ float m_ortho_projection[] = {1, 0, 0, 0, bool manipulate_only_marked_gizmo = false; Session session; -bool simple_gui = true; +PointClouds::PointCloudDimensions session_dims; bool session_loaded = false; -int num_edge_extended_before = 0; -int num_edge_extended_after = 0; - -void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session); -void reshape(int w, int h); - -// this functions performs experiment from paper +// these functions performs experiment from paper //@article //{BEDKOWSKI2023113199, // title = {Benchmark of multi-view Terrestrial Laser Scanning Point Cloud data registration algorithms}, @@ -247,11 +244,12 @@ void reshape(int w, int h); // keywords = {TLS, Point cloud, Open-source, Multi-view data registration, LiDAR data metrics, Robust loss function, Tait-bryan angles, Quaternions, Rodrigues’ formula, Lie algebra, Rotation matrix parameterization}, // abstract = {This study addresses multi-view Terrestrial Laser Scanning Point Cloud data registration methods. Multiple rigid point cloud data registration is mandatory for aligning all scans into a common reference frame and it is still considered a challenge looking from a large-scale surveys point of view. The goal of this work is to support the development of cutting-edge registration methods in geoscience and mobile robotics domains. This work evaluates 3 data sets of total 20 scenes available in the literature. This paper provides a novel open-source framework for multi-view Terrestrial Laser Scanning Point Cloud data registration benchmarks. The goal was to verify experimentally which registration variant can improve the open-source data looking from the quantitative and qualitative points of view. In particular, the following scanners provided measurement data: Z+F TLS Imager 5006i, Z+F TLS Imager 5010C, Leica ScanStation C5, Leica ScanStation C10, Leica P40 and Riegl VZ-400. The benchmark shows an impact of the metric e.g. point to point, point to projection onto a plane, plane to plane etc..., rotation matrix parameterization (Tait-Bryan, quaternion, Rodrigues) and other implementation variations (e.g. multi-view Normal Distributions Transform, Pose Graph SLAM approach) onto the multi-view data registration accuracy and performance. An open-source project is created and it can be used for improving existing data sets reported in the literature, it is the added value of the presented research. The combination of metrics, rotation matrix parameterization and optimization algorithms creates hundreds of possible approaches. It is shown that chosen metric is a dominant factor in data registration. The rotation parameterization and other degrees of freedom of proposed variants are rather negligible compared with chosen metric. Most of the proposed approaches improve registered reference data provided by other researchers. Only for 2 from 20 scenes it was not possible to provide significant improvement. The largest improvements are evident for large-scale scenes. The project is available and maintained at https://github.com/MapsHD/HDMapping.} // } + +void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session); void perform_experiment_on_windows(Session &session, ObservationPicking &observation_picking, ICP &icp, NDT &ndt, RegistrationPlaneFeature ®istration_plane_feature, PoseGraphSLAM &pose_graph_slam); void perform_experiment_on_linux(Session &session, ObservationPicking &observation_picking, ICP &icp, NDT &ndt, RegistrationPlaneFeature ®istration_plane_feature, PoseGraphSLAM &pose_graph_slam); - double compute_rms(bool initial, Session &session, ObservationPicking &observation_picking); void reset_poses(Session &session); @@ -898,7 +896,7 @@ void observation_picking_gui() observation_picking.add_intersection(Eigen::Vector3d(0.0, 0.0, 0.0)); int index_intersection_to_remove = -1; - for (int i = 0; i < observation_picking.intersections.size(); i++) + for (size_t i = 0; i < observation_picking.intersections.size(); i++) { ImGui::Separator(); ImGui::SetWindowFontScale(1.25f); @@ -918,7 +916,7 @@ void observation_picking_gui() if (index_intersection_to_remove != -1) { std::vector intersections; - for (int i = 0; i < observation_picking.intersections.size(); i++) + for (size_t i = 0; i < observation_picking.intersections.size(); i++) { if (i != index_intersection_to_remove) intersections.push_back(observation_picking.intersections[i]); @@ -954,37 +952,39 @@ void loop_closure_gui() { ImGui::Begin("Manual Pose Graph Loop Closure", &is_loop_closure_gui); { + const auto point_cloud_upper = session.point_clouds_container.point_clouds.size() - 1; + ImGui::Text("Num edge extended:"); ImGui::Text("before: "); ImGui::SameLine(); ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::SliderInt("##fs", &num_edge_extended_before, 0, session.point_clouds_container.point_clouds.size() - 1); + ImGui::SliderInt("##fs", &num_edge_extended_before, 0, point_cloud_upper); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", session.point_clouds_container.point_clouds.size() - 1); + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); ImGui::SameLine(); ImGui::InputInt("##fi", &num_edge_extended_before, 1, 5); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", session.point_clouds_container.point_clouds.size() - 1); + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); if (num_edge_extended_before < 0) num_edge_extended_before = 0; - if (num_edge_extended_before >= session.point_clouds_container.point_clouds.size() - 1) - num_edge_extended_before = session.point_clouds_container.point_clouds.size() - 1; + if (num_edge_extended_before >= point_cloud_upper) + num_edge_extended_before = point_cloud_upper; ImGui::Text(" after: "); ImGui::SameLine(); - ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast(session.point_clouds_container.point_clouds.size() - 1)); + ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast(point_cloud_upper)); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", session.point_clouds_container.point_clouds.size() - 1); + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); ImGui::SameLine(); ImGui::InputInt("##ti", &num_edge_extended_after, 1, 5); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", session.point_clouds_container.point_clouds.size() - 1); + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); if (num_edge_extended_after < 0) num_edge_extended_after = 0; - if (num_edge_extended_after >= session.point_clouds_container.point_clouds.size() - 1) - num_edge_extended_after = session.point_clouds_container.point_clouds.size() - 1; + if (num_edge_extended_after >= point_cloud_upper) + num_edge_extended_after = point_cloud_upper; ImGui::PopItemWidth(); int prev_index_active_edge = session.pose_graph_loop_closure.index_active_edge; @@ -1276,7 +1276,7 @@ void lio_segments_gui() std::cout << "pose before: " << session.point_clouds_container.point_clouds[index_target].m_pose.matrix() << std::endl; std::vector all_m_poses; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) { all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); } @@ -1291,7 +1291,7 @@ void lio_segments_gui() std::cout << "update all poses after current pose" << std::endl; Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[index_target].m_pose; - for (int j = index_target + 1; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = index_target + 1; j < session.point_clouds_container.point_clouds.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; @@ -1411,24 +1411,21 @@ void openSession() } } -void saveSession(std::string output_file_name = "") +std::string saveSession() { - if (output_file_name.empty()){ - const auto output_file_name = mandeye::fd::SaveFileDialog("Save session as", mandeye::fd::Session_filter, ".mjs", session_file_name); - } + const std::string output_file_name = mandeye::fd::SaveFileDialog("Save session as", mandeye::fd::Session_filter, ".mjs", session_file_name); if (output_file_name.size() > 0) { std::cout << "Session file to save: '" << output_file_name << "'" << std::endl; - //creating filename proposal based on current selection + //creating filenames proposal based on current selection std::filesystem::path path(output_file_name); // Extract parts const auto dir = path.parent_path(); const auto stem = path.stem().string(); - const auto ext = path.extension().string(); - // Build new name + // Build new names std::string initial_poses_file_name = (dir / (stem + "_ini_poses.mri")).string(); std::string poses_file_name = (dir / (stem + "_poses.mrp")).string(); @@ -1489,13 +1486,15 @@ void saveSession(std::string output_file_name = "") catch (const fs::filesystem_error& e) { std::cerr << "Error copying poses file: " << e.what() << '\n'; } + + return output_file_name; } else{ std::cout << "saving canceled" << std::endl; } } -void openLaz(bool ground_truth) +void openLaz(bool fillInSession) { session.point_clouds_container.point_clouds.clear(); std::vector input_file_names; @@ -1525,17 +1524,8 @@ void openLaz(bool ground_truth) session_dims = session.point_clouds_container.compute_point_cloud_dimension(); - [[maybe_unused]] - pfd::message message( - "Information", - "If you can not see point cloud --> 1. Change 'Points render subsampling', 2. Check console 'min max coordinates should be small numbers to see points in our local coordinate system'. 3. Set checkbox 'calculate_offset for WHU-TLS'. 4. Later on You can change offset directly in session json file.", - pfd::choice::ok, pfd::icon::info); - message.result(); - - if (ground_truth) + if (fillInSession) { - session.is_ground_truth = true; - for (auto &pc : session.point_clouds_container.point_clouds) { Eigen::Affine3d m = Eigen::Affine3d::Identity(); @@ -1544,7 +1534,7 @@ void openLaz(bool ground_truth) int counter = 1; Eigen::Vector3d mean(pc.points_local[0]); // std::cout << "mean " << mean << std::endl; - for (int i = 100; i < pc.points_local.size(); i += 100) + for (size_t i = 100; i < pc.points_local.size(); i += 100) { mean += pc.points_local[i]; counter++; @@ -1574,76 +1564,41 @@ void openLaz(bool ground_truth) pc.pose = pose_tait_bryan_from_affine_matrix(m); } - std::filesystem::path path_ground_truth_session_folder; - // save to folder - for (auto &pc : session.point_clouds_container.point_clouds) - { - // std::cout << "pc.file_name: '" << pc.file_name << "'" << std::endl; - - std::filesystem::path path(pc.file_name); + std::string session_fn = get_next_result_path(session.working_directory).string(); + std::filesystem::create_directory(session_fn); - // std::cout << path.parent_path() << std::endl; - // std::cout << path.relative_path() << std::endl; - // std::cout << path.root_directory() << std::endl; + session_file_name = (fs::path(session_fn) / "newSession.mjs").string(); + session.point_clouds_container.initial_poses_file_name = "dummy"; //non empty file names signal poses present, new file names will be created on save + session.point_clouds_container.poses_file_name = "dummy"; //non empty file names signal poses present, new file names will be created on save - auto fn = path.filename(); + std::string output_file_name = saveSession(); - path.remove_filename(); - - // std::cout << path << std::endl; - - path /= "session_ground_truth"; - - path_ground_truth_session_folder = path; + if (output_file_name.size() > 0) + { + //creating filenames proposal based on current selection + std::filesystem::path path(output_file_name); + // Extract parts + const auto dir = path.parent_path(); + const auto stem = path.stem().string(); - if (std::filesystem::create_directory(path)) - { - std::cout << "Directory '" << path << "' created successfully.\n"; - } - else + // save to folder + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) { - std::cout << "Directory '" << path << "' already exists or failed to create.\n"; - } - - auto fpath = path; - fpath /= fn; - - std::cout << fpath << std::endl; - - std::cout << "saving file: '" << fpath << "'" << std::endl; - - exportLaz(fpath.string(), pc.points_local, pc.intensities, pc.timestamps); + auto pc = session.point_clouds_container.point_clouds[i]; + auto fpath = dir / fs::path(pc.file_name).filename(); - //-- - fpath = path; - fpath /= "session_ground_truth.mjs"; + session.point_clouds_container.point_clouds[i].file_name = fpath.string(); + exportLaz(fpath.string(), pc.points_local, pc.intensities, pc.timestamps); - auto initial_poses_file_name = path; - initial_poses_file_name /= "session_ini_poses.mri"; - session.point_clouds_container.initial_poses_file_name = initial_poses_file_name.string(); + //saving terajectory file + auto pathtrj = dir / ("trajectory_lio_" + std::to_string(i) + ".csv"); + std::cout << "Saving trajectory: " << pathtrj << std::endl; - auto poses_file_name = path; - poses_file_name /= "session_poses.mrp"; - session.point_clouds_container.poses_file_name = poses_file_name.string(); - - saveSession(fpath.string()); - - //saving terajectory files - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++){ - // save trajectory - - auto pathtrj = path; - - std::string trajectory_filename = ("trajectory_lio_" + std::to_string(i) + ".csv"); - pathtrj /= trajectory_filename; - std::cout << "saving to: " << pathtrj << std::endl; - - /// std::ofstream outfile; outfile.open(pathtrj); if (!outfile.good()) { - std::cout << "can not save file: " << pathtrj << std::endl; + std::cout << "Can not save file: " << pathtrj << std::endl; return; } @@ -1674,15 +1629,20 @@ void openLaz(bool ground_truth) } outfile.close(); } - - } - std::string mes = "Session saved to folder '" + path_ground_truth_session_folder.string() + "'"; - [[maybe_unused]] pfd::message message( - "Ground truth session info", mes, + [[maybe_unused]] + pfd::message message( + "Information", + "If you can not see point cloud --> 1. Change 'Points render subsampling', 2. Check console 'min max coordinates should be small numbers to see points in our local coordinate system'. 3. Set checkbox 'calculate_offset for WHU-TLS'. 4. Later on You can change offset directly in session json file.", pfd::choice::ok, pfd::icon::info); message.result(); + + //std::string mes = "Session saved to folder '" + path_ground_truth_session_folder.string() + "'"; + //[[maybe_unused]] pfd::message message( + // "Ground truth session info", mes, + // pfd::choice::ok, pfd::icon::info); + //message.result(); } } } @@ -1692,14 +1652,14 @@ void saveSubsession() int inx_begin = 0; int inx_end = 0; - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++){ + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++){ if (session.point_clouds_container.point_clouds[i].visible){ inx_begin = i; break; } } - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) if (session.point_clouds_container.point_clouds[i].visible) inx_end = i; @@ -1745,31 +1705,18 @@ void saveSubsession() void project_gui() { ImGui::Begin("Settings"); + { + std::string wd = "Working directory: '" + session.working_directory + "'"; - ImGui::Checkbox("Simple_gui", &simple_gui); - ImGui::SameLine(); - ImGui::Checkbox("Is ground truth", &session.is_ground_truth); - - std::string wd = "working directory: '" + session.working_directory + "'"; - - ImGui::Checkbox("show_imu_to_lio_diff", &session.point_clouds_container.show_imu_to_lio_diff); - - ImGui::Text(wd.c_str()); + ImGui::Text(wd.c_str()); - ImGui::NewLine(); + ImGui::NewLine(); - if (ImGui::Button("Set initial pose to Identity and update other poses")) - initial_pose_to_identity(session); + if (ImGui::Button("Set initial pose to Identity and update other poses")) + initial_pose_to_identity(session); - if (!simple_gui) - { ImGui::NewLine(); - ImGui::Text("Offset [m] x: %.10f y: %.10f z: %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); - ImGui::SameLine(); - if (ImGui::Button("Print offset to console")) - std::cout << "Offset: " << std::setprecision(10) << session.point_clouds_container.offset << std::endl; - ImGui::Checkbox("Downsample during load", &tls_registration.is_decimate); ImGui::Text("Bucket [m]:"); @@ -1848,19 +1795,6 @@ void project_gui() ImGui::NewLine(); - if (ImGui::Button("Load aligned point cloud from WHU-TLS")) - openLaz(false); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Select all *.las/*.laz files in folder 2-AlignedPointCloud"); - - ImGui::SameLine(); - ImGui::Checkbox("Calculate_offset for WHU-TLS", &tls_registration.calculate_offset); - ImGui::Text("WHU-TLS dataset: "); - ImGui::SameLine(); - ImGuiHyperlink("http://3s.whu.edu.cn/ybs/en/benchmark.htm"); - - ImGui::NewLine(); - if (ImGui::Button("Load 3DTK files (select all *.txt files)")) { session.point_clouds_container.point_clouds.clear(); @@ -1960,13 +1894,10 @@ void project_gui() } ImGui::SameLine(); ImGui::Text(session.point_clouds_container.poses_file_name.c_str()); - } - ImGui::Separator(); + ImGui::Separator(); - if (!is_loop_closure_gui) - { - if (!simple_gui) + if (!is_loop_closure_gui) { static double x_origin = 0.0; static double y_origin = 0.0; @@ -1993,7 +1924,7 @@ void project_gui() if (session.point_clouds_container.point_clouds.size() != 0) { std::vector all_m_poses2; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) { all_m_poses2.push_back(session.point_clouds_container.point_clouds[j].m_pose); } @@ -2013,7 +1944,7 @@ void project_gui() session.point_clouds_container.point_clouds[0].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); Eigen::Affine3d curr_m_pose2 = session.point_clouds_container.point_clouds[0].m_pose; - for (int j = 1; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = 1; j < session.point_clouds_container.point_clouds.size(); j++) { curr_m_pose2 = curr_m_pose2 * (all_m_poses2[j - 1].inverse() * all_m_poses2[j]); @@ -2033,52 +1964,47 @@ void project_gui() } ImGui::Separator(); - } - - ImGui::Text("Set offsets to export point cloud in global coordinate system [m]:"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("each local coordinate of the point += offset"); - ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputDouble("X##t", &session.point_clouds_container.offset.x(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(xText); - ImGui::SameLine(); - ImGui::InputDouble("Y##t", &session.point_clouds_container.offset.y(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(yText); - ImGui::SameLine(); - ImGui::InputDouble("Z##t", &session.point_clouds_container.offset.z(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(zText); - ImGui::PopItemWidth(); - if (ImGui::Button("Set offset_to_apply --> from session (X, Y, Z)")) - session.point_clouds_container.offset = session.point_clouds_container.offset_to_apply; + ImGui::Text("Set offsets to export point cloud in global coordinate system [m]:"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("each local coordinate of the point += offset"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("X##t", &session.point_clouds_container.offset.x(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("Y##t", &session.point_clouds_container.offset.y(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("Z##t", &session.point_clouds_container.offset.z(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); - if (!simple_gui) - { + if (ImGui::Button("Set offset_to_apply --> from session (X, Y, Z)")) + session.point_clouds_container.offset = session.point_clouds_container.offset_to_apply; + /* ImGui::NewLine(); ImGui::NewLine(); ImGui::NewLine(); - ImGui::Separator(); ImGui::NewLine(); ImGui::NewLine(); ImGui::NewLine(); + ImGui::Separator(); ImGui::Text("Perform experiment on:"); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Experiments to compare methods and approaches for multi-view TLS registration"); ImGui::SameLine(); if (ImGui::Button("WINDOWS")) - { perform_experiment_on_windows(session, observation_picking, tls_registration.icp, tls_registration.ndt, tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); - } ImGui::SameLine(); if (ImGui::Button("LINUX")) - { perform_experiment_on_linux(session, observation_picking, tls_registration.icp, tls_registration.ndt, tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); - } + */ } } + ImGui::End(); } @@ -2254,7 +2180,7 @@ void display() if (session.point_clouds_container.point_clouds[i].gizmo) { std::vector all_m_poses; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) { all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); } @@ -2307,7 +2233,7 @@ void display() if (!manipulate_only_marked_gizmo) { Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; - for (int j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) + for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; @@ -2559,26 +2485,19 @@ void display() ImGui::SameLine(); if (ImGui::ArrowButton("##menuArrow", ImGuiDir_Down)) - { ImGui::OpenPopup("OpenMenu"); - } if (ImGui::BeginPopup("OpenMenu")) { - if (ImGui::MenuItem("Open las/laz")){ - openLaz(false); - } - if (ImGui::MenuItem("Create ground truth session from las/laz")) - { - openLaz(true); - } - - if (ImGui::IsItemHovered()){ - ImGui::SetTooltip("Create session from las/laz files"); - } - - ImGui::MenuItem("Calculate_offset", nullptr, &tls_registration.calculate_offset); + ImGui::MenuItem("Fill in session", nullptr, &fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Fill in data for trajectory and pose to create complete session"); + + if (ImGui::MenuItem("Open las/laz")) + openLaz(fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Create session from las/laz file(s)"); ImGui::EndPopup(); } @@ -2880,7 +2799,7 @@ void display() { if (p.visible) { - for (int i = 0; i < p.points_local.size(); i++) + for (size_t i = 0; i < p.points_local.size(); i++) { const auto& pp = p.points_local[i]; Eigen::Vector3d vp; @@ -3116,6 +3035,8 @@ void display() } ImGui::EndDisabled(); + ImGui::MenuItem("Show IMU to LIO difference", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + ImGui::Separator(); } ImGui::EndDisabled(); @@ -3185,7 +3106,6 @@ void display() if (ImGui::BeginMenu("Console")) { #ifdef _WIN32 - if (ImGui::MenuItem("Use Windows console", nullptr, &consWin)) { if (consWin) @@ -3372,7 +3292,7 @@ void mouse(int glut_button, int state, int x, int y) int i = session.control_points.index_pose; if (session.control_points.index_pose >= 0 && session.control_points.index_pose < session.point_clouds_container.point_clouds.size()) { - for (int j = 0; j < session.point_clouds_container.point_clouds[i].points_local.size(); j++) + for (size_t j = 0; j < session.point_clouds_container.point_clouds[i].points_local.size(); j++) { const auto &p = session.point_clouds_container.point_clouds[i].points_local[j]; Eigen::Vector3d vp = session.point_clouds_container.point_clouds[i].m_pose * p; diff --git a/core/include/export_laz.h b/core/include/export_laz.h index 3e61be4b..abd08db8 100644 --- a/core/include/export_laz.h +++ b/core/include/export_laz.h @@ -21,8 +21,8 @@ inline bool exportLaz(const std::string &filename, constexpr float scale = 0.0001f; // one tenth of milimeter // find max - Eigen::Vector3d _max(-1000000000.0, -1000000000.0, -1000000000.0); - Eigen::Vector3d _min(1000000000.0, 1000000000.0, 1000000000.0); + Eigen::Vector3d _max(-std::numeric_limits::max(), -std::numeric_limits::max(), -std::numeric_limits::max()); + Eigen::Vector3d _min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); for (auto &p : pointcloud) { diff --git a/core/include/utils.hpp b/core/include/utils.hpp index 14b9e537..9d925fc7 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -113,7 +113,9 @@ Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationP LaserBeam GetLaserBeam(int x, int y); double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); void getClosestTrajectoryPoint(Session& session, int x, int y, bool gcpPicking, int& picked_index); -void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, - int &index_loop_closure_source, int &index_loop_closure_target, int ctrl_shift); +void getClosestTrajectoriesPoint(std::vector& sessions, int x, int y, + const int first_session_index, const int second_session_index, const int number_visible_sessions, + int& index_loop_closure_source, int& index_loop_closure_target, bool KeyShift); + void setNewRotationCenter(int x, int y); \ No newline at end of file diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp index fef8efc2..2d733185 100644 --- a/core/src/pfd_wrapper.cpp +++ b/core/src/pfd_wrapper.cpp @@ -21,6 +21,11 @@ namespace mandeye::fd{ files = pfd::open_file(title, internal::lastLocationHint, filter, multiselect).result(); + for (auto& f : files) + { + f = std::filesystem::path(f).lexically_normal().string(); + } + if (!files.empty()) { std::filesystem::path pfile(files.back()); diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 0ab0c3f9..343606fc 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #ifdef _WIN32 #include @@ -38,7 +39,7 @@ bool compass_ruler = true; Eigen::Affine3f viewLocal; Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); -float rotate_x = 0.0, rotate_y = 0.0; +float rotate_x = -35.264f, rotate_y = 135.0f; float translate_x, translate_y = 0.0; float translate_z = -50.0; @@ -165,6 +166,8 @@ std::string truncPath(const std::string &fullPath) return "..\\" + parent + "\\..\\" + filename; } + + void wheel(int button, int dir, int x, int y) { ImGuiIO &io = ImGui::GetIO(); @@ -1287,48 +1290,17 @@ void getClosestTrajectoryPoint(Session &session, int x, int y, bool gcpPicking, camera_transition_active = true; } -void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, int &index_loop_closure_source, int &index_loop_closure_target, int ctrl_shift) +void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, const int first_session_index, const int second_session_index, const int number_visible_sessions, int &index_loop_closure_source, int &index_loop_closure_target, bool KeyShift) { - std::cout << "getClosestTrajectoriesPoint" << std::endl; - - int first_session_index = -1; - int second_session_index = -1; - int number_visible_sessions = 0; - - bool first_session_index_found = false; - for (int index = 0; index < sessions.size(); index++) - { - if (sessions[index].visible) - { - number_visible_sessions++; - if (!first_session_index_found) - { - first_session_index = index; - second_session_index = index; - first_session_index_found = true; - } - else - { - second_session_index = index; - } - } - } - - std::cout << "first_session_index: " << first_session_index << std::endl; - std::cout << "second_session_index: " << second_session_index << std::endl; - std::cout << "number_visible_sessions: " << number_visible_sessions << std::endl; - // picked_index = -1; - // picked_session = nullptr; - const auto laser_beam = GetLaserBeam(x, y); double min_distance = std::numeric_limits::max(); if (number_visible_sessions == 1) { // std::cout << "first_session_index " << first_session_index << std::endl; - for (int i = 0; i < sessions[first_session_index].point_clouds_container.point_clouds.size(); i++) + for (size_t i = 0; i < sessions[first_session_index].point_clouds_container.point_clouds.size(); i++) { - for (int j = 0; j < sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) + for (size_t j = 0; j < sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) { const auto &p = sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); Eigen::Vector3d vp = sessions[first_session_index].point_clouds_container.point_clouds[i].m_pose * p; @@ -1341,14 +1313,14 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, i if (number_visible_sessions == 1) { - if (ctrl_shift == 0) + if (!KeyShift) // io.KeyCtrl { new_rotation_center.x() = static_cast(vp.x()); new_rotation_center.y() = static_cast(vp.y()); new_rotation_center.z() = static_cast(vp.z()); index_loop_closure_source = i; } - if (ctrl_shift == 1) + else // io.KeyShift { index_loop_closure_target = i; } @@ -1357,51 +1329,21 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, i } } } - - if (number_visible_sessions > 2) - { - // std::cout << "first_session_index " << first_session_index << std::endl; - for (int s = 0; s < sessions.size(); s++){ - if (sessions[s].visible){ - - for (int i = 0; i < sessions[s].point_clouds_container.point_clouds.size(); i++) - { - for (int j = 0; j < sessions[s].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) - { - const auto &p = sessions[s].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); - Eigen::Vector3d vp = sessions[s].point_clouds_container.point_clouds[i].m_pose * p; - - double dist = distance_point_to_line(vp, laser_beam); - - if (dist < min_distance) - { - min_distance = dist; - - new_rotation_center.x() = static_cast(vp.x()); - new_rotation_center.y() = static_cast(vp.y()); - new_rotation_center.z() = static_cast(vp.z()); - } - } - } - } - } - } - - if (number_visible_sessions == 2) + else if (number_visible_sessions == 2) { int index = -1; - if (ctrl_shift == 0) + if (!KeyShift) // io.KeyCtrl { index = first_session_index; } - if (ctrl_shift == 1) + else // io.KeyShift { index = second_session_index; } // std::cout << "first_session_index " << first_session_index << std::endl; - for (int i = 0; i < sessions[index].point_clouds_container.point_clouds.size(); i++) + for (size_t i = 0; i < sessions[index].point_clouds_container.point_clouds.size(); i++) { - for (int j = 0; j < sessions[index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) + for (size_t j = 0; j < sessions[index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) { const auto &p = sessions[index].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); Eigen::Vector3d vp = sessions[index].point_clouds_container.point_clouds[i].m_pose * p; @@ -1412,92 +1354,49 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, i { min_distance = dist; - if (ctrl_shift == 0) + if (!KeyShift) // io.KeyCtrl { index_loop_closure_source = i; new_rotation_center.x() = static_cast(vp.x()); new_rotation_center.y() = static_cast(vp.y()); new_rotation_center.z() = static_cast(vp.z()); } - if (ctrl_shift == 1) + else // io.KeyShift { index_loop_closure_target = i; - } - + } } } } } - -#if 0 - // for (auto &session : sessions) - for (int index = 0; index < sessions.size(); index++) + else // (number_visible_sessions > 2) { - auto &session = sessions[index]; - - if (number_visible_sessions > 2) - { - if (!session.visible) - { - continue; - } + // std::cout << "first_session_index " << first_session_index << std::endl; + for (size_t s = 0; s < sessions.size(); s++) { + if (sessions[s].visible) { - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - for (int j = 0; j < session.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) + for (size_t i = 0; i < sessions[s].point_clouds_container.point_clouds.size(); i++) { - const auto &p = session.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); - Eigen::Vector3d vp = session.point_clouds_container.point_clouds[i].m_pose * p; - - double dist = distance_point_to_line(vp, laser_beam); - - if (dist < min_distance) + for (size_t j = 0; j < sessions[s].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) { - min_distance = dist; + const auto& p = sessions[s].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); + Eigen::Vector3d vp = sessions[s].point_clouds_container.point_clouds[i].m_pose * p; - new_rotation_center.x() = static_cast(vp.x()); - new_rotation_center.y() = static_cast(vp.y()); - new_rotation_center.z() = static_cast(vp.z()); + double dist = distance_point_to_line(vp, laser_beam); + + if (dist < min_distance) + { + min_distance = dist; - // picked_index = i; - // picked_session = session; + new_rotation_center.x() = static_cast(vp.x()); + new_rotation_center.y() = static_cast(vp.y()); + new_rotation_center.z() = static_cast(vp.z()); + } } } } } - - - if (number_visible_sessions == 2) - { - } - /*if (!session.visible) - { - continue; - } - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - for (int j = 0; j < session.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) - { - const auto &p = session.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); - Eigen::Vector3d vp = session.point_clouds_container.point_clouds[i].m_pose * p; - - double dist = distance_point_to_line(vp, laser_beam); - - if (dist < min_distance) - { - min_distance = dist; - - new_rotation_center.x() = static_cast(vp.x()); - new_rotation_center.y() = static_cast(vp.y()); - new_rotation_center.z() = static_cast(vp.z()); - - // picked_index = i; - // picked_session = session; - } - } - }*/ } -#endif new_translate_x = -new_rotation_center.x(); new_translate_y = -new_rotation_center.y();