From a2fa976bc140228ead1220f532ad5ef7cfc0931b Mon Sep 17 00:00:00 2001 From: yuckinus <40965122+yuckinus@users.noreply.github.com> Date: Wed, 3 Dec 2025 23:00:01 +0200 Subject: [PATCH] Fixes and improvements - improve docking support: + no docking for Info window + docking only active while Shift key pressed while window dragging + docking only active on screen edges - step1 / optimize step1 (load_data, calculate_trajectory, compute_step_1) (insignificant impact in overall processing time/effort) - step 2 / unify center of rotation selection to closest trajectory point (like step3) - viewer / unify center of rotation selection to closest point from cloud segment - added Shift+R for manual selection of new center of rotation - small fixes --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 697 +++++++++--------- .../lidar_odometry_gui.cpp | 25 +- .../lidar_odometry_utils_optimizers.cpp | 33 +- .../mandeye_mission_recorder_calibration.cpp | 2 +- .../mandeye_single_session_viewer.cpp | 41 +- .../multi_session_registration.cpp | 21 +- .../multi_view_tls_registration_gui.cpp | 77 +- core/include/utils.hpp | 6 + core/src/utils.cpp | 178 ++++- 9 files changed, 582 insertions(+), 498 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index e0db967c..9fdfb91d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -4,13 +4,15 @@ namespace fs = std::filesystem; -bool load_data(std::vector &input_file_names, - LidarOdometryParams ¶ms, - std::vector> &pointsPerFile, - Imu &imu_data, +const float RAD_TO_DEG = 180.0f / M_PI; + +bool load_data(std::vector& input_file_names, + LidarOdometryParams& params, + std::vector>& pointsPerFile, + Imu& imu_data, bool debugMsg) { - std::sort(std::begin(input_file_names), std::end(input_file_names)); + std::sort(input_file_names.begin(), input_file_names.end()); std::vector csv_files, laz_files; std::string sn_file; @@ -18,318 +20,289 @@ bool load_data(std::vector &input_file_names, std::cout << "Loading data..\n--------------\n"; + // --- Separate files by type + for (const auto& fileName : input_file_names) + { + std::string ext = fs::path(fileName).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); - std::for_each(std::begin(input_file_names), std::end(input_file_names), [&](const std::string &fileName) + if (ext == ".csv") + csv_files.push_back(fileName); + else if (ext == ".json") { - if (fileName.ends_with(".csv")) - csv_files.push_back(fileName); - - if (fileName.ends_with(".json")) - { - std::string stem = fs::path(fileName).filename().stem().string(); // get filename without extension - - // skip files not related to calibration - if (!stem.starts_with("status") && !stem.starts_with("cam0") && !stem.starts_with("cam1")) - { - if (calibrationFile.empty()) - calibrationFile = fileName; - else - std::cout << "Unexpected JSON file found (" << fileName << "). Ignored.." << std::endl; - } - } + std::string stem = fs::path(fileName).filename().stem().string(); // get filename without extension - if (fileName.ends_with(".laz") || fileName.ends_with(".las")) - laz_files.push_back(fileName); - - if (fileName.ends_with(".mjc")) - { + // skip files not related to calibration + if (!stem.starts_with("status") && !stem.starts_with("cam0") && !stem.starts_with("cam1")) if (calibrationFile.empty()) calibrationFile = fileName; else - std::cout << "Unexpected calibration file found (" << fileName << "). Ignored.." << std::endl; - } + std::cout << "Unexpected JSON file found (" << fileName << "). Ignored..\n"; + } + else if ((ext == ".laz") || (ext == ".las")) + laz_files.push_back(fileName); + else if (ext == ".mjc") + { + if (calibrationFile.empty()) + calibrationFile = fileName; + else + std::cout << "Unexpected calibration file found (" << fileName << "). Ignored.." << std::endl; + } + else if ((ext == ".sn") && sn_file.empty()) + { + sn_file = fileName; + std::cout << "Only using first SN file found\n"; + } + } - if (fileName.ends_with(".sn")) - { - if (sn_file.empty()) - { - sn_file = fileName; - std::cout << "Only using first SN file found" << std::endl; - } - } - }); + auto get4index = [](const std::string& path) -> std::string { + auto s = fs::path(path).stem().string(); + return s.size() > 4 ? s.substr(s.size() - 4) : s; + }; - for (int i = 0; i < laz_files.size(); i++) + for (size_t i = 0; i < laz_files.size(); i++) + { + std::string lfs = get4index(laz_files[i]); + + bool found = false; + for (size_t j = 0; j < csv_files.size(); j++) + { + std::string cfs = get4index(csv_files[j]); + if (lfs.compare(cfs) == 0) { - fs::path lf(laz_files[i]); - std::string lfs = lf.filename().stem().string().substr(lf.filename().stem().string().size() - 4); + found = true; + break; + } + } - bool found = false; - for (int j = 0; j < csv_files.size(); j++) - { - fs::path cf(csv_files[j]); - std::string cfs = cf.filename().stem().string().substr(cf.filename().stem().string().size() - 4); - if (lfs.compare(cfs) == 0) - { - found = true; - break; - } - } + if (!found) + { + std::cout << "\n!!! there is no CSV file for: " << laz_files[i] << " !!! \n\n"; - if (!found) - { - std::cout << "\n!!! there is no CSV file for: " << laz_files[i] << " !!! \n\n"; - - if (i < (laz_files.size() - 1)) - { - std::cout << "!!! can't continue with missmatched middle file pairs.. !!!\n" - << std::endl; - return false; - } - } + if (i < (laz_files.size() - 1)) + { + std::cout << "!!! can't continue with missmatched middle file pairs.. !!!\n\n"; + return false; } + } + } - for (int i = 0; i < csv_files.size(); i++) + for (size_t i = 0; i < csv_files.size(); i++) + { + std::string lfs = get4index(csv_files[i]); + + bool found = false; + for (size_t j = 0; j < laz_files.size(); j++) + { + std::string cfs = get4index(laz_files[j]); + if (lfs.compare(cfs) == 0) { - fs::path lf(csv_files[i]); - std::string lfs = lf.filename().stem().string().substr(lf.filename().stem().string().size() - 4); + found = true; + break; + } + } - bool found = false; - for (int j = 0; j < laz_files.size(); j++) - { - fs::path cf(laz_files[j]); - std::string cfs = cf.filename().stem().string().substr(cf.filename().stem().string().size() - 4); - if (lfs.compare(cfs) == 0) - { - found = true; - break; - } - } + if (!found) + { + std::cout << "\n!!! there is no LAZ file for: " << csv_files[i] << " !!! \n\n"; - if (!found) - { - std::cout << "\n!!! there is no LAZ file for: " << csv_files[i] << " !!! \n\n"; - - if (i < (csv_files.size() - 1)) - { - std::cout << "!!! can't continue with missmatched middle file pairs.. !!!\n" - << std::endl; - return false; - } - } + if (i < (csv_files.size() - 1)) + { + std::cout << "!!! can't continue with missmatched middle file pairs.. !!!\n\n"; + return false; } - std::string working_directory = ""; - std::string imuSnToUse; - std::size_t minSize = std::min(laz_files.size(), csv_files.size()); + } + } - if (input_file_names.size() > 2) - { - working_directory = fs::path(input_file_names[0]).parent_path().string(); + if (input_file_names.size() > 2) + { + std::string working_directory = fs::path(input_file_names.front()).parent_path().string(); - if (laz_files.size() != csv_files.size()) - { - std::cout << "!!! file number issue. continuing but ignoring missmatched last file pair.. !!!\n" - << std::endl; - } + if (laz_files.size() != csv_files.size()) + std::cout << "!!! file number issue. continuing but ignoring missmatched last file pair.. !!!\n" << std::endl; - // check if folder exists! - if (!fs::exists(working_directory)) - { - std::cout << "folder '" << working_directory << "' does not exist" << std::endl; + // check if folder exists! + if (!fs::exists(working_directory)) + { + std::cerr << "Folder '" << working_directory << "' does not exist.\n"; + return false; + } - std::string message_info = "folder '" + working_directory + "' does not exist --> PLEASE REMOVE e.g. POLISH LETTERS from path. !!!PROGRAM WILL SHUT DOWN AFTER THIS MESSAGE!!!"; - return false; - } + // --- Calibration and IMU setup + const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); + const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); + const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); + const int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); + std::size_t minSize = std::min(laz_files.size(), csv_files.size()); - const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); - imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); + if (!preloadedCalibration.empty()) + { + std::cout << "Loaded calibration for:\n"; + for (const auto& [sn, _] : preloadedCalibration) + std::cout << " -> " << sn << "\n\n"; - if (!preloadedCalibration.empty()) - { - std::cout << "Loaded calibration for: \n"; - for (const auto &[sn, _] : preloadedCalibration) - { - std::cout << " -> " << sn << "\n"; - } - std::cout << std::endl; - } - else - { - std::cout << "There is no calibration file in folder!" << std::endl; - std::cout << "IGNORE THIS MESSAGE IF YOU ONLY HAVE 1 LiDAR" << std::endl; - - // example file for 2x LiDAR": - /* - { - "calibration" : { - "47MDL9T0020193" : { - "identity" : "true" - }, - "47MDL9S0020300" : - { - "order" : "ROW", - "inverted" : "TRUE", - "data" : [ - 0.999824, 0.00466397, -0.0181595, -0.00425984, - -0.0181478, -0.00254457, -0.999832, -0.151599, - -0.0047094, 0.999986, -0.00245948, -0.146408, - 0, 0, 0, 1 - ] - } - }, - "imuToUse" : "47MDL9T0020193" - }*/ - } + bool hasError = false; - fs::path wdp = fs::path(input_file_names[0]).parent_path(); - wdp /= "preview"; - if (!fs::exists(wdp)) + for (const auto& [id, sn] : idToSn) + { + if (preloadedCalibration.find(sn) == preloadedCalibration.end()) { - std::cout << "creating folder: '" << wdp << "'" << std::endl; - fs::create_directory(wdp); + std::cerr << "WRONG CALIBRATION FILE! THE SERIAL NUMBER SHOULD BE " << sn << "!!!\n"; + hasError = true; } + } - params.working_directory_preview = wdp.string(); - - fs::path wdp2 = fs::path(input_file_names[0]).parent_path(); - wdp2 /= "cache"; - if (!fs::exists(wdp2)) - { - std::cout << "creating folder: '" << wdp2 << "'" << std::endl; - fs::create_directory(wdp2); - } - params.working_directory_cache = wdp2.string(); + if (!hasError && preloadedCalibration.find(imuSnToUse) == preloadedCalibration.end()) + { + std::cerr << "MISSING CALIBRATION FOR imuSnToUse: " << imuSnToUse << "!!!\n"; + std::cerr << "Available serial numbers in calibration file are:\n"; + for (const auto& [snKey, _] : preloadedCalibration) + std::cerr << " - " << snKey << "\n"; - // for (size_t i = 0; i < input_file_names.size(); i++) - //{ - // std::cout << input_file_names[i] << std::endl; - // } + hasError = true; + } - const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); - // GetId of Imu to use - int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); + if (hasError) + { + std::cerr << "Press ENTER to exit...\n"; + std::cin.get(); + std::exit(EXIT_FAILURE); + } + } + else + { + std::cout << "There is no calibration file in folder!" << std::endl; + std::cout << "IGNORE THIS MESSAGE IF YOU ONLY HAVE 1 LiDAR" << std::endl; - std::cout << "loading IMU data from 'imu****.csv' using ID " << imuNumberToUse - << " from reference '" << sn_file << "' ..\n" - << std::endl; + // example file for 2x LiDAR": + /* + { + "calibration" : { + "47MDL9T0020193" : { + "identity" : "true" + }, + "47MDL9S0020300" : + { + "order" : "ROW", + "inverted" : "TRUE", + "data" : [ + 0.999824, 0.00466397, -0.0181595, -0.00425984, + -0.0181478, -0.00254457, -0.999832, -0.151599, + -0.0047094, 0.999986, -0.00245948, -0.146408, + 0, 0, 0, 1 + ] + } + }, + "imuToUse" : "47MDL9T0020193" + }*/ + } - for (size_t fileNo = 0; fileNo < minSize; fileNo++) - { - const std::string &imufn = csv_files.at(fileNo); - auto imu = load_imu(imufn.c_str(), imuNumberToUse); - imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); + // --- Working directories + fs::path wdp = fs::path(working_directory) / "preview"; + if (!fs::exists(wdp)) + { + std::cout << "Creating folder: " << wdp << "\n"; + fs::create_directory(wdp); + } + params.working_directory_preview = wdp.string(); - bool hasError = false; + fs::path wdp_cache = fs::path(working_directory) / "cache"; + if (!fs::exists(wdp_cache)) + { + std::cout << "Creating folder: " << wdp_cache << "\n"; + fs::create_directory(wdp_cache); + } + params.working_directory_cache = wdp_cache.string(); - if (!preloadedCalibration.empty()) - { - for (const auto &[id, sn] : idToSn) - { - if (preloadedCalibration.find(sn) == preloadedCalibration.end()) - { - std::cerr << "WRONG CALIBRATION FILE! THE SERIAL NUMBER SHOULD BE " << sn << "!!!\n"; - hasError = true; - } - } + // --- Load IMU data (still sequential, could parallelize later) + std::cout << "loading IMU data from 'imu****.csv' using ID " << imuNumberToUse + << " from reference '" << sn_file << "' ..\n\n"; - if (!hasError && preloadedCalibration.find(imuSnToUse) == preloadedCalibration.end()) - { - std::cerr << "MISSING CALIBRATION FOR imuSnToUse: " << imuSnToUse << "!!!\n"; - std::cerr << "Available serial numbers in calibration file are:\n"; - for (const auto &[snKey, _] : preloadedCalibration) - { - std::cerr << " - " << snKey << "\n"; - } - hasError = true; - } + // Each thread loads into its own local vector + std::vector, FusionVector, FusionVector>>> imuChunks(minSize); - if (hasError) - { - std::cerr << "Press ENTER to exit...\n"; - std::cin.get(); - std::exit(EXIT_FAILURE); - } - } + tbb::parallel_for( + tbb::blocked_range(0, minSize), + [&](const tbb::blocked_range& range) { + for (size_t i = range.begin(); i < range.end(); ++i) + { + imuChunks[i] = load_imu(csv_files[i].c_str(), imuNumberToUse); } + }); - std::sort(imu_data.begin(), imu_data.end(), - [](const std::tuple, FusionVector, FusionVector> &a, const std::tuple, FusionVector, FusionVector> &b) - { - return std::get<0>(a).first < std::get<0>(b).first; - }); + // Merge all chunks sequentially at the end + for (auto& chunk : imuChunks) + imu_data.insert(imu_data.end(), std::make_move_iterator(chunk.begin()), std::make_move_iterator(chunk.end())); - std::cout << "loading points..\n"; - pointsPerFile.resize(minSize); - std::mutex mtx; + // Sort once after merging + std::sort(imu_data.begin(), imu_data.end(), + [](const auto& a, const auto& b) { + return std::get<0>(a).first < std::get<0>(b).first; + }); - std::cout << "total - filter = kept\n"; - std::cout << "-----------------------" << std::endl; + // --- Precompute calibration once (major optimization) + const auto combinedCalibration = MLvxCalib::CombineIntoCalibration(idToSn, preloadedCalibration); - std::transform(std::execution::par_unseq, std::begin(laz_files), std::begin(laz_files) + minSize, std::begin(pointsPerFile), [&](const std::string &fn) - { - auto calibration = MLvxCalib::CombineIntoCalibration(idToSn, preloadedCalibration); - auto data = load_point_cloud(fn.c_str(), true, params.filter_threshold_xy_inner, params.filter_threshold_xy_outer, calibration); + std::cout << "Loading point clouds (" << minSize << " files)..\n"; + std::cout << "total - filter = kept\n-----------------------\n"; - std::sort(data.begin(), data.end(), [](const Point3Di &a, const Point3Di &b) - { return a.timestamp < b.timestamp; }); + if (debugMsg) //if true this will break parallelism + { + for (const auto& [id, calib] : combinedCalibration) + std::cout << " id : " << id << "\n" << calib.matrix() << "\n"; + } - if ((fn == laz_files.front()) && (params.save_calibration_validation)) - { - fs::path calibrationValidtationFile = wdp / "calibrationValidation.asc"; - std::ofstream testPointcloud{calibrationValidtationFile.c_str()}; - int row_index = 0; - for (const auto &p : data) - { - if (row_index++ >= params.calibration_validation_points) - { - break; - } - testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid << "\n"; - } - } + pointsPerFile.resize(minSize); - std::unique_lock lck(mtx); + // --- Parallel load of LAZ files + tbb::parallel_for(size_t(0), minSize, [&](size_t i) + { + const std::string& fn = laz_files[i]; - if (debugMsg) - { - for (const auto &[id, calib] : calibration) - { - std::cout << " id : " << id << std::endl; - std::cout << calib.matrix() << std::endl; - } - } + auto data = load_point_cloud(fn.c_str(), true, params.filter_threshold_xy_inner, params.filter_threshold_xy_outer, combinedCalibration); - return data; - // std::cout << fn << std::endl; - // - }); + std::sort(data.begin(), data.end(), [](const Point3Di& a, const Point3Di& b) + { return a.timestamp < b.timestamp; }); - if (pointsPerFile.size() > 0) + // Optional calibration validation (first file only) + if ((i == 0) && params.save_calibration_validation) { - pointsPerFile[0].clear(); + fs::path outFile = wdp / "calibrationValidation.asc"; + std::ofstream testPointcloud(outFile); + if (!testPointcloud) + std::cerr << "Failed to open file: " << outFile << "\n"; + else + for (size_t j = 0; j < std::min(data.size(), (size_t)params.calibration_validation_points); ++j) + { + const auto& p = data[j]; + testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid << "\n"; + } } - int number_of_points = 0; - for (const auto &pp : pointsPerFile) - { - number_of_points += pp.size(); - } - std::cout << "TOTAL: " << number_of_points << std::endl; + // Store results + pointsPerFile[i] = std::move(data); + }); - std::cout << "..loading finished\n" - << std::endl; + if (pointsPerFile.size() > 0) + pointsPerFile.front().clear(); - return true; - } - else - { - std::cout << "Not enought files to continue: " << input_file_names.size() << std::endl; - return false; - } + // --- Summary + size_t totalPoints = 0; + for (const auto& pp : pointsPerFile) + totalPoints += pp.size(); + + std::cout << "TOTAL: " << totalPoints << "\n..loading finished.\n\n"; + return true; + } + else + { + std::cout << "Not enough files to continue: " << input_file_names.size() << std::endl; + return false; + } } void calculate_trajectory( - Trajectory &trajectory, Imu &imu_data, bool fusionConventionNwu, bool fusionConventionEnu, bool fusionConventionNed, double ahrs_gain, bool debugMsg, bool use_removie_imu_bias_from_first_stationary_scan) + Trajectory& trajectory, Imu& imu_data, bool fusionConventionNwu, bool fusionConventionEnu, bool fusionConventionNed, double ahrs_gain, bool debugMsg, bool use_remove_imu_bias_from_first_stationary_scan) { FusionAhrs ahrs; FusionAhrsInitialise(&ahrs); @@ -354,7 +327,7 @@ void calculate_trajectory( double bias_gyr_y = 0.0; double bias_gyr_z = 0.0; - if (use_removie_imu_bias_from_first_stationary_scan) + if (use_remove_imu_bias_from_first_stationary_scan) { if (imu_data.size() > 1000) { @@ -364,16 +337,17 @@ void calculate_trajectory( for (int i = 0; i < 1000; i++) { - const auto &[timestamp_pair, gyr, acc] = imu_data[i]; + const auto& [timestamp_pair, gyr, acc] = imu_data[i]; - gyr_x.push_back(gyr.axis.x * 180.0 / M_PI); - gyr_y.push_back(gyr.axis.y * 180.0 / M_PI); - gyr_z.push_back(gyr.axis.z * 180.0 / M_PI); + gyr_x.push_back(gyr.axis.x * RAD_TO_DEG); + gyr_y.push_back(gyr.axis.y * RAD_TO_DEG); + gyr_z.push_back(gyr.axis.z * RAD_TO_DEG); } - std::sort(gyr_x.begin(), gyr_x.end()); - std::sort(gyr_y.begin(), gyr_y.end()); - std::sort(gyr_z.begin(), gyr_z.end()); + // Median without full sort + std::nth_element(gyr_x.begin(), gyr_x.begin() + 500, gyr_x.end()); + std::nth_element(gyr_y.begin(), gyr_y.begin() + 500, gyr_y.end()); + std::nth_element(gyr_z.begin(), gyr_z.begin() + 500, gyr_z.end()); bias_gyr_x = gyr_x[500]; bias_gyr_y = gyr_y[500]; @@ -388,10 +362,10 @@ void calculate_trajectory( std::cout << "------------------" << std::endl; } - for (const auto &[timestamp_pair, gyr, acc] : imu_data) + for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = {static_cast(gyr.axis.x * 180.0 / M_PI) - bias_gyr_x, static_cast(gyr.axis.y * 180.0 / M_PI) - bias_gyr_y, static_cast(gyr.axis.z * 180.0 / M_PI) - bias_gyr_z}; - const FusionVector accelerometer = {acc.axis.x, acc.axis.y, acc.axis.z}; + const FusionVector gyroscope = { static_cast(gyr.axis.x * RAD_TO_DEG) - bias_gyr_x, static_cast(gyr.axis.y * RAD_TO_DEG) - bias_gyr_y, static_cast(gyr.axis.z * RAD_TO_DEG) - bias_gyr_z }; + const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; /*if (provious_time_stamp == 0.0) { @@ -447,15 +421,15 @@ void calculate_trajectory( FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - Eigen::Quaterniond d{quat.element.w, quat.element.x, quat.element.y, quat.element.z}; - Eigen::Affine3d t{Eigen::Matrix4d::Identity()}; + Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; t.rotate(d); trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); if (debugMsg) { - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); + const FusionEuler euler = FusionQuaternionToEuler(quat); counter++; if (counter % 100 == 0) { @@ -468,14 +442,18 @@ void calculate_trajectory( } bool compute_step_1( - std::vector> &pointsPerFile, LidarOdometryParams ¶ms, Trajectory &trajectory, std::vector &worker_data, const std::atomic& pause) + std::vector>& pointsPerFile, LidarOdometryParams& params, Trajectory& trajectory, std::vector& worker_data, const std::atomic& pause) { int number_of_initial_points = 0; double timestamp_begin = 0.0; - for (const auto &pp : pointsPerFile) + + // Step 1: Pre-reserve to avoid vector reallocations + params.initial_points.reserve(params.threshold_initial_points); + + for (const auto& pp : pointsPerFile) { // number_of_points += pp.size(); - for (const auto &p : pp) + for (const auto& p : pp) { number_of_initial_points++; params.initial_points.push_back(p); @@ -486,26 +464,28 @@ bool compute_step_1( } } if (number_of_initial_points > params.threshold_initial_points) - { break; - } } // std::cout << "timestamp_begin: " << timestamp_begin << std::endl; std::vector> timestamps; std::vector poses; - for (const auto &t : trajectory) + timestamps.reserve(trajectory.size()); + poses.reserve(trajectory.size()); + + for (const auto& [time, pose_pair] : trajectory) { - if (t.first >= timestamp_begin) - { - timestamps.emplace_back(t.first, t.second.second); - Eigen::Affine3d m; - m.matrix() = t.second.first; - poses.push_back(m); - } + if (time < timestamp_begin) + continue; + + timestamps.emplace_back(time, pose_pair.second); + poses.emplace_back(pose_pair.first); } + timestamps.shrink_to_fit(); + poses.shrink_to_fit(); + std::cout << "number of poses: " << poses.size() << std::endl; if (poses.empty()) @@ -516,57 +496,56 @@ bool compute_step_1( int threshold = params.threshold_nr_poses; - int index_begin = 0; - const int n_iter = std::floor(poses.size() / threshold); + const int n_iter = poses.size() / threshold; + worker_data.clear(); worker_data.reserve(n_iter); - for (int i = 0; i < n_iter; i++) + for (size_t i = 0; i < n_iter; i++) { if (i % 50 == 0) - { std::cout << "\rrunning iterations: " << i + 1 << "/" << n_iter << std::flush; - } - WorkerData wd; + + WorkerData& wd = worker_data.emplace_back(); // construct in-place wd.intermediate_trajectory.reserve(threshold); wd.intermediate_trajectory_motion_model.reserve(threshold); wd.intermediate_trajectory_timestamps.reserve(threshold); wd.imu_om_fi_ka.reserve(threshold); - for (int ii = 0; ii < threshold; ii++) + + const size_t start_idx = i * threshold; + const size_t end_idx = (i + 1) * threshold; + + for (size_t idx = start_idx; idx < end_idx; idx++) { - int idx = i * threshold + ii; wd.intermediate_trajectory.emplace_back(poses[idx]); wd.intermediate_trajectory_motion_model.emplace_back(poses[idx]); wd.intermediate_trajectory_timestamps.emplace_back(timestamps[idx]); - TaitBryanPose tb = pose_tait_bryan_from_affine_matrix(poses[idx]); + + const TaitBryanPose tb = pose_tait_bryan_from_affine_matrix(poses[idx]); // wd.imu_roll_pitch.emplace_back(tb.om, tb.fi); wd.imu_om_fi_ka.emplace_back(tb.om, tb.fi, tb.ka); } + std::vector points; - bool found = false; - auto lower = pointsPerFile[0].begin(); - for (int index = index_begin; index < pointsPerFile.size(); index++) + + for (size_t index = 0; index < pointsPerFile.size(); index++) { auto lower = std::lower_bound( pointsPerFile[index].begin(), pointsPerFile[index].end(), wd.intermediate_trajectory_timestamps[0].first, - [](const Point3Di &point, double timestamp) + [](const Point3Di& point, double timestamp) { return point.timestamp < timestamp; }); auto upper = std::lower_bound( pointsPerFile[index].begin(), pointsPerFile[index].end(), wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first, - [](const Point3Di &point, double timestamp) + [](const Point3Di& point, double timestamp) { return point.timestamp < timestamp; }); - auto tmp = std::distance(lower, upper); points.reserve(points.size() + std::distance(lower, upper)); if (lower != upper) - { points.insert(points.end(), std::make_move_iterator(lower), std::make_move_iterator(upper)); - found = true; - } } // wd.original_points = points; @@ -584,96 +563,76 @@ bool compute_step_1( // std::cout << "ts_step " << ts_step << std::endl; // std::cout << "ts_end " << wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first << std::endl; - for (int pp = 0; pp < points.size(); pp++) - { + for (size_t pp = 0; pp < points.size(); pp++) points[pp].timestamp = ts_begin + pp * ts_step; - } } ////////////////////////////////////////// - for (unsigned long long int k = 0; k < points.size(); k++) + for (size_t k = 0; k < points.size(); k++) { - Point3Di &p = points[k]; + Point3Di& p = points[k]; auto lower = std::lower_bound(wd.intermediate_trajectory_timestamps.begin(), wd.intermediate_trajectory_timestamps.end(), p.timestamp, [](std::pair lhs, double rhs) -> bool { return lhs.first < rhs; }); // p.index_pose = std::distance(wd.intermediate_trajectory_timestamps.begin(), lower); int index_pose = std::distance(wd.intermediate_trajectory_timestamps.begin(), lower) - 1; - p.index_pose = index_pose; - if (index_pose >= 0 && index_pose < wd.intermediate_trajectory_timestamps.size()) - { - if (fabs(p.timestamp - wd.intermediate_trajectory_timestamps[index_pose].first) > 0.01) - { - p.index_pose = -1; - } - } + if (index_pose >= 0 && index_pose < wd.intermediate_trajectory_timestamps.size() && + std::abs(p.timestamp - wd.intermediate_trajectory_timestamps[index_pose].first) <= 0.01) + p.index_pose = index_pose; else - { p.index_pose = -1; - } } - std::vector filtered_points; - for (unsigned long long int k = 0; k < points.size(); k++) - { - if (points[k].index_pose != -1) - { - filtered_points.push_back(points[k]); - } - } - // wd.original_points = filtered_points; + auto it = std::remove_if(points.begin(), points.end(), + [](const Point3Di& p) { return p.index_pose == -1; }); + points.erase(it, points.end()); - std::filesystem::path path = params.working_directory_cache; // ToDo - // path /= "cache"; - std::string original_points_cache_filename = "original_points_" + std::to_string(worker_data.size()) + ".cache"; - path /= original_points_cache_filename; - wd.original_points_cache_file_name = path; - - // std::cout << "original_points_cache_file_name " << path.string() << std::endl; - // wd.save_points(filtered_points, wd.original_points_cache_file_name); - // wd.original_points = filtered_points; - if (!save_vector_data(wd.original_points_cache_file_name.string(), filtered_points)) + if (points.size() <= 1000) + worker_data.pop_back(); + else { - std::cout << "problem with save_vector_data file '" << wd.original_points_cache_file_name.string() << "'" << std::endl; - std::cout << __FILE__ << " " << __LINE__ << std::endl; - } + std::vector intermediate_points; - std::vector intermediate_points; + if (params.decimation > 0.0 && points.size() > 1000) + intermediate_points = decimate(points, params.decimation, params.decimation, params.decimation); - if (params.decimation > 0.0 && filtered_points.size() > 1000) - { - intermediate_points = decimate(filtered_points, params.decimation, params.decimation, params.decimation); - } + std::filesystem::path path = params.working_directory_cache; - std::filesystem::path path2 = params.working_directory_cache; - // path2 /= "cache"; - std::string intermediate_points_cache_filename = "intermediate_points_" + std::to_string(worker_data.size()) + ".cache"; - path2 /= intermediate_points_cache_filename; - wd.intermediate_points_cache_file_name = path2; + std::string original_points_cache_filename = "original_points_" + std::to_string(worker_data.size()) + ".cache"; + wd.original_points_cache_file_name = path / original_points_cache_filename; - // remove "flooding" messages from console. this also interfered with previous message of "running iterations: i/total" - // std::cout << "intermediate_points_cache_file_name " << path2.string() << std::endl; + // std::cout << "original_points_cache_file_name " << path.string() << std::endl; + // wd.save_points(filtered_points, wd.original_points_cache_file_name); + // wd.original_points = filtered_points; + if (!save_vector_data(wd.original_points_cache_file_name.string(), points)) + { + std::cout << "problem with save_vector_data file '" << wd.original_points_cache_file_name.string() << "'" << std::endl; + std::cout << __FILE__ << " " << __LINE__ << std::endl; + } - // wd.save_points(intermediate_points, wd.intermediate_points_cache_file_name); - if (!save_vector_data(wd.intermediate_points_cache_file_name.string(), intermediate_points)) - { - std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" << std::endl; - } + std::string intermediate_points_cache_filename = "intermediate_points_" + std::to_string(worker_data.size()) + ".cache"; + wd.intermediate_points_cache_file_name = path / intermediate_points_cache_filename; - if (filtered_points.size() > 1000) - { - worker_data.push_back(wd); + // remove "flooding" messages from console. this also interfered with previous message of "running iterations: i/total" + // std::cout << "intermediate_points_cache_file_name " << path2.string() << std::endl; + + if (intermediate_points.size() > 0) + { + if (!save_vector_data(wd.intermediate_points_cache_file_name.string(), intermediate_points)) + std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" << std::endl; + } + else //ensure we always have intermediate points + { + if (!save_vector_data(wd.intermediate_points_cache_file_name.string(), points)) + std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" << std::endl; + } } if (pause) - { while (pause) - { std::this_thread::sleep_for(std::chrono::seconds(1)); - } - } } std::cout << "\rrunning iterations: " << n_iter << " (done)" << std::flush; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 5e76a4e0..b74365bf 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -86,6 +86,7 @@ static const std::vector appShortcuts = { {"", "Ctrl+Q", ""}, {"", "R", ""}, {"", "Ctrl+R", ""}, + {"", "Shift+R", ""}, {"", "S", ""}, {"", "Ctrl+S", ""}, {"", "Ctrl+Shift+S", ""}, @@ -125,6 +126,7 @@ static const std::vector appShortcuts = { {"", "Right click + drag", "n"}, {"", "Scroll", ""}, {"", "Shift + scroll", ""}, + {"", "Shift + drag", ""}, {"", "Ctrl + left click", ""}, {"", "Ctrl + right click", ""}, {"", "Ctrl + middle click", ""} }; @@ -638,9 +640,7 @@ void step2(const std::atomic &loPause) { double ts_failure = 0.0; if (compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages)) - { step_2_done = true; - } else { for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) @@ -655,9 +655,7 @@ void step2(const std::atomic &loPause) if (imu.size() > 0) { if (std::get<0>(imu[imu.size() - 1]).first > ts_failure) - { break; - } } std::cout << "file: '" << imufn << "' [OK]" << std::endl; } @@ -1459,7 +1457,7 @@ void progress_window() { ImGui::Begin("Progress", nullptr, ImGuiWindowFlags_AlwaysAutoResize); - ImGui::Text(("Working directory: '" + working_directory + "'").c_str()); + ImGui::Text(("Working directory:\n'" + working_directory + "'").c_str()); // Calculate elapsed time and ETA auto currentTime = std::chrono::system_clock::now(); @@ -1507,9 +1505,8 @@ void progress_window() if (!loPause) { if (ImGui::Button("Pause")) - { loPause.store(true); - } + if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); @@ -1521,9 +1518,7 @@ void progress_window() else { if (ImGui::Button("Resume")) - { loPause.store(false); - } } ImGui::SameLine(); ImGui::Text("Also check console for progress.."); @@ -1579,9 +1574,7 @@ void openData() loThread.detach(); } else //no data loaded - { loRunning = false; - } } void step1(const std::string &folder, @@ -1680,9 +1673,7 @@ void display() if (show_covs) { for (const auto &b : params.buckets_indoor) - { draw_ellipse(b.second.cov, b.second.mean, Eigen::Vector3f(0.0f, 0.0f, 1.0f), 3); - } } #if 0 // ToDo @@ -1724,9 +1715,7 @@ void display() glColor3f(1, 0, 0); glBegin(GL_POINTS); for (int 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(); } @@ -1762,9 +1751,7 @@ void display() for (const auto &wd : worker_data) { for (const auto &it : wd.intermediate_trajectory) - { glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - } } glEnd(); glPointSize(1); @@ -1796,9 +1783,7 @@ void display() glColor3f(1, 0, 0); glBegin(GL_POINTS); for (const auto &b : params.reference_buckets) - { glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); - } glEnd(); } @@ -2039,7 +2024,7 @@ void display() toml_io.LoadParametersFromTomlFile(input_file_names[0], params); std::cout << "Parameters loaded from: " << input_file_names[0] << std::endl; } - catch (const std::exception &e) + catch (const std::exception& e) { std::cerr << "Error loading TOML file: " << e.what() << std::endl; } 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 7538be30..53cb3d0d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1510,13 +1510,9 @@ void optimize_lidar_odometry(std::vector &intermediate_points, }; if (multithread) - { std::for_each(std::execution::par_unseq, std::begin(intermediate_points), std::end(intermediate_points), hessian_fun_indoor); - } else - { std::for_each(std::begin(intermediate_points), std::end(intermediate_points), hessian_fun_indoor); - } if (ablation_study_use_hierarchical_rgd) { @@ -1525,9 +1521,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, const auto hessian_fun_outdoor = [&](const Point3Di &intermediate_points_i) { if (intermediate_points_i.point.norm() < 5.0 || intermediate_points_i.point.norm() > max_distance) // ToDo - { return; - } Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; auto index_of_bucket = get_rgd_index(point_global, b_outdoor); @@ -1535,22 +1529,18 @@ void optimize_lidar_odometry(std::vector &intermediate_points, auto bucket_it = buckets_outdoor.find(index_of_bucket); // no bucket found if (bucket_it == buckets_outdoor.end()) - { return; - } + auto &this_bucket = bucket_it->second; const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) - { return; - } + if ((infm.array() < -threshold).any()) - { return; - } if (ablation_study_use_view_point_and_normal_vectors) { @@ -1558,9 +1548,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, Eigen::Vector3d &nv = this_bucket.normal_vector; Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); if (nv.dot(viewport - this_bucket.mean) < 0) - { return; - } } const Eigen::Affine3d &m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; @@ -1617,32 +1605,23 @@ void optimize_lidar_odometry(std::vector &intermediate_points, }; if (multithread) - { std::for_each(std::execution::par_unseq, std::begin(intermediate_points), std::end(intermediate_points), hessian_fun_outdoor); - } else - { std::for_each(std::begin(intermediate_points), std::end(intermediate_points), hessian_fun_outdoor); - } } std::vector> odo_edges; for (size_t i = 1; i < intermediate_trajectory.size(); i++) - { odo_edges.emplace_back(i - 1, i); - } std::vector poses; std::vector poses_desired; for (size_t i = 0; i < intermediate_trajectory.size(); i++) - { poses.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory[i])); - } + for (size_t i = 0; i < intermediate_trajectory_motion_model.size(); i++) - { poses_desired.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory_motion_model[i])); - } for (size_t i = 0; i < odo_edges.size(); i++) { @@ -1804,12 +1783,8 @@ void optimize_lidar_odometry(std::vector &intermediate_points, Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; for (int k = 0; k < x.outerSize(); ++k) - { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { h_x.push_back(it.value()); - } - } delta = 1000000.0; if (h_x.size() == 6 * intermediate_trajectory.size()) @@ -1843,9 +1818,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, } delta = 0.0; for (int i = 0; i < h_x.size(); i++) - { delta += sqrt(h_x[i] * h_x[i]); - } } return; } 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 59d34d1d..6cfba692 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -42,7 +42,7 @@ std::vector infoLines = { }; //App specific shortcuts (using empty dummy until needed) -std::vector appShortcuts(78, { "", "", "" }); +std::vector appShortcuts(80, { "", "", "" }); #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; 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 7c6aa0d4..610a0dfc 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -82,6 +82,7 @@ static const std::vector appShortcuts = { {"", "Ctrl+Q", ""}, {"", "R", ""}, {"", "Ctrl+R", ""}, + {"", "Shift+R", ""}, {"", "S", ""}, {"", "Ctrl+S", ""}, {"", "Ctrl+Shift+S", ""}, @@ -121,6 +122,7 @@ static const std::vector appShortcuts = { {"", "Right click + drag", "n"}, {"", "Scroll", ""}, {"", "Shift + scroll", ""}, + {"", "Shift + drag", ""}, {"", "Ctrl + left click", ""}, {"", "Ctrl + right click", ""}, {"", "Ctrl + middle click", ""} @@ -151,6 +153,7 @@ int oldcolorScheme = 0; bool usePose = false; Session session; +bool session_loaded = false; //built in console output redirection to imgui window /////////////////////////////////////////////////////////////////////////////////// @@ -695,7 +698,7 @@ void openSession() std::string newTitle = winTitle + " - " + truncPath(session_file_name); glutSetWindowTitle(newTitle.c_str()); - session.load(fs::path(session_file_name).string(), false, 0.0, 0.0, 0.0, false); + session_loaded = session.load(fs::path(session_file_name).string(), false, 0.0, 0.0, 0.0, false); index_rendered_points_local = 0; if (gl_useVBOs) @@ -1255,7 +1258,41 @@ void mouse(int glut_button, int state, int x, int y) if (!io.WantCaptureMouse) { if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && io.KeyCtrl) - setNewRotationCenter(x, y); + { + if (session_loaded) + { + 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++) + { + auto vp = session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[j]; + + if (usePose == true) + vp = session.point_clouds_container.point_clouds[index_rendered_points_local].m_pose * vp; + + double dist = distance_point_to_line(vp, laser_beam); + + if (dist < min_distance && dist < 0.1) + { + min_distance = dist; + + new_rotation_center.x() = vp.x(); + new_rotation_center.y() = vp.y(); + new_rotation_center.z() = vp.z(); + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; + } + else + setNewRotationCenter(x, y); + } if (state == GLUT_DOWN) mouse_buttons |= 1 << glut_button; diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 1821674b..b4647f1f 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -101,6 +101,7 @@ static const std::vector appShortcuts = { {"", "Ctrl+Q", ""}, {"", "R", ""}, {"", "Ctrl+R", "Remove session(s)"}, + {"", "Shift+R", ""}, {"", "S", ""}, {"", "Ctrl+S", "Save project"}, {"", "Ctrl+Shift+S", ""}, @@ -140,6 +141,7 @@ static const std::vector appShortcuts = { {"", "Right click + drag", "n"}, {"", "Scroll", ""}, {"", "Shift + scroll", ""}, + {"", "Shift + drag", ""}, {"", "Ctrl + left click", ""}, {"", "Ctrl + right click", ""}, {"", "Ctrl + middle click", ""}}; @@ -3399,7 +3401,7 @@ void display() ImGui::Separator(); - if (ImGui::BeginMenu("Save all marked trajectories", loaded_sessions)) + if (ImGui::BeginMenu("Save all marked trajectories", sessions.size() > 0)) { if (ImGui::MenuItem("Save all as las/laz files")) { @@ -3861,7 +3863,7 @@ void display() if (ImGui::BeginMenu("Tools")) { - ImGui::MenuItem("Normal Distributions Transform", nullptr, &is_ndt_gui, !is_loop_closure_gui && loaded_sessions); + ImGui::MenuItem("Normal Distributions Transform", nullptr, &is_ndt_gui, !is_loop_closure_gui && (sessions.size() > 0)); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); @@ -3981,17 +3983,17 @@ void display() if (remove_gui) { - ImGui::OpenPopup("Remove Sessions"); + ImGui::OpenPopup("Remove session(s)"); remove_gui = false; } - if (ImGui::BeginPopupModal("Remove Sessions", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + if (ImGui::BeginPopupModal("Remove session(s)", NULL, ImGuiWindowFlags_AlwaysAutoResize)) { static std::vector session_marked_for_removal; if (session_marked_for_removal.size() != project_settings.session_file_names.size()) session_marked_for_removal.resize(project_settings.session_file_names.size(), false); - ImGui::Text("Select sessions to remove:"); + ImGui::Text("Select session(s) to remove:"); ImGui::Separator(); for (int i = 0; i < project_settings.session_file_names.size(); i++) @@ -4011,7 +4013,7 @@ void display() { std::cout << "Removing session: " << project_settings.session_file_names[i] << std::endl; project_settings.session_file_names.erase(project_settings.session_file_names.begin() + i); - if (loaded_sessions && i < sessions.size()) + if ((sessions.size() > 0) && i < sessions.size()) sessions.erase(sessions.begin() + i); } } @@ -4448,7 +4450,12 @@ void mouse(int glut_button, int state, int x, int y) if (!io.WantCaptureMouse) { if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && io.KeyCtrl) - setNewRotationCenter(x, y); + { + if (sessions.size() > 0) + getClosestTrajectoriesPoint(sessions, x, y); + else + setNewRotationCenter(x, y); + } if (state == GLUT_DOWN) { 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 45b1734d..a21328c7 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 @@ -110,6 +110,7 @@ static const std::vector appShortcuts = { {"", "Ctrl+Q", ""}, {"", "R", ""}, {"", "Ctrl+R", "Random cloud colors"}, + {"", "Shift+R", ""}, {"", "S", ""}, {"", "Ctrl+S", "Save session"}, {"", "Ctrl+Shift+S", "Save subsession"}, @@ -149,6 +150,7 @@ static const std::vector appShortcuts = { {"", "Right click + drag", "n"}, {"", "Scroll", ""}, {"", "Shift + scroll", ""}, + {"", "Shift + drag", ""}, {"", "Ctrl + left click", ""}, {"", "Ctrl + right click", ""}, {"", "Ctrl + middle click", ""} @@ -1553,63 +1555,6 @@ void saveSubsession() } } -double distance_point_to_line(const Eigen::Vector3d &point, const LaserBeam &line) -{ - Eigen::Vector3d AP = point - line.position; - - double dist = (AP.cross(line.direction)).norm(); - return dist; -} - -void getClosestTrajectoryPoint(Session& session, int x, int y, bool gcpPicking, int &picked_index) -{ - picked_index = -1; - - const auto laser_beam = GetLaserBeam(x, y); - double min_distance = std::numeric_limits::max(); - int index_i = -1; - int index_j = -1; - - 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; - index_i = i; - index_j = j; - - new_rotation_center.x() = vp.x(); - new_rotation_center.y() = vp.y(); - new_rotation_center.z() = vp.z(); - - if (gcpPicking) - { - session.ground_control_points.picking_mode_index_to_node_inner = index_i; - session.ground_control_points.picking_mode_index_to_node_outer = index_j; - } - - picked_index = index_i; - - // if (picking_mode_index_to_node_inner != -1 && picking_mode_index_to_node_outer != -1) - } - } - } - - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - camera_transition_active = true; -} - void project_gui() { ImGui::Begin("Single session processing"); @@ -3199,6 +3144,7 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observ return pos; } + void mouse(int glut_button, int state, int x, int y) { ImGuiIO &io = ImGui::GetIO(); @@ -3272,25 +3218,24 @@ void mouse(int glut_button, int state, int x, int y) else { if (glut_button == GLUT_MIDDLE_BUTTON) - if (session_loaded){ + if (session_loaded) + { int tmp = -1; getClosestTrajectoryPoint(session, x, y, false, tmp); - if (io.KeyCtrl){ - if (tmp != -1){ + if (io.KeyCtrl) + { + if (tmp != -1) index_loop_closure_target = tmp; - } } - if (io.KeyShift){ + else if (io.KeyShift) + { if (tmp != -1) - { index_loop_closure_source = tmp; - } } } - else{ + else setNewRotationCenter(x, y); - } } } diff --git a/core/include/utils.hpp b/core/include/utils.hpp index e745441c..4b4d4c82 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -1,8 +1,10 @@ #pragma once + #include #include #include #include +#include /////////////////////////////////////////////////////////////////////////////////// @@ -110,4 +112,8 @@ void drawMiniCompassWithRuler( Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); 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); + void setNewRotationCenter(int x, int y); \ No newline at end of file diff --git a/core/src/utils.cpp b/core/src/utils.cpp index d3e5c40c..617051ca 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -5,6 +5,8 @@ #include "utils.hpp" +#include "imgui_internal.h" + #include #include #include @@ -52,6 +54,8 @@ float new_translate_x = translate_x; float new_translate_y = translate_y; float new_translate_z = translate_z; +bool cor_gui = false; + // Transition timing bool camera_transition_active = false; @@ -101,6 +105,7 @@ static const std::vector shortcuts = { {"", "Ctrl+Q", ""}, {"", "R", "camera Right"}, {"", "Ctrl+R", ""}, + {"", "Shift+R", "Rotation center"}, {"", "S", ""}, {"", "Ctrl+S", ""}, {"", "Ctrl+Shift+S", ""}, @@ -140,6 +145,7 @@ static const std::vector shortcuts = { {"", "Right click + drag", "camera pan"}, {"", "Scroll", "camera zoom"}, {"", "Shift + scroll", "camera 5x zoom"}, + {"", "Shift + drag", "Dock window to screen edges"}, {"", "Ctrl + left click", ""}, {"", "Ctrl + right click", "change center of rotation"}, {"", "Ctrl + middle click", "change center of rotation (if no CP GUI active)"} @@ -361,6 +367,8 @@ void keyboardUp(unsigned char key, int x, int y) { //std::cout << "Up key: " << key << ", mod: " << mods << std::endl; } +static bool first_time = true; + void ShowMainDockSpace() { ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking @@ -387,7 +395,18 @@ void ShowMainDockSpace() // This is the dockspace! ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); - ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode); + ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode | ImGuiDockNodeFlags_NoDockingInCentralNode); + + if (first_time) { + first_time = false; + + auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); + auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); + + ImGui::DockBuilderDockWindow("Settings", dock_id_left); + ImGui::DockBuilderDockWindow("Console", dock_id_bottom); + ImGui::DockBuilderFinish(dockspace_id); + } ImGui::End(); } @@ -430,6 +449,8 @@ bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display) io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard // Enable Keyboard Controls | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; + io.ConfigDockingWithShift = true; + io.MouseDrawCursor = false; // use OS cursor (for future ImGUI update to 1.93+) ImGui::StyleColorsDark(); ImGui_ImplGLUT_Init(); @@ -765,6 +786,9 @@ void view_kbd_shortcuts() breakCameraTransition(); } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) + cor_gui = true; + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !is_ortho) lock_z = !lock_z; @@ -796,7 +820,6 @@ void view_kbd_shortcuts() if (ImGui::IsKeyPressed(ImGuiKey_X, false)) show_axes = !show_axes; - if (ImGui::IsKeyPressed(ImGuiKey_1)) point_size = 1; if (ImGui::IsKeyPressed(ImGuiKey_2)) @@ -916,9 +939,57 @@ void ShowShortcutsTable(const std::vector appShortcuts) void info_window(const std::vector& infoLines, const std::vector& appShortcuts, bool* open) { + //temporary location for new rotation center (minimal impact change in all apps) + if (cor_gui) + { + ImGui::OpenPopup("Center of rotation"); + cor_gui = false; + } + + if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Select new center of rotation [m]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + if (ImGui::Button("Set")) + { + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + + camera_transition_active = true; + + ImGui::CloseCurrentPopup(); + } + + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } + if (!open || !*open) return; - if (ImGui::Begin("Info", open, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize)) + if (ImGui::Begin("Info", open, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking)) { bool firstLine = true; for (const auto& line : infoLines) @@ -1145,6 +1216,107 @@ LaserBeam GetLaserBeam(int x, int y) return laser_beam; } +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) +{ + Eigen::Vector3d AP = point - line.position; + + double dist = (AP.cross(line.direction)).norm(); + return dist; +} + +void getClosestTrajectoryPoint(Session& session, int x, int y, bool gcpPicking, int& picked_index) +{ + picked_index = -1; + + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + int index_i = -1; + int index_j = -1; + + 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; + index_i = i; + index_j = j; + + 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 WITH_GUI == 1 + if (gcpPicking) + { + session.ground_control_points.picking_mode_index_to_node_inner = index_i; + session.ground_control_points.picking_mode_index_to_node_outer = index_j; + } +#endif + + picked_index = index_i; + + // if (picking_mode_index_to_node_inner != -1 && picking_mode_index_to_node_outer != -1) + } + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; +} + +void getClosestTrajectoriesPoint(std::vector& sessions, int x, int y) +{ + //picked_index = -1; + //picked_session = nullptr; + + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + + for (auto& session : sessions) + { + 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; + } + } + } + } + + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + camera_transition_active = true; +} + void setNewRotationCenter(int x, int y)