diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 7ce60d8b..5a7ad649 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -132,7 +132,7 @@ static const std::vector appShortcuts = { namespace fs = std::filesystem; -bool full_lidar_odometry_gui = false; +bool is_settings_gui = false; bool full_debug_messages = false; NDT ndt; bool show_reference_buckets = true; @@ -679,9 +679,9 @@ void save_results(bool info, double elapsed_seconds) } } -void project_gui() +void settings_gui() { - if (ImGui::Begin("Settings", &full_lidar_odometry_gui)) + if (ImGui::Begin("Settings", &is_settings_gui)) { ImGui::Checkbox("simple_gui", &simple_gui); if (ImGui::IsItemHovered()) @@ -1526,7 +1526,7 @@ void progress_window() void openData() { - full_lidar_odometry_gui = false; + is_settings_gui = false; info_gui = false; loRunning.store(true); @@ -2048,20 +2048,6 @@ void display() if (ImGui::IsItemHovered()) ImGui::SetTooltip("Processing parameter list"); - ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); - ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0, 0, 0, 0)); - ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); - ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); - if (ImGui::SmallButton(full_lidar_odometry_gui ? "[X] Full GUI" : "[ ] Full GUI")) - full_lidar_odometry_gui = !full_lidar_odometry_gui; - ImGui::PopStyleVar(2); - ImGui::PopStyleColor(3); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Show power user settings window with more parameters"); - - ImGui::SameLine(); - ImGui::SameLine(); ImGui::Dummy(ImVec2(20, 0)); ImGui::SameLine(); @@ -2114,6 +2100,12 @@ void display() bg_color = params.clear_color; + ImGui::Separator(); + + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + ImGui::EndMenu(); } if (ImGui::IsItemHovered()) @@ -2206,11 +2198,11 @@ void display() stretch_gizmo_m(3, 3) = m_gizmo[15]; } - if (full_lidar_odometry_gui) + if (is_settings_gui) { lastPar = 0; - project_gui(); + settings_gui(); } if (loRunning) 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 75eb24fd..63b704f4 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -3,6 +3,10 @@ #include #include + +const double DEG_TO_RAD = M_PI / 180.0f; +const double RAD_TO_DEG = 180.0f / M_PI; + // extern std::vector> global_tmp; namespace { @@ -49,9 +53,7 @@ std::vector> nns(std::vector points_global, const { unsigned long long int index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) - { buckets[index_of_bucket].second = i; - } else { buckets[index_of_bucket].first = i; @@ -108,12 +110,8 @@ std::vector> nns(std::vector points_global, const // std::cout << "------------" << std::endl; for (size_t y = 0; y < min_distances.size(); y++) - { if (indexes_target[y] != -1) - { nn.emplace_back(index_element_source, indexes_target[y]); - } - } } //} @@ -200,21 +198,16 @@ void optimize_icp(std::vector &intermediate_points, 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++) { @@ -343,9 +336,7 @@ void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_points, std::vector 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()); - } - } if (h_x.size() == 6 * intermediate_trajectory.size()) { @@ -411,9 +398,7 @@ void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_points, std::vector 0.5 && intermediate_points[i].index_pose != -1 && r_l < 30) { - double polar_angle_deg_l = atan2(intermediate_points[i].point.y(), intermediate_points[i].point.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(intermediate_points[i].point.z() / r_l) / M_PI * 180.0; + double polar_angle_deg_l = atan2(intermediate_points[i].point.y(), intermediate_points[i].point.x()) * RAD_TO_DEG; + double azimutal_angle_deg_l = acos(intermediate_points[i].point.z() / r_l) * RAD_TO_DEG; Eigen::Vector3d pp = intermediate_points[i].point; @@ -462,8 +447,8 @@ void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_points, std::vector &intermediate_points, std::vectorsecond; Eigen::Vector3d mean(this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); @@ -529,15 +513,9 @@ void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_points, 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.size(); i++) - { poses_desired.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory[i])); - } for (size_t i = 0; i < odo_edges.size(); i++) { @@ -719,13 +688,8 @@ void optimize_sf(std::vector &intermediate_points, 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()); - } - } if (h_x.size() == 6 * int_tr.size()) { @@ -749,9 +713,7 @@ void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_points, std::vector &intermediate_points_sf, std::vector &intermediate_trajectory, @@ -775,7 +737,7 @@ void optimize_sf2(std::vector &intermediate_points, std::vector &intermediate_points, std::vector threshold).any()) - { - return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; - } + if ((infm.array() < -threshold).any()) - { return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; - } const Eigen::Affine3d &m_pose = intermediate_trajectory[intermediate_points[indexes_i].index_pose]; // intermediate_trajectory[intermediate_points_i.index_pose]; const Eigen::Vector3d &p_s = intermediate_points[indexes_i].point; @@ -855,13 +813,9 @@ void optimize_sf2(std::vector &intermediate_points, std::vector AtPAndtBlocksToSum(intermediate_points.size()); if (useMultithread) - { std::transform(std::execution::par_unseq, std::begin(indexes), std::end(indexes), std::begin(AtPAndtBlocksToSum), hessian_fun); - } else - { std::transform(std::begin(indexes), std::end(indexes), std::begin(AtPAndtBlocksToSum), hessian_fun); - } SumBlocks(AtPAndtBlocksToSum, AtPAndt, AtPBndt); @@ -870,23 +824,17 @@ void optimize_sf2(std::vector &intermediate_points, 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])); - } std::vector> odo_edges; for (size_t i = 1; i < intermediate_trajectory.size(); i++) - { odo_edges.emplace_back(i - 1, i); - } for (size_t i = 0; i < odo_edges.size(); i++) { - Eigen::Matrix AtPAodo; relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified(AtPAodo, poses[odo_edges[i].first].px, @@ -1009,12 +957,8 @@ void optimize_sf2(std::vector &intermediate_points, std::vector 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()); - } - } if (h_x.size() == 6 * intermediate_trajectory.size()) { @@ -1086,19 +1030,13 @@ void optimize_rigid_sf( rgd_params_sc.resolution_Z = azimutal_angle_deg; for (auto &t : _intermediate_trajectory) - { t.translation() -= shift; - } for (auto &t : _intermediate_trajectory_motion_model) - { t.translation() -= shift; - } for (auto &b : _buckets) - { b.second.mean -= shift; - } std::vector points_rgd; std::vector point_cloud_global; @@ -1111,16 +1049,14 @@ void optimize_rigid_sf( auto pinv = minv * b.second.mean; if (pinv.norm() < max_distance_lidar_rigid_sf) - { points_rgd.push_back(b.second.mean); - } } for (int i = 0; i < points_rgd.size(); i++) { double r_l = points_rgd[i].norm(); - double polar_angle_deg_l = atan2(points_rgd[i].y(), points_rgd[i].x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(points_rgd[i].z() / r_l) / M_PI * 180.0; + double polar_angle_deg_l = atan2(points_rgd[i].y(), points_rgd[i].x()) * RAD_TO_DEG; + double azimutal_angle_deg_l = acos(points_rgd[i].z() / r_l) * RAD_TO_DEG; point_cloud_global_sc.emplace_back(r_l, polar_angle_deg_l, azimutal_angle_deg_l); @@ -1158,17 +1094,16 @@ void optimize_rigid_sf( auto point_global = first_pose * point_local; double r_l = point_global.norm(); - double polar_angle_deg_l = atan2(point_global.y(), point_global.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(point_global.z() / r_l) / M_PI * 180.0; + double polar_angle_deg_l = atan2(point_global.y(), point_global.x()) * RAD_TO_DEG; + double azimutal_angle_deg_l = acos(point_global.z() / r_l) * RAD_TO_DEG; auto index_of_bucket = get_rgd_index({r_l, polar_angle_deg_l, azimutal_angle_deg_l}, {rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z}); auto bucket_it = buckets_sf.find(index_of_bucket); // no bucket found if (bucket_it == buckets_sf.end()) - { return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; - } + auto &this_bucket = bucket_it->second; const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); @@ -1227,13 +1162,10 @@ void optimize_rigid_sf( std::vector blocks(indexes.size()); if (useMultithread) // ToDo fix for this case - { std::transform(std::execution::par_unseq, std::begin(indexes), std::end(indexes), std::begin(blocks), hessian_fun); - } else - { std::transform(std::begin(indexes), std::end(indexes), std::begin(blocks), hessian_fun); - } + Eigen::MatrixXd AtPAndt(6, 6); AtPAndt.setZero(); Eigen::MatrixXd AtPBndt(6, 1); @@ -1260,13 +1192,13 @@ void optimize_rigid_sf( double wy = 1.0 / (rgd_sf_sigma_y_m * rgd_sf_sigma_y_m); double wz = 1.0 / (rgd_sf_sigma_z_m * rgd_sf_sigma_z_m); - double a_om = rgd_sf_sigma_om_deg / 180.0 * M_PI; + double a_om = rgd_sf_sigma_om_deg * DEG_TO_RAD; double w_om = 1.0 / (a_om * a_om); - double a_fi = rgd_sf_sigma_fi_deg / 180.0 * M_PI; + double a_fi = rgd_sf_sigma_fi_deg * DEG_TO_RAD; double w_fi = 1.0 / (a_fi * a_fi); - double a_ka = rgd_sf_sigma_ka_deg / 180.0 * M_PI; + double a_ka = rgd_sf_sigma_ka_deg * DEG_TO_RAD; double w_ka = 1.0 / (a_ka * a_ka); tripletListP.emplace_back(ir, ir, wx); @@ -1347,9 +1279,7 @@ void optimize_rigid_sf( /// for (int i = 0; i < _intermediate_trajectory.size(); i++) - { _intermediate_trajectory[i].translation() += shift; - } intermediate_trajectory = _intermediate_trajectory; intermediate_trajectory_motion_model = _intermediate_trajectory; @@ -1381,9 +1311,9 @@ void optimize_lidar_odometry(std::vector &intermediate_points, bool ablation_study_use_view_point_and_normal_vectors) { - double sigma_motion_model_om = lidar_odometry_motion_model_om_1_sigma_deg * M_PI / 180.0; - double sigma_motion_model_fi = lidar_odometry_motion_model_fi_1_sigma_deg * M_PI / 180.0; - double sigma_motion_model_ka = lidar_odometry_motion_model_ka_1_sigma_deg * M_PI / 180.0; + double sigma_motion_model_om = lidar_odometry_motion_model_om_1_sigma_deg * DEG_TO_RAD; + double sigma_motion_model_fi = lidar_odometry_motion_model_fi_1_sigma_deg * DEG_TO_RAD; + double sigma_motion_model_ka = lidar_odometry_motion_model_ka_1_sigma_deg * DEG_TO_RAD; double w_motion_model_om = 1.0 / (sigma_motion_model_om * sigma_motion_model_om); double w_motion_model_fi = 1.0 / (sigma_motion_model_fi * sigma_motion_model_fi); @@ -1408,9 +1338,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, const auto hessian_fun_indoor = [&](const Point3Di &intermediate_points_i) { if (intermediate_points_i.point.norm() < 0.1 || 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_indoor); @@ -1418,9 +1346,8 @@ void optimize_lidar_odometry(std::vector &intermediate_points, auto bucket_it = buckets_indoor.find(index_of_bucket); // no bucket found if (bucket_it == buckets_indoor.end()) - { return; - } + auto &this_bucket = bucket_it->second; // if(buckets[index_of_bucket].number_of_points >= 5){ @@ -1428,14 +1355,10 @@ void optimize_lidar_odometry(std::vector &intermediate_points, const double threshold = 100000.0; if ((infm.array() > threshold).any()) - { return; - } - if ((infm.array() < -threshold).any()) - { + if ((infm.array() < -threshold).any()) return; - } if (ablation_study_use_view_point_and_normal_vectors) { @@ -1443,9 +1366,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]; @@ -1487,15 +1408,11 @@ void optimize_lidar_odometry(std::vector &intermediate_points, double norm = 1.0; if (ablation_study_use_norm) - { norm = p_s.norm(); - } double w = planarity * norm; if (w > 10.0) - { w = 10.0; - } if (ablation_study_use_planarity || ablation_study_use_norm) { @@ -1647,9 +1564,9 @@ void optimize_lidar_odometry(std::vector &intermediate_points, // relative_pose_measurement_odo(2, 0) += relative.norm() * sin(poses_desired[odo_edges[i].first].fi); // TaitBryanPose relative_pose_measurement_odo // relative_pose_measurement_odo(2, 0) += 0.01; - relative_pose_measurement_odo(3, 0) += (motion_model_correction.om / 180.0) * M_PI; - relative_pose_measurement_odo(4, 0) += (motion_model_correction.fi / 180.0) * M_PI; - relative_pose_measurement_odo(5, 0) += (motion_model_correction.ka / 180.0) * M_PI; + relative_pose_measurement_odo(3, 0) += motion_model_correction.om * DEG_TO_RAD; + relative_pose_measurement_odo(4, 0) += motion_model_correction.fi * DEG_TO_RAD; + relative_pose_measurement_odo(5, 0) += motion_model_correction.ka * DEG_TO_RAD; Eigen::Matrix AtPAodo; @@ -1808,13 +1725,9 @@ void optimize_lidar_odometry(std::vector &intermediate_points, // std::cout << "(p1 - p2).norm() " << (p1 - p2).norm() << std::endl; if ((p1 - p2).norm() < 1.0) - { intermediate_trajectory[i] = affine_matrix_from_pose_tait_bryan(pose); - } else - { std::cout << "big jump on trajectory: " << (p1 - p2).norm() << std::endl; - } } delta = 0.0; for (int i = 0; i < h_x.size(); i++) @@ -1832,27 +1745,21 @@ void align_to_reference(NDT::GridParameters &rgd_params, std::vector & for (int i = 0; i < initial_points.size(); i += 1) { - Eigen::Vector3d point_global = m_g * initial_points[i].point; auto index_of_bucket = get_rgd_index(point_global, b); if (!reference_buckets.contains(index_of_bucket)) - { continue; - } Eigen::Matrix3d infm = reference_buckets[index_of_bucket].cov.inverse(); constexpr double threshold = 10000.0; if ((infm.array() > threshold).any()) - { continue; - } + if ((infm.array() < -threshold).any()) - { continue; - } const Eigen::Affine3d &m_pose = m_g; const Eigen::Vector3d &p_s = initial_points[i].point; @@ -1876,18 +1783,11 @@ void align_to_reference(NDT::GridParameters &rgd_params, std::vector & int c = 0; for (int row = 0; row < 6; row++) - { for (int col = 0; col < 6; col++) - { AtPAndt.coeffRef(c + row, c + col) += AtPA(row, col); - } - } for (int row = 0; row < 6; row++) - { AtPBndt.coeffRef(c + row, 0) -= AtPB(row, 0); - } - //} } AtPAndt.coeffRef(0, 0) += 10000.0; @@ -1901,12 +1801,8 @@ void align_to_reference(NDT::GridParameters &rgd_params, std::vector & Eigen::SparseMatrix x = solver.solve(AtPBndt); 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()); - } - } if (h_x.size() == 6) { @@ -1921,9 +1817,7 @@ void align_to_reference(NDT::GridParameters &rgd_params, std::vector & m_g = affine_matrix_from_pose_tait_bryan(pose); } else - { std::cout << "align_to_reference FAILED" << std::endl; - } } bool compute_step_2(std::vector &worker_data, LidarOdometryParams ¶ms, double &ts_failure, std::atomic &loProgress, const std::atomic &pause, bool debugMsg) @@ -1953,9 +1847,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p auto pp = params.initial_points; for (int i = 0; i < pp.size(); i++) - { pp[i].point = params.m_g * pp[i].point; - } + update_rgd(params.in_out_params_indoor, params.buckets_indoor, pp, params.m_g.translation()); update_rgd(params.in_out_params_outdoor, params.buckets_outdoor, pp, params.m_g.translation()); @@ -1980,7 +1873,6 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p } } - // auto tmp_trj = Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); if (i > 1 && params.use_motion_from_previous_step) { @@ -2014,9 +1906,7 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p } for (int tr = 0; tr < new_trajectory.size(); tr++) - { new_trajectory[tr].translation() += mean_shift * tr; - } worker_data[i].intermediate_trajectory = new_trajectory; worker_data[i].intermediate_trajectory_motion_model = new_trajectory; @@ -2037,13 +1927,10 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p auto firstm = tr[0]; for (auto &t : tr) - { t.translation() -= firstm.translation(); - } + for (auto &t : trmm) - { t.translation() -= firstm.translation(); - } NDT::GridParameters rgd_params_sc; @@ -2062,8 +1949,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p // std::cout << worker_data[i].intermediate_points[ii].index_pose << " "; if (r_l > 0.5 && intermediate_points[ii].index_pose != -1 && r_l < params.max_distance_lidar_rigid_sf) { - double polar_angle_deg_l = atan2(intermediate_points[ii].point.y(), intermediate_points[ii].point.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(intermediate_points[ii].point.z() / r_l) / M_PI * 180.0; + double polar_angle_deg_l = atan2(intermediate_points[ii].point.y(), intermediate_points[ii].point.x()) * RAD_TO_DEG; + double azimutal_angle_deg_l = acos(intermediate_points[ii].point.z() / r_l) * RAD_TO_DEG; points_local.push_back(intermediate_points[ii]); @@ -2100,15 +1987,13 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p double wx = 1000000; double wy = 1000000; double wz = 1000000; - double angle_sigma_rad = 0.1 / 180.0 * M_PI; + double angle_sigma_rad = 0.1 * DEG_TO_RAD; double wom = 1.0 / (angle_sigma_rad * angle_sigma_rad); double wfi = 1.0 / (angle_sigma_rad * angle_sigma_rad); double wka = 1.0 / (angle_sigma_rad * angle_sigma_rad); for (int iter = 0; iter < params.robust_and_accurate_lidar_odometry_iterations; iter++) - { optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, params.useMultithread, wx, wy, wz, wom, wfi, wka); - } if (debug) { @@ -2126,20 +2011,14 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::string output_file_name = "optimize_sf2_" + std::to_string(index_fn++) + ".laz"; if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) - { std::cout << "problem with saving file: " << output_file_name << std::endl; - } } for (auto &t : tr) - { t.translation() += firstm.translation(); - } for (auto &t : trmm) - { t.translation() += firstm.translation(); - } worker_data[i].intermediate_trajectory = tr; worker_data[i].intermediate_trajectory_motion_model = tr; @@ -2207,9 +2086,7 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p if (iter % 10 == 0 && iter > 0) { if (debugMsg) - { std::cout << "\nlm_factor " << lm_factor << ", delta " << std::setprecision(10) << delta << "\n"; - } lm_factor *= 10.0; } @@ -2218,9 +2095,7 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::chrono::duration elapsed_seconds = end - start; if (elapsed_seconds.count() > params.real_time_threshold_seconds) - { break; - } } end1 = std::chrono::system_clock::now(); @@ -2234,13 +2109,9 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p << fixed << std::setprecision(2) << elapsed_seconds1.count() << "[s], delta "; if (delta > 1e-12) - { std::cout << std::setprecision(10) << delta << "!!!"; - } else - { std::cout << "< 1e-12"; - } std::cout << "\n"; @@ -2312,18 +2183,15 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::vector points_global_new; points_global_new.reserve(points_global.size() / 2 + 1); for (int k = points_global.size() / 2; k < points_global.size(); k++) - { points_global_new.emplace_back(points_global[k]); - } acc_distance = 0; points_global = points_global_new; // decimate if (params.decimation > 0) - { decimate(points_global, params.decimation, params.decimation, params.decimation); - } + update_rgd(params.in_out_params_indoor, params.buckets_indoor, points_global, worker_data[i].intermediate_trajectory[0].translation()); update_rgd(params.in_out_params_outdoor, params.buckets_outdoor, points_global, worker_data[i].intermediate_trajectory[0].translation()); // @@ -2359,9 +2227,7 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p } for (int i = 0; i < worker_data.size(); i++) - { worker_data[i].intermediate_trajectory_motion_model = worker_data[i].intermediate_trajectory; - } end = std::chrono::system_clock::now(); @@ -2374,9 +2240,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p params.total_length_of_calculated_trajectory = 0; for (int i = 1; i < worker_data.size(); i++) - { params.total_length_of_calculated_trajectory += (worker_data[i].intermediate_trajectory[0].translation() - worker_data[i - 1].intermediate_trajectory[0].translation()).norm(); - } + std::cout << "total_length_of_calculated_trajectory: " << params.total_length_of_calculated_trajectory << " [m]" << std::endl; } @@ -2413,16 +2278,11 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ for (int i = 0; i < worker_data.size(); i++) { std::vector intermediate_points; - if (!load_vector_data(worker_data[i].intermediate_points_cache_file_name.string(), intermediate_points)) - { std::cout << "problem with load_vector_data file '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" << std::endl; - } for (int j = 0; j < intermediate_points.size(); j++) - { all_points_local.push_back(intermediate_points[j]); - } } std::vector> tripletListA; @@ -2435,18 +2295,14 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ std::vector> odo_edges; for (size_t i = 1; i < trajectory.size(); i++) - { odo_edges.emplace_back(i - 1, i); - } std::vector poses; for (size_t i = 0; i < trajectory.size(); i++) - { poses.push_back(pose_tait_bryan_from_affine_matrix(trajectory[i])); - } - double angle = 0.1 / 180.0 * M_PI; + double angle = 0.1 * DEG_TO_RAD; double wangle = 1.0 / (angle * angle); // smoothness @@ -2618,9 +2474,7 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ const auto hessian_fun = [&](const Point3Di &intermediate_points_i) { if (intermediate_points_i.point.norm() < 1.0) - { return; - } Eigen::Vector3d point_global = trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; auto index_of_bucket = get_rgd_index(point_global, b); @@ -2628,22 +2482,18 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ auto bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.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; - } const Eigen::Affine3d &m_pose = trajectory[intermediate_points_i.index_pose]; const Eigen::Vector3d &p_s = intermediate_points_i.point; @@ -2670,15 +2520,10 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ int ir = tripletListB.size(); for (int row = 0; row < 3; row++) - { for (int col = 0; col < 6; col++) - { if (jacobian(row, col) != 0.0) - { tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } + tripletListB.emplace_back(ir, 0, delta_x); tripletListB.emplace_back(ir + 1, 0, delta_y); tripletListB.emplace_back(ir + 2, 0, delta_z); @@ -2695,13 +2540,9 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ }; if (multithread) - { std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun); - } else - { std::for_each(std::begin(points_local), std::end(points_local), hessian_fun); - } Eigen::SparseMatrix matAndt(tripletListB.size(), trajectory.size() * 6); Eigen::SparseMatrix matPndt(tripletListB.size(), tripletListB.size()); @@ -2733,12 +2574,8 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ 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()); - } - } if (h_x.size() == 6 * trajectory.size()) { @@ -2758,9 +2595,7 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ } for (int i = 0; i < indexes.size(); i++) - { worker_data[indexes[i].first].intermediate_trajectory[indexes[i].second] = trajectory[i]; - } } void Consistency2(std::vector &worker_data, LidarOdometryParams ¶ms) @@ -2792,14 +2627,10 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par { std::vector intermediate_points; if (!load_vector_data(worker_data[i].intermediate_points_cache_file_name.string(), intermediate_points)) - { std::cout << "problem with load_vector_data for file '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" << std::endl; - } for (int j = 0; j < intermediate_points.size(); j++) - { all_points_local.push_back(intermediate_points[j]); - } } std::vector> tripletListA; @@ -2812,18 +2643,14 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par std::vector> odo_edges; for (size_t i = 1; i < trajectory.size(); i++) - { odo_edges.emplace_back(i - 1, i); - } std::vector poses; for (size_t i = 0; i < trajectory.size(); i++) - { poses.push_back(pose_tait_bryan_from_affine_matrix(trajectory[i])); - } - double angle = 0.01 / 180.0 * M_PI; + double angle = 0.01 * DEG_TO_RAD; double wangle = 1.0 / (angle * angle); // smoothness @@ -3022,9 +2849,7 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par { unsigned long long int index_of_bucket = index_pairs[i].first; if (buckets.contains(index_of_bucket)) - { buckets[index_of_bucket].index_end_exclusive = i; - } else { buckets[index_of_bucket].index_begin_inclusive = i; @@ -3039,9 +2864,7 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par long long unsigned int index_point_segment = points_global[index_pairs[i].second].index_point; if (b.second.buckets.contains(index_point_segment)) - { b.second.buckets[index_point_segment].point_indexes.push_back(index_pairs[i].second); - } else { NDT::BucketCoef bc; @@ -3058,9 +2881,8 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par bb.second.mean = Eigen::Vector3d(0, 0, 0); for (const auto &p : bb.second.point_indexes) - { bb.second.mean += points_global[p].point; - } + bb.second.mean /= bb.second.point_indexes.size(); bb.second.cov.setZero(); @@ -3089,17 +2911,14 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par const auto &m = trajectory[points_global[bb.second.point_indexes[0]].index_pose]; if (nv.dot(m.translation() - bb.second.mean) < 0.0) - { nv *= -1.0; - } + bb.second.normal_vector = nv; bb.second.point_indexes.clear(); } else - { bb.second.valid = false; - } } } std::cout << "build regular grid decomposition FINISHED" << std::endl; @@ -3111,21 +2930,17 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par for (int i = 0; i < points_global.size(); i++) { if (i % 10000 == 0) - { std::cout << "computing " << i << " of " << points_global.size() << std::endl; - } + if (points_local[i].point.norm() < 1.0) - { continue; - } auto index_of_bucket = get_rgd_index(points_global[i].point, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.end()) - { continue; - } + for (auto &bb : bucket_it->second.buckets) { if (bb.second.valid) @@ -3136,13 +2951,10 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par const double threshold = 100000.0; if ((infm.array() > threshold).any()) - { continue; - } + if ((infm.array() < -threshold).any()) - { continue; - } // check nv Eigen::Vector3d &nv = this_bucket.normal_vector; @@ -3178,15 +2990,10 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par int ir = tripletListB.size(); for (int row = 0; row < 3; row++) - { for (int col = 0; col < 6; col++) - { if (jacobian(row, col) != 0.0) - { tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } + tripletListB.emplace_back(ir, 0, delta_x); tripletListB.emplace_back(ir + 1, 0, delta_y); tripletListB.emplace_back(ir + 2, 0, delta_z); @@ -3265,12 +3072,8 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par 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()); - } - } if (h_x.size() == 6 * trajectory.size()) { @@ -3290,7 +3093,5 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par } for (int i = 0; i < indexes.size(); i++) - { worker_data[indexes[i].first].intermediate_trajectory[indexes[i].second] = trajectory[i]; - } } \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/resource.rc b/apps/lidar_odometry_step_1/resource.rc index 1a74ec5f..6b4eb256 100644 --- a/apps/lidar_odometry_step_1/resource.rc +++ b/apps/lidar_odometry_step_1/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 2 -PRODUCTVERSION 0, 0, 9, 2 +FILEVERSION 0, 0, 9, 5 +PRODUCTVERSION 0, 0, 9, 5 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 1\0" -VALUE "FileVersion", "0.9.2\0" +VALUE "FileVersion", "0.9.5\0" VALUE "InternalName", "Odometry\0" VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "lidar_odometry_step_1.exe\0" -VALUE "ProductVersion", "0.9.2\0" +VALUE "ProductVersion", "0.9.5\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 3318798e..47fd8f23 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -225,7 +225,7 @@ void load_pc(const std::string &lazFile, std::vector& points, bool omm laszip_close_reader(laszip_reader); } -void project_gui() +void settings_gui() { if (ImGui::Begin("Settings", nullptr, ImGuiWindowFlags_AlwaysAutoResize)) { @@ -896,7 +896,7 @@ void display() if (compass_ruler) drawMiniCompassWithRuler(); - project_gui(); + settings_gui(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); diff --git a/apps/mandeye_mission_recorder_calibration/resource.rc b/apps/mandeye_mission_recorder_calibration/resource.rc index ef2b9e31..6f2f975e 100644 --- a/apps/mandeye_mission_recorder_calibration/resource.rc +++ b/apps/mandeye_mission_recorder_calibration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,2 - PRODUCTVERSION 0,0,9,2 + FILEVERSION 0,0,9,5 + PRODUCTVERSION 0,0,9,5 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping MR Calibration\0" - VALUE "FileVersion", "0.9.2\0" + VALUE "FileVersion", "0.9.5\0" VALUE "InternalName", "Mission Recorder Calibration\0" VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_mission_recorder_calibration.exe\0" - VALUE "ProductVersion", "0.9.2\0" + VALUE "ProductVersion", "0.9.5\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 006c9013..089ea9bd 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -95,15 +95,13 @@ std::vector laz_files; std::vector photos_files; std::map photo_files_ts; -std::string working_directory = ""; -std::string working_directory_preview; double filter_threshold_xy_inner = 0.0; //no filtering for raw viewing double filter_threshold_xy_outer = 300.0; //no filtering for raw viewing bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; int number_of_points_threshold = 20000; -bool is_init = true; +bool is_init = false; int index_rendered_points_local = -1; // std::vector> all_points_local; // std::vector> all_points_local; @@ -131,6 +129,8 @@ bool show_mean_cov = false; bool show_rgd_nn = false; bool show_imu_data = false; +bool is_settings_gui = false; + namespace photos { cv::Mat imgToShow; @@ -880,194 +880,6 @@ void loadData() std::vector> pointsPerFile; std::vector, FusionVector, FusionVector>> imu_data; - /* - //start loading process - std::sort(std::begin(input_file_names), std::end(input_file_names)); - - std::string calibrationFile; - - std::cout << "Selected files for open:\n"; - - std::for_each(std::begin(input_file_names), std::end(input_file_names), [&](const std::string& fileName) - { - std::cout << fileName << "\n"; - - std::string ext = fs::path(fileName).extension().string(); - std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); - - if (ext == ".laz" || ext == ".las") - { - laz_files.push_back(fileName); - } - else if (ext == ".csv") - csv_files.push_back(fileName); - else if ((ext == ".sn") && sn_file.empty()) - { - sn_file = fileName; - std::cout << "Only using first SN file found\n"; - } - else if (ext == ".mjc") - { - if (calibrationFile.empty()) - calibrationFile = fileName; - else - std::cout << "Unexpected calibration file found (" << fileName << "). Ignored.." << std::endl; - } - else if (ext == ".jpg" || ext == ".jpeg") { - photos_files.push_back(fileName); - // decode filename e.g.: ` cam0_1761264773592270949.jpg` - const std::string filename = fs::path(fileName).stem().string(); - std::string cam_id = filename.substr(0, filename.find("_")); - std::string timestamp = filename.substr(filename.find("_") + 1, filename.size()); - - std::cout << "cam_id: " << cam_id << std::endl; - std::cout << "timestamp: " << timestamp << std::endl; - std::cout << "filename: " << filename << std::endl; - - if (cam_id == "cam0" && !timestamp.empty()) - { - try { - uint64_t ts = std::stoull(timestamp); - photo_files_ts[ts] = fileName; - } - catch (const std::exception& e) { - std::cerr << "Error parsing timestamp from filename: " << filename << " - " << e.what() << std::endl; - } - } - } - }); - - std::cout << "\n"; - - if (input_file_names.size() > 0 && laz_files.size() == csv_files.size()) - { - working_directory = fs::path(input_file_names[0]).parent_path().string(); - - if (calibrationFile.empty()) - calibrationFile = (fs::path(working_directory) / "calibration.json").string(); //if no calibration file provided, try to load old default from working directory - - // --- Calibration and IMU setup - const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); - const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); - const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); - - if (!preloadedCalibration.empty()) - { - std::cout << "Loaded calibration for:\n"; - for (const auto& [sn, _] : preloadedCalibration) - std::cout << " -> " << sn << "\n\n"; - - bool hasError = false; - - 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; - } - } - - 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; - } - - 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; - - // example file for 2x LiDAR": - } - - std::cout << "imuSnToUse: " << imuSnToUse << std::endl; - - std::cout << "Number of found photos: " << photos_files.size() << std::endl; - - fs::path wdp = fs::path(input_file_names[0]).parent_path(); - wdp /= "preview"; - if (!fs::exists(wdp)) - fs::create_directory(wdp); - - working_directory_preview = wdp.string(); - - std::cout << "Loading IMU" << std::endl; - - std::vector, FusionVector, FusionVector>> imu_data; - - for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) - { - const std::string& imufn = csv_files.at(fileNo); - // GetId of Imu to use - int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); - std::cout << "imuNumberToUse " << imuNumberToUse << " at: '" << imufn << "'" << std::endl; - auto imu = load_imu(imufn.c_str(), imuNumberToUse); - std::cout << imufn << " with mapping " << snFn << std::endl; - imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); - } - - // sort IMU data - // imu_data - 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; - }); - - std::cout << "Loading points\n"; - std::vector> pointsPerFile; - pointsPerFile.resize(laz_files.size()); - // std::vector> indexesPerFile; - - std::mutex mtx; - std::cout << "Start std::transform\n"; - - std::transform(std::execution::par_unseq, std::begin(laz_files), std::end(laz_files), std::begin(pointsPerFile), [&](const std::string& fn) - { - // Load mapping from id to sn - fs::path fnSn(fn); - fnSn.replace_extension(".sn"); - - // GetId of Imu to use - const auto idToSn = MLvxCalib::GetIdToSnMapping(fnSn.string()); - auto calibration = MLvxCalib::CombineIntoCalibration(idToSn, preloadedCalibration); - auto data = load_point_cloud(fn.c_str(), true, filter_threshold_xy_inner, filter_threshold_xy_outer, calibration); - - if (fn == laz_files.front()) - { - fs::path calibrationValidtationFile = wdp / "calibrationValidation.asc"; - - std::ofstream testPointcloud{ calibrationValidtationFile.c_str() }; - for (const auto& p : data) - testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid << "\n"; - } - - std::unique_lock lck(mtx); - for (const auto& [id, calib] : calibration) - { - std::cout << " id : " << id << std::endl; - std::cout << calib.matrix() << std::endl; - } - return data; - // std::cout << fn << std::endl; - // - }); - std::cout << "std::transform finished" << std::endl; - */ - if (load_data(input_file_names, params, pointsPerFile, imu_data, false)) { //clear possible previous data @@ -1352,7 +1164,7 @@ void loadData() if (all_data.size() > 0) { - is_init = false; + is_init = true; index_rendered_points_local = 0; } } @@ -1426,7 +1238,7 @@ void imu_data_gui() if (ImGui::Button("Save 'ts gyr_x gyr_y gyr_z acc_x acc_y acc_z yaw_rad pitch_rad roll_rad' to csv")) { std::string output_file_name = ""; - output_file_name = mandeye::fd::SaveFileDialog("Save imu data", {}, ""); + output_file_name = mandeye::fd::SaveFileDialog("Save IMU data", {}, ""); std::cout << "file to save: '" << output_file_name << "'" << std::endl; ofstream file; @@ -1453,12 +1265,12 @@ void imu_data_gui() ImGui::End(); } -void project_gui() +void settings_gui() { - if (ImGui::Begin("Settings")) + if (ImGui::Begin("Settings", &is_settings_gui)) { ImGui::PushItemWidth(ImGuiNumberWidth); - if (is_init) + if (!is_init) { ImGui::InputInt("number_of_points_threshold", &number_of_points_threshold); if (number_of_points_threshold < 0) @@ -1494,14 +1306,16 @@ void project_gui() } } + ImGui::BeginDisabled(!is_init); if (ImGui::Button("Save point cloud")) { std::string output_file_name = ""; output_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter); - std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; - + if (output_file_name.size() > 0) { + std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; + std::vector pointcloud; std::vector intensity; std::vector timestamps; @@ -1520,8 +1334,10 @@ void project_gui() std::cout << "problem with saving file: " << output_file_name << std::endl; } } + ImGui::EndDisabled(); ImGui::Separator(); + ImGui::PushItemWidth(ImGuiNumberWidth); ImGui::InputDouble("distance_bucket", &distance_bucket); ImGui::InputDouble("polar_angle_deg", &polar_angle_deg); @@ -1536,12 +1352,13 @@ void project_gui() ImGui::InputDouble("wfi", &wfi); ImGui::InputDouble("wka", &wka); ImGui::PopItemWidth(); - ImGui::Separator(); + + ImGui::NewLine(); if (ImGui::Button("Optimize")) optimize(); - ImGui::SameLine(); + ImGui::Separator(); if (ImGui::Button("Print to console debug text")) { @@ -1895,6 +1712,12 @@ void display() ImGui::EndMenu(); } + ImGui::Separator(); + + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + ImGui::EndMenu(); } if (ImGui::IsItemHovered()) @@ -1993,7 +1816,8 @@ void display() if (show_imu_data) imu_data_gui(); - project_gui(); + if (is_settings_gui) + settings_gui(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); diff --git a/apps/mandeye_raw_data_viewer/resource.rc b/apps/mandeye_raw_data_viewer/resource.rc index d399a03f..d8c77965 100644 --- a/apps/mandeye_raw_data_viewer/resource.rc +++ b/apps/mandeye_raw_data_viewer/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,2 - PRODUCTVERSION 0,0,9,2 + FILEVERSION 0,0,9,5 + PRODUCTVERSION 0,0,9,5 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Raw Data Viewer\0" - VALUE "FileVersion", "0.9.2\0" + VALUE "FileVersion", "0.9.5\0" VALUE "InternalName", "Raw data viewer\0" VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_raw_data_viewer.exe\0" - VALUE "ProductVersion", "0.9.2\0" + VALUE "ProductVersion", "0.9.5\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 56c2899e..3f6513fc 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -1374,7 +1374,7 @@ void display() ImGui::BeginTooltip(); ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; + - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); ImGui::NewLine(); ImGui::Text("Check Properties (Ctrl+P) for more info"); @@ -1387,7 +1387,7 @@ void display() ImGui::BeginTooltip(); ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; + - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); ImGui::NewLine(); ImGui::Text("Check Properties (Ctrl+P) for more info"); diff --git a/apps/mandeye_single_session_viewer/resource.rc b/apps/mandeye_single_session_viewer/resource.rc index 9ab99052..ae9959ec 100644 --- a/apps/mandeye_single_session_viewer/resource.rc +++ b/apps/mandeye_single_session_viewer/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,2 - PRODUCTVERSION 0,0,9,2 + FILEVERSION 0,0,9,5 + PRODUCTVERSION 0,0,9,5 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Session Viewer\0" - VALUE "FileVersion", "0.9.2\0" + VALUE "FileVersion", "0.9.5\0" VALUE "InternalName", "Single session viewer\0" VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_single_session_viewer.exe\0" - VALUE "ProductVersion", "0.9.2\0" + VALUE "ProductVersion", "0.9.5\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index cb13fd6d..5b2401b2 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -173,6 +173,8 @@ bool is_loop_closure_gui = false; bool remove_gui = false; NDT ndt; +bool is_settings_gui = true; + int number_visible_sessions = 0; int index_gt = -1; int old_index_gt = -1; @@ -186,8 +188,6 @@ struct ProjectSettings std::vector session_file_names; }; - - std::vector edges; int index_active_edge = -1; bool manipulate_active_edge = false; @@ -1993,9 +1993,9 @@ void loadSessions() } } -void project_gui() +void settings_gui() { - if (ImGui::Begin("Settings")) + if (ImGui::Begin("Settings", &is_settings_gui)) { ImGui::Checkbox("Downsample during load", &is_decimate); ImGui::SameLine(); @@ -3561,6 +3561,12 @@ else ImGui::ColorEdit3("Background", (float *)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::Separator(); + + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + ImGui::EndMenu(); } if (ImGui::IsItemHovered()) @@ -3695,7 +3701,8 @@ else // if (is_loop_closure_gui) // manual_pose_graph_loop_closure.Gui(); - project_gui(); + if (is_settings_gui) + settings_gui(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); diff --git a/apps/multi_session_registration/resource.rc b/apps/multi_session_registration/resource.rc index 433dda28..89cc80ae 100644 --- a/apps/multi_session_registration/resource.rc +++ b/apps/multi_session_registration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 2 -PRODUCTVERSION 0, 0, 9, 2 +FILEVERSION 0, 0, 9, 5 +PRODUCTVERSION 0, 0, 9, 5 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 3\0" -VALUE "FileVersion", "0.9.2\0" +VALUE "FileVersion", "0.9.5\0" VALUE "InternalName", "Multi session registration\0" VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "multi_session_registration_step_3.exe\0" -VALUE "ProductVersion", "0.9.2\0" +VALUE "ProductVersion", "0.9.5\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 99b31b91..86c03d6a 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 @@ -194,6 +194,7 @@ bool is_pose_graph_slam = false; bool is_manual_analisys = false; bool is_loop_closure_gui = false; bool is_lio_segments_gui = false; +bool is_settings_gui = true; bool fillInSession = true; @@ -1731,9 +1732,9 @@ void saveSubsession() std::cout << "saving canceled" << std::endl; } -void project_gui() +void settings_gui() { - ImGui::Begin("Settings"); + ImGui::Begin("Settings", &is_settings_gui); { std::string wd = "Working directory: '" + session.working_directory + "'"; @@ -3134,6 +3135,10 @@ void display() ImGui::Separator(); + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + if (ImGui::BeginMenu("Console")) { #ifdef _WIN32 @@ -3228,7 +3233,8 @@ void display() ImGui::EndMainMenuBar(); } - project_gui(); + if (is_settings_gui) + settings_gui(); // my_display_code(); diff --git a/apps/multi_view_tls_registration/resource.rc b/apps/multi_view_tls_registration/resource.rc index a790594a..1f965547 100644 --- a/apps/multi_view_tls_registration/resource.rc +++ b/apps/multi_view_tls_registration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 2 -PRODUCTVERSION 0, 0, 9, 2 +FILEVERSION 0, 0, 9, 5 +PRODUCTVERSION 0, 0, 9, 5 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 2\0" -VALUE "FileVersion", "0.9.2\0" +VALUE "FileVersion", "0.9.5\0" VALUE "InternalName", "Multi view TLS registration\0" VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "multi_view_tls_registration_step_2.exe\0" -VALUE "ProductVersion", "0.9.2\0" +VALUE "ProductVersion", "0.9.5\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/core/include/structures.h b/core/include/structures.h index ef30db72..a202ed17 100644 --- a/core/include/structures.h +++ b/core/include/structures.h @@ -72,6 +72,21 @@ struct Point3Di int index_point; }; +// Point3Di POD (Plain Old Data) variant +// Used for safe binary I/O via read()/write() or memcpy +// Contains only trivially copyable members +#pragma pack(push, 1) +struct Point3DiDisk +{ + double x, y, z; + double timestamp; + float intensity; + int32_t index_pose; + uint8_t lidarid; + int32_t index_point; +}; +#pragma pack(pop) + struct Point { double x = 0.0f; @@ -94,7 +109,35 @@ struct ChunkFile { double time_end_inclusive; }; -template +//Extra measures are needed when writting/reading non-trivial types +//Point3Di containing Eigen::Vector3d to avoid: +//- structure alignment & padding written to file +//- unreadable file on another build +//- undefined behavior if read back via raw load + +inline void convert_to_disk(const Point3Di& src, Point3DiDisk& dst) +{ + dst.x = src.point.x(); + dst.y = src.point.y(); + dst.z = src.point.z(); + dst.timestamp = src.timestamp; + dst.intensity = src.intensity; + dst.index_pose = src.index_pose; + dst.lidarid = src.lidarid; + dst.index_point = src.index_point; +} + +inline void convert_from_disk(const Point3DiDisk& src, Point3Di& dst) +{ + dst.point = Eigen::Vector3d(src.x, src.y, src.z); + dst.timestamp = src.timestamp; + dst.intensity = src.intensity; + dst.index_pose = src.index_pose; + dst.lidarid = src.lidarid; + dst.index_point = src.index_point; +} + +/*template inline bool save_vector_data(const std::string& file_name, std::vector& vector_data) { std::ofstream ofs(file_name, std::ios::binary); if (!ofs.good()) { @@ -102,9 +145,35 @@ inline bool save_vector_data(const std::string& file_name, std::vector& vecto } ofs.write(reinterpret_cast(vector_data.data()), vector_data.size()* sizeof(T)); return true; -} +}*/ template +bool save_vector_data(const std::string& file_name, const std::vector& out) +{ + static_assert( + std::is_trivially_copyable_v || std::is_same_v, + "Binary loading only supported for POD or Point3Di" + ); + + std::ofstream file(file_name, std::ios::binary); + if (!file) + return false; + + if constexpr (std::is_trivially_copyable_v) + // direct write for POD + return file.write(reinterpret_cast(out.data()), out.size() * sizeof(T)).good(); + else + { + // convert to disk type first + std::vector disk(out.size()); + for (size_t i = 0; i < out.size(); ++i) + convert_to_disk(out[i], disk[i]); + + return file.write(reinterpret_cast(disk.data()), disk.size() * sizeof(Point3DiDisk)).good(); + } +} + +/*template inline bool load_vector_data(const std::string& file_name, std::vector& vector_data) { std::basic_ifstream vd_str(file_name, std::ios::binary); if (!vd_str.good()) { @@ -115,6 +184,78 @@ inline bool load_vector_data(const std::string& file_name, std::vector& vecto memcpy(v.data(), data.data(), data.size()); vector_data = v; return true; +}*/ + +/*template +bool load_vector_data(const std::string& file_name, std::vector& out) +{ + //static_assert(std::is_trivially_copyable_v, + // "T must be trivially copyable"); + + std::ifstream file(file_name, std::ios::binary | std::ios::ate); + if (!file) + return false; + + const std::streamsize size = file.tellg(); + if (size < 0 || size % sizeof(T) != 0) + return false; + + const size_t count = size / sizeof(T); + out.resize(count); + + file.seekg(0, std::ios::beg); + if (!file.read(reinterpret_cast(out.data()), size)) + return false; + + return true; +}*/ + +template +bool load_vector_data(const std::string& file_name, std::vector& out) +{ + static_assert( + std::is_trivially_copyable_v || std::is_same_v, + "Binary loading only supported for POD or Point3Di" + ); + + std::ifstream file(file_name, std::ios::binary | std::ios::ate); + if (!file) + return false; + + const std::streamsize size = file.tellg(); + if (size < 0) + return false; + + file.seekg(0, std::ios::beg); + + if constexpr (std::is_trivially_copyable_v) + { + if (size % sizeof(T) != 0) + return false; + + const size_t count = size / sizeof(T); + out.resize(count); + + return file.read(reinterpret_cast(out.data()), size).good(); + } + else + { + // Non-trivial type path (Point3Di) + if (size % sizeof(Point3DiDisk) != 0) + return false; + + const size_t count = size / sizeof(Point3DiDisk); + std::vector disk(count); + + if (!file.read(reinterpret_cast(disk.data()), size)) + return false; + + out.resize(count); + for (size_t i = 0; i < count; ++i) + convert_from_disk(disk[i], out[i]); + + return true; + } } struct LaserBeam { diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index 5b266b66..8cf02ff7 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -1318,18 +1318,16 @@ bool PointClouds::load_whu_tls(std::vector input_file_names, bool i pc.local_trajectory = local_trajectory; } else - { std::cout << "trajectory path: '" << trj_path.string() << "' does not exist" << std::endl; - } } //// load actual pointclouds point_clouds.resize(point_clouds_nodata.size()); std::transform(std::execution::par_unseq, std::begin(point_clouds_nodata), std::end(point_clouds_nodata), std::begin(point_clouds), [&](auto &pc) - { - - if(!load_cache_mode){ + { + if(!load_cache_mode) + { if (load_pc(pc, pc.file_name, load_cache_mode)) { if (is_decimate && pc.points_local.size() > 0) @@ -1346,8 +1344,11 @@ bool PointClouds::load_whu_tls(std::vector input_file_names, bool i } } } - return pc; }); + + return pc; + }); + //calculate average position of a subset of points from all clouds to center the point clouds around the origin if (calculate_offset) { int num = 0; @@ -1362,12 +1363,12 @@ bool PointClouds::load_whu_tls(std::vector input_file_names, bool i this->offset /= num; } - for (int i = 0; i < point_clouds.size(); i++) + //recenter point clouds around calculated offset + if (!this->offset.isZero()) { - for (int j = 0; j < point_clouds[i].points_local.size(); j++) - { - point_clouds[i].points_local[j] -= this->offset; - } + for (int i = 0; i < point_clouds.size(); i++) + for (int j = 0; j < point_clouds[i].points_local.size(); j++) + point_clouds[i].points_local[j] -= this->offset; } print_point_cloud_dimension();