From 8eda6a4b21fc366cac7a8f819f142b8203da8667 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 17:50:30 +0200 Subject: [PATCH 1/9] Fix NDT multi-session crash: build Session overload per GUI flavour core_math is compiled once with WITH_GUI=0, but Session's layout depends on WITH_GUI (GUI builds add ManualPoseGraphLoopClosure, GroundControlPoints and ControlPoints). NDT::optimize(std::vector&) in core_math walked the GUI app's session vector with a 0x1e0 stride instead of 0x250, reading garbage for sessions[1] and segfaulting in multi_session_registration. Move that overload to ndt_session.cpp in CORE_BASE_SOURCES so it is built with the caller's WITH_GUI value; the rest of ndt.cpp stays in core_math (pose_graph_slam.cpp depends on it). Declare ndt_job in ndt.h for the split-out file. Co-Authored-By: Claude Opus 5.5 (1M context) --- core/CMakeLists.txt | 3 +- core/include/Core/ndt.h | 27 ++ core/src/ndt.cpp | 752 -------------------------------------- core/src/ndt_session.cpp | 759 +++++++++++++++++++++++++++++++++++++++ 4 files changed, 788 insertions(+), 753 deletions(-) create mode 100644 core/src/ndt_session.cpp diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 3c353cbc..a91da772 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -8,6 +8,7 @@ set(CORE_BASE_SOURCES src/gnss.cpp src/ground_control_points.cpp src/imu_preintegration.cpp + src/ndt_session.cpp src/nmea.cpp src/point_cloud.cpp src/point_clouds.cpp @@ -16,7 +17,7 @@ set(CORE_BASE_SOURCES # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... ) -# core_math holds the registration/optimization sources with auto-generated Jacobian headers (up to ~24k chars/line, expensive to compile); built once as a static lib shared by core and core_no_gui since none of it branches on WITH_GUI. +# core_math holds the registration/optimization sources with auto-generated Jacobian headers (up to ~24k chars/line, expensive to compile); built once as a static lib shared by core and core_no_gui, so nothing here may touch a type whose layout depends on WITH_GUI (e.g. Session) -- that code goes in CORE_BASE_SOURCES (see ndt_session.cpp). # hash_utils.cpp lives here (not CORE_BASE_SOURCES) because pair_wise_iterative_closest_point.cpp needs get_rgd_index_3d() from it -- keeping both in the same archive avoids a circular static-lib link dependency between core_math and core/core_no_gui. set(CORE_MATH_SOURCES src/hash_utils.cpp diff --git a/core/include/Core/ndt.h b/core/include/Core/ndt.h index 5c081654..b484fc07 100644 --- a/core/include/Core/ndt.h +++ b/core/include/Core/ndt.h @@ -203,3 +203,30 @@ class NDT double sigma_azimuthal_angle = 0.0001; int num_extended_points = 10; }; + +//! Per-thread NDT worker (defined in ndt.cpp); declared here so ndt_session.cpp can dispatch it. +void ndt_job( + int i, + NDT::Job* job, + std::vector* buckets, + Eigen::SparseMatrix* AtPA, + Eigen::SparseMatrix* AtPB, + std::vector* index_pair_internal, + std::vector* pp, + std::vector* mposes, + std::vector* mposes_inv, + size_t trajectory_size, + NDT::PoseConvention pose_convention, + NDT::RotationMatrixParametrization rotation_matrix_parametrization, + int number_of_unknowns, + double* sumssr, + int* sums_obs, + bool is_generalized, + double sigma_r, + double sigma_polar_angle, + double sigma_azimuthal_angle, + int num_extended_points, + double* md_out, + double* md_count_out, + bool compute_only_mean_and_cov, + bool compute_mean_and_cov_for_bucket); diff --git a/core/src/ndt.cpp b/core/src/ndt.cpp index 3cca056a..8e5e4f76 100644 --- a/core/src/ndt.cpp +++ b/core/src/ndt.cpp @@ -2736,758 +2736,6 @@ bool NDT::optimize(std::vector& point_clouds, bool compute_only_maha return true; } -bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) -{ - std::cout << "optimize sessions" << std::endl; - - auto start = std::chrono::system_clock::now(); - - Session tmp_session; - - if (sessions.size() > 1) - { - if (sessions[0].is_ground_truth) - { - tmp_session = sessions[0]; - PointClouds point_clouds_container; - - std::vector point_clouds; - - std::vector points_local; - - for (const auto& pc : sessions[0].point_clouds_container.point_clouds) - { - for (const auto& p : pc.points_local) - { - Eigen::Vector3d pg = pc.m_pose * p; - points_local.push_back(pg); - } - } - - PointCloud pc; - pc.points_local = points_local; - pc.m_initial_pose = Eigen::Affine3d::Identity(); - pc.m_pose = Eigen::Affine3d::Identity(); - - point_clouds.push_back(pc); - point_clouds_container.point_clouds = point_clouds; - - sessions[0].point_clouds_container = point_clouds_container; - } - } - - OptimizationAlgorithm optimization_algorithm; - if (is_gauss_newton) - { - optimization_algorithm = OptimizationAlgorithm::gauss_newton; - } - if (is_levenberg_marguardt) - { - optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; - } - - PoseConvention pose_convention; - if (is_wc) - { - pose_convention = PoseConvention::wc; - } - if (is_cw) - { - pose_convention = PoseConvention::cw; - } - - RotationMatrixParametrization rotation_matrix_parametrization; - if (is_tait_bryan_angles) - { - rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; - } - else if (is_rodrigues) - { - rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; - } - else if (is_quaternion) - { - rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; - } - else if (is_lie_algebra_left_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; - } - else if (is_lie_algebra_right_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; - } - - if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - TaitBryanPose pose; - pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); - pc.m_pose = pc.m_pose * m; - } - } - } - - int number_of_unknowns; - if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - number_of_unknowns = 6; - } - if (is_quaternion) - { - number_of_unknowns = 7; - } - - double lm_lambda = 0.0001; - double previous_rms = std::numeric_limits::max(); - int number_of_lm_iterations = 0; - - std::vector m_poses_tmp; - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - // for (size_t i = 0; i < point_clouds.size(); i++) - //{ - // m_poses_tmp.push_back(point_clouds[i].m_pose); - // } - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - m_poses_tmp.push_back(pc.m_pose); - } - } - } - - for (int iter = 0; iter < number_of_iterations; iter++) - { - std::cout << "building points_global_external begin" << std::endl; - std::vector points_global_external; - size_t num_total_points = 0; - - // for (int i = 0; i < point_clouds.size(); i++) - //{ - // num_total_points += point_clouds[i].points_local.size(); - // } - - int num_point_clouds = 0; - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - num_total_points += pc.points_local.size(); - num_point_clouds++; - } - } - - points_global_external.reserve(num_total_points); - Eigen::Vector3d vt; - Point3D p; - - int index_pose = 0; - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - // num_total_points += pc.points_local.size(); - std::cout << "processing point_cloud [" << index_pose + 1 << "] of " << num_point_clouds << std::endl; - - for (int j = 0; j < pc.points_local.size(); j++) - { - vt = pc.m_pose * pc.points_local[j]; - p.x = vt.x(); - p.y = vt.y(); - p.z = vt.z(); - p.index_pose = index_pose; - points_global_external.emplace_back(p); - } - index_pose++; - } - } - - std::cout << "building points_global_external end" << std::endl; - - std::vector index_pair_external; - std::vector buckets_external; - - GridParameters rgd_params_external; - rgd_params_external.resolution_X = this->bucket_size_external[0]; - rgd_params_external.resolution_Y = this->bucket_size_external[1]; - rgd_params_external.resolution_Z = this->bucket_size_external[2]; - - int bbext = this->bucket_size_external[0]; - if (this->bucket_size_external[1] > bbext) - bbext = this->bucket_size_external[1]; - if (this->bucket_size_external[2] > bbext) - bbext = this->bucket_size_external[2]; - rgd_params_external.bounding_box_extension = bbext; - - std::cout << "building external grid begin" << std::endl; - grid_calculate_params(points_global_external, rgd_params_external); - int num_threads = 1; - if (buckets_external.size() > this->number_of_threads) - { - num_threads = this->number_of_threads; - } - build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); - std::vector buckets_external_reduced; - - for (const auto& b : buckets_external) - { - if (b.number_of_points > 1000) - { - buckets_external_reduced.push_back(b); - } - } - buckets_external = buckets_external_reduced; - buckets_external_reduced.clear(); - - std::sort( - buckets_external.begin(), - buckets_external.end(), - [](const Bucket& a, const Bucket& b) - { - return (a.number_of_points > b.number_of_points); - }); - - std::cout << "building external grid end" << std::endl; - std::cout << "number active buckets external: " << buckets_external.size() << std::endl; - - bool init = false; - Eigen::SparseMatrix AtPA_ndt(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - Eigen::SparseMatrix AtPB_ndt(num_point_clouds * number_of_unknowns, 1); - double rms = 0.0; - int sum = 0; - double md = 0.0; - double md_sum = 0.0; - - for (int bi = 0; bi < buckets_external.size(); bi++) - { - if (compute_only_mahalanobis_distance) - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; - } - else - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " - << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; - } - std::vector points_global; - - for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) - { - points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); - } - - GridParameters rgd_params; - rgd_params.resolution_X = this->bucket_size[0]; - rgd_params.resolution_Y = this->bucket_size[1]; - rgd_params.resolution_Z = this->bucket_size[2]; - rgd_params.bounding_box_extension = 1.0; - - std::vector index_pair; - std::vector buckets; - - std::cout << "building rgd begin" << std::endl; - grid_calculate_params(points_global, rgd_params); - build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); - std::cout << "building rgd end" << std::endl; - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); - // double *md_out, double *md_count_out - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } - - std::vector mposes; - std::vector mposes_inv; - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - mposes.push_back(pc.m_pose); - mposes_inv.push_back(pc.m_pose.inverse()); - } - } - - std::cout << "computing AtPA AtPB start" << std::endl; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back( - std::thread( - ndt_job, - k, - &jobs[k], - &buckets, - &(AtPAtmp[k]), - &(AtPBtmp[k]), - &index_pair, - &points_global, - &mposes, - &mposes_inv, - num_point_clouds, - pose_convention, - rotation_matrix_parametrization, - number_of_unknowns, - &(sumrmss[k]), - &(sums[k]), - is_generalized, - sigma_r, - sigma_polar_angle, - sigma_azimuthal_angle, - num_extended_points, - &(md_out[k]), - &(md_count_out[k]), - false, - compute_mean_and_cov_for_bucket)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - std::cout << "computing AtPA AtPB finished" << std::endl; - - for (size_t k = 0; k < jobs.size(); k++) - { - rms += sumrmss[k]; - sum += sums[k]; - md += md_out[k]; - md_sum += md_count_out[k]; - } - - for (size_t k = 0; k < jobs.size(); k++) - { - if (!init) - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt = AtPAtmp[k]; - AtPB_ndt = AtPBtmp[k]; - init = true; - } - } - else - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt += AtPAtmp[k]; - AtPB_ndt += AtPBtmp[k]; - } - } - } - } - std::cout << "cleaning start" << std::endl; - points_global_external.clear(); - index_pair_external.clear(); - buckets_external.clear(); - std::cout << "cleaning finished" << std::endl; - - rms /= sum; - std::cout << "rms " << rms << std::endl; - - md /= md_sum; - std::cout << "mean mahalanobis distance: " << md << std::endl; - - if (compute_only_mahalanobis_distance) - { - return true; - } - - ////////////////////////////////////////////////////////////////// - - if (is_fix_first_node) - { - Eigen::SparseMatrix I(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - for (int ii = 0; ii < number_of_unknowns; ii++) - { - I.coeffRef(ii, ii) = 1000000; - } - AtPA_ndt += I; - } - - std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; - if (is_levenberg_marguardt) - { - if (rms < previous_rms) - { - if (lm_lambda < 1000000) - { - lm_lambda *= 10.0; - } - previous_rms = rms; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - } - else - { - lm_lambda /= 10.0; - number_of_lm_iterations++; - iter--; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - int index = 0; - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - pc.m_pose = m_poses_tmp[index++]; - } - } - - previous_rms = std::numeric_limits::max(); - continue; - } - } - else - { - previous_rms = rms; - } - - if (is_quaternion) - { - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - int index = 0; - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - // pc.m_pose = m_poses_tmp[index++]; - int ic = index * 7; - int ir = 0; - QuaternionPose pose; - if (is_wc) - { - pose = pose_quaternion_from_affine_matrix(pc.m_pose); - } - else - { - pose = pose_quaternion_from_affine_matrix(pc.m_pose.inverse()); - } - double delta; - quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); - - Eigen::Matrix jacobian; - quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); - - tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); - tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); - - tripletListP.emplace_back(ir, ir, 1000000.0); - - tripletListB.emplace_back(ir, 0, delta); - - index++; - } - } - - Eigen::SparseMatrix matA(tripletListB.size(), num_point_clouds * 7); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(num_point_clouds * 7, num_point_clouds * 7); - Eigen::SparseMatrix AtPB(num_point_clouds * 7, 1); - - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = AtP * matA; - AtPB = AtP * matB; - - AtPA_ndt += AtPA; - AtPB_ndt += AtPB; - } - - if (is_levenberg_marguardt) - { - Eigen::SparseMatrix LM(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - LM.setIdentity(); - LM *= lm_lambda; - AtPA_ndt += LM; - } - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA_ndt); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB_ndt); - - std::vector h_x; - std::cout << "redult: row,col,value" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - if (it.value() == it.value()) - { - h_x.push_back(it.value()); - std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; - } - } - } - - if (h_x.size() == num_point_clouds * number_of_unknowns) - { - std::cout << "AtPA=AtPB SOLVED" << std::endl; - int counter = 0; - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - Eigen::Affine3d m_pose; - - if (is_wc) - { - m_pose = pc.m_pose; - } - else - { - m_pose = pc.m_pose.inverse(); - } - - if (is_tait_bryan_angles) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; - m_pose = affine_matrix_from_pose_tait_bryan(pose); - } - else if (is_rodrigues) - { - RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.sx += h_x[counter++]; - pose.sy += h_x[counter++]; - pose.sz += h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose); - } - else if (is_quaternion) - { - QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); - - QuaternionPose poseq; - poseq.px = h_x[counter++]; - poseq.py = h_x[counter++]; - poseq.pz = h_x[counter++]; - poseq.q0 = h_x[counter++]; - poseq.q1 = h_x[counter++]; - poseq.q2 = h_x[counter++]; - poseq.q3 = h_x[counter++]; - - if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && - fabs(poseq.pz) < this->bucket_size[0] && fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && - fabs(poseq.q3) < 10) - { - pose.px += poseq.px; - pose.py += poseq.py; - pose.pz += poseq.pz; - pose.q0 += poseq.q0; - pose.q1 += poseq.q1; - pose.q2 += poseq.q2; - pose.q3 += poseq.q3; - m_pose = affine_matrix_from_pose_quaternion(pose); - } - } - else if (is_lie_algebra_left_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; - } - else if (is_lie_algebra_right_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); - } - - if (is_wc) - { - } - else - { - m_pose = m_pose.inverse(); - } - - auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); - auto pose_src = pose_tait_bryan_from_affine_matrix(pc.m_pose); - - if (!pc.fixed_x) - { - pose_src.px = pose_res.px; - } - if (!pc.fixed_y) - { - pose_src.py = pose_res.py; - } - if (!pc.fixed_z) - { - pose_src.pz = pose_res.pz; - } - if (!pc.fixed_om) - { - pose_src.om = pose_res.om; - } - if (!pc.fixed_fi) - { - pose_src.fi = pose_res.fi; - } - if (!pc.fixed_ka) - { - pose_src.ka = pose_res.ka; - } - - pc.pose = pose_src; - pc.gui_translation[0] = pose_src.px; - pc.gui_translation[1] = pose_src.py; - pc.gui_translation[2] = pose_src.pz; - pc.gui_rotation[0] = rad2deg(pose_src.om); - pc.gui_rotation[1] = rad2deg(pose_src.fi); - pc.gui_rotation[2] = rad2deg(pose_src.ka); - - /* - if (is_wc) - { - // if (!s.is_ground_truth) - //{ - pc.m_pose = m_pose; // ToDo check if !pc.fixed needed - //} - } - else - { - // if (!s.is_ground_truth) - //{ - pc.m_pose = m_pose.inverse(); // ToDo check if !pc.fixed needed - //} - } - - if (!pc.fixed) - { - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - }*/ - } - } - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - m_poses_tmp.push_back(pc.m_pose); - } - } - } - - std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; - } - else - { - std::cout << "AtPA=AtPB FAILED" << std::endl; - break; - } - } - - ////////// - - if (sessions.size() > 1) - { - if (sessions[0].is_ground_truth) - { - Eigen::Affine3d pose_inv0 = sessions[0].point_clouds_container.point_clouds[0].m_pose.inverse(); - - sessions[0].point_clouds_container = tmp_session.point_clouds_container; - - for (int i = 1; i < sessions.size(); i++) - { - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - { - sessions[i].point_clouds_container.point_clouds[j].m_pose = - sessions[i].point_clouds_container.point_clouds[j].m_pose * pose_inv0; - sessions[i].point_clouds_container.point_clouds[j].pose = - pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); - sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = - sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = - sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = - sessions[i].point_clouds_container.point_clouds[j].pose.pz; - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = - rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.om); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = - rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.fi); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = - rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.ka); - } - } - } - } - ////////// - - auto end = std::chrono::system_clock::now(); - auto elapsed = std::chrono::duration_cast(end - start); - - std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; - - return true; -} - std::vector> NDT::compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms) { OptimizationAlgorithm optimization_algorithm; diff --git a/core/src/ndt_session.cpp b/core/src/ndt_session.cpp new file mode 100644 index 00000000..5b8af793 --- /dev/null +++ b/core/src/ndt_session.cpp @@ -0,0 +1,759 @@ +#include + +// Split out of ndt.cpp: Session has a WITH_GUI-dependent layout, so this must be built per GUI flavour, not in core_math. +#include +#include +#include + +#include + +bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) +{ + std::cout << "optimize sessions" << std::endl; + + auto start = std::chrono::system_clock::now(); + + Session tmp_session; + + if (sessions.size() > 1) + { + if (sessions[0].is_ground_truth) + { + tmp_session = sessions[0]; + PointClouds point_clouds_container; + + std::vector point_clouds; + + std::vector points_local; + + for (const auto& pc : sessions[0].point_clouds_container.point_clouds) + { + for (const auto& p : pc.points_local) + { + Eigen::Vector3d pg = pc.m_pose * p; + points_local.push_back(pg); + } + } + + PointCloud pc; + pc.points_local = points_local; + pc.m_initial_pose = Eigen::Affine3d::Identity(); + pc.m_pose = Eigen::Affine3d::Identity(); + + point_clouds.push_back(pc); + point_clouds_container.point_clouds = point_clouds; + + sessions[0].point_clouds_container = point_clouds_container; + } + } + + OptimizationAlgorithm optimization_algorithm; + if (is_gauss_newton) + { + optimization_algorithm = OptimizationAlgorithm::gauss_newton; + } + if (is_levenberg_marguardt) + { + optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; + } + + PoseConvention pose_convention; + if (is_wc) + { + pose_convention = PoseConvention::wc; + } + if (is_cw) + { + pose_convention = PoseConvention::cw; + } + + RotationMatrixParametrization rotation_matrix_parametrization; + if (is_tait_bryan_angles) + { + rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; + } + else if (is_rodrigues) + { + rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; + } + else if (is_quaternion) + { + rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; + } + else if (is_lie_algebra_left_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; + } + else if (is_lie_algebra_right_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; + } + + if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + TaitBryanPose pose; + pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); + pc.m_pose = pc.m_pose * m; + } + } + } + + int number_of_unknowns; + if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + number_of_unknowns = 6; + } + if (is_quaternion) + { + number_of_unknowns = 7; + } + + double lm_lambda = 0.0001; + double previous_rms = std::numeric_limits::max(); + int number_of_lm_iterations = 0; + + std::vector m_poses_tmp; + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + // for (size_t i = 0; i < point_clouds.size(); i++) + //{ + // m_poses_tmp.push_back(point_clouds[i].m_pose); + // } + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + m_poses_tmp.push_back(pc.m_pose); + } + } + } + + for (int iter = 0; iter < number_of_iterations; iter++) + { + std::cout << "building points_global_external begin" << std::endl; + std::vector points_global_external; + size_t num_total_points = 0; + + // for (int i = 0; i < point_clouds.size(); i++) + //{ + // num_total_points += point_clouds[i].points_local.size(); + // } + + int num_point_clouds = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + num_total_points += pc.points_local.size(); + num_point_clouds++; + } + } + + points_global_external.reserve(num_total_points); + Eigen::Vector3d vt; + Point3D p; + + int index_pose = 0; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + // num_total_points += pc.points_local.size(); + std::cout << "processing point_cloud [" << index_pose + 1 << "] of " << num_point_clouds << std::endl; + + for (int j = 0; j < pc.points_local.size(); j++) + { + vt = pc.m_pose * pc.points_local[j]; + p.x = vt.x(); + p.y = vt.y(); + p.z = vt.z(); + p.index_pose = index_pose; + points_global_external.emplace_back(p); + } + index_pose++; + } + } + + std::cout << "building points_global_external end" << std::endl; + + std::vector index_pair_external; + std::vector buckets_external; + + GridParameters rgd_params_external; + rgd_params_external.resolution_X = this->bucket_size_external[0]; + rgd_params_external.resolution_Y = this->bucket_size_external[1]; + rgd_params_external.resolution_Z = this->bucket_size_external[2]; + + int bbext = this->bucket_size_external[0]; + if (this->bucket_size_external[1] > bbext) + bbext = this->bucket_size_external[1]; + if (this->bucket_size_external[2] > bbext) + bbext = this->bucket_size_external[2]; + rgd_params_external.bounding_box_extension = bbext; + + std::cout << "building external grid begin" << std::endl; + grid_calculate_params(points_global_external, rgd_params_external); + int num_threads = 1; + if (buckets_external.size() > this->number_of_threads) + { + num_threads = this->number_of_threads; + } + build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); + std::vector buckets_external_reduced; + + for (const auto& b : buckets_external) + { + if (b.number_of_points > 1000) + { + buckets_external_reduced.push_back(b); + } + } + buckets_external = buckets_external_reduced; + buckets_external_reduced.clear(); + + std::sort( + buckets_external.begin(), + buckets_external.end(), + [](const Bucket& a, const Bucket& b) + { + return (a.number_of_points > b.number_of_points); + }); + + std::cout << "building external grid end" << std::endl; + std::cout << "number active buckets external: " << buckets_external.size() << std::endl; + + bool init = false; + Eigen::SparseMatrix AtPA_ndt(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + Eigen::SparseMatrix AtPB_ndt(num_point_clouds * number_of_unknowns, 1); + double rms = 0.0; + int sum = 0; + double md = 0.0; + double md_sum = 0.0; + + for (int bi = 0; bi < buckets_external.size(); bi++) + { + if (compute_only_mahalanobis_distance) + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; + } + else + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " + << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; + } + std::vector points_global; + + for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) + { + points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); + } + + GridParameters rgd_params; + rgd_params.resolution_X = this->bucket_size[0]; + rgd_params.resolution_Y = this->bucket_size[1]; + rgd_params.resolution_Z = this->bucket_size[2]; + rgd_params.bounding_box_extension = 1.0; + + std::vector index_pair; + std::vector buckets; + + std::cout << "building rgd begin" << std::endl; + grid_calculate_params(points_global, rgd_params); + build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + std::cout << "building rgd end" << std::endl; + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + // double *md_out, double *md_count_out + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + mposes.push_back(pc.m_pose); + mposes_inv.push_back(pc.m_pose.inverse()); + } + } + + std::cout << "computing AtPA AtPB start" << std::endl; + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + num_point_clouds, + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + false, + compute_mean_and_cov_for_bucket)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + std::cout << "computing AtPA AtPB finished" << std::endl; + + for (size_t k = 0; k < jobs.size(); k++) + { + rms += sumrmss[k]; + sum += sums[k]; + md += md_out[k]; + md_sum += md_count_out[k]; + } + + for (size_t k = 0; k < jobs.size(); k++) + { + if (!init) + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt = AtPAtmp[k]; + AtPB_ndt = AtPBtmp[k]; + init = true; + } + } + else + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt += AtPAtmp[k]; + AtPB_ndt += AtPBtmp[k]; + } + } + } + } + std::cout << "cleaning start" << std::endl; + points_global_external.clear(); + index_pair_external.clear(); + buckets_external.clear(); + std::cout << "cleaning finished" << std::endl; + + rms /= sum; + std::cout << "rms " << rms << std::endl; + + md /= md_sum; + std::cout << "mean mahalanobis distance: " << md << std::endl; + + if (compute_only_mahalanobis_distance) + { + return true; + } + + ////////////////////////////////////////////////////////////////// + + if (is_fix_first_node) + { + Eigen::SparseMatrix I(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + for (int ii = 0; ii < number_of_unknowns; ii++) + { + I.coeffRef(ii, ii) = 1000000; + } + AtPA_ndt += I; + } + + std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; + if (is_levenberg_marguardt) + { + if (rms < previous_rms) + { + if (lm_lambda < 1000000) + { + lm_lambda *= 10.0; + } + previous_rms = rms; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + } + else + { + lm_lambda /= 10.0; + number_of_lm_iterations++; + iter--; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + int index = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + pc.m_pose = m_poses_tmp[index++]; + } + } + + previous_rms = std::numeric_limits::max(); + continue; + } + } + else + { + previous_rms = rms; + } + + if (is_quaternion) + { + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + int index = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + // pc.m_pose = m_poses_tmp[index++]; + int ic = index * 7; + int ir = 0; + QuaternionPose pose; + if (is_wc) + { + pose = pose_quaternion_from_affine_matrix(pc.m_pose); + } + else + { + pose = pose_quaternion_from_affine_matrix(pc.m_pose.inverse()); + } + double delta; + quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); + + Eigen::Matrix jacobian; + quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); + + tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); + tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); + + tripletListP.emplace_back(ir, ir, 1000000.0); + + tripletListB.emplace_back(ir, 0, delta); + + index++; + } + } + + Eigen::SparseMatrix matA(tripletListB.size(), num_point_clouds * 7); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(num_point_clouds * 7, num_point_clouds * 7); + Eigen::SparseMatrix AtPB(num_point_clouds * 7, 1); + + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = AtP * matA; + AtPB = AtP * matB; + + AtPA_ndt += AtPA; + AtPB_ndt += AtPB; + } + + if (is_levenberg_marguardt) + { + Eigen::SparseMatrix LM(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + LM.setIdentity(); + LM *= lm_lambda; + AtPA_ndt += LM; + } + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA_ndt); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB_ndt); + + std::vector h_x; + std::cout << "redult: row,col,value" << std::endl; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + if (it.value() == it.value()) + { + h_x.push_back(it.value()); + std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; + } + } + } + + if (h_x.size() == num_point_clouds * number_of_unknowns) + { + std::cout << "AtPA=AtPB SOLVED" << std::endl; + int counter = 0; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + Eigen::Affine3d m_pose; + + if (is_wc) + { + m_pose = pc.m_pose; + } + else + { + m_pose = pc.m_pose.inverse(); + } + + if (is_tait_bryan_angles) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + m_pose = affine_matrix_from_pose_tait_bryan(pose); + } + else if (is_rodrigues) + { + RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.sx += h_x[counter++]; + pose.sy += h_x[counter++]; + pose.sz += h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose); + } + else if (is_quaternion) + { + QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); + + QuaternionPose poseq; + poseq.px = h_x[counter++]; + poseq.py = h_x[counter++]; + poseq.pz = h_x[counter++]; + poseq.q0 = h_x[counter++]; + poseq.q1 = h_x[counter++]; + poseq.q2 = h_x[counter++]; + poseq.q3 = h_x[counter++]; + + if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && + fabs(poseq.pz) < this->bucket_size[0] && fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && + fabs(poseq.q3) < 10) + { + pose.px += poseq.px; + pose.py += poseq.py; + pose.pz += poseq.pz; + pose.q0 += poseq.q0; + pose.q1 += poseq.q1; + pose.q2 += poseq.q2; + pose.q3 += poseq.q3; + m_pose = affine_matrix_from_pose_quaternion(pose); + } + } + else if (is_lie_algebra_left_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; + } + else if (is_lie_algebra_right_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); + } + + if (is_wc) + { + } + else + { + m_pose = m_pose.inverse(); + } + + auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); + auto pose_src = pose_tait_bryan_from_affine_matrix(pc.m_pose); + + if (!pc.fixed_x) + { + pose_src.px = pose_res.px; + } + if (!pc.fixed_y) + { + pose_src.py = pose_res.py; + } + if (!pc.fixed_z) + { + pose_src.pz = pose_res.pz; + } + if (!pc.fixed_om) + { + pose_src.om = pose_res.om; + } + if (!pc.fixed_fi) + { + pose_src.fi = pose_res.fi; + } + if (!pc.fixed_ka) + { + pose_src.ka = pose_res.ka; + } + + pc.pose = pose_src; + pc.gui_translation[0] = pose_src.px; + pc.gui_translation[1] = pose_src.py; + pc.gui_translation[2] = pose_src.pz; + pc.gui_rotation[0] = rad2deg(pose_src.om); + pc.gui_rotation[1] = rad2deg(pose_src.fi); + pc.gui_rotation[2] = rad2deg(pose_src.ka); + + /* + if (is_wc) + { + // if (!s.is_ground_truth) + //{ + pc.m_pose = m_pose; // ToDo check if !pc.fixed needed + //} + } + else + { + // if (!s.is_ground_truth) + //{ + pc.m_pose = m_pose.inverse(); // ToDo check if !pc.fixed needed + //} + } + + if (!pc.fixed) + { + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + }*/ + } + } + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + m_poses_tmp.push_back(pc.m_pose); + } + } + } + + std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; + } + else + { + std::cout << "AtPA=AtPB FAILED" << std::endl; + break; + } + } + + ////////// + + if (sessions.size() > 1) + { + if (sessions[0].is_ground_truth) + { + Eigen::Affine3d pose_inv0 = sessions[0].point_clouds_container.point_clouds[0].m_pose.inverse(); + + sessions[0].point_clouds_container = tmp_session.point_clouds_container; + + for (int i = 1; i < sessions.size(); i++) + { + for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + sessions[i].point_clouds_container.point_clouds[j].m_pose = + sessions[i].point_clouds_container.point_clouds[j].m_pose * pose_inv0; + sessions[i].point_clouds_container.point_clouds[j].pose = + pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); + sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = + sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = + sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = + sessions[i].point_clouds_container.point_clouds[j].pose.pz; + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.om); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.fi); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.ka); + } + } + } + } + ////////// + + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; + + return true; +} From b77346b7c050f342572d3e9aae318c29e1b8feb5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 18:50:13 +0200 Subject: [PATCH 2/9] Format ndt_session.cpp with clang-format 21 (as CI does) It had been formatted with clang-format 18, whose layout of the std::thread argument list CI's clang-format 21 rejects. The function body is again byte-identical to the original in ndt.cpp. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- core/src/ndt_session.cpp | 53 ++++++++++++++++++++-------------------- 1 file changed, 27 insertions(+), 26 deletions(-) diff --git a/core/src/ndt_session.cpp b/core/src/ndt_session.cpp index 5b8af793..52feba7e 100644 --- a/core/src/ndt_session.cpp +++ b/core/src/ndt_session.cpp @@ -312,32 +312,33 @@ bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis std::cout << "computing AtPA AtPB start" << std::endl; for (size_t k = 0; k < jobs.size(); k++) { - threads.push_back(std::thread( - ndt_job, - k, - &jobs[k], - &buckets, - &(AtPAtmp[k]), - &(AtPBtmp[k]), - &index_pair, - &points_global, - &mposes, - &mposes_inv, - num_point_clouds, - pose_convention, - rotation_matrix_parametrization, - number_of_unknowns, - &(sumrmss[k]), - &(sums[k]), - is_generalized, - sigma_r, - sigma_polar_angle, - sigma_azimuthal_angle, - num_extended_points, - &(md_out[k]), - &(md_count_out[k]), - false, - compute_mean_and_cov_for_bucket)); + threads.push_back( + std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + num_point_clouds, + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + false, + compute_mean_and_cov_for_bucket)); } for (size_t j = 0; j < threads.size(); j++) From 09c72a2630822cee0dcb43625fad01c08224b2be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 18:50:13 +0200 Subject: [PATCH 3/9] Make Session's layout independent of WITH_GUI Session held ManualPoseGraphLoopClosure + GroundControlPoints + ControlPoints with WITH_GUI=1 but only PoseGraphLoopClosure with WITH_GUI=0, so core_math (built WITH_GUI=0) and GUI apps disagreed on sizeof(Session) -- the cause of the NDT multi-session crash. Data members are now unconditional in all four classes; only GUI methods stay behind #if. sizeof(Session) is 592 in both. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- core/include/Core/control_points.h | 3 ++- core/include/Core/ground_control_points.h | 3 ++- core/include/Core/manual_pose_graph_loop_closure.h | 11 +++++++---- core/include/Core/session.h | 10 +--------- 4 files changed, 12 insertions(+), 15 deletions(-) diff --git a/core/include/Core/control_points.h b/core/include/Core/control_points.h index c6385bf3..b06c0878 100644 --- a/core/include/Core/control_points.h +++ b/core/include/Core/control_points.h @@ -30,7 +30,7 @@ class ControlPoints bool is_imgui = false; std::vector cps; -#if WITH_GUI == 1 + // Data members stay outside WITH_GUI so the class layout is identical in GUI and non-GUI builds. bool picking_mode = false; bool draw_uncertainty = false; int index_picked_point = -1; @@ -38,6 +38,7 @@ class ControlPoints int index_pose = 0; +#if WITH_GUI == 1 void imgui(PointClouds& point_clouds_container, const Eigen::Vector3f& rotation_center); void render(const PointClouds& point_clouds_container, bool show_pc); void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd = 1); diff --git a/core/include/Core/ground_control_points.h b/core/include/Core/ground_control_points.h index 4eb73a0a..9f22eaef 100644 --- a/core/include/Core/ground_control_points.h +++ b/core/include/Core/ground_control_points.h @@ -27,13 +27,14 @@ class GroundControlPoints std::vector gpcs; double default_lidar_height_above_ground = 0.15; -#if WITH_GUI == 1 + // Data members stay outside WITH_GUI so the class layout is identical in GUI and non-GUI builds. bool is_imgui = false; bool picking_mode = false; int picking_mode_index_to_node_inner = -1; int picking_mode_index_to_node_outer = -1; bool draw_uncertainty = false; +#if WITH_GUI == 1 void imgui(PointClouds& point_clouds_container); void render(const PointClouds& point_clouds_container); void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd = 1); diff --git a/core/include/Core/manual_pose_graph_loop_closure.h b/core/include/Core/manual_pose_graph_loop_closure.h index d43bc92a..58e51d42 100644 --- a/core/include/Core/manual_pose_graph_loop_closure.h +++ b/core/include/Core/manual_pose_graph_loop_closure.h @@ -1,12 +1,15 @@ #pragma once -#if WITH_GUI == 1 #include -#include #include #include #include +#if WITH_GUI == 1 +#include +#endif + +// Defined in every build (not only WITH_GUI) so Session has one layout; only the GUI methods are conditional. class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure { public: @@ -18,6 +21,7 @@ class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure ManualPoseGraphLoopClosure() = default; ~ManualPoseGraphLoopClosure() = default; +#if WITH_GUI == 1 void Gui( PointClouds& point_clouds_container, int& index_loop_closure_source, @@ -35,6 +39,5 @@ class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure int index_loop_closure_target, int num_edge_extended_before, int num_edge_extended_after); -}; - #endif +}; diff --git a/core/include/Core/session.h b/core/include/Core/session.h index 57f13717..35e22c26 100644 --- a/core/include/Core/session.h +++ b/core/include/Core/session.h @@ -5,15 +5,10 @@ #include #include -#if WITH_GUI == 1 #include #include #include -#else -#include -#endif - class Session { public: @@ -30,13 +25,10 @@ class Session // bool show_rgb = true; bool load_cache_mode = false; -#if WITH_GUI == 1 + // No WITH_GUI branches here: Session is shared between core_math (WITH_GUI=0) and GUI apps, so its layout must not depend on it. ManualPoseGraphLoopClosure pose_graph_loop_closure; GroundControlPoints ground_control_points; ControlPoints control_points; -#else - PoseGraphLoopClosure pose_graph_loop_closure; -#endif bool load(const std::string& file_name, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset); bool save( From 1e0d9e051e1e2d05319f10f6e4f29210f8fb724c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 18:50:13 +0200 Subject: [PATCH 4/9] Keep the GLUT step 3 as multi_session_registration_step_3_legacy Independent copy of apps/multi_session_registration as it is before the raylib port, mirroring multi_view_tls_registration_legacy for step 2. Added to the top-level CMake and to deploy_mandeye.bat's expected binaries. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- CMakeLists.txt | 1 + .../CMakeLists.txt | 73 + .../icon.ico | Bin 0 -> 23088 bytes .../multi_session_factor_graph.cpp | 928 ++++ .../multi_session_factor_graph.h | 21 + .../multi_session_registration.cpp | 4391 +++++++++++++++++ .../resource.h | 17 + .../resource.rc | 49 + deploy_mandeye.bat | 2 +- 9 files changed, 5481 insertions(+), 1 deletion(-) create mode 100644 apps/multi_session_registration_legacy/CMakeLists.txt create mode 100644 apps/multi_session_registration_legacy/icon.ico create mode 100644 apps/multi_session_registration_legacy/multi_session_factor_graph.cpp create mode 100644 apps/multi_session_registration_legacy/multi_session_factor_graph.h create mode 100644 apps/multi_session_registration_legacy/multi_session_registration.cpp create mode 100644 apps/multi_session_registration_legacy/resource.h create mode 100644 apps/multi_session_registration_legacy/resource.rc diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c027b32..2f1eff56 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -135,6 +135,7 @@ add_subdirectory(apps/mandeye_single_session_viewer) add_subdirectory(apps/multi_view_tls_registration) add_subdirectory(apps/multi_view_tls_registration_legacy) add_subdirectory(apps/multi_session_registration) +add_subdirectory(apps/multi_session_registration_legacy) if(BUILD_WITH_UTILITY_APPLICATION) add_subdirectory(apps/manual_color) diff --git a/apps/multi_session_registration_legacy/CMakeLists.txt b/apps/multi_session_registration_legacy/CMakeLists.txt new file mode 100644 index 00000000..01c03fbe --- /dev/null +++ b/apps/multi_session_registration_legacy/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 4.0.0) + +# Legacy GLUT + legacy immediate-mode OpenGL build of step3, kept side by +# side with apps/multi_session_registration (raylib-based since it was +# ported off GLUT), the same way step2 keeps multi_view_tls_registration_legacy. +# This is a full, independent copy of the app as it was before that port -- +# not sharing translation units with apps/multi_session_registration -- so +# the two can diverge or be retired independently. +project(multi_session_registration_step_3_legacy) + +# Source files +set(SOURCES + multi_session_registration.cpp + multi_session_factor_graph.cpp + "../../core/src/utils.cpp" +) + +# Windows: add resource file +if(WIN32) + list(APPEND SOURCES "resource.rc") +endif() + +add_executable( + multi_session_registration_step_3_legacy ${SOURCES} + ) + +target_compile_definitions(multi_session_registration_step_3_legacy PRIVATE -DWITH_GUI=1) + +target_include_directories( + multi_session_registration_step_3_legacy + PRIVATE include + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/core_hd_mapping/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/freeglut/include + ${LASZIP_INCLUDE_DIR}/LASzip/include) + +target_link_libraries( + multi_session_registration_step_3_legacy + # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib + ${FREEGLUT_LIBRARY} + ${OPENGL_gl_LIBRARY} + OpenGL::GLU + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} + ${CORE_LIBRARIES} + ${GUI_LIBRARIES}) + +if(WIN32) + add_custom_command( + TARGET multi_session_registration_step_3_legacy + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if (MSVC) + target_compile_options(multi_session_registration_step_3_legacy PRIVATE /bigobj) +endif() + +hdmapping_install_app(multi_session_registration_step_3_legacy) \ No newline at end of file diff --git a/apps/multi_session_registration_legacy/icon.ico b/apps/multi_session_registration_legacy/icon.ico new file mode 100644 index 0000000000000000000000000000000000000000..f8a2cb27bbcc89f5edfb3ecc7b3b1c71e1f87324 GIT binary patch literal 23088 zcmXV11y~zhlm&{rd(q%l9Ew}9Vuc{Z-6246_hQA0d!e`#cXtgGcemp1u=DTk@bM)i zVKVdHm2=NM7X}6v`1$XEg`t8W{R#s^2t1EaRhGp-B}E0EV#v!$fBEm-|9+7Xfd@V3 zVhb1;olSXZ2@Q{>lPq^1eDl}Ab?#c;cA|thg)@`?wwb{ zA}S(PI?B8F*X2Le)yn16yTc-{X>|wIUNnlPLC}t_tGBx z_B=bFcTc_`>JV{R+5JrExeVZu2$B`OaS%t^c<|V z@-s0pp;HK_?(CTL^!B={O->y;z)``QSyFC~a&($a7b+dEG$)!4)|8Z%?r2oYCV+6r z$v>a1wUdbY2uv5LSZ?*9$tfsECxBi`4-e|_zIe;Rwdk>TyuEsBR_YBu-W&}N4Oz|2 ziV5M%P%|+_5t-QAL51z_g0$@DnG8m1WSf2%9Z!a#dQbB$A3`ZV$U<4 zhex6HKIerb z-j{J_mgoO*2ZNjpw5jI%LS2kKu-nTmJ_o>r zU}wMasiQ#OMw_w%YT%XOA8wRI*($ciSH8Gu%=~I_YTAa11XXVj9BOIE7SY0Q zdKfQq6)LCwG6cVWZ;09z7DJ}vG0IjEy$;QVNJ-*HC78GFzs_4cQQX z`SGT+b9RKHwL00`1{Pv|ZRjrKq+LN;TH{lxxdzw zKTA>ABhdF=Us1FPniu;Cg`bXVeV$Gl9|`8VcbzYK(4a0C{X{OaV6BUXWHzsS4}l6j zn2hlObrq3ag4H6q^RgKOJ#w7r(yYf44sUeP*0t`5k&kZR*#sS}*zw=6aALuMJ1}Uy z!7s!l3J9cBkNFPRdUonH<-U9FogJt$e)ZN0hcqRQ;Ji)USeV+&^*PFI@ z^`6HbROb^6`-%P5P?=wiIcbWfy{TlB5{CY0>GaA1HQ3pSO(Jy3Rim~GhT!?@#c)c$ zAuU7y3Kfj(ZG3!uRW-G#Yf)O-;K)c+eHeil>VQ>;{r(h#J_{kyI|>J`^P?F9cqY{J zLF8pVzX&f_4s#;zvsNkAQO1qbir0_zJ}bP@{zR7DYRjzcGIX1U7-uV1QRqS=2P8jZ zO&Uu+vZ?YE8*l0;uz9Dvb2Hk&dp_u^Bb=NarDVl$gHcLRw9#}fUUibe=dd+M=|59d z_d}U1(;4}EYLHT0M`!U`bj)m{r3o2-Lt4HAcan;SLM(GsF)g%$hlYzQ!D)Md(|JeE z#Ka^nZir>bbP!8up}{^QrK+0=o`akX2G;+8ELJ{Y>G~9n8kv)EO!2#!u1?G{6Uxs1 zey#NtJt7uGx#5Y+&xY(*ulTfS?0}IG1<}>EMcd`35AKq&ZV3);AjR?tw%w$18jkJ8 zuN7n2A{lgw$sfKQOlnRDwysW`>bH709DZ5zI$|RwU1>*zZTqPs7ugw#fD&KKuu#to zpV$+8NMvR1e{(@SS{Rd|{cV|sl`zV$9}4^L=wxSS=W;tX{QhDv*r)b{tjgF(#vH-_ z?P;Av&<6DKc+0@XmNYapwP`2s7aqOw1kCpCycmP+wHQYK+#^^FgRJ5u#G!rmZC z%i1$|1cZx&{MZ`EW)Gg%ZiObx_Jml@CheyWHZzQ`*F|Eym4E;FUtSmR~p@`<~@YiCKVi`4ntP`b%NTwkdC65+Zv$h zu569K?vc1wgXFgwmL0bLl~vs?byWTddiG+tMDbI=3+jL+G+`?&w8sn&2F`vk_cs_@ z@{jA0=H7V5VCGTP{z>DyDDAdmR9G0I-@UPrf76?v#aL=LZLF}Qgz;uG6AWfiaK3D; zm;DiuQU9+YhUn`=x(Lcrn_-9B>)7w#jes0uNf2#X|5c~PEJEwE|Bs5m)mc7#&{Db> zHk>2|qL*&_o%`_u=XKXcwgebV5G#C%Ja6F5cGiAJ;nvzdentX{u!@R+7mJ#I*IPbR z0I!Y0OR6kT`!g+V_#KvT7q6D#IDiGb?X(|I5V{;{y%AJkKo2^q%3LH20lr|qD$m#d z9LcBeyjHXe2e22f>x05OcIpLP{rw5D+(kz5kACR%IyA%Ux-iZz&Je+@mYZ)bHMO

*)l4@4U+3>pw%Aw$QUimynzMoQKSSeRi8V4~p4slAx zl)r+2h93&necv9YyjC)JJzZB@`?|6=-AWqzGAiq|)t z8BZ%~%$d`Ew+e;E0v7!TxZXm0*`t3>Hf-6iO{R@tphjr~LR@>9>O^wCM4bZrS>X{VP93e({HK!(>F z4n1XtSkXuASSD$foW8KHf+xy*oOZ`QyknA-Y3uLGu`OTU%cxyLCLI*a&FSt|-3?i` z`4w=lykswy4m!pnF?yL51?soK#USxZX3;}mfBrR40!&_DDXNLmr&Hy_ewO?vE^3;g zBNYwscJqF6&9}oZJQbGr@XA)xk-vv*pWZ=KNP>aw7g+j}YDp1RjFzbF$UQNg-_ZRs?DXxZ}aPlnppuOr*BVy6Vr7Rly*>tcbP z7x9yy*wb0%k>$5?E3%8YP*WRUu>C(m&@H(U7%SZ&N4MVyPbQ4OeoS=i1Tq)M616Sg z(y`%(aPRJ-_P4AY#bzq?Tkr3tRo-^Y&sxf1QlkenG23YC-E8zrg$6rgI;R3QyI&0? z1VTd*Samy&%SlT+ImURa@g+QJg%A##lCnxygWq(v6zl`w zCxhj^n;$!c9`5f^sYB-P4dOx0WaQ5es+jY>-f@F|W1D4vZ4^k`iu}RUL&atLFV9?l zwL-yy>+em>4Vc`h<8;x9c&Jv2&j(*G3J^V@78TmnLH5fHH=kkSKqX*A{f3IbAjEK8 z^JQEZ>H~XDPP&oS$)2OC_JNTk7U)4&2zJ|f4_d{5pE|9STwVI4^2U68py@5*2>IX( z0ex-6>&w#-;cU#%_^9c_B~iO(1}XV5^nFSD2i|e%_jg>QHDg)?_0J|x#s&gk#noO8 z`+cn7Zo#uoXQ!Y28sCT?Ffj2KpHPZyf?!`Y{j!!uZF$T7{`~^L(6Cyv!IVrMe2>Yl z!25>>-gXWbfkiFbeGt?0P(t6zyBF6e{ha-7 zB+BdbNOXRZv}vKgBK0moL#)*D$7KyulXhb9IPqQ-;{4meSfWPj9qmeIF$p^cjy-lZ zNyzl0xJQc2IyFbg-s#{F8|45A>Nyjdzjl4BjU6ZYlIIr6D4mOm@;~;e-N{kFczQT^ zBL&E3;ma+qM+^1dT2TCej+aLqBBJ?g1A=p3&x)r*?;ss)B|LPj%l$%W$3b~xV`}%6 z>X7v{cgn-VLwigVJ$51>y>DxEzVhC!xL|?7V1u0XpZMe7sowDBsPM(x3om>_-FN3Z ze4qYF1vp6S>%X^M_njZTa0hCv6zR|=78QP}c)A9y*^6-BiqT$O{4^#t>Std$uCjq! z%kd`IauWW4`GM=*4!;X@)Jxm?Gd4P~FoY^Y0jhfEk*6yWK*QbXj-PCSV5O*_;PKXe z*J`hu!$bDrBYoIrZ@4!b+@+KZMiM(wccgPqxj0F8h5QtGarK4#=Ea4#k`jiB@CE!? z+ZARay^_gJu^NbhA-{xKHkRD{>Gs6uVwY%Vcenm=T+u|bnV0D?MD2Twji&0EkztZM zXg-zG^!)B_o~2^q9s zts9<^%fUTRNIbW*Gql9GdnFaSzf79|hc@!KyAt-7zTvd!lSl)w;UAOf}F$g*R*()JfhS=%E)4b%PSN%5+vDRL$8 zVu{VEzzCk=AgBU!yC`&IgocGB3ds56?`^62(wPv?- zXgs{~6aQq^-sZ>Xp>Whz@KpEEf?GflkRO*?7Rc z({6fp(J11-4{fEOuPxl(Yvolk1$Nb784>9L&yvp*;ZaUJZMy%>S)t7#f;cKd7Kxsd zbrLIOQkWaTqci*?LCMU#dJ5Bt< zj~gc}XlrEx#y5=o%WLk&5^3a*OBo#UvT76((Pmqsa z!|YIrVqvm_Os!BBqsZNI2IW~3|=M}y@HAI1_| zr&Q`0bQ12%>mpYyeo!S2_nC8B{u*+gw|ila4S{_pTN-=$-)&Shz3jJvm3nub@8Ecq z@_)5uK0iNOgkk!=WBWd^fw$P6oEZn|C@a26KaoG@b}_eWau1)TN%@Dj;JFugyEll7J{+mxFgkI`$~}c1S<{R?3f1_N88Wz3AxZuBC-77Vm7E$f zA1N~D5HSb^jLhNKDR5|y3T98bLmO#tz#{$-5P`?=&&?;9=g}9P5r2l@@?b)?*>vN* zUIJuT<<&lAw7>PNW7PYsT^0y-2_WiGF1OaK=ODPMvR3-WXnS*BHr#PgL$b%+?WYh# zY)6r9)eHPlW!rS4pA6sGmbX>yCV8?2)1iA?sL+m8F&cSX9k#z~od78Z6b#|4qe{(4 zYHDhF{x3IrkZy!kU6{v$-<!erH)4$M7K}7}vK# z>@YBfDVi6q*0W4^`WhO2z*;QZJTc_>^eolel5m;HrLuU>Rc-N70CyOhs8Tvn&ChmI(W%bS^b@qCu8lTXCeo+IAZUG3i&d zsu&5bT)E4OvPm<}gni2H$Ca3$3yRH_L#rSJ`mCwy7!vLI^+N#A4b#?VLZcBiPl%_B%OIGrx!J&XFtr`cMNbHP|68cZ|B15M&bHDL!|G&zu0R9Koj3|q&p|5hh%?_=_IR1$!1b1qN4v`~5L>JoQoznOD! zAo*kB%}HtDKlHMEwm-kVGCYm+IzBJ6u4-Ty7!sn!XB3A%lu*QzUW6T{rT2Rfg@u$q zV%xm_hau5fnMoW5d6~#Sj{9>O8-lLir6)WhqSvlH@`h22Q1j^Th{6>0Sz;XA@AWze zrR2Kr&h}EKG&@>m<}wC?#BnuBNZ&@8E}X>~w8ueD8P=-c%=9qDX;t1B-0(8+#j@Ya zqpQ%im?Ju ze;|~|z+-~=wR*{v^~s6E-JvzH^W;Iukib_erj{_+tc!|FctUvUm;LHPU61KX1^$95 ze#}{WxWjm5W>HWW4b#ur6qka>0~Jw9F)=YjbmEPAdlm3|zFDF3&v-$3ftrYoatEG* zW#6kt$~fg}=I>MU2$ z?a1=(w9w?izrk;t6ZY;G|2F4`D#oRff70hWUWMH`|B*X2Nj1RWXoGLAIMm`0)eo<8KzF{(ID|zOu37{#0|{QiCXSF*dAN z_s|#bgoIx)#I`$pEnHn%goI2E@%Qr3RjT3pN+_HBB$2x_xtwr8HgSu68x zW2ud+;V0NMPS+h@*X>Bf?3k_7zlY$vSwwxPxgFbu=CPp5{s%uFsPc1(>+I#+6o0e# zvPijbO~U0P7kUkjjwZ~M#d74{sfN=t9k!LE6ai^z;V)Ezl)_&&%#v2)CTF_QMZdSh zI25_&iwvN?QfG84xNGeD0&Zj4RmS4Yz5Rt-rJDNQVC2g07^coqa6I9*C#;LEKj)nl zOaAiU>-12`GuDIzg-9gCo2O+WGQ?7h>;eR@+Ul>36O8tJV%(kZCDsS=gcVpzGTku& z3`)+Rf`S4EPqD0|j1_>qX+3R;sxY+sT~{#UVp=Nf!jps3{Tf1ouB4_rzE!lubEvNL zP!G^KKbA;6MDx28tJS}J(XR23k2SdS)mmgX5Pe1k0!9ENwlgEjzKqkVR?iJ%+@`srWH7EAfmg^+bp%C>vHzhG^$hJAb97t3hyRoCn-6Qo;7!dW=xb+`BZ$>wSOQACZTLbv#K>}0lF>&D;#o6`4S#+tu8$u^5&O53i~s-cnMMrvcy zUrpsvGwp>uU?{;~83=M+Sl0SHbeZo}A{LpM?B8~0;{-<1sTAq#`Ndo*Vd(8X~0^W*` z7;y>dJ<%j7Zp+?BOi)3+&HXld9KeG3}_nzOn~Ev726@T6*H z5wVfX9t>48M~?JL>3u;6XkG;uL<^`MryKqYU+x8QaS?SkrC;a+#cv9_5Ho`0Fci$q z`sBhwG3huz#AB-u9fqGp&S?$4Jh9`x!vzwgNOUCeVeM76&!!oB(+1pJmErm~fwys1 z69wrHFVCz>h^(H{MhYI-Zxq=hQb<2Ml`#0WW*cFX+#X)rYSh z(t~gt>W0LcPFofW3JbHn;b2ZHmwfM52YW&rRSnNH*AfsJ-sGUWp_|qrq&tPyzZ*u7 zv&S}P6d<686)y&r_rrHz{aAT;e!e|*zR?N`w?*-us95^<`(>{@;Ukq{8pkCi3hb_^ zc8_PhA%2^=Yw-j~*;Q`*`YnB>agGQAQaB0O+1aix~v zbYF9ktsbE?@c8L$X}1R>VUk}l3(W}}A1^mLHg-TkQW(x0~1>KC(x)ey1`uYJK0kEkwEhl8LqYiU^<5r{D8FIJqvWDHFx}_IMNM zG3*^oZF3@8=U%_W9 z+!k(zXSTZJl^=W7@1BEAysPIEJDVY;=~3dQFi}Wr6O>`A{sZy?MWf%p#}^UkeOhcbWe__SF!<|f??(cV zy-gxadp}Wv*~HZ8B7T#E+A)`2FNu0ynw@q$IjG##S*lvGY~^^r^#e#s$@F#a7bA8P z4^c#*{EFcDd~^)y_VjkzoGtUdOt$PFrs8SS6%5380E^$NkIIh35g@csh^feIAn+S9@i4$`67oO$U^EwlBf&;NU!V;$@p2 z)62Zae2avyC#A6J=VL=dLMDZ(`Ez_~C|XzNzQx~a%yNDhiWPk&tV`Un^nRbpdSR@F z5c__ld+IoLYisKsdi7>3{+7+X@947V;k>TyqkCE5y{3;__c_AZnmJe!>7wggj0-e& z=d_;yGFD^Nr|<4Je4giac0nf@+-Dl@yzu4At}8UqyWy}ZR&3%Vijev)mo)K& z8Ag0M^T0C4cW&mUREzgkbCH-5=Q9Ahtb0$XQ@wTeee2y$S2N_DYD3?TTX4sU+!AXU z&br`hQ+aF;q0Uk0M$rt9hbauU*0L*^d-Opli2cMO0wYBBR-m8YfqWxvXU7UO7=E?1 zwCD)65QQ%4=?xDgDBR93lsK0(1sr=J<=}&PM0}nxPlbzP-mU(|s{t~Wwg#0*^u5JAraliiwxyA| zFQ5qAqNx7X*ap-V?Qc&f!Vf!m+G8RNvv*NF91o)tvc*~_atWVD?`2c=OdtzJ6vVHx zTQ4`4doezMkqAGZ=je3$^`gut#RZ7x2gxTh*+RTlP5lr_S`~#hxA-n`}eA3Ovk)wkDy=-o$Q(W5>mks1S9~AQJs1$EcDF8E7Sto(q>xX*)1@u`OQz z_{Lqm@UwA$BthT6z-{UPv|@DoZv8F0Jl)j(ky~S2<|~90E5;|TAON)bPgfPt36z4% zv|GOY*F3hLU#H;l!0v3>)*8zaO6ltAa#)hgVg!f=zX!Ujy$L!YhW&d?x+XTkWU=cQ z$FF-jmp-|0Zyyr|)nzZ+!%zU6*7mTMk?FqXso%r|MK9Xhxmb<8l0OYAETG`nCSsnN zvw9W2lt2u3gKA9wpkZJLZ(R5L56Jwngi_ep*i8I$XC*`nM#R`!(y2{#dAR}6-#%|9 zt02H^0J0WKpaDBGN5QdZ9|!VV=g6iTu0Ad5#2%^{D_iV;KHT7}dUy>8fXViM^#mB4 z`FzsX?M&BaQ>fUg%cFrGpZiYM?8YEv*h@B==|t8Ry;u^UBDg(X>`kuFUA!IV2MAJi zJ-yHR`dRZfGYOzPMpVtRr0?I6|Ni|85RUkG49aF>shrZEK4o*;?_HIqx$IBS0UC}@ z;pY#fmX8B;@G9gJvWBfb> zg(wo#hD?Xjs;Wk;UA@Hsivv=Q`gA zG&M-_g>`7!L_7ruB8sH#DwG(otX86WMz925!=v6T4}D*>rRE6hK^X5|3o2n05O3gd zadC@FOD`|io*o{Yreuijni&7GGr?@26LtC7WL!uche% zGPnKeL5d?M%v7NKUr24l(c$vg72=`4mDbJIUbrKC#&~TZXvAZm^|n#TG!cAsiNMU5 ziT?3pFU>N?cne_-K}L}*(?EtC=F<=IVW1JHsHX=_zpZJuD;I7__^9>122_uHgY#cY zXQsITq3EXg>eTMqPlE^>CSc~L#_P*9H-kPC9WQTEQI20aP(a@_4ZO0xal8FIxxRel zU&{od$G1K!cBRz3^(4`*!43yN>63y(8&(VSPJ!dAZ*pel7#c_0B(2tUg;lYU(wQfORhvP@=+jySxYFOeDv511v++_fI^UC z+pxn&5YPl3#bdlUO?<@EqI3}2kS*53cgK$LB8Tbe=m%=VP?11mZs~_jG&XO1Dr~|Ncki_CQ~^edbl|}qo;?LkC)rz1_)R5GT??AYXHUwaSq>v_;)A6>szRlm*c{f@;#0-5 zP7fFDZTswd#tXS2Yr`lC%;YxF<$`)c_>JHZryMF@XrRH_b;6l{>9`iM~8arba_B zg@ZSri;94^_IVcT7t6=AXDu%90=pQ=nnfQz2E}xHnI0a%O(sHYlnsQQNgrGv7}?rh z;oK*oeG3tvzo+H?|7lg6=W7MuxbcT4nK8k5K@dfF{(^~KP{#GZ-zkJ#h6~f(*tX19 zu2KkfeX-{p@s$|M!`DYd_cH?X|1_Z15;9T<(D&L3)$Q<$({)O`FLds{oj9AcOVD1xy8w#Fkd)@7dB zTl3nQrNhf$NiQr6OR$j?(3b{eParK)UIOFSo=xGJY&Wvpr;hZh1eQu-SlGWW?yMdPm*=eY=rZ{f7CS@s z`M-+^P;{xjUHEDse2vSOQZX(kMh&09dNJC8)&zPrX`ce_*$ORiLv@$BXUuq&hPLvr z+AvGF!6pEJolsJ=oUk@sHG_^4ES>Z(kJ)QmDt{=a;K86ojC*E0bR2s-9Ql)N(+t)- z7q}AKCZCxu#*Qw4JIG`b;+H?zH6GgPC4D2{ziV$EXB7ujtI|cu@b4HM8Ta!}uxg5& z#kA-Rx?P#(agOF)31KQA8s~mncKT8dXH7pX(GlXQKAP}=d`yHZ6E$$>p_j}a%0;mqVi#{@avIp_|&w+&SV1| z!dGv&^C1249zV3|e*;zdXkBSe1&_#BXZL8~WwA!8=+`Kpw=dUHTqMS%Y1rO3YMaVP z(HRo?jw1|Fb*ry|~^eidgLC z>aRsq;Vy`VId`Vidx`KW>}5H1|)I@z$GSg&StCQ!P`K09Ek$t%eM8eK1AyN9 z?(ExkR_r`=NY|mW(<*@$WI2ZaAjSo^#f*~aFX`}E*y|5b311zjZ*)mN6wqsef`VvB zu&n@o@{0+Fh2^1g90*^2fRT)onI2zWB4H(zKPw%r2$E}4x~z^Rmk<&i4U$)?f6OQ| z(#4BUpTtIf48Ptl6uly$6v+c)$0_e`Kt6iDj5m7TZL0t(+ITfEijtMrc4iR!;ybH} z?928x=X-*Nl-Gd5l58$rxIQOtdy0=#OCQWj7F!du>_$8Bd-Pik@q(HH1S$_+xY-p` z<%ak9cf2a513T%QPgP-exzk=t1{7L=n-C3`0b!dOlN{ ztJK}^7)S^yEtQu2U?`u@{5VGEQeFjhx?d z9iBy$VMwdX-2SpmKQ!Fy0@-&*Tc3~j2Wg{|T!Llv*{kEIXlE{`nHEn4iDMiAT#N(= z5yZu80y3o;h$GC5l)2Dpl)p+k&)OW; z1qi?6aZ^7>8?q*I zXKD)DPnA{`6*qos2Gm3<{Ihe@D@9XsxicV5!AS%ynjW8KkUP$Y zTYleW=htHlk);Y5#G=U?;<8AL*yw_x>Ft&O9+husgX*iEAb*-A?%|=`c)~_5njSd% z+k}hMsU@=jMDKjIC39`vu+uhCa+oXgjgT=)shXP^j#5THhYg(T612)F%z>=WWlby%3xQrCRSQ+FV>=-lVvNy$z6q0~<4O@Y@SflHHfB^!<@|Cog}jWF#)$*gT=z=sT)DnyhYH z8=wAjXti;*l&q5-CM&AYdWV4UqW4t9=jO!?llKMLtfh$0 zYKYTxK^06S$@9Z8OjnBI(GL<8AgZb2Bqhh>QKD+Zra?go6rWvzGvU;gQ}}NB?3tg&wdwQtyU>V-^d_ntO?*DDxFE#Q z{TTFtDxjKHcEj={^SHG68I3e8H~i`vZOKagH!@yG+1Q`492wHo-l9=g-g6V{_Rvrcc|a5 zftgu6e%_5>SnROmN0c$X3_)QvfZ$2I=kX(ugdE5ro;fZI6&d;}smYfVwXP8>uu}72 zfWh$uY3FgOz`cGj8KOeGJ5zB!^N_6BdM(KQ_JP zV;l&57dWN)=~Rn)bMQ!P0)1Xk9pVGoi`9A3Zyx zZFYBDb%ugrhu9c5__3ZNK4xIVsq!Y3;4wH4au8x~oSjYk5+(gSzbxfnth-ShVP;Xo z@1IU`d)fxiz8A(PpqK!Rnb?1gF${xLy6T{qeL1GGqKtN1n{WMWWt?B^m`a2;7}!%Y zollx(P;6KpX!Gyu)A489s;!!+!g^iDUFm_h+i=Q@e;JOyN=a)#o8Ow7-X;vWJ$pP> zotGmV_%Vn{fx)0Ak6Q}i@7+e?@dTnTmq7e584k+up@)-1Ht;beaP&LJq0h(3?>=CM zp@&cZM*|py@w-QE^DU2oXkz86RI40PId7beGw5XCJK~QJ7?-N%1X_|byN5m92EQjZ z9l_=%J$C&5fUPukVXtFim9}#l>tT8HgXKYn!Gcm|NB?`As?Pet;d?Nc_9IuqPmoC6 zT_z!nznGXqhX1>_XQ;nGa>h}IAYG`EZl|~vsiNNn8r-`jLwG=gw&?l57}?4dSK5*o z^^2A^mFv}qJWFTN?;CpwB=qY68R;z*C!wX*H)B|c;pBuhgs#DG?Eq$82wz1B!Q?| zu3N?qQ!zXY-KSK1>+QboB}PXKp-0BT*esEy5G_KlYn`6l*l>>(9tphuh8`JITH5zX z*5zz#`DVe7^6v@DSJ3sp{`N+WUye4Ym_>L+qw=3MG>ZBW4>I2)1_v$_%V~i7H^1Q$ zkPPepZw!t-l?A!^YA9_7-;jA6y33c69I;t?oThnGO^UUJPjAJDZ`_>#tFO|u_E{7GBxyULRI&9^5* z?<;%v$U26GbtrVi{>ss4V1}uM;2+$Ex7*3`p!%o*sPhkYzY#=CK6K;XsKsTJ;Jm4w zrONdW5pSK>0p{Bx%EH3RiFT8iV}oirIG!o(GU&++cSbZk0wOjAse-gNF=spuAVR<< z#g~?Ye6Xk`36`+tvIDzrLIg@G%JR#9CHzabiBwPkU{QWuTV`ab&GF5RRr($d`7k%S zGfR+Alv=;hhd5%&LG$TC$ZC?7QGJ~u-Oj+zHLcgXLZPgML}>yX!htOWeom)j;#D|c z5Jmv=z2HdVXmX;{acV0A75t~*_`%aR5vj*J$HtchJ2$_VL9uGzJA>#?iLKFw>aJ7- z0We}>Uj5S340LIMzE5%K3mFESoFVW?#`W5q#1n0{mDZ#fm`H#{;HM-fII)JmI}8`} zxc{)&?5Qz7)0`sYs9BG^qGAmw1V{Zc`2 zhdIe33-P@qg5Qt<^9DdtU52{y?F@0E>Tez+|RLr=-=N# zInBh()cw1;Up(HN922Sk_wSRny>v^t{AM0Wso;n43jU*k^-uu@T3CN!=a3Z5N_u{= z9JGNSyD2VJj_xYgORh(ro3s27fh^FS@qeYtIVjHZyf)s7h%VJd#BCTkq>E&5(uN&r7NLpno7OG^drr&B z)0e2Q(t%73#JR3R2rk|#Z(<^l?wm_QQNFrA(V^OB?u5i z>?wGO)lZtYhpd_GL^TX@@-D!8!`eNOv1`nN)93Zm4Km5m#4N7ySOs>2YoZ%;HJrh+0d_$hek; zaO=aQ8`myN!1d<_VEgZDxak$*b!EG_Nk#1XQyfYN9-#}D?CXNR5tOfzsR@c@zcCu)uJ?6;b z3(eRc?>FS&1qzn7tErC$-dAko%1AY!d=!k}sUeQajBzkG$H^ZDh>i}4ZiC%smiwCV z(OkvO-rnCNhr`rVV)o8wXF_(pp@XUX)|GQ{q5!^yc7L%qy+>ggIyy!~scA1y?jj}g zDJA~w=^ql*@Mo2}gZ^5T4qs8^r+I*K0Ijm~2aAQ?mJar8BR(PFgX8*- zaf_qBCxn!gP!0gh{WmmTWzY`LpN}+<00u*kz~^5k=-C&Y^ze5peq==Y0*9IQDU`9~ zo3DcuMH3c|Bmb_hYHJP(AM||k&RSn)XJ^axTay84VD`y!2Ei+*Bxbg?DGO34O+!llt4~r!0Th+xOx{oT zduDEym!xyi&SVqg>SN!1qy6&SH9jz`O6ga zU1SMAW9o43ng#!Z4@j}()C~NwHIbP&5evYy$SW^@7_Ts+-2uOs>nA$2oi;Kem zvu2F$D^9XSg8$85n$7v#b8=K_%jn3^DW^n&n%}9AqIqYQq*|pzik?Tf0$*RFNk0O! zCDq@Hzn^Nks38A01KE=U2%g-a3&+eM;o(=$S5TLe`gs5!Wle@`WSlH7R$FrTs((op zHhw0td`JRo)SRELCnRO;XTAMT8)q61bsP0@Dz}uZW#9J@lE@50NvUilWEuOuri3&` zi4aO8WEo2}cA`?odQiK-07v5l&|86uOc7O?c+Pd z0glhtBG1ariH73^QY6$uX8KIn4&d8cYhrwye}6R3NL{B#0>yxkL6|#nqI2V@;@n#J zyNwNhm#*~I-rUO%HfmJt>gmi%Z8~9k7Ks_HVjYujZE2fmolQ2mZW`U|CIQ)()l8>m z?o-55d3$BP5hEW{oxWd053Y*TcoFhXl&n~KN{e3DJ88tg2p3Zc3TgKaRr%!!@GvoZH4Wx_vsw z)%ftFRdoK0-xCt` z^rNKZkj*Bhm+hCOaakU+5V=y_$%mwjc)FuRAG2Rz6Yvj^BpiEs!PpKA{?QwWEtj_N zb;y_tJUy9?vZyb!xriR(uk>=>;g{1i2~mEHe>j_$t%LXw62S)i0o)mKVyDX_Xn;5vl@4cQ^KshNXy1pmPe`~j`@JGgZC(APYn(KpLBDKiO zK}_6ec>Anq&}!nQ26@yxFTeQwI!~R0af;R=-z8T5{_N#KcfOAwKhFNp{OCQbN}B-9 zru<+SlUDK{qg$~mPifi?mtv21PTj)MG}Hb-;J-% zHbcBP_PSkRam`&;^y2fhE#wKfKdPx)!#|g|ScdVaL)+2JE2Gb~{?3^HC4HGyPfvZ6 z!(;3e5oh&@jhCxUamIRdy4!wuOE_q=Q_U}5wtT5%Sa@u)yz1klFgQ2}om>C5ACn+f?1Z@}q}L_z z;{hU2npm;?5MPI)GOO?G@EkwL!SR4~FN!$7^oK!rOBfMelPy%y8~%<7+<0YIT{baR z_*~(+s8`z*${DvFk#En1yI~uQ_V=jKadDC&B6Nw(UxrkQ3)~@A@bL76k#OhcXM&hN ztWbw^Q6|O`kR#w-a zW%eB)(-A}llz9y*_ZL|ZMNv!!M40R6uLBhTOMc#)quNMi|1oW$>RF%K?JZ>3;{z$E z{0C0Q4EfZ!u7ruFS}^mg#FH+?W>qZygtFoe^Mw>pfY2^@csuB|KZdo$S*QPSc&~*0 z8$|0117hR;Ty`WvLct!E0wM&CUiJ$53zzXRKF z%Ul)A7Ubo%#H3|%JOYN8-@_yR#*hp^S8zG}IZ2Qd+h=#9SIRe|2jI_YOiT&TE3dy?Bh&l@~KCgrQOcGAXO=;Dk7Z`2M8^jmP*X;^S86K6S@}-h4zM;Fqht_*X5J>toKkGIGnP@1bDaGhg033rLau6noV#ZO zrJ<2N>_dnP91TeZfmNl+RnJs?DNw_k;a04ODsAoU$SKSQd;l9ChrY)RG33^g?&;6? z!vmO&)5ngn_++Z#(aBRjF}|OCXGh!juibZ+EHCM+?mO~o+>djz zm>MYjs>Z8sV_Ru6ldH2a@=6Wgx+}|<;!c2z7HMCR>~*%7*_8|J`g}dN3NvhwsgVjj z`?70&18RJ6`Z!buOjJI(#6Ot^wiTFk?;8w?tu zVh}z=9@S<{gdhW5dFqY1ek?(nP9~wF-+LG~&9jo~MRl<<*|f=KVt)E> z*rLnbdyEQ0y@~52lDLY>57)ZU>k;QSklCCNCA7gCNFc-xL^ehfr^U`G<)QHm;^JZ& zk`2}^PdlnTOUDa9)4Z7EMc{}l{D*Qt_j*)p>{1G;N6~p!$*V_XZ!OK1w9+h09J_s| zuY))IgX6>26Uo-GBQ{1*_TrA^Z+5U&6r^e4zL?H#b-KO8^_la;zh5>JdMTmvBKN|^FMHKusVDg@!~?!H@8Z}A6|c;&u0Uyl55vAjB#mX^&Ku{Ht!lEEED!M%d+_`Gq0 zor7b76vlxM-dxF$fuuo3HFP(>Y+zuKdycuKt&Iby3R|uBPV>c0z{vAr52=@-J`$EG zn%m}<6uhf=>?KD$KepZ6_;!4X=7Ue$8$_ds#_=A-pC`+PmnWVfSJnnMX@f!+vDjJ# zx)H(1M$zA)`yqbg5!^>~Py4Gk`tBTIVqyn6L;?+!`V%MJq;huYY$vSA{_a5C#X2k%Gt~9ZjG(7`qkpOO`z9bm4^OykF_j z2tbmkeORKL?Ck9JU+uJ4Db{W~B*!k1HsxexuVOa)%+V8kKK*=%oB>|FY2B=jIQ!VVb^W)xtww0qypMSO`SUqdub=hxj}9W2 zdHRd3nrVkGu|PwsHA{jivGt;{F&k}gS`hK_sKV!mOq^2jFJFfDx-H#kE@Y)Qqg$Gh0jWjRykk8vmAu8UYYMU<&S9X=$L|U$0QM%T}fB?vBK0vu7WP7rO-BeK|jlq)VNp1=>$x!14HFKC03b`1w=A*43c6bd$*UNO~hq17cP6<;vXCJj@xYEQ+$S6A9N8>;qX- z63(V|cHp#djKY|BC=)%ad9v$`32rGbGVfi!0156H%=Vg)DBZDR$Evp{Vqm#R-~a>N zOzo)8`sZIkH-;ve1YT8ehk6K&gl1En($UfJUoLTu2?}PItwC2Hd41T`fB2wU@k+X( z^!7G6?b7dNEl&a82Y(q721cW$w8FwGZR;mv-BdHRuqTKA$OTc3@jY)_PCQprQNKv0 zXTBuf2wiK4bOk@XHV`i;D92KRe}|CKM~)mZRKLjCxj+wT;KITJILf#0-aP?Fvaq;F zxEMcBWKj_Ws!xPdh&IR=wE-d%!*yx`07w1JAm9-b3k%g;SM*5_b;K{(nwko*G`sVb zvK=rW_Nc_|?goQ5f{P6vIuXin0OlC1^c9X|5rlC04YVl|Bxrh4p^%O1`cjF$f$wIw zK6ZkpQnrX|Q9XG*@KY)2TkO2D`id5Vsj05B^V^^A9V3H4#o6>W*+%w)jt=szPyIq) zVFKRIA4RUs93f$EW!w_<)~YJ&>5whGat!%rwB`tO{HMv~yl$ z{UyPcx?p~*SkQ!Z9Ky`8o@{?K9YSbCE%;LLy=U7Axw*MJi?;i1!h36sFb$8Xt)sc$ zfMo*YNGjzy+N{g{y9X);V;h2envyJv%|I}Vg5js{q9(Z2x_uOEI{c#qm9mNV#i4?A z(xoP)^+jZr7t+;jD+W7gBSpI0=qXYNl*GDS*`Ei?tT6%faU#_IqPhdhRdEbR{(m zTYT>ss~^A3O}~C z^uG4jjZSF4-kJ>&!;34G%R}Y3dr%`9NJm`qxBlCsDx6#?u!{=K?BCxS+6>$jsz0fe zW{}b;RU_(ecsn%Z%kDy<1tFl=$eagpXX-G_H!Ro2rmP(7>D`H@$-E^bB)0v9VQEl) zhZdl7Na6(dmJK{F)W~IT?_vcy1Om+-^Ywy4-v5}-uEgFyiEB;u&A^Gru7 zikA_0s?e->BFWrleFw|I$;qn`RAz*=5%WEl%R`04KD}K?jOM7 z@TlLBpv4k^gcO&QWO$rC=`#ubV-Gase||bXzkc)P6p3mf&!s?EQpRQE+OX^4uf0f2 zYy=x_f?BxYt|rs~u3mL$`;l+0dRNM^`I)4Y)Hm0pXeiEW{arg~q(xO(t)-M0#e}N3 z0@bM+_=x~EpiGfBg&rCv|Lam=UomL7qX|vqmKG7v$tTH~_27+!rS#M5r;bXW0URJq zs|-n=$HT@!=CB4i<2ulIAUqzm_Xd{3-IqzBzxk&WkFT%=M{Q)!O^bsf+gMny%@oN0VK z+l31kz|k0x7UHf&nBTlU`W^)iN6s+UPW6uzFedb~t&RtQ=LArN z_*qDcrZ`+WGp9+^IpRYE`pDyk9?`;Of9GKGO6NXxbpM`S$79-Sj^y?Rb)8cBE)ete z-&HIb*`>Armt_$5-=7NmhE`&1{kfeiW?+$=8^Ra_D~+hHi1Sy0cf>9rfHBOZ09G$( z>=7fvsm~rPze<9ho*qTU1nQPlEy%*0SttiPdv7+S4=nVWy$T=iS&tv;cWa;KD247} zGVjmuLcol~>{rpv`KwDAWiF8r6!aq)rC6yU+hk&Q-Eoo>&;<~ec}_bV(xMD@2XClv zK_KOtdH)Fjv{HI{dJK^)n#XV9uO+~Z;)^Bmqae7(9q0Yo2cgnm7^IyAiFCwEfUD-3AZ?*W4u~F#WEGyFl1FFZ$CbP?0v)x$!O3Gx zOykw0E=O8XDP6sSn2v!5-8jd0cSFtRR|^er0EN2zyu7)M%=5QzE5Yc1W82G<)^}PZ zVUDa#x^Rc4iI~hTDvA{j{rdv$f=t>ERFM*sK{IEXWW>GnQX(3b%fZocezZ;t&dM~@ z&s|&MMFoXAa=|^Ztdyexi~+H4VrFI+9ItbjZ6$SebvW8EffF&}gU`;UhgP8JW_Zm% z7OxlbIX6RE=OxgffGI*c za>K^P(A)dey?gg;97L{g01Hkz6?Om?lnsc)dU}UpuAbIUghVViA&G`bR*&ozr@sdp ojQ>MS{(mvb|NF~ex_wr{MF|rZ>)bS8KGR*%HN04I!7l870F=0lwEzGB literal 0 HcmV?d00001 diff --git a/apps/multi_session_registration_legacy/multi_session_factor_graph.cpp b/apps/multi_session_registration_legacy/multi_session_factor_graph.cpp new file mode 100644 index 00000000..e4afff84 --- /dev/null +++ b/apps/multi_session_registration_legacy/multi_session_factor_graph.cpp @@ -0,0 +1,928 @@ +#include "multi_session_factor_graph.h" + +#include +#include + +#include +#include +#include +#include +#include + +bool optimize(std::vector& sessions, const std::vector& edges, TaitBryanPose motion_model_weights) +{ + for (auto& session : sessions) + { + for (auto& pc : session.point_clouds_container.point_clouds) + pc.m_pose_temp = pc.m_pose; + } + + std::vector sums; + sums.push_back(0); + int sum = 0; + std::vector vfixed_x; + std::vector vfixed_y; + std::vector vfixed_z; + std::vector vfixed_om; + std::vector vfixed_fi; + std::vector vfixed_ka; + + for (size_t i = 0; i < sessions.size(); i++) + { + sum += sessions[i].point_clouds_container.point_clouds.size(); + sums.push_back(sum); + + for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + vfixed_x.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_x); + vfixed_y.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_y); + vfixed_z.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_z); + + vfixed_om.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_om); + vfixed_fi.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_fi); + vfixed_ka.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_ka); + } + } + + bool is_ok = true; + std::vector m_poses; + std::vector poses_motion_model; + std::vector index_trajectory; + + for (size_t j = 0; j < sessions.size(); j++) + { + for (size_t i = 0; i < sessions[j].point_clouds_container.point_clouds.size(); i++) + { + m_poses.push_back(sessions[j].point_clouds_container.point_clouds[i].m_pose); + poses_motion_model.push_back(sessions[j].point_clouds_container.point_clouds[i].m_initial_pose); + // poses_motion_model.push_back(sessions[j].point_clouds_container.point_clouds[i].m_pose); + index_trajectory.push_back(j); + } + } + + std::vector poses; + + bool is_wc = true; + bool is_cw = false; + int iterations = 1; + bool is_fix_first_node = true; + // std::vector indexes_ground_truth; + for (size_t j = 0; j < sessions.size(); j++) + { + for (size_t i = 0; i < sessions[j].point_clouds_container.point_clouds.size(); i++) + { + if (is_wc) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(sessions[j].point_clouds_container.point_clouds[i].m_pose)); + } + else if (is_cw) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(sessions[j].point_clouds_container.point_clouds[i].m_pose.inverse())); + } + // if (sessions[j].is_ground_truth) + //{ + // indexes_ground_truth.push_back(poses.size() - 1); + // } + } + } + + std::vector all_edges; + + // motion model edges; + // double angle = * DEG_TO_RAD; + // double wangle = 1.0 / (angle * angle); + + for (size_t i = 1; i < poses_motion_model.size(); i++) + { + if (index_trajectory[i - 1] == index_trajectory[i]) + { + Eigen::Affine3d m_rel = poses_motion_model[i - 1].inverse() * poses_motion_model[i]; + Edge edge; + edge.index_from = i - 1; + edge.index_to = i; + edge.index_session_from = -1; + edge.index_session_to = -1; + edge.relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_rel); + edge.relative_pose_tb_weights.om = 1.0 / (motion_model_weights.om * DEG_TO_RAD * motion_model_weights.om * DEG_TO_RAD); + edge.relative_pose_tb_weights.fi = 1.0 / (motion_model_weights.fi * DEG_TO_RAD * motion_model_weights.fi * DEG_TO_RAD); + edge.relative_pose_tb_weights.ka = 1.0 / (motion_model_weights.ka * DEG_TO_RAD * motion_model_weights.ka * DEG_TO_RAD); + edge.relative_pose_tb_weights.px = + motion_model_weights.px > 0 ? 1.0 / (motion_model_weights.px * motion_model_weights.px) : 1000000.0; + edge.relative_pose_tb_weights.py = + motion_model_weights.py > 0 ? 1.0 / (motion_model_weights.py * motion_model_weights.py) : 1000000.0; + edge.relative_pose_tb_weights.pz = + motion_model_weights.pz > 0 ? 1.0 / (motion_model_weights.pz * motion_model_weights.pz) : 1000000.0; + all_edges.push_back(edge); + } + } + + for (size_t i = 0; i < sessions.size(); i++) + { + for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) + { + Edge edge; + edge.index_from = sessions[i].pose_graph_loop_closure.edges[j].index_from + sums[i]; + edge.index_to = sessions[i].pose_graph_loop_closure.edges[j].index_to + sums[i]; + edge.relative_pose_tb = sessions[i].pose_graph_loop_closure.edges[j].relative_pose_tb; + edge.relative_pose_tb_weights = sessions[i].pose_graph_loop_closure.edges[j].relative_pose_tb_weights; + // edge.is_fixed_fi = ToDo + all_edges.push_back(edge); + } + } + + for (size_t i = 0; i < edges.size(); i++) + { + Edge edge; + edge.index_from = edges[i].index_from + sums[edges[i].index_session_from]; + edge.index_to = edges[i].index_to + sums[edges[i].index_session_to]; + edge.relative_pose_tb = edges[i].relative_pose_tb; + edge.relative_pose_tb_weights = edges[i].relative_pose_tb_weights; + // edge.is_fixed_fi = ToDo + all_edges.push_back(edge); + } + + for (int iter = 0; iter < iterations; iter++) + { + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + for (size_t i = 0; i < all_edges.size(); i++) + { + Eigen::Matrix delta; + Eigen::Matrix jacobian; + // auto relative_pose = pose_tait_bryan_from_affine_matrix(all_edges[i].relative_pose_tb); + if (is_wc) + { + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka), + all_edges[i].relative_pose_tb.px, + all_edges[i].relative_pose_tb.py, + all_edges[i].relative_pose_tb.pz, + normalize_angle(all_edges[i].relative_pose_tb.om), + normalize_angle(all_edges[i].relative_pose_tb.fi), + normalize_angle(all_edges[i].relative_pose_tb.ka)); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka)); + } + else if (is_cw) + { + relative_pose_obs_eq_tait_bryan_cw_case1( + delta, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka), + all_edges[i].relative_pose_tb.px, + all_edges[i].relative_pose_tb.py, + all_edges[i].relative_pose_tb.pz, + normalize_angle(all_edges[i].relative_pose_tb.om), + normalize_angle(all_edges[i].relative_pose_tb.fi), + normalize_angle(all_edges[i].relative_pose_tb.ka)); + relative_pose_obs_eq_tait_bryan_cw_case1_jacobian( + jacobian, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka)); + } + + int ir = tripletListB.size(); + + int ic_1 = all_edges[i].index_from * 6; + int ic_2 = all_edges[i].index_to * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, normalize_angle(delta(3, 0))); + tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); + tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); + + tripletListP.emplace_back(ir, ir, all_edges[i].relative_pose_tb_weights.px * get_cauchy_w(delta(0, 0), 1)); + tripletListP.emplace_back(ir + 1, ir + 1, all_edges[i].relative_pose_tb_weights.py * get_cauchy_w(delta(1, 0), 1)); + tripletListP.emplace_back(ir + 2, ir + 2, all_edges[i].relative_pose_tb_weights.pz * get_cauchy_w(delta(2, 0), 1)); + tripletListP.emplace_back(ir + 3, ir + 3, all_edges[i].relative_pose_tb_weights.om * get_cauchy_w(delta(3, 0), 1)); + tripletListP.emplace_back(ir + 4, ir + 4, all_edges[i].relative_pose_tb_weights.fi * get_cauchy_w(delta(4, 0), 1)); + tripletListP.emplace_back(ir + 5, ir + 5, all_edges[i].relative_pose_tb_weights.ka * get_cauchy_w(delta(5, 0), 1)); + } + + if (is_fix_first_node) + { + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir + 1, 1, 1); + tripletListA.emplace_back(ir + 2, 2, 1); + tripletListA.emplace_back(ir + 3, 3, 1); + tripletListA.emplace_back(ir + 4, 4, 1); + tripletListA.emplace_back(ir + 5, 5, 1); + + tripletListP.emplace_back(ir, ir, 0.0001); + tripletListP.emplace_back(ir + 1, ir + 1, 0.0001); + tripletListP.emplace_back(ir + 2, ir + 2, 0.0001); + tripletListP.emplace_back(ir + 3, ir + 3, 0.0001); + tripletListP.emplace_back(ir + 4, ir + 4, 0.0001); + tripletListP.emplace_back(ir + 5, ir + 5, 0.0001); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + for (size_t i = 1; i < index_trajectory.size(); i++) + { + if (index_trajectory[i - 1] != index_trajectory[i]) + { + ir = tripletListB.size(); + tripletListA.emplace_back(ir, i * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, i * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, i * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, i * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, i * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, i * 6 + 5, 1); + + tripletListP.emplace_back(ir, ir, 0.0001); + tripletListP.emplace_back(ir + 1, ir + 1, 0.0001); + tripletListP.emplace_back(ir + 2, ir + 2, 0.0001); + tripletListP.emplace_back(ir + 3, ir + 3, 0.0001); + tripletListP.emplace_back(ir + 4, ir + 4, 0.0001); + tripletListP.emplace_back(ir + 5, ir + 5, 0.0001); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + } + } + } + + /*double angle = 1.0 * DEG_TO_RAD; + double wangle = 1.0 / (angle * angle); + + for (size_t index = 0; index < indexes_ground_truth.size(); index++) + { + int ir = tripletListB.size(); + int ic = indexes_ground_truth[index] * 6; + tripletListA.emplace_back(ir, ic + 0, 1); + tripletListA.emplace_back(ir + 1, ic + 1, 1); + tripletListA.emplace_back(ir + 2, ic + 2, 1); + tripletListA.emplace_back(ir + 3, ic + 3, 1); + tripletListA.emplace_back(ir + 4, ic + 4, 1); + tripletListA.emplace_back(ir + 5, ic + 5, 1); + + tripletListP.emplace_back(ir , ir , 10000.0); + tripletListP.emplace_back(ir + 1, ir + 1, 10000.0); + tripletListP.emplace_back(ir + 2, ir + 2, 10000.0); + tripletListP.emplace_back(ir + 3, ir + 3, wangle); + tripletListP.emplace_back(ir + 4, ir + 4, wangle); + tripletListP.emplace_back(ir + 5, ir + 5, wangle); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + }*/ + + // gnss + // for (const auto &pc : point_clouds_container.point_clouds) + /*for (size_t index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) + { + const auto &pc = point_clouds_container.point_clouds[index_pose]; + for (size_t i = 0; i < gnss.gnss_poses.size(); i++) + { + double time_stamp = gnss.gnss_poses[i].timestamp; + + auto it = std::lower_bound(pc.local_trajectory.begin(), pc.local_trajectory.end(), + time_stamp, [](const PointCloud::LocalTrajectoryNode &lhs, const double &time) -> bool + { return lhs.timestamp < time; }); + + int index = it - pc.local_trajectory.begin(); + + if (index > 0 && index < pc.local_trajectory.size()) + { + + if (fabs(time_stamp - pc.local_trajectory[index].timestamp) < 10e12) + { + + Eigen::Matrix jacobian; + TaitBryanPose pose_s; + pose_s = pose_tait_bryan_from_affine_matrix(m_poses[index_pose]); + Eigen::Vector3d p_s = pc.local_trajectory[index].m_pose.translation(); + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, + pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + + double delta_x; + double delta_y; + double delta_z; + Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y, + gnss.gnss_poses[i].alt - gnss.offset_alt); point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, + pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + + std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; + + int ir = tripletListB.size(); + int ic = index_pose * 6; + 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, ic + col, -jacobian(row, col)); + } + } + } + tripletListP.emplace_back(ir, ir, get_cauchy_w(delta_x, 1)); + tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta_y, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta_z, 1)); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + // jacobian3x6 = get_point_to_point_jacobian_tait_bryan(pose_convention, + point_clouds_container.point_clouds[i].m_pose, p_s, p_t); + + // auto m = pc.m_pose * pc.local_trajectory[index].m_pose; + // glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + // glVertex3f(gnss_poses[i].x - offset_x, gnss_poses[i].y - offset_y, gnss_poses[i].alt - offset_alt); + } + } + } + }*/ + // + + for (size_t j = 0; j < sessions.size(); j++) + { + for (size_t jj = 0; jj < sessions[j].ground_control_points.gpcs.size(); jj++) + { + Eigen::Vector3d p_s = + sessions[j] + .point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner] + .local_trajectory[sessions[j].ground_control_points.gpcs[jj].index_to_node_outer] + .m_pose.translation(); + Eigen::Matrix jacobian; + TaitBryanPose pose_s; + pose_s = pose_tait_bryan_from_affine_matrix( + sessions[j].point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner].m_pose); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + + double delta_x; + double delta_y; + double delta_z; + Eigen::Vector3d p_t( + sessions[j].ground_control_points.gpcs[jj].x, + sessions[j].ground_control_points.gpcs[jj].y, + sessions[j].ground_control_points.gpcs[jj].z + sessions[j].ground_control_points.gpcs[jj].lidar_height_above_ground); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + int ir = tripletListB.size(); + int ic = sessions[j].ground_control_points.gpcs[jj].index_to_node_inner * 6 + sums[j] * 6; + + 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, ic + col, -jacobian(row, col)); + } + } + } + tripletListP.emplace_back( + ir + 0, + ir + 0, + (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_x * sessions[j].ground_control_points.gpcs[jj].sigma_x)) * + get_cauchy_w(delta_x, 1)); + tripletListP.emplace_back( + ir + 1, + ir + 1, + (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_y * sessions[j].ground_control_points.gpcs[jj].sigma_y)) * + get_cauchy_w(delta_y, 1)); + tripletListP.emplace_back( + ir + 2, + ir + 2, + (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_z * sessions[j].ground_control_points.gpcs[jj].sigma_z)) * + get_cauchy_w(delta_z, 1)); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + std::cout << "gcp: delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; + } + } + + /////////////////////////////////////////////////////// + for (size_t i = 0; i < sessions.size(); i++) + { + for (size_t index_pose = 0; index_pose < sessions[i].point_clouds_container.point_clouds.size(); index_pose++) + { + if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_x || sessions[i].is_ground_truth) + { + int ir = tripletListB.size(); + int ic = (index_pose + sums[i]) * 6; + tripletListA.emplace_back(ir + 0, ic, 1000000000000); + tripletListP.emplace_back(ir, ir, 1); + tripletListB.emplace_back(ir, 0, 0); + } + + if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_y || sessions[i].is_ground_truth) + { + int ir = tripletListB.size(); + int ic = (index_pose + sums[i]) * 6 + 1; + tripletListA.emplace_back(ir + 0, ic, 1000000000000); + tripletListP.emplace_back(ir, ir, 1); + tripletListB.emplace_back(ir, 0, 0); + } + + if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_z || sessions[i].is_ground_truth) + { + int ir = tripletListB.size(); + int ic = (index_pose + sums[i]) * 6 + 2; + tripletListA.emplace_back(ir + 0, ic, 1000000000000); + tripletListP.emplace_back(ir, ir, 1); + tripletListB.emplace_back(ir, 0, 0); + } + + if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_om || sessions[i].is_ground_truth) + { + int ir = tripletListB.size(); + int ic = (index_pose + sums[i]) * 6 + 3; + tripletListA.emplace_back(ir + 0, ic, 1000000000000); + tripletListP.emplace_back(ir, ir, 1); + tripletListB.emplace_back(ir, 0, 0); + } + + if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_fi || sessions[i].is_ground_truth) + { + int ir = tripletListB.size(); + int ic = (index_pose + sums[i]) * 6 + 4; + tripletListA.emplace_back(ir + 0, ic, 1000000000000); + tripletListP.emplace_back(ir, ir, 1); + tripletListB.emplace_back(ir, 0, 0); + } + + if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_ka || sessions[i].is_ground_truth) + { + int ir = tripletListB.size(); + int ic = (index_pose + sums[i]) * 6 + 5; + tripletListA.emplace_back(ir + 0, ic, 1000000000000); + tripletListP.emplace_back(ir, ir, 1); + tripletListB.emplace_back(ir, 0, 0); + } + } + } + + double error_imu = 0; + double error_imu_sum = 0; + + for (size_t j = 0; j < sessions.size(); j++) + { + for (size_t index_pose = 0; index_pose < sessions[j].point_clouds_container.point_clouds.size(); index_pose++) + { + const auto& pc = sessions[j].point_clouds_container.point_clouds[index_pose]; + if (!pc.fuse_inclination_from_IMU) + { + continue; + } + if (pc.local_trajectory.size() == 0) + { + continue; + } + + TaitBryanPose target_pose; + target_pose.om = pc.local_trajectory[0].imu_om_fi_ka.x(); + target_pose.fi = pc.local_trajectory[0].imu_om_fi_ka.y(); + target_pose.ka = pc.local_trajectory[0].imu_om_fi_ka.z(); + target_pose.px = pc.m_pose(0, 3); + target_pose.py = pc.m_pose(1, 3); + target_pose.pz = pc.m_pose(2, 3); + + Eigen::Affine3d target_mpose = affine_matrix_from_pose_tait_bryan(target_pose); + + double xtg = target_mpose(0, 3); + double ytg = target_mpose(1, 3); + double ztg = target_mpose(2, 3); + double vzx = target_mpose(0, 2); + double vzy = target_mpose(1, 2); + double vzz = target_mpose(2, 2); + + TaitBryanPose current_pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + + Eigen::Matrix delta; + point_to_plane_tait_bryan_wc( + delta, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 1, + 0, + 0, // add 0,1,0 + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); + + Eigen::Matrix delta_jacobian; + point_to_plane_tait_bryan_wc_jacobian( + delta_jacobian, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 1, + 0, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); + + int ir = tripletListB.size(); + int ic = (index_pose + sums[j]) * 6; + tripletListA.emplace_back(ir + 0, ic + 3, -delta_jacobian(0, 3)); + tripletListA.emplace_back(ir + 0, ic + 4, -delta_jacobian(0, 4)); + + tripletListP.emplace_back(ir, ir, /*get_cauchy_w(delta(0, 0), 1) * 10000*/ 1); + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + + /////////////////////////////// + point_to_plane_tait_bryan_wc( + delta, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 0, + 1, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); + + point_to_plane_tait_bryan_wc_jacobian( + delta_jacobian, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 0, + 1, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); + + ir = tripletListB.size(); + // ic = index_pose * 6; + ic = (index_pose + sums[j]) * 6; + + tripletListA.emplace_back(ir + 0, ic + 3, -delta_jacobian(0, 3)); + tripletListA.emplace_back(ir + 0, ic + 4, -delta_jacobian(0, 4)); + + tripletListP.emplace_back(ir, ir, /*get_cauchy_w(delta(0, 0), 1) * 10000*/ 1); + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + } + } + + // fuse control points + for (size_t j = 0; j < sessions.size(); j++) + { + // CPs + auto& cps = sessions[j].control_points; + auto& point_clouds_container = sessions[j].point_clouds_container; + + for (int i = 0; i < cps.cps.size(); i++) + { + if (!cps.cps[i].is_z_0) + { + Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local); + + Eigen::Matrix jacobian; + TaitBryanPose pose_s; + pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + + double delta_x; + double delta_y; + double delta_z; + Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, cps.cps[i].z_target_global); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + int ir = tripletListB.size(); + + //(index_pose + sums[j]) * 6; + int ic = (cps.cps[i].index_to_pose + sums[j]) * 6; + + 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, ic + col, -jacobian(row, col)); + } + } + } + tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (cps.cps[i].sigma_x * cps.cps[i].sigma_x)) * get_cauchy_w(delta_x, 1)); + tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (cps.cps[i].sigma_y * cps.cps[i].sigma_y)) * get_cauchy_w(delta_y, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, (1.0 / (cps.cps[i].sigma_z * cps.cps[i].sigma_z)) * get_cauchy_w(delta_z, 1)); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + std::cout << "cp [not z == 0]: delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; + } + else + { + Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local); + + Eigen::Matrix jacobian; + TaitBryanPose pose_s; + pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + + double delta_x; + double delta_y; + double delta_z; + Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, 0.0 /*cps.cps[i].z_target_global*/); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + int ir = tripletListB.size(); + // int ic = cps.cps[i].index_to_pose * 6; + int ic = (cps.cps[i].index_to_pose + sums[j]) * 6; + + for (int row = 2; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir, ic + col, -jacobian(row, col)); + } + } + } + // tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (cps.cps[i].sigma_x * cps.cps[i].sigma_x)) * get_cauchy_w(delta_x, + // 1)); tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (cps.cps[i].sigma_y * cps.cps[i].sigma_y)) * + // get_cauchy_w(delta_y, 1)); + tripletListP.emplace_back(ir, ir, (1.0 / (cps.cps[i].sigma_z * cps.cps[i].sigma_z))); + + // tripletListB.emplace_back(ir, 0, delta_x); + // tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir, 0, delta_z); + + std::cout << "cp [not z == 0]: delta_z " << delta_z << std::endl; + } + } + } + + ////////////////////////////////////////////////////// + // for (size_t i = 0; i < gcps.gpcs.size(); i++) + //{ + + Eigen::SparseMatrix matA(tripletListB.size(), poses.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(poses.size() * 6, poses.size() * 6); + Eigen::SparseMatrix AtPB(poses.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + Eigen::SimplicialCholesky> solver(AtPA); + + 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 * poses.size()) + { + int counter = 0; + + for (size_t i = 0; i < poses.size(); i++) + { + TaitBryanPose pose = poses[i]; + pose.px += h_x[counter++] * 0.1; + pose.py += h_x[counter++] * 0.1; + pose.pz += h_x[counter++] * 0.1; + pose.om += h_x[counter++] * 0.1; + pose.fi += h_x[counter++] * 0.1; + pose.ka += h_x[counter++] * 0.1; + + poses[i].px = pose.px; + poses[i].py = pose.py; + poses[i].pz = pose.pz; + poses[i].om = pose.om; + poses[i].fi = pose.fi; + poses[i].ka = pose.ka; + } + } + else + { + std::cout << "optimizing with tait bryan FAILED" << std::endl; + std::cout << "h_x.size(): " << h_x.size() << " should be: " << 6 * poses.size() << std::endl; + is_ok = false; + break; + } + + if (is_ok) + { + for (size_t i = 0; i < m_poses.size(); i++) + { + if (is_wc) + m_poses[i] = affine_matrix_from_pose_tait_bryan(poses[i]); + else if (is_cw) + m_poses[i] = affine_matrix_from_pose_tait_bryan(poses[i]).inverse(); + } + } + + if (is_ok) + { + int index = 0; + for (size_t i = 0; i < m_poses.size(); i++) + { + if (i > 0) + { + if (index_trajectory[i - 1] != index_trajectory[i]) + index = 0; + } + // if (!sessions[index_trajectory[i]].is_ground_truth) + //{ + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose = m_poses[i]; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose = + pose_tait_bryan_from_affine_matrix(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[0] = + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.px; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[1] = + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.py; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[2] = + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.pz; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[0] = + rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.om); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[1] = + rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.fi); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[2] = + rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.ka); + //} + index++; + } + } + } + return true; +} \ No newline at end of file diff --git a/apps/multi_session_registration_legacy/multi_session_factor_graph.h b/apps/multi_session_registration_legacy/multi_session_factor_graph.h new file mode 100644 index 00000000..b173f04e --- /dev/null +++ b/apps/multi_session_registration_legacy/multi_session_factor_graph.h @@ -0,0 +1,21 @@ +#pragma once + +#include + +struct Edge +{ + TaitBryanPose relative_pose_tb; + TaitBryanPose relative_pose_tb_weights; + int index_session_from; + int index_session_to; + int index_from; + int index_to; + bool is_fixed_px = false; + bool is_fixed_py = false; + bool is_fixed_pz = false; + bool is_fixed_om = false; + bool is_fixed_fi = false; + bool is_fixed_ka = false; +}; + +bool optimize(std::vector& sessions, const std::vector& edges, TaitBryanPose motion_model_weights); diff --git a/apps/multi_session_registration_legacy/multi_session_registration.cpp b/apps/multi_session_registration_legacy/multi_session_registration.cpp new file mode 100644 index 00000000..ca6a4bbf --- /dev/null +++ b/apps/multi_session_registration_legacy/multi_session_registration.cpp @@ -0,0 +1,4391 @@ +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#ifdef _WIN32 +#include "resource.h" +#include + +#endif + +#include "multi_session_factor_graph.h" + +std::string winTitle = std::string("Step 3 (Multi session registration) ") + HDMAPPING_VERSION_STRING; + +std::vector infoLines = { + "This program is third/final step in MANDEYE process", + "", + "First step: create project by adding sessions (result of 'multi_view_tls_registration_step_2' program)", + "Last step: save project", + "To produce map use 'multi_view_tls_registration_step_2' export functionality" +}; + +// App specific shortcuts (Type and Shortcut are just for easy reference) +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "Add session(s)" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "Load sessions" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open project" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "Remove session(s)" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "Save project" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "" }, + { "", "Ctrl+T", "" }, + { "", "U", "" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "" }, + { "", "1-9", "" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "" }, + { "", "Right click + drag", "n" }, + { "", "Scroll", "" }, + { "", "Shift + scroll", "" }, + { "", "Shift + drag", "" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "" }, + { "", "Ctrl + middle click", "" } }; + +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + +bool is_decimate = true; +double bucket_x = 0.1; +double bucket_y = 0.1; +double bucket_z = 0.1; +bool calculate_offset = false; +ObservationPicking observation_picking; +int index_loop_closure_source = -1; +int index_loop_closure_target = -1; +int first_session_index = -1; +int second_session_index = -1; +double search_radius = 0.3; +bool loaded_sessions = false; +bool optimized = false; +bool gizmo_all_sessions = false; +bool is_ndt_gui = false; +bool is_loop_closure_gui = false; +bool remove_gui = false; +NDT ndt; + +bool update_rotation_center = false; + +bool is_settings_gui = true; + +int number_visible_sessions = 0; +int index_gt = -1; +int old_index_gt = -1; +int index_gizmo = -1; +int old_index_gizmo = -1; + +double time_stamp_offset = 0.0; + +struct ProjectSettings +{ + std::vector session_file_names; +}; + +std::vector edges; +int index_active_edge = -1; +bool manipulate_active_edge = false; +bool edge_gizmo = false; + +ProjectSettings project_settings; +std::vector sessions; + +int viewer_reduce_rendered_trajectory = 1; +namespace fs = std::filesystem; + +int num_edge_extended_before = 0; +int num_edge_extended_after = 0; + +int gui_point_size = 2; + +TaitBryanPose motion_model_weights = { 0.01, 0.01, 0.01, 0.1, 0.1, 0.1 }; +/////////////////////////////////////////////////////////////////////////////////// + +void ndt_gui() +{ + static bool compute_mean_and_cov_for_bucket = false; + if (ImGui::Begin("Normal Distributions Transform", &is_ndt_gui, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::InputFloat3("Bucket size [m] (x, y,z)", ndt.bucket_size); + if (ndt.bucket_size[0] < 0.01) + ndt.bucket_size[0] = 0.01f; + if (ndt.bucket_size[1] < 0.01) + ndt.bucket_size[1] = 0.01f; + if (ndt.bucket_size[2] < 0.01) + ndt.bucket_size[2] = 0.01f; + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Number of threads", &ndt.number_of_threads); + if (ndt.number_of_threads < 1) + ndt.number_of_threads = 1; + ImGui::SameLine(); + ImGui::InputInt("Number of iterations", &ndt.number_of_iterations); + if (ndt.number_of_iterations < 1) + ndt.number_of_iterations = 1; + ImGui::PopItemWidth(); + + if (ImGui::Button("NDT optimization")) + { + for (auto& s : sessions) + { + s.is_gizmo = false; + } + + double rms_initial = 0.0; + double rms_final = 0.0; + double mui = 0.0; + + ndt.optimize(sessions, false, compute_mean_and_cov_for_bucket); + } + ImGui::End(); + } + +#if 0 + ImGui::Checkbox("ndt fix_first_node (add I to first pose in Hessian)", &ndt.is_fix_first_node); + + ImGui::Checkbox("ndt Gauss-Newton", &ndt.is_gauss_newton); + if (ndt.is_gauss_newton) + { + ndt.is_levenberg_marguardt = false; + } + + ImGui::SameLine(); + ImGui::Checkbox("ndt Levenberg-Marguardt", &ndt.is_levenberg_marguardt); + if (ndt.is_levenberg_marguardt) + { + ndt.is_gauss_newton = false; + } + + ImGui::Checkbox("ndt poses expressed as camera<-world (cw)", &ndt.is_cw); + if (ndt.is_cw) + { + ndt.is_wc = false; + } + ImGui::SameLine(); + ImGui::Checkbox("ndt poses expressed as camera->world (wc)", &ndt.is_wc); + if (ndt.is_wc) + { + ndt.is_cw = false; + } + + ImGui::Checkbox("ndt Tait-Bryan angles (om fi ka: RxRyRz)", &ndt.is_tait_bryan_angles); + if (ndt.is_tait_bryan_angles) + { + ndt.is_quaternion = false; + ndt.is_rodrigues = false; + } + + ImGui::SameLine(); + ImGui::Checkbox("ndt Quaternion (q0 q1 q2 q3)", &ndt.is_quaternion); + if (ndt.is_quaternion) + { + ndt.is_tait_bryan_angles = false; + ndt.is_rodrigues = false; + } + + ImGui::SameLine(); + ImGui::Checkbox("ndt Rodrigues (sx sy sz)", &ndt.is_rodrigues); + if (ndt.is_rodrigues) + { + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = false; + } + + if (ImGui::Button("compute mean mahalanobis distance")) + { + double rms_initial = 0.0; + double rms_final = 0.0; + double mui = 0.0; + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + } + + ImGui::Text("--------------------------------------------------------------------------------------------------------"); + + if (ImGui::Button("ndt_optimization(Lie-algebra left Jacobian)")) + { + // icp.optimize_source_to_target_lie_algebra_left_jacobian(point_clouds_container); + ndt.optimize_lie_algebra_left_jacobian(session.point_clouds_container.point_clouds, compute_mean_and_cov_for_bucket); + } + if (ImGui::Button("ndt_optimization(Lie-algebra right Jacobian)")) + { + // icp.optimize_source_to_target_lie_algebra_right_jacobian(point_clouds_container); + ndt.optimize_lie_algebra_right_jacobian(session.point_clouds_container.point_clouds, compute_mean_and_cov_for_bucket); + } + + ImGui::Text("--------------------------------------------------------------------------------------------------------"); + + ImGui::Checkbox("generalized", &ndt.is_generalized); + + if (ndt.is_generalized) + { + ImGui::InputDouble("sigma_r", &ndt.sigma_r, 0.01, 0.01); + ImGui::InputDouble("sigma_polar_angle_rad", &ndt.sigma_polar_angle, 0.0001, 0.0001); + ImGui::InputDouble("sigma_azimuthal_angle_rad", &ndt.sigma_azimuthal_angle, 0.0001, 0.0001); + ImGui::InputInt("num_extended_points", &ndt.num_extended_points, 1, 1); + + ImGui::Checkbox("compute_mean_and_cov_for_bucket", &compute_mean_and_cov_for_bucket); + } + + if (ImGui::Button("Set Zoller+Fröhlich TLS Imager 5006i errors")) + { + ndt.sigma_r = 0.0068; + ndt.sigma_polar_angle = 0.007 * DEG_TO_RAD; + ndt.sigma_azimuthal_angle = 0.007 * DEG_TO_RAD; + } + + if (ImGui::Button("Set Zoller+Fröhlich TLS Imager 5010C errors")) + { + ndt.sigma_r = 0.01; + ndt.sigma_polar_angle = 0.007 * DEG_TO_RAD; + ndt.sigma_azimuthal_angle = 0.007 * DEG_TO_RAD; + } + + if (ImGui::Button("Set Zoller+Fröhlich TLS Imager 5016 errors")) + { + ndt.sigma_r = 0.00025; + ndt.sigma_polar_angle = 0.004 * DEG_TO_RAD; + ndt.sigma_azimuthal_angle = 0.004 * DEG_TO_RAD; + } + if (ImGui::Button("Set Faro Focus3D errors")) + { + ndt.sigma_r = 0.001; + ndt.sigma_polar_angle = 19.0 * (1.0 / 3600.0) * DEG_TO_RAD; + ndt.sigma_azimuthal_angle = 19.0 * (1.0 / 3600.0) * DEG_TO_RAD; + } + if (ImGui::Button("Set Leica ScanStation C5 C10 errors")) + { + ndt.sigma_r = 0.006; + ndt.sigma_polar_angle = 0.00006; + ndt.sigma_azimuthal_angle = 0.00006; + } + if (ImGui::Button("Set Riegl VZ400 errors")) + { + ndt.sigma_r = 0.005; + ndt.sigma_polar_angle = 0.0005 * DEG_TO_RAD + 0.0003; // Laser Beam Dicvergence + ndt.sigma_azimuthal_angle = 0.0005 * DEG_TO_RAD + 0.0003; // Laser Beam Dicvergence + } + if (ImGui::Button("Set Leica HDS6100 errors")) + { + ndt.sigma_r = 0.009; + ndt.sigma_polar_angle = 0.000125; + ndt.sigma_azimuthal_angle = 0.000125; + } + if (ImGui::Button("Set Leica P40 errors")) + { + ndt.sigma_r = 0.0012; + ndt.sigma_polar_angle = 8.0 / 3600; + ndt.sigma_azimuthal_angle = 8.0 / 3600; + } +#endif +} + +void loop_closure_gui() +{ + if (ImGui::Begin("Manual Pose Graph Loop Closure Mode", &is_loop_closure_gui, ImGuiWindowFlags_AlwaysAutoResize)) + { + if (ImGui::Button("Optimize GRAPH")) + { + for (int i = 0; i < 100; i++) + { + std::cout << "Iteration [" << i + 1 << "] of: " << 100 << std::endl; + optimize(sessions, edges, motion_model_weights); + } + optimized = true; + } + + ImGui::Checkbox("update_rotation_center", &update_rotation_center); + + // + auto point_cloud_upper = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1; + + ImGui::InputInt("gui_point_size", &gui_point_size); + if (gui_point_size < 1) + gui_point_size = 1; + + ImGui::Text("Num edge extended:"); + + ImGui::Text("before: "); + ImGui::SameLine(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::SliderInt("##fs", &num_edge_extended_before, 0, point_cloud_upper); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SameLine(); + ImGui::InputInt("##fi", &num_edge_extended_before, 1, 5); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + if (num_edge_extended_before < 0) + num_edge_extended_before = 0; + if (num_edge_extended_before >= point_cloud_upper) + num_edge_extended_before = point_cloud_upper; + + point_cloud_upper = sessions[second_session_index].point_clouds_container.point_clouds.size() - 1; + + ImGui::Text(" after: "); + ImGui::SameLine(); + + ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast(point_cloud_upper)); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SameLine(); + ImGui::InputInt("##ti", &num_edge_extended_after, 1, 5); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + if (num_edge_extended_after < 0) + num_edge_extended_after = 0; + if (num_edge_extended_after >= point_cloud_upper) + num_edge_extended_after = point_cloud_upper; + ImGui::PopItemWidth(); + // + + if (!manipulate_active_edge) + { + ImGui::InputInt("index_loop_closure_source", &index_loop_closure_source); + if (index_loop_closure_source < 0) + index_loop_closure_source = 0; + if (index_loop_closure_source >= sessions[first_session_index].point_clouds_container.point_clouds.size() - 1) + index_loop_closure_source = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1; + ImGui::InputInt("index_loop_closure_target", &index_loop_closure_target); + if (index_loop_closure_target < 0) + index_loop_closure_target = 0; + if (index_loop_closure_target >= sessions[second_session_index].point_clouds_container.point_clouds.size() - 1) + index_loop_closure_target = sessions[second_session_index].point_clouds_container.point_clouds.size() - 1; + } + if (ImGui::Button("Add edge")) + { + Edge edge; + edge.index_from = index_loop_closure_source; + edge.index_to = index_loop_closure_target; + edge.index_session_from = first_session_index; + edge.index_session_to = second_session_index; + + edge.relative_pose_tb = pose_tait_bryan_from_affine_matrix( + sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.inverse() * + sessions[second_session_index].point_clouds_container.point_clouds[index_loop_closure_target].m_pose); + + edge.relative_pose_tb_weights.px = 1000000.0; + edge.relative_pose_tb_weights.py = 1000000.0; + edge.relative_pose_tb_weights.pz = 1000000.0; + edge.relative_pose_tb_weights.om = 1000000.0; + edge.relative_pose_tb_weights.fi = 1000000.0; + edge.relative_pose_tb_weights.ka = 1000000.0; + + edges.push_back(edge); + + index_active_edge = edges.size() - 1; + } + + std::string number_active_edges = "number_edges: " + std::to_string(edges.size()); + ImGui::Text(number_active_edges.c_str()); + if (edges.size() > 0) + { + ImGui::Checkbox("manipulate_active_edge", &manipulate_active_edge); + if (manipulate_active_edge) + { + int remove_edge_index = -1; + if (ImGui::Button("remove active edge")) + { + edge_gizmo = false; + remove_edge_index = index_active_edge; + } + + int prev_index_active_edge = index_active_edge; + + if (!edge_gizmo) + { + bool is_gizmo = false; + + for (const auto& s : sessions) + { + if (s.is_gizmo) + is_gizmo = true; + } + + if (!is_gizmo) + { + ImGui::InputInt("index_active_edge", &index_active_edge); + + if (index_active_edge < 0) + index_active_edge = 0; + if (index_active_edge >= (int)edges.size()) + index_active_edge = (int)edges.size() - 1; + } + } + + std::string txt = "index_session_from: " + std::to_string(edges[index_active_edge].index_session_from); + ImGui::Text(txt.c_str()); + txt = "index_session_to: " + std::to_string(edges[index_active_edge].index_session_to); + ImGui::Text(txt.c_str()); + txt = "index_from: " + std::to_string(edges[index_active_edge].index_from); + ImGui::Text(txt.c_str()); + txt = "index_to: " + std::to_string(edges[index_active_edge].index_to); + ImGui::Text(txt.c_str()); + + if (remove_edge_index != -1) + { + std::vector new_edges; + for (size_t i = 0; i < edges.size(); i++) + { + if (remove_edge_index != i) + new_edges.push_back(edges[i]); + } + edges = new_edges; + + index_active_edge = remove_edge_index - 1; + manipulate_active_edge = false; + } + + bool prev_gizmo = edge_gizmo; + ImGui::Checkbox("gizmo", &edge_gizmo); + + if (prev_gizmo != edge_gizmo) + { + auto m_to = sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .m_pose * + affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + m_gizmo[0] = (float)m_to(0, 0); + m_gizmo[1] = (float)m_to(1, 0); + m_gizmo[2] = (float)m_to(2, 0); + m_gizmo[3] = (float)m_to(3, 0); + m_gizmo[4] = (float)m_to(0, 1); + m_gizmo[5] = (float)m_to(1, 1); + m_gizmo[6] = (float)m_to(2, 1); + m_gizmo[7] = (float)m_to(3, 1); + m_gizmo[8] = (float)m_to(0, 2); + m_gizmo[9] = (float)m_to(1, 2); + m_gizmo[10] = (float)m_to(2, 2); + m_gizmo[11] = (float)m_to(3, 2); + m_gizmo[12] = (float)m_to(0, 3); + m_gizmo[13] = (float)m_to(1, 3); + m_gizmo[14] = (float)m_to(2, 3); + m_gizmo[15] = (float)m_to(3, 3); + } + if (!edge_gizmo) + { + if (ImGui::Button("ICP")) + { + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + } + else + { + bool is_with_ground_truth = false; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + is_with_ground_truth = true; + } + + if (is_with_ground_truth) + { + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + int number_of_iterations = 10; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + else + { + int number_of_iterations = 10; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + std::vector source; + auto& e = edges[index_active_edge]; + for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) + { + int index_src = e.index_to + i; + if (index_src >= 0 && + index_src < + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.size()) + { + Eigen::Affine3d m_src = sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds.at(index_src) + .m_pose; + for (int k = 0; k < sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[index_src] + .points_local.size(); + k++) + { + Eigen::Vector3d p_g = m_src * + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[index_src] + .points_local[k]; + source.push_back(p_g); + } + // point_clouds_container.point_clouds.at(index_src).render(m_src, 1); + } + } + std::vector target; + + for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) + { + int index_trg = e.index_from + i; + if (index_trg >= 0 && + index_trg < sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds.size()) + { + Eigen::Affine3d m_trg = sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds.at(index_trg) + .m_pose; + for (int k = 0; k < sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[index_trg] + .points_local.size(); + k++) + { + Eigen::Vector3d p_g = m_trg * + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[index_trg] + .points_local[k]; + target.push_back(p_g); + } + // point_clouds_container.point_clouds.at(index_src).render(m_src, 1); + } + } + + Eigen::Affine3d m_src_inv = sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[e.index_to] + .m_pose.inverse(); + + for (auto& p : source) + { + p = m_src_inv * p; + } + + Eigen::Affine3d m_trg_inv = sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[e.index_from] + .m_pose.inverse(); + + for (auto& p : target) + { + p = m_trg_inv * p; + } + + if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + } + ImGui::SameLine(); + ImGui::InputDouble("search_radius", &search_radius); + if (search_radius < 0.01) + search_radius = 0.01; + + ///////////////////////////////// + if (ImGui::Button("ICP [search radius 2m]")) + { + float sr = 2.0; + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + } + else + { + bool is_with_ground_truth = false; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + is_with_ground_truth = true; + } + + if (is_with_ground_truth) + { + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + else + { + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + } + + ImGui::SameLine(); + if (ImGui::Button("ICP [search radius 1m]")) + { + float sr = 1.0; + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + } + else + { + bool is_with_ground_truth = false; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + is_with_ground_truth = true; + } + + if (is_with_ground_truth) + { + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + else + { + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + } + ImGui::SameLine(); + if (ImGui::Button("ICP [search radius 0.5m]")) + { + float sr = 0.5; + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + } + else + { + bool is_with_ground_truth = false; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + is_with_ground_truth = true; + } + + if (is_with_ground_truth) + { + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + else + { + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + } + } + ImGui::SameLine(); + if (ImGui::Button("ICP [search radius 0.25m]")) + { + float sr = 0.25; + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + } + else + { + bool is_with_ground_truth = false; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + is_with_ground_truth = true; + } + + if (is_with_ground_truth) + { + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + else + { + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + } + } + ImGui::SameLine(); + if (ImGui::Button("ICP [search radius 0.1m]")) + { + float sr = 0.1; + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + } + else + { + bool is_with_ground_truth = false; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + is_with_ground_truth = true; + } + + if (is_with_ground_truth) + { + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + else + { + int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + const std::vector& source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + const std::vector& target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } + } + } + } +#if 0 + if (ImGui::Button("Save src")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog("Output file name", mandeye::fd::LAS_LAZ_filter, ".laz"); + std::cout << "laz file to save: '" << output_file_name << "'" << std::endl; + + if (output_file_name.size() > 0) + { + std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + for (size_t i = 0; i < source.size(); i++) + { + pointcloud.push_back(source[i]); + intensity.push_back(0); + timestamps.push_back(0.0); + } + + exportLaz( + output_file_name[0], + pointcloud, + intensity, + timestamps); + } + + /*int number_of_iterations = 30; + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; + std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + { + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + }*/ + //save + } + ImGui::SameLine(); + if (ImGui::Button("Save trg (transfromed only by rotation)")) + { + } +#endif + ////////////////////////////////// + } + } + } + + ImGui::End(); + } +} + +void save_trajectories_to_laz( + const Session& session, + const std::string& output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling) +{ + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + float consecutive_distance = 0; + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (size_t i = 0; i < p.local_trajectory.size(); i++) + { + const auto& pp = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d vp; + vp = p.m_pose * pp; // + session.point_clouds_container.offset; + + if (i > 0) + { + double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); + consecutive_distance += dist; + } + + bool is_curve = false; + + if (i > 100 && i < p.local_trajectory.size() - 100) + { + Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); + Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); + + Eigen::Vector3d v1 = position_curr - position_prev; + Eigen::Vector3d v2 = position_next - position_curr; + + if (v1.norm() > 0 && v2.norm() > 0) + { + double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * RAD_TO_DEG); + + if (angle_deg > 10.0) + { + is_curve = true; + } + } + } + double tol = not_curve_consecutive_distance_meters; + + if (is_curve) + { + tol = curve_consecutive_distance_meters; + } + + if (!is_trajectory_export_downsampling) + { + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(p.local_trajectory[i].timestamps.first); + } + else + { + if (consecutive_distance >= tol) + { + consecutive_distance = 0; + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(p.local_trajectory[i].timestamps.first); + } + } + } + } + } + // if (!exportLaz(output_file_name, pointcloud, intensity, gnss.offset_x, gnss.offset_y, gnss.offset_alt)) + if (!exportLaz( + output_file_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z())) + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } +} + +void createDXFPolyline(const std::string& filename, const std::vector& points) +{ + std::ofstream dxfFile(filename); + dxfFile << std::setprecision(20); + if (!dxfFile.is_open()) + { + std::cerr << "Failed to open file: " << filename << std::endl; + return; + } + + // DXF header + dxfFile << "0\nSECTION\n2\nHEADER\n0\nENDSEC\n"; + dxfFile << "0\nSECTION\n2\nTABLES\n0\nENDSEC\n"; + + // Start the ENTITIES section + dxfFile << "0\nSECTION\n2\nENTITIES\n"; + + // Start the POLYLINE entity + dxfFile << "0\nPOLYLINE\n"; + dxfFile << "8\n0\n"; // Layer 0 + dxfFile << "66\n1\n"; // Indicates the presence of vertices + dxfFile << "70\n8\n"; // 1 = Open polyline + + // Write the VERTEX entities + for (const auto& point : points) + { + dxfFile << "0\nVERTEX\n"; + dxfFile << "8\n0\n"; // Layer 0 + dxfFile << "10\n" << point.x() << "\n"; // X coordinate + dxfFile << "20\n" << point.y() << "\n"; // Y coordinate + dxfFile << "30\n" << point.z() << "\n"; // Z coordinate + } + + // End the POLYLINE + dxfFile << "0\nSEQEND\n"; + + // End the ENTITIES section + dxfFile << "0\nENDSEC\n"; + + // End the DXF file + dxfFile << "0\nEOF\n"; + + dxfFile.close(); + std::cout << "DXF file created: " << filename << std::endl; +} + +void save_trajectories( + Session& session, + const std::string& output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling, + bool write_lidar_timestamp, + bool write_unix_timestamp, + bool use_quaternions, + bool save_to_dxf) +{ + std::ofstream outfile; + if (!save_to_dxf) + { + outfile.open(output_file_name); + } + if (save_to_dxf || outfile.good()) + { + float consecutive_distance = 0; + std::vector polylinePoints; + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (size_t i = 0; i < p.local_trajectory.size(); i++) + { + const auto& m = p.local_trajectory[i].m_pose; + Eigen::Affine3d pose = p.m_pose * m; + pose.translation() += session.point_clouds_container.offset; + + if (i > 0) + { + double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); + consecutive_distance += dist; + } + + bool is_curve = false; + + if (i > 100 && i < p.local_trajectory.size() - 100) + { + Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); + Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); + + Eigen::Vector3d v1 = position_curr - position_prev; + Eigen::Vector3d v2 = position_next - position_curr; + + if (v1.norm() > 0 && v2.norm() > 0) + { + double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * RAD_TO_DEG); + + if (angle_deg > 10.0) + is_curve = true; + } + } + double tol = not_curve_consecutive_distance_meters; + + if (is_curve) + tol = curve_consecutive_distance_meters; + + if (!is_trajectory_export_downsampling || (is_trajectory_export_downsampling && consecutive_distance >= tol)) + { + if (is_trajectory_export_downsampling) + consecutive_distance = 0; + if (save_to_dxf) + polylinePoints.push_back(pose.translation()); + else + { + outfile << std::setprecision(20); + + if (write_lidar_timestamp) + outfile << p.local_trajectory[i].timestamps.first << ","; + if (write_unix_timestamp) + outfile << p.local_trajectory[i].timestamps.second << ","; + + outfile << pose(0, 3) << "," << pose(1, 3) << "," << pose(2, 3) << ","; + if (use_quaternions) + { + Eigen::Quaterniond q(pose.rotation()); + outfile << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << std::endl; + } + else + outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1) + << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl; + } + } + } + } + } + if (!save_to_dxf) + outfile.close(); + else + createDXFPolyline(output_file_name, polylinePoints); + } +} + +bool save_project_settings(const std::string& file_name, const ProjectSettings& _project_settings) +{ + std::cout << "saving file: '" << file_name << "'" << std::endl; + + nlohmann::json jj; + + nlohmann::json jsession_file_names; + for (const auto& pc : _project_settings.session_file_names) + { + nlohmann::json jfn{ { "session_file_name", pc } }; + jsession_file_names.push_back(jfn); + } + jj["session_file_names"] = jsession_file_names; + + nlohmann::json jloop_closure_edges; + for (const auto& edge : edges) + { + nlohmann::json jloop_closure_edge{ + { "px", edge.relative_pose_tb.px }, + { "py", edge.relative_pose_tb.py }, + { "pz", edge.relative_pose_tb.pz }, + { "om", edge.relative_pose_tb.om }, + { "fi", edge.relative_pose_tb.fi }, + { "ka", edge.relative_pose_tb.ka }, + { "w_px", edge.relative_pose_tb_weights.px }, + { "w_py", edge.relative_pose_tb_weights.py }, + { "w_pz", edge.relative_pose_tb_weights.pz }, + { "w_om", edge.relative_pose_tb_weights.om }, + { "w_fi", edge.relative_pose_tb_weights.fi }, + { "w_ka", edge.relative_pose_tb_weights.ka }, + { "index_from", edge.index_from }, + { "index_to", edge.index_to }, + { "is_fixed_px", edge.is_fixed_px }, + { "is_fixed_py", edge.is_fixed_py }, + { "is_fixed_pz", edge.is_fixed_pz }, + { "is_fixed_om", edge.is_fixed_om }, + { "is_fixed_fi", edge.is_fixed_fi }, + { "is_fixed_ka", edge.is_fixed_ka }, + { "index_session_from", edge.index_session_from }, + { "index_session_to", edge.index_session_to }, + }; + jloop_closure_edges.push_back(jloop_closure_edge); + } + jj["loop_closure_edges"] = jloop_closure_edges; + + std::ofstream fs(file_name); + if (!fs.good()) + return false; + fs << jj.dump(2); + fs.close(); + + return true; +} + +void update_timestamp_offset() +{ + std::cout << "update_timestamp" << std::endl; + time_stamp_offset = std::numeric_limits::max(); + + for (const auto& s : sessions) + { + if (!s.point_clouds_container.point_clouds.empty() && !s.point_clouds_container.point_clouds[0].local_trajectory.empty()) + { + double ts = s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first; + if (ts < time_stamp_offset) + time_stamp_offset = ts; + } + } + + std::cout << "new time_stamp_offset = " << time_stamp_offset << std::endl; +} + +bool revert(std::vector& sessions) +{ + for (auto& session : sessions) + { + for (auto& pc : session.point_clouds_container.point_clouds) + pc.m_pose = pc.m_pose_temp; + } + return true; +} + +bool revert_to_initial(std::vector& sessions) +{ + for (auto& session : sessions) + { + for (auto& pc : session.point_clouds_container.point_clouds) + pc.m_pose = pc.m_initial_pose; + } + return true; +} + +bool save_results(std::vector& sessions) +{ + for (auto& session : sessions) + { + if (!session.is_ground_truth) + { + std::cout << "saving result to: " << session.point_clouds_container.poses_file_name << std::endl; + session.point_clouds_container.save_poses(fs::path(session.point_clouds_container.poses_file_name).string(), false); + } + } + return true; +} + +Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observation_picking) +{ + const auto laser_beam = GetLaserBeam(x, y); + + RegistrationPlaneFeature::Plane pl; + + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = -observation_picking.picking_plane_height; + + Eigen::Vector3d pos = rayIntersection(laser_beam, pl); + + std::cout << "intersection: " << pos.x() << " " << pos.y() << " " << pos.z() << std::endl; + + return pos; +} + +bool loadProject(const std::string& file_name, ProjectSettings& _project_settings) +{ + std::cout << "Opening project file: '" << file_name << "'\n"; + + try + { + std::ifstream fs(file_name); + if (!fs.good()) + return false; + nlohmann::json data = nlohmann::json::parse(fs); + fs.close(); + + _project_settings.session_file_names.clear(); + + std::cout << "Contained sessions:\n"; + + for (const auto& fn_json : data["session_file_names"]) + { + const std::string fn = fn_json["session_file_name"]; + _project_settings.session_file_names.push_back(fn); + std::cout << "'" << fn << "'"; + if (!fs::exists(fn)) + std::cout << " (WARNING: session file does not exist! Please manually adapt path)"; + std::cout << "\n"; + } + + edges.clear(); + for (const auto& edge_json : data["loop_closure_edges"]) + { + Edge edge; + edge.index_from = edge_json["index_from"]; + edge.index_to = edge_json["index_to"]; + edge.is_fixed_fi = edge_json["is_fixed_fi"]; + edge.is_fixed_ka = edge_json["is_fixed_ka"]; + edge.is_fixed_om = edge_json["is_fixed_om"]; + edge.is_fixed_px = edge_json["is_fixed_px"]; + edge.is_fixed_py = edge_json["is_fixed_py"]; + edge.is_fixed_pz = edge_json["is_fixed_pz"]; + edge.relative_pose_tb.fi = edge_json["fi"]; + edge.relative_pose_tb.ka = edge_json["ka"]; + edge.relative_pose_tb.om = edge_json["om"]; + edge.relative_pose_tb.px = edge_json["px"]; + edge.relative_pose_tb.py = edge_json["py"]; + edge.relative_pose_tb.pz = edge_json["pz"]; + edge.relative_pose_tb_weights.fi = edge_json["w_fi"]; + edge.relative_pose_tb_weights.ka = edge_json["w_ka"]; + edge.relative_pose_tb_weights.om = edge_json["w_om"]; + edge.relative_pose_tb_weights.px = edge_json["w_px"]; + edge.relative_pose_tb_weights.py = edge_json["w_py"]; + edge.relative_pose_tb_weights.pz = edge_json["w_pz"]; + edge.index_session_from = edge_json["index_session_from"]; + edge.index_session_to = edge_json["index_session_to"]; + edges.push_back(edge); + } + + std::cout << "Found " << edges.size() << "edges\nOpening done\n"; + + return true; + } catch (std::exception& e) + { + std::cout << "can't load project settings: " << e.what() << std::endl; + return false; + } + + std::string newTitle = winTitle + " - " + truncPath(file_name); + glutSetWindowTitle(newTitle.c_str()); + + loaded_sessions = false; + time_stamp_offset = 0.0; + + return true; +} + +void openProject() +{ + std::string input_file_name = ""; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Open project", mandeye::fd::Project_filter); + + if (input_file_name.size() > 0) + { + loadProject(fs::path(input_file_name).string(), project_settings); + } +} + +void saveProject() +{ + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save project file", mandeye::fd::Project_filter, ".mjp", "project"); + + if (output_file_name.size() > 0) + if (save_project_settings(fs::path(output_file_name).string(), project_settings)) + { + std::string newTitle = winTitle + " - " + truncPath(output_file_name); + glutSetWindowTitle(newTitle.c_str()); + } +} + +void addSession() +{ + auto input_file_names = mandeye::fd::OpenFileDialog("Add session(s)", mandeye::fd::Session_filter, true); + + if (input_file_names.size() > 0) + { + for (const auto& input_file_name : input_file_names) + { + std::cout << "Adding session file: '" << input_file_name << "'" << std::endl; + project_settings.session_file_names.push_back(input_file_name); + } + + loaded_sessions = false; + time_stamp_offset = 0.0; + } +} + +void loadSessions() +{ + sessions.clear(); + for (const auto& ps : project_settings.session_file_names) + { + Session session; + session.load(fs::path(ps).string(), is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset); + + // making sure irelevant session specific settings that could affect rendering are off + session.point_clouds_container.xz_intersection = false; + session.point_clouds_container.yz_intersection = false; + session.point_clouds_container.xy_intersection = false; + session.point_clouds_container.xz_grid_10x10 = false; + session.point_clouds_container.xz_grid_1x1 = false; + session.point_clouds_container.xz_grid_01x01 = false; + session.point_clouds_container.yz_grid_10x10 = false; + session.point_clouds_container.yz_grid_1x1 = false; + session.point_clouds_container.yz_grid_01x01 = false; + session.point_clouds_container.xy_grid_10x10 = false; + session.point_clouds_container.xy_grid_1x1 = false; + session.point_clouds_container.xy_grid_01x01 = false; + + sessions.push_back(session); + if (session.is_ground_truth) + index_gt = sessions.size() - 1; + } + loaded_sessions = true; + + // reorder + std::vector sessions_reorder; + std::vector session_file_names_reordered; + + std::map map_reorder; + // project_settings.session_file_names. + int new_index = 0; + for (size_t i = 0; i < sessions.size(); i++) + { + if (sessions[i].is_ground_truth) + { + sessions_reorder.push_back(sessions[i]); + session_file_names_reordered.push_back(project_settings.session_file_names[i]); + map_reorder[i] = new_index++; + } + } + for (size_t i = 0; i < sessions.size(); i++) + { + if (!sessions[i].is_ground_truth) + { + sessions_reorder.push_back(sessions[i]); + session_file_names_reordered.push_back(project_settings.session_file_names[i]); + map_reorder[i] = new_index++; + } + } + sessions = sessions_reorder; + project_settings.session_file_names = session_file_names_reordered; + + for (auto& e : edges) + { + e.index_session_from = map_reorder[e.index_session_from]; + e.index_session_to = map_reorder[e.index_session_to]; + } + + std::cout << "sessions reordered, ground truth should be in front" << std::endl; + for (const auto& s : sessions) + { + std::cout << "session: '" << s.session_file_name << "' ground truth [" << int(s.is_ground_truth) << "]" << std::endl; + } + + // update time_stamp_offset + std::cout << "update time_stamp_offset" << std::endl; + for (const auto& s : sessions) + { + if (s.point_clouds_container.point_clouds.size() > 0) + { + if (s.point_clouds_container.point_clouds[0].local_trajectory.size() > 0) + { + if (s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first > time_stamp_offset) + { + time_stamp_offset = s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first; + } + } + } + } +} + +void generate_loop_closures(const std::vector& sessions, std::vector& edges) +{ + edges.clear(); + // Implementation for generating loop closures + + // bool found_edge = false; + + for (int i1 = 0; i1 < sessions.size(); i1++) + { + for (int j1 = 0; j1 < sessions[i1].point_clouds_container.point_clouds.size(); j1++) + { + Eigen::Affine3d pose_i = sessions[i1].point_clouds_container.point_clouds[j1].m_pose; + + for (int i2 = i1 + 1; i2 < sessions.size(); i2++) + { + for (int j2 = 0; j2 < sessions[i2].point_clouds_container.point_clouds.size(); j2++) + { + Eigen::Affine3d pose_j = sessions[i2].point_clouds_container.point_clouds[j2].m_pose; + + if ((pose_i.translation() - pose_j.translation()).norm() < 10.0) // Example threshold for proximity + { + // found_edge = true; + // Create a loop closure edge between pose_i and pose_j + /*Edge edge; + edge.index_session_from = i1; + edge.index_from = j1; + edge.index_session_to = i2; + edge.index_to = j2; + + // Initialize other edge parameters as needed + edges.push_back(edge);*/ + + Edge edge; + + edge.index_session_from = i1; + edge.index_session_to = i2; + + edge.index_from = j1; + edge.index_to = j2; + + std::cout << "Found loop closure edge between session " << i1 << " (pose " << j1 << ") and session " << i2 + << " (pose " << j2 << ")" << std::endl; + + edge.relative_pose_tb = pose_tait_bryan_from_affine_matrix( + sessions[edge.index_session_from].point_clouds_container.point_clouds[edge.index_from].m_pose.inverse() * + sessions[edge.index_session_to].point_clouds_container.point_clouds[edge.index_to].m_pose); + + edge.relative_pose_tb_weights.px = 1.0; + edge.relative_pose_tb_weights.py = 1.0; + edge.relative_pose_tb_weights.pz = 1.0; + edge.relative_pose_tb_weights.om = 1.0; + edge.relative_pose_tb_weights.fi = 1.0; + edge.relative_pose_tb_weights.ka = 1.0; + + edges.push_back(edge); + + // j1 += 20; + // j2 += 20; + } + } + } // for(int i2 = i1 + 1; i2 < sessions.size(); i2++) + } // for(int j1 = 0; j1 < sessions[i1].point_clouds_container.point_clouds.size(); j1++) + } // for(int i1 = 0; i1 < sessions.size(); i1++) +} + +void icp_all_edges(std::vector& sessions, std::vector& edges, float sr) +{ + std::cout << "icp_all_edges" << std::endl; + + int number_of_iterations = 10; + + for (size_t index_active_edge = 0; index_active_edge < edges.size(); index_active_edge++) + { + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + std::vector source; + auto& e = edges[index_active_edge]; + for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) + { + int index_src = e.index_to + i; + if (index_src >= 0 && + index_src < sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.size()) + { + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_src).m_pose; + for (int k = 0; k < + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[index_src].points_local.size(); + k++) + { + Eigen::Vector3d p_g = m_src * + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[index_src].points_local[k]; + source.push_back(p_g); + } + // point_clouds_container.point_clouds.at(index_src).render(m_src, 1); + } + } + std::vector target; + + for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) + { + int index_trg = e.index_from + i; + if (index_trg >= 0 && + index_trg < sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.size()) + { + Eigen::Affine3d m_trg = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_trg).m_pose; + for (int k = 0; k < sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[index_trg] + .points_local.size(); + k++) + { + Eigen::Vector3d p_g = m_trg * + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[index_trg] + .points_local[k]; + target.push_back(p_g); + } + // point_clouds_container.point_clouds.at(index_src).render(m_src, 1); + } + } + + Eigen::Affine3d m_src_inv = + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[e.index_to].m_pose.inverse(); + + for (auto& p : source) + { + p = m_src_inv * p; + } + + Eigen::Affine3d m_trg_inv = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.inverse(); + + for (auto& p : target) + { + p = m_trg_inv * p; + } + + if (icp.compute(source, target, sr, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); + } +} + +void settings_gui() +{ + if (ImGui::Begin("Settings", &is_settings_gui)) + { + ImGui::Checkbox("Downsample during load", &is_decimate); + ImGui::SameLine(); + ImGui::Text("Bucket [m]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("X##b", &bucket_x, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("Y##b", &bucket_y, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("Z##b", &bucket_z, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + ImGui::NewLine(); + + ImGui::Separator(); + + ImGui::Text("Benchmark settings:"); + + ImGui::PushItemWidth(ImGuiNumberWidth * 2); + + static double fast_plus = 100000000.0; + static double fast_plus_plus = 1000000000.0; + + ImGui::InputDouble("Increment", &fast_plus); + ImGui::InputDouble("Fast increment", &fast_plus_plus); + ImGui::InputDouble("Timestamp offset", &time_stamp_offset, fast_plus, fast_plus_plus); + ImGui::PopItemWidth(); + ImGui::SameLine(); + if (ImGui::Button("Set to origin")) + { + bool is_first_gt = false; + + if (sessions.size() > 0) + { + if (sessions[0].is_ground_truth) + is_first_gt = true; + } + + Eigen::Affine3d m_gt = Eigen::Affine3d::Identity(); + if (sessions.size() > 0) + { + int index_point_clouds = -1; + int index_local_trajectory = -1; + bool found = false; + for (size_t a = 0; a < sessions[0].point_clouds_container.point_clouds.size(); a++) + { + for (size_t b = 0; b < sessions[0].point_clouds_container.point_clouds[a].local_trajectory.size(); b++) + { + if (sessions[0].point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) + { + if (!found) + { + found = true; + index_point_clouds = a; + index_local_trajectory = b; + break; + } + } + } + } + + if (index_point_clouds != -1 && index_local_trajectory != -1) + { + m_gt = sessions[0].point_clouds_container.point_clouds[index_point_clouds].m_pose * + sessions[0].point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; + } + } + + // for (auto& session : sessions) + for (auto& session : sessions) + { + if (is_first_gt) + { + if (session.is_ground_truth) + continue; + } + + int index_point_clouds = -1; + int index_local_trajectory = -1; + bool found = false; + for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++) + { + for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) + { + if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) + { + if (!found) + { + found = true; + index_point_clouds = a; + index_local_trajectory = b; + break; + } + } + } + } + + if (index_point_clouds != -1 && index_local_trajectory != -1) + { + auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose; + auto m2 = + session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; + + auto inv = Eigen::Affine3d::Identity(); + inv = (m1 * m2).inverse(); + for (size_t index = 0; index < session.point_clouds_container.point_clouds.size(); index++) + session.point_clouds_container.point_clouds[index].m_pose = + inv * session.point_clouds_container.point_clouds[index].m_pose; + + for (size_t index = 0; index < session.point_clouds_container.point_clouds.size(); index++) + session.point_clouds_container.point_clouds[index].m_pose = + m_gt * session.point_clouds_container.point_clouds[index].m_pose; + } + } + } + + static bool open_import_popup = false; + static bool show_instruction = false; + + ImGui::Dummy(ImVec2(0, 10)); + + if (ImGui::Button("Import Benchmark Output Folders")) + { + open_import_popup = true; + ImGui::OpenPopup("Import Benchmark"); + } + + if (ImGui::BeginPopupModal("Import Benchmark", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Import benchmark sessions"); + ImGui::Separator(); + ImGui::SetNextWindowSize(ImVec2(740, 340), ImGuiCond_Once); + if (ImGui::Button("Instruction")) + { + show_instruction = !show_instruction; + } + + ImGui::Dummy(ImVec2(0, 10)); + if (ImGui::Button("Select Folder")) + { + const std::string algorithms[] = { "ct-icp", "glim", "super-lio", "dlio", + "i2ekf-lo", "superOdom", "lego-loam", "faster-lio", + "kiss-icp", "fast-lio", "lio-ekf", "genz-icp", + "point-lio", "ig-lio", "dlo", "lidar_odometry_ros_wrapper" }; + + std::vector missing; + std::unordered_map algo_map; + + for (const auto& algo : algorithms) + { + if (algo == "kiss-icp") + { + algo_map[algo] = "output_hdmapping-kiss"; + } + else if (algo == "genz-icp") + { + algo_map[algo] = "output_hdmapping-genz"; + } + else if (algo == "lidar_odometry_ros_wrapper") + { + algo_map[algo] = "output_hdmapping-lidar-odometry-ros"; + } + else + { + algo_map[algo] = "output_hdmapping-" + algo; + } + } + + fs::path path = fs::path(mandeye::fd::SelectFolder("Add sessions")); + + for (const auto& algo : algorithms) + { + fs::path output_folder = path / algo / algo_map[algo]; + fs::path session_file = output_folder / "session.json"; + + if (fs::is_directory(output_folder)) + { + auto it = + std::find(project_settings.session_file_names.begin(), project_settings.session_file_names.end(), session_file); + + if (it == project_settings.session_file_names.end()) + { + std::cout << "Adding session file: '" << session_file << "'" << std::endl; + project_settings.session_file_names.push_back(session_file.string()); + } + } + else + { + missing.push_back(output_folder); + } + } + + for (const auto& miss : missing) + { + std::cout << miss << " doesn't exist" << std::endl; + } + } + + ImGui::SameLine(); + + if (ImGui::Button("Close")) + { + ImGui::CloseCurrentPopup(); + show_instruction = false; + } + + if (show_instruction) + { + ImGui::BeginChild("InstructionChild", ImVec2(720, 300), true, ImGuiWindowFlags_HorizontalScrollbar); + ImGui::Separator(); + + ImGui::TextWrapped("Required folder structure:"); + + ImGui::Spacing(); + + ImGui::TextWrapped("Folders must follow the structure generated in benchmark-HDMapping-Orchestration (step 3):"); + ImGui::TextWrapped("https://github.com/MapsHD/benchmark-HDMapping-Orchestration"); + + ImGui::Spacing(); + ImGui::Separator(); + + ImGui::BulletText("chosen_folder/"); + ImGui::BulletText(" ct-icp/output_hdmapping-ct-icp/session.json"); + ImGui::BulletText(" glim/output_hdmapping-glim/session.json"); + ImGui::BulletText(" kiss-icp/output_hdmapping-kiss/session.json"); + ImGui::BulletText(" fast-lio/output_hdmapping-fast-lio/session.json"); + ImGui::BulletText(" ... (same pattern for other algorithms)"); + + ImGui::Spacing(); + ImGui::EndChild(); + } + + ImGui::EndPopup(); + } + if (project_settings.session_file_names.size() > 0) + { + ImGui::Separator(); + + ImGui::Text("Sessions:"); + + for (size_t i = 0; i < project_settings.session_file_names.size(); i++) + { + ImGui::Text(truncPath(project_settings.session_file_names[i]).c_str()); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(project_settings.session_file_names[i].c_str()); + + if (project_settings.session_file_names.size() == sessions.size()) + { + ImGui::BeginDisabled(is_loop_closure_gui); + { + ImGui::SameLine(); + ImGui::Checkbox(("Visible##" + std::to_string(i)).c_str(), &sessions[i].visible); + } + ImGui::EndDisabled(); + + ImGui::SameLine(); + if (ImGui::RadioButton(("Ground truth##" + std::to_string(i)).c_str(), &index_gt, i)) + if (old_index_gt == i) + index_gt = -1; // unselect + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select session as unmovable reference"); + + ImGui::BeginDisabled(!sessions[i].visible); + { + ImGui::BeginDisabled(sessions[i].is_ground_truth); + { + ImGui::SameLine(); + if (ImGui::RadioButton(("Gizmo##" + std::to_string(i)).c_str(), &index_gizmo, i)) + if (old_index_gizmo == i) + index_gizmo = -1; // unselect + } + ImGui::EndDisabled(); + + ImGui::SameLine(); + ImGui::ColorEdit3( + ("Color##" + std::to_string(i)).c_str(), (float*)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); + for (auto& pc : sessions[i].point_clouds_container.point_clouds) + { + pc.traj_color[0] = sessions[i].render_color[0]; + pc.traj_color[1] = sessions[i].render_color[1]; + pc.traj_color[2] = sessions[i].render_color[2]; + pc.render_color[0] = sessions[i].render_color[0]; + pc.render_color[1] = sessions[i].render_color[1]; + pc.render_color[2] = sessions[i].render_color[2]; + } + } + ImGui::EndDisabled(); + + // + if (sessions[i].point_clouds_container.point_clouds.size() > 0) + { + if (sessions[i].point_clouds_container.point_clouds[0].local_trajectory.size() > 0) + { + if (sessions[i] + .point_clouds_container.point_clouds[sessions[i].point_clouds_container.point_clouds.size() - 1] + .local_trajectory.size() > 0) + { + ImGui::SameLine(); + + int index_last = sessions[i].point_clouds_container.point_clouds.size() - 1; + int index_last2 = sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory.size() - 1; + + ImGui::Text( + "Timestamp range: <%.0f, %.0f>", + sessions[i].point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first, + sessions[i] + .point_clouds_container.point_clouds[index_last] + .local_trajectory[index_last2] + .timestamps.first); + } + } + } + } + } + + if (project_settings.session_file_names.size() == sessions.size()) + { + if ((old_index_gt != index_gt) || (old_index_gizmo != index_gizmo)) + { + for (size_t i = 0; i < sessions.size(); i++) + { + sessions[i].is_ground_truth = (i == index_gt); + sessions[i].is_gizmo = (i == index_gizmo); + } + + old_index_gt = index_gt; + old_index_gizmo = index_gizmo; + } + + if (index_gizmo != -1 && index_gizmo < sessions.size()) + { + // sessions[index_gizmo].is_gizmo = true; + m_gizmo[0] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 0); + m_gizmo[1] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 0); + m_gizmo[2] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 0); + m_gizmo[3] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 0); + m_gizmo[4] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 1); + m_gizmo[5] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 1); + m_gizmo[6] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 1); + m_gizmo[7] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 1); + m_gizmo[8] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 2); + m_gizmo[9] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 2); + m_gizmo[10] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 2); + m_gizmo[11] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 2); + m_gizmo[12] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 3); + m_gizmo[13] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 3); + m_gizmo[14] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 3); + m_gizmo[15] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 3); + } + } + + ImGui::BeginDisabled((project_settings.session_file_names.size() < 2) || (index_gizmo == -1)); + { + ImGui::Checkbox("Gizmo all sessions", &gizmo_all_sessions); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Gizmo will move all sessions except ground truth one"); + } + ImGui::EndDisabled(); + + ImGui::Separator(); + + ImGui::NewLine(); + + if (project_settings.session_file_names.size() == sessions.size()) + { + number_visible_sessions = 0; + + bool first_session_index_found = false; + for (size_t index = 0; index < sessions.size(); index++) + { + if (sessions[index].visible) + { + number_visible_sessions++; + if (!first_session_index_found) + { + first_session_index = index; + second_session_index = index; + first_session_index_found = true; + } + else + { + second_session_index = index; + } + } + } + + if (!is_loop_closure_gui) + { + static int nr_iter = 100; + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Number of iterations", &nr_iter); + if (nr_iter < 1) + nr_iter = 1; + + ImGui::InputDouble("Motion Model Weight (1 sigma m): position x [px]", &motion_model_weights.px, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("Motion Model Weight (1 sigma m): position y [py]", &motion_model_weights.py, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("Motion Model Weight (1 sigma m): position z [pz]", &motion_model_weights.pz, 0.0, 0.0, "%.3f"); + ImGui::InputDouble( + "Motion Model Weight (1 sigma degree): orientation om [om]", &motion_model_weights.om, 0.0, 0.0, "%.3f"); + ImGui::InputDouble( + "Motion Model Weight (1 sigma degree): orientation fi [fi]", &motion_model_weights.fi, 0.0, 0.0, "%.3f"); + ImGui::InputDouble( + "Motion Model Weight (1 sigma degree): orientation ka [ka]", &motion_model_weights.ka, 0.0, 0.0, "%.3f"); + + std::string bn = "Optimize (number of iterations: " + std::to_string(nr_iter) + ")"; + + if (ImGui::Button(bn.c_str())) + { + for (int i = 0; i < nr_iter; i++) + { + std::cout << "Iteration [" << i + 1 << "] of: " << nr_iter << std::endl; + optimize(sessions, edges, motion_model_weights); + } + optimized = true; + } + + // if (optimized) + //{ + ImGui::SameLine(); + if (ImGui::Button("Revert")) + revert(sessions); + ImGui::SameLine(); + if (ImGui::Button("Save results")) + save_results(sessions); + ImGui::SameLine(); + if (ImGui::Button("Revert to initial")) + revert_to_initial(sessions); + //} + + if (ImGui::Button("Generate loop closures")) + { + generate_loop_closures(sessions, edges); + } + + if (ImGui::Button("ICP all edges")) + { + icp_all_edges(sessions, edges, search_radius); + } + ImGui::SameLine(); + ImGui::InputDouble("search_radius", &search_radius); + if (search_radius < 0.01) + search_radius = 0.01; + } + + // if (!is_loop_closure_gui && prev_is_loop_closure_gui) + //{ + // exit(1); + // } + } + } + } + + ImGui::End(); +} + +void display() +{ + ImGuiIO& io = ImGui::GetIO(); + glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + pc.point_size = gui_point_size; + } + } + + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + glTranslatef(translate_x, translate_y, translate_z); + + // janusz + if (is_loop_closure_gui) + { + // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, + // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, + // false, false, 100000); + // sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false, + // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, + // false, false, 100000); + + if (first_session_index < sessions[first_session_index].point_clouds_container.point_clouds.size()) + { + if (update_rotation_center) + { + rotation_center.x() = sessions[first_session_index] + .point_clouds_container.point_clouds[index_loop_closure_source] + .m_pose.translation() + .x(); + rotation_center.y() = sessions[first_session_index] + .point_clouds_container.point_clouds[index_loop_closure_source] + .m_pose.translation() + .y(); + rotation_center.z() = sessions[first_session_index] + .point_clouds_container.point_clouds[index_loop_closure_source] + .m_pose.translation() + .z(); + } + } + + if (manipulate_active_edge) + { + if (edges.size() > 0) + { + int index_src = edges[index_active_edge].index_from; + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + + if (update_rotation_center) + { + rotation_center.x() = m_src(0, 3); + rotation_center.y() = m_src(1, 3); + rotation_center.z() = m_src(2, 3); + } + } + } + + /*if (session.pose_graph_loop_closure.manipulate_active_edge) + { + if (session.pose_graph_loop_closure.edges.size() > 0) + { + if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) + { + rotation_center.x() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(0, + 3); rotation_center.y() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(1, + 3); rotation_center.z() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(2, + 3); + } + } + }*/ + } + + viewLocal.translate(rotation_center); + + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + else + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + viewLocal.translate(-rotation_center); + + glLoadMatrixf(viewLocal.matrix().data()); + } + else + updateOrthoView(); + + showAxes(); + + if (is_loop_closure_gui) + { + if (manipulate_active_edge) + { + if (edges.size() > 0) + { + /*int index_src = edges[index_active_edge].index_from; + int index_trg = edges[index_active_edge].index_to; + + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + Eigen::Affine3d m_trg = m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render( + m_src, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render_color); + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render( + m_trg, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render_color);*/ + + int index_src = edges[index_active_edge].index_from; + int index_trg = edges[index_active_edge].index_to; + + Eigen::Affine3d _m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + Eigen::Affine3d _m_trg = _m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + Eigen::Affine3d m_src_0 = + sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).m_pose; // Todo + + for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; + i++) + { + if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && + sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + + Eigen::Affine3d m_src_curr = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo + Eigen::Affine3d m_src = _m_src * (m_src_0.inverse() * m_src_curr); + + // sessions[first_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; + + sessions[first_session_index].point_clouds_container.point_clouds.at(i).render( + m_src, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(i).render_color); + } + } + + Eigen::Affine3d m_trg_0 = + sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).m_pose; // Todo + + for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; + i++) + { + if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && + sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + Eigen::Affine3d m_trg_curr = + sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo + Eigen::Affine3d m_trg = _m_trg * (m_trg_0.inverse() * m_trg_curr); + + // sessions[second_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; + sessions[second_session_index].point_clouds_container.point_clouds.at(i).render( + m_trg, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(i).render_color); + } + } + } + } + else + { + ObservationPicking observation_picking; + + /*sessions[first_session_index] + .point_clouds_container.point_clouds.at(index_loop_closure_source) + .render( + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false);*/ + + for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; + i++) + { + if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && + sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + Eigen::Affine3d m_src = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; + + sessions[first_session_index].point_clouds_container.point_clouds.at(i).render( + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false); + } + } + + /*sessions[second_session_index] + .point_clouds_container.point_clouds.at(index_loop_closure_target) + .render( + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false);*/ + + for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; + i++) + { + if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && + sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + Eigen::Affine3d m_src = sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; + + sessions[second_session_index].point_clouds_container.point_clouds.at(i).render( + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false); + } + } + } + + // sessions[first_session_index].point_clouds_container.render(); + + glBegin(GL_LINE_STRIP); + for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) + { + glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + } + glEnd(); + + int i = 0; + for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) + { + glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); + i++; + } + + glBegin(GL_LINE_STRIP); + for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) + { + glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + } + glEnd(); + + i = 0; + for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) + { + glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); + i++; + } + + for (size_t i = 0; i < sessions.size(); i++) + { + for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) + { + int index_src = sessions[i].pose_graph_loop_closure.edges[j].index_from; + int index_trg = sessions[i].pose_graph_loop_closure.edges[j].index_to; + + glColor3f(0.0f, 0.0f, 1.0f); + glBegin(GL_LINES); + auto v1 = sessions[i].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); + auto v2 = sessions[i].point_clouds_container.point_clouds.at(index_trg).m_pose.translation(); + glVertex3f(v1.x(), v1.y(), v1.z()); + glVertex3f(v2.x(), v2.y(), v2.z()); + + glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5); + glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10); + glEnd(); + + glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(j).c_str()); + } + } + + for (size_t i = 0; i < edges.size(); i++) + { + int index_src = edges[i].index_from; + int index_trg = edges[i].index_to; + + int index_session_from = edges[i].index_session_from; + int index_session_to = edges[i].index_session_to; + + if (sessions[index_session_from].is_ground_truth || sessions[index_session_to].is_ground_truth) + glColor3f(0.0f, 1.0f, 1.0f); + else + glColor3f(1.0f, 1.0f, 0.0f); + + glBegin(GL_LINES); + auto v1 = sessions[index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); + auto v2 = sessions[index_session_to].point_clouds_container.point_clouds.at(index_trg).m_pose.translation(); + glVertex3f(v1.x(), v1.y(), v1.z()); + glVertex3f(v2.x(), v2.y(), v2.z()); + + glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5); + glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10); + glEnd(); + + glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); + } + } + else + { + for (auto& session : sessions) + { + if (session.visible) + { + session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory); + session.ground_control_points.render(session.point_clouds_container); + session.control_points.render(session.point_clouds_container, false); + + //// + int index_point_clouds = -1; + int index_local_trajectory = -1; + bool found = false; + for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++) + { + for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) + { + if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) + { + if (!found) + { + found = true; + index_point_clouds = a; + index_local_trajectory = b; + break; + } + } + } + } + + if (index_point_clouds != -1 && index_local_trajectory != -1) + { + if (index_local_trajectory < session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory.size()) + { + glColor3f( + session.point_clouds_container.point_clouds[index_point_clouds].render_color[0], + session.point_clouds_container.point_clouds[index_point_clouds].render_color[1], + session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]); + glBegin(GL_LINES); + + auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose; + auto m2 = + session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; + + auto v1 = (m1 * m2).translation(); + + glVertex3f(v1.x() - 5.0, v1.y(), v1.z()); + glVertex3f(v1.x() + 5.0, v1.y(), v1.z()); + + glVertex3f(v1.x(), v1.y() - 5.0, v1.z()); + glVertex3f(v1.x(), v1.y() + 5.0, v1.z()); + + glVertex3f(v1.x(), v1.y(), v1.z() - 5.0); + glVertex3f(v1.x(), v1.y(), v1.z() + 5.0); + + glEnd(); + } + } + } + } + } + + /*if (is_loop_closure_gui) + { + session.manual_pose_graph_loop_closure.Render(session.point_clouds_container, index_loop_closure_source, index_loop_closure_target); + } + else + { + for (const auto &g : available_geo_points) + { + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + auto c = g.coordinates - session.point_clouds_container.offset; + glVertex3f(c.x() - 0.5, c.y(), c.z()); + glVertex3f(c.x() + 0.5, c.y(), c.z()); + + glVertex3f(c.x(), c.y() - 0.5, c.z()); + glVertex3f(c.x(), c.y() + 0.5, c.z()); + + glVertex3f(c.x(), c.y(), c.z() - 0.5); + glVertex3f(c.x(), c.y(), c.z() + 0.5); + glEnd(); + } + + // + for (const auto &pc : session.point_clouds_container.point_clouds) + { + for (const auto &gp : pc.available_geo_points) + { + if (gp.choosen) + { + auto c = pc.m_pose * gp.coordinates; + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(c.x() - 0.5, c.y(), c.z()); + glVertex3f(c.x() + 0.5, c.y(), c.z()); + + glVertex3f(c.x(), c.y() - 0.5, c.z()); + glVertex3f(c.x(), c.y() + 0.5, c.z()); + + glVertex3f(c.x(), c.y(), c.z() - 0.5); + glVertex3f(c.x(), c.y(), c.z() + 0.5); + glEnd(); + + glBegin(GL_LINES); + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(c.x(), c.y(), c.z()); + glVertex3f(gp.coordinates.x(), gp.coordinates.y(), gp.coordinates.z()); + glEnd(); + + glColor3f(0.0f, 0.0f, 0.0f); + glBegin(GL_LINES); + glVertex3f(c.x(), c.y(), c.z()); + glVertex3f(c.x() + 10, c.y(), c.z()); + glEnd(); + + glRasterPos3f(c.x() + 10, c.y(), c.z()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str()); + } + } + } + }*/ + + // gnss.render(session.point_clouds_container); + + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + + if (!is_loop_closure_gui) + { + Eigen::Affine3d prev_pose_manipulated = Eigen::Affine3d::Identity(); + Eigen::Affine3d prev_pose_after_gismo = Eigen::Affine3d::Identity(); + + for (size_t i = 0; i < sessions.size(); i++) + { + // guizmo_all_sessions; + if (sessions[i].is_gizmo && !sessions[i].is_ground_truth) + { + if (sessions[i].point_clouds_container.point_clouds.size() > 0) + { + prev_pose_manipulated = sessions[i].point_clouds_container.point_clouds[0].m_pose; + std::vector all_m_poses; + for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + ImGuiIO& io = ImGui::GetIO(); + + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate( + modelview, + projection, + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); + } + else + ImGuizmo::Manipulate( + m_ortho_gizmo_view, + m_ortho_projection, + ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, + ImGuizmo::WORLD, + m_gizmo, + NULL); + + sessions[i].point_clouds_container.point_clouds[0].m_pose = Eigen::Map(m_gizmo).cast(); + prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose; + sessions[i].point_clouds_container.point_clouds[0].pose = + pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); + + sessions[i].point_clouds_container.point_clouds[0].gui_translation[0] = + (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; + sessions[i].point_clouds_container.point_clouds[0].gui_translation[1] = + (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; + sessions[i].point_clouds_container.point_clouds[0].gui_translation[2] = + (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; + + sessions[i].point_clouds_container.point_clouds[0].gui_rotation[0] = + (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[0].gui_rotation[1] = + (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[0].gui_rotation[2] = + (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); + + Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; + for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + sessions[i].point_clouds_container.point_clouds[j].pose = + pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = + (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = + (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = + (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; + + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = + (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = + (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = + (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + //} + } + } + } + if (gizmo_all_sessions) + { + for (size_t i = 0; i < sessions.size(); i++) + { + // guizmo_all_sessions; + if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth) + { + std::vector all_m_poses; + for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * sessions[i].point_clouds_container.point_clouds[0].m_pose; + + Eigen::Affine3d m_new = prev_pose_after_gismo * m_rel_org; + + sessions[i].point_clouds_container.point_clouds[0].m_pose = m_new; + sessions[i].point_clouds_container.point_clouds[0].pose = + pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); + + sessions[i].point_clouds_container.point_clouds[i].gui_translation[0] = + (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; + sessions[i].point_clouds_container.point_clouds[i].gui_translation[1] = + (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; + sessions[i].point_clouds_container.point_clouds[i].gui_translation[2] = + (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; + + sessions[i].point_clouds_container.point_clouds[i].gui_rotation[0] = + (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[i].gui_rotation[1] = + (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[i].gui_rotation[2] = + (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); + + Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; + for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + sessions[i].point_clouds_container.point_clouds[j].pose = + pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = + (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = + (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = + (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; + + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = + (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = + (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = + (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + } + } + } + } + else + { + // ImGuizmo ----------------------------------------------- + if (edge_gizmo && edges.size() > 0) + { + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate( + &modelview[0], + &projection[0], + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); + } + else + ImGuizmo::Manipulate( + m_ortho_gizmo_view, + m_ortho_projection, + ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, + ImGuizmo::WORLD, + m_gizmo, + NULL); + + Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); + + m_g.matrix() = Eigen::Map(m_gizmo).cast(); + + const int& index_src = edges[index_active_edge].index_from; + + const Eigen::Affine3d& m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); + } + } + + /*if (!is_loop_closure_gui) +{ + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].gizmo) + { + std::vector all_m_poses; + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); + + ImGuiIO &io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | +ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | +ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; + session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; + session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; + session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; + session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; + session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; + session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; + session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; + session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; + session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; + session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; + session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; + session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; + session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; + session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; + session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; + session.point_clouds_container.point_clouds[i].pose = +pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); + + session.point_clouds_container.point_clouds[i].gui_translation[0] = +(float)session.point_clouds_container.point_clouds[i].pose.px; session.point_clouds_container.point_clouds[i].gui_translation[1] = +(float)session.point_clouds_container.point_clouds[i].pose.py; session.point_clouds_container.point_clouds[i].gui_translation[2] = +(float)session.point_clouds_container.point_clouds[i].pose.pz; + + session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om +* RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[1] = +(float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[2] += (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); + + if (!manipulate_only_marked_gizmo) + { + Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; + for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + session.point_clouds_container.point_clouds[j].pose = +pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); + + session.point_clouds_container.point_clouds[j].gui_translation[0] = +(float)session.point_clouds_container.point_clouds[j].pose.px; session.point_clouds_container.point_clouds[j].gui_translation[1] = +(float)session.point_clouds_container.point_clouds[j].pose.py; session.point_clouds_container.point_clouds[j].gui_translation[2] = +(float)session.point_clouds_container.point_clouds[j].pose.pz; + + session.point_clouds_container.point_clouds[j].gui_rotation[0] = +(float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); session.point_clouds_container.point_clouds[j].gui_rotation[1] += (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[j].gui_rotation[2] = +(float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + } + } + } + + session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud); + observation_picking.render(); + + glPushAttrib(GL_ALL_ATTRIB_BITS); + glPointSize(5); + for (const auto &obs : observation_picking.observations) + { + for (const auto &[key1, value1] : obs) + { + for (const auto &[key2, value2] : obs) + { + if (key1 != key2) + { + Eigen::Vector3d p1, p2; + if (session.point_clouds_container.show_with_initial_pose) + { + p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; + } + else + { + p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; + } + glColor3f(0, 1, 0); + glBegin(GL_POINTS); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + glColor3f(1, 0, 0); + glBegin(GL_LINES); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + } + } + } + } + glPopAttrib(); + + for (const auto &obs : observation_picking.observations) + { + Eigen::Vector3d mean(0, 0, 0); + int counter = 0; + for (const auto &[key1, value1] : obs) + { + mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + counter++; + } + if (counter > 0) + { + mean /= counter; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glEnd();F + } + } + + glColor3f(1, 0, 1); + glBegin(GL_POINTS); + for (auto p : picked_points) + { + glVertex3f(p.x(), p.y(), p.z()); + } + glEnd(); +} +else +{ + // ImGuizmo ----------------------------------------------- + if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0) + { + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | +ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, +ImGuizmo::WORLD, m_gizmo, NULL); + + Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); + + m_g(0, 0) = m_gizmo[0]; + m_g(1, 0) = m_gizmo[1]; + m_g(2, 0) = m_gizmo[2]; + m_g(3, 0) = m_gizmo[3]; + m_g(0, 1) = m_gizmo[4]; + m_g(1, 1) = m_gizmo[5]; + m_g(2, 1) = m_gizmo[6]; + m_g(3, 1) = m_gizmo[7]; + m_g(0, 2) = m_gizmo[8]; + m_g(1, 2) = m_gizmo[9]; + m_g(2, 2) = m_gizmo[10]; + m_g(3, 2) = m_gizmo[11]; + m_g(0, 3) = m_gizmo[12]; + m_g(1, 3) = m_gizmo[13]; + m_g(2, 3) = m_gizmo[14]; + m_g(3, 3) = m_gizmo[15]; + + const int &index_src = +session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from; + + const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; + session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb = +pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); + } +}*/ + + view_kbd_shortcuts(); + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) + { + addSession(); + + // workaround + io.AddKeyEvent(ImGuiKey_A, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + if ((project_settings.session_file_names.size() > 0) && !loaded_sessions) + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_L, false)) + { + loadSessions(); + + // workaround + io.AddKeyEvent(ImGuiKey_L, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) + { + openProject(); + + // workaround + io.AddKeyEvent(ImGuiKey_O, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (project_settings.session_file_names.size() > 0) + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_R, false)) + { + remove_gui = true; + + // workaround + io.AddKeyEvent(ImGuiKey_R, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (sessions.size() > 0) + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_S, false)) + { + saveProject(); + + // workaround + io.AddKeyEvent(ImGuiKey_S, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (ImGui::BeginMainMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + if (ImGui::MenuItem("Open project", "Ctrl+O")) + openProject(); + if (ImGui::MenuItem("Save project", "Ctrl+S", nullptr, project_settings.session_file_names.size() > 0)) + saveProject(); + + ImGui::Separator(); + + if (ImGui::MenuItem("Add session(s)", "Ctrl+A")) + addSession(); + if (ImGui::MenuItem("Remove session(s)", "Ctrl+R", nullptr, project_settings.session_file_names.size() > 0)) + remove_gui = true; + + if (ImGui::MenuItem("Load sessions", "Ctrl+L", nullptr, (project_settings.session_file_names.size() > 0) && !loaded_sessions)) + loadSessions(); + + ImGui::Separator(); + + if (ImGui::BeginMenu("Save all marked trajectories", sessions.size() > 0)) + { + if (ImGui::MenuItem("Save all as las/laz files")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string laz_path = (dir / (folder_name + "_trajectory_laz.laz")).string(); + + std::cout << "Saving trajectory to LAZ: " << laz_path << std::endl; + + save_trajectories_to_laz(session, laz_path, 0.0f, 0.0f, false); + } + + std::cout << "Finished saving all trajectories to .laz files." << std::endl; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("As one global scan"); + + ImGui::Separator(); + + ImGui::Text("(x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22)"); + if (ImGui::MenuItem("Save all as csv (timestamp Lidar)##1")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidar_r.csv")).string(); + + std::cout << "Saving trajectory to CSV: " << csv_path << std::endl; + + try + { + std::ofstream outfile(csv_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << csv_path << std::endl; + continue; + } + + outfile << "timestampLidar,x,y,z," << "r00,r01,r02," << "r10,r11,r12," << "r20,r21,r22\n"; + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Matrix3d rot = pose.rotation(); + + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," << std::setprecision(10) + << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) << "," << rot(0, 1) << "," + << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," << rot(1, 2) << "," << rot(2, 0) + << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << csv_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; + } + } + + std::cout << "Finished saving all trajectories to CSV files." << std::endl; + } + if (ImGui::MenuItem("Save all as csv (timestamp Unix)##1")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string csv_path = (dir / (folder_name + "_trajectory_timestampUnix_r.csv")).string(); + + try + { + std::ofstream outfile(csv_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << csv_path << std::endl; + continue; + } + + outfile << "timestampUnix,x,y,z," << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n"; + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Matrix3d rot = pose.rotation(); + outfile << std::fixed << std::setprecision(0) << traj.timestamps.second << "," // Unix timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) + << "," << rot(0, 1) << "," << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," + << rot(1, 2) << "," << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << csv_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; + } + } + } + if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##1")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidarUnix_r.csv")).string(); + + try + { + std::ofstream outfile(csv_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << csv_path << std::endl; + continue; + } + + outfile << "timestampLidar,timestampUnix,x,y,z," << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n"; + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Matrix3d rot = pose.rotation(); + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp + << traj.timestamps.second << "," // Unix timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) + << "," << rot(0, 1) << "," << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," + << rot(1, 2) << "," << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << csv_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; + } + } + } + + ImGui::Separator(); + ImGui::Text("(x,y,z,qx,qy,qz,qw)"); + + if (ImGui::MenuItem("Save all as csv (timestamp Lidar)##2")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidar_q.csv")).string(); + + std::cout << "Saving trajectory to CSV: " << csv_path << std::endl; + + try + { + std::ofstream outfile(csv_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << csv_path << std::endl; + continue; + } + + outfile << "timestampLidar,x,y,z,qx,qy,qz,qw\n"; + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Quaterniond q(pose.rotation()); + + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << "," + << q.y() << "," << q.z() << "," << q.w() << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << csv_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; + } + } + + std::cout << "Finished saving all trajectories to CSV files." << std::endl; + } + if (ImGui::MenuItem("Save all as csv (timestamp Unix)##2")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string csv_path = (dir / (folder_name + "_trajectory_timestampUnix_q.csv")).string(); + + std::cout << "Saving trajectory to CSV: " << csv_path << std::endl; + + try + { + std::ofstream outfile(csv_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << csv_path << std::endl; + continue; + } + + outfile << "timestampUnix,x,y,z,qx,qy,qz,qw\n"; + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Quaterniond q(pose.rotation()); + + outfile << std::fixed << std::setprecision(0) << traj.timestamps.second << "," // Unix timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << "," + << q.y() << "," << q.z() << "," << q.w() << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << csv_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; + } + } + + std::cout << "Finished saving all trajectories to CSV files." << std::endl; + } + if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##2")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidarUnix_q.csv")).string(); + + std::cout << "Saving trajectory to CSV: " << csv_path << std::endl; + + try + { + std::ofstream outfile(csv_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << csv_path << std::endl; + continue; + } + + outfile << "timestampLidar,timestampUnix,x,y,z,qx,qy,qz,qw\n"; + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Quaterniond q(pose.rotation()); + + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp + << traj.timestamps.second << "," // Unix timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << "," + << q.y() << "," << q.z() << "," << q.w() << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << csv_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; + } + } + + std::cout << "Finished saving all trajectories to CSV files." << std::endl; + } + if (ImGui::MenuItem("Save all as TUM TXT")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + Session& session = sessions[i]; + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string txt_path = (dir / (folder_name + "_trajectory_tum.txt")).string(); + + std::cout << "Saving trajectory to TUM TXT: " << txt_path << std::endl; + try + { + std::ofstream outfile(txt_path); + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << txt_path << std::endl; + continue; + } + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + Eigen::Vector3d pos = pose.translation(); + Eigen::Quaterniond q(pose.rotation()); + + double t_s = static_cast(traj.timestamps.first) / 1e9; + + outfile << std::fixed << std::setprecision(9) << t_s << " " << std::setprecision(10) << pos.x() << " " + << pos.y() << " " << pos.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() + << "\n"; + } + } + + outfile.close(); + std::cout << "Saved: " << txt_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << txt_path << ": " << e.what() << std::endl; + } + } + std::cout << "Finished saving all trajectories to TUM TXT files." << std::endl; + } + if (ImGui::MenuItem("Save all as TUM TXT (single folder)")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + + std::filesystem::path session_dir = std::filesystem::path(session_path).parent_path(); + + std::filesystem::path base_dir = session_dir.parent_path(); + + std::filesystem::path output_dir = base_dir / "all_tum_files"; + + try + { + std::filesystem::create_directories(output_dir); + } catch (const std::exception& e) + { + std::cerr << "Failed to create export directory: " << e.what() << std::endl; + continue; + } + + std::string folder_name = session_dir.filename().string(); + + std::filesystem::path txt_path = output_dir / (folder_name + "_trajectory_tum.txt"); + + std::cout << "Saving trajectory to TUM TXT: " << txt_path << std::endl; + + try + { + std::ofstream outfile(txt_path); + + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " << txt_path << std::endl; + continue; + } + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + + Eigen::Vector3d pos = pose.translation(); + + Eigen::Quaterniond q(pose.rotation()); + + double t_s = static_cast(traj.timestamps.first) / 1e9; + + outfile << std::fixed << std::setprecision(9) << t_s << " " << std::setprecision(10) << pos.x() << " " + << pos.y() << " " << pos.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() + << "\n"; + } + } + + outfile.close(); + + std::cout << "Saved: " << txt_path << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error creating " << txt_path << ": " << e.what() << std::endl; + } + } + std::cout << "Finished saving all trajectories to single folder." << std::endl; + } + + ImGui::EndMenu(); + } + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Tools")) + { + ImGui::MenuItem("Normal Distributions Transform", nullptr, &is_ndt_gui, !is_loop_closure_gui && (sessions.size() > 0)); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Point cloud alignment (registration) algorithm"); + ImGui::Text( + "Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions " + "rather than raw points"); + ImGui::Text( + "Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large"); + ImGui::Text( + "Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences " + "with continuous probability density functions."); + ImGui::EndTooltip(); + } + + // bool prev_is_loop_closure_gui + ImGui::MenuItem( + "Manual Loop Closure", "Ctrl+L", &is_loop_closure_gui, (number_visible_sessions == 1 || number_visible_sessions == 2)); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Manually connect overlapping scan sections"); + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("View")) + { + ImGui::BeginDisabled(!(sessions.size() > 0)); + { + auto tmp = point_size; + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Points size", &point_size); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("keyboard 1-9 keys"); + if (point_size < 1) + point_size = 1; + else if (point_size > 10) + point_size = 10; + + if (tmp != point_size) + for (auto& session : sessions) + for (auto& point_cloud : session.point_clouds_container.point_clouds) + point_cloud.point_size = point_size; + + ImGui::Separator(); + } + ImGui::EndDisabled(); + + if (ImGui::MenuItem("Orthographic", "key O", &is_ortho)) + { + if (is_ortho) + { + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); + + ImGui::MenuItem("Show axes", "key X", &show_axes); + ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); + + ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); + + // ImGui::MenuItem("show_covs", nullptr, &show_covs); + + ImGui::Separator(); + + ImGui::Text("Colors:"); + + 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()) + ImGui::SetTooltip("Scene view relevant parameters"); + + camMenu(); + + ImGui::BeginDisabled(sessions.size() <= 0); + { + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); + // ImGui::SameLine(); + + if (viewer_decimate_point_cloud < 1) + viewer_decimate_point_cloud = 1; + + ImGui::SameLine(); + + ImGui::InputInt("Trajectory reduce render", &viewer_reduce_rendered_trajectory, 10, 100); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("increase for better performance, decrease for rendering more nodes in the trajectory"); + // ImGui::SameLine(); + + if (viewer_reduce_rendered_trajectory < 1) + viewer_reduce_rendered_trajectory = 1; + + ImGui::SameLine(); + + ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); + } + ImGui::EndDisabled(); + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); + + 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("Info")) + info_gui = !info_gui; + + ImGui::PopStyleVar(2); + ImGui::PopStyleColor(3); + + ImGui::EndMainMenuBar(); + } + + if (remove_gui) + { + ImGui::OpenPopup("Remove session(s)"); + remove_gui = false; + } + + 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 session(s) to remove:"); + ImGui::Separator(); + + for (size_t i = 0; i < project_settings.session_file_names.size(); i++) + { + bool checked = session_marked_for_removal[i]; + if (ImGui::Checkbox(project_settings.session_file_names[i].c_str(), &checked)) + session_marked_for_removal[i] = checked; + } + + ImGui::Separator(); + + if (ImGui::Button("Remove")) + { + for (size_t i = project_settings.session_file_names.size(); i > 0; --i) + { + size_t idx = i - 1; + + if (session_marked_for_removal[idx]) + { + std::cout << "Removing session: " << project_settings.session_file_names[idx] << std::endl; + + project_settings.session_file_names.erase(project_settings.session_file_names.begin() + idx); + + if (idx < sessions.size()) + sessions.erase(sessions.begin() + idx); + } + } + session_marked_for_removal.clear(); + + if (!sessions.empty()) + update_timestamp_offset(); + else + { + loaded_sessions = false; + time_stamp_offset = 0.0; + } + + ImGui::CloseCurrentPopup(); + } + + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + session_marked_for_removal.clear(); + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } + + if (is_ndt_gui) + ndt_gui(); + + if (is_loop_closure_gui) + loop_closure_gui(); + + cor_window(); + + info_window(infoLines, appShortcuts); + + if (compass_ruler) + drawMiniCompassWithRuler(); + + // my_display_code(); + /*if (is_ndt_gui) + ndt_gui(); + if (is_icp_gui) + icp_gui(); + if (is_pose_graph_slam) + pose_graph_slam_gui(); + if (is_registration_plane_feature) + registration_plane_feature_gui(); + if (is_manual_analisys) + observation_picking_gui();*/ + // if (is_loop_closure_gui) + // manual_pose_graph_loop_closure.Gui(); + + if (is_settings_gui) + settings_gui(); + + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + + glutSwapBuffers(); + glutPostRedisplay(); +} + +void mouse(int glut_button, int state, int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + int button = -1; + if (glut_button == GLUT_LEFT_BUTTON) + button = 0; + if (glut_button == GLUT_RIGHT_BUTTON) + button = 1; + if (glut_button == GLUT_MIDDLE_BUTTON) + button = 2; + if (button != -1 && state == GLUT_DOWN) + io.MouseDown[button] = true; + if (button != -1 && state == GLUT_UP) + io.MouseDown[button] = false; + + static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) + wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + + if (!io.WantCaptureMouse) + { + if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) && + !manipulate_active_edge) + { + // if (s_loop_closure_gui) + if ((sessions.size() > 0) && (number_visible_sessions > 0) && update_rotation_center) + { + getClosestTrajectoriesPoint( + sessions, + x, + y, + first_session_index, + second_session_index, + number_visible_sessions, + index_loop_closure_source, + index_loop_closure_target, + io.KeyShift, + time_stamp_offset); + } + else + { + if (update_rotation_center) + { + setNewRotationCenter(x, y); + } + } + } + + if (state == GLUT_DOWN) + { + mouse_buttons |= 1 << glut_button; + + /*if (observation_picking.is_observation_picking_mode) + { + Eigen::Vector3d p = GLWidgetGetOGLPos(x, y, observation_picking); + int number_active_pcs = 0; + int index_picked = -1; + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].visible) + { + number_active_pcs++; + index_picked = i; + } + } + if (number_active_pcs == 1) + { + observation_picking.add_picked_to_current_observation(index_picked, p); + } + }*/ + } + else if (state == GLUT_UP) + { + mouse_buttons = 0; + } + mouse_old_x = x; + mouse_old_y = y; + } +} + +int main(int argc, char* argv[]) +{ + try + { + if (checkClHelp(argc, argv)) + { + std::cout << winTitle << "\n\n" + << "USAGE:\n" + << std::filesystem::path(argv[0]).stem().string() << " /?\n\n" + << "where\n" + << " Path to Mandeye JSON Project file (*.mjp)\n" + << " -h, /h, --help, /? Show this help and exit\n\n"; + + return 0; + } + + initGL(&argc, argv, winTitle, display, mouse); + + if (argc > 1) + { + for (int i = 1; i < argc; i++) + { + std::string ext = fs::path(argv[i]).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + + if (ext == ".mjp") + { + loadProject(argv[i], project_settings); + + break; + } + } + } + + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + ImGui::DestroyContext(); + } catch (const std::bad_alloc& e) + { + std::cerr << "System is out of memory : " << e.what() << std::endl; + mandeye::fd::OutOfMemMessage(); + } catch (const std::exception& e) + { + std::cout << e.what(); + } catch (...) + { + std::cerr << "Unknown fatal error occurred." << std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/apps/multi_session_registration_legacy/resource.h b/apps/multi_session_registration_legacy/resource.h new file mode 100644 index 00000000..4f3204ca --- /dev/null +++ b/apps/multi_session_registration_legacy/resource.h @@ -0,0 +1,17 @@ +//{{NO_DEPENDENCIES}} +// Microsoft Visual C++ generated include file. +// Used by resource.rc +// +#define IDI_ICON1 101 // application icon +#define VS_VERSION_INFO 1 // version info + +// Next default values for new objects +// +#ifdef APSTUDIO_INVOKED +#ifndef APSTUDIO_READONLY_SYMBOLS +#define _APS_NEXT_RESOURCE_VALUE 106 +#define _APS_NEXT_COMMAND_VALUE 40001 +#define _APS_NEXT_CONTROL_VALUE 1000 +#define _APS_NEXT_SYMED_VALUE 101 +#endif +#endif diff --git a/apps/multi_session_registration_legacy/resource.rc b/apps/multi_session_registration_legacy/resource.rc new file mode 100644 index 00000000..e8821486 --- /dev/null +++ b/apps/multi_session_registration_legacy/resource.rc @@ -0,0 +1,49 @@ +// Microsoft Visual C++ generated resource script. +// +#include "resource.h" +///////////////////////////////////////////////////////////////////////////// +// English (United States) resources + +///////////////////////////////////////////////////////////////////////////// +// +// Icon +// + +// Icon with lowest ID value placed first to ensure application icon +// remains consistent on all systems. +IDI_ICON1 ICON "icon.ico" + + +///////////////////////////////////////////////////////////////////////////// +// +// Version +// + +VS_VERSION_INFO VERSIONINFO +FILEVERSION 0, 0, 100, 1 +PRODUCTVERSION 0, 0, 100, 1 +FILEFLAGSMASK 0x3fL +FILEOS 0x40004 +FILETYPE 0x1 +BEGIN +BLOCK "StringFileInfo" +BEGIN +BLOCK "040904B0" +BEGIN +VALUE "CompanyName", "Mandeye\0" +VALUE "FileDescription", "HDMapping Step 3\0" +VALUE "FileVersion", "0.100.1\0" +VALUE "InternalName", "Multi session registration\0" +VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" +VALUE "OriginalFilename", "multi_session_registration_step_3.exe\0" +VALUE "ProductVersion", "0.100.1\0" +VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" +VALUE "ProductName", "HDMapping\0" +END +END +BLOCK "VarFileInfo" +BEGIN +VALUE "Translation", 0x409, 0x04B0 +END +END +///////////////////////////////////////////////////////////////////////////// \ No newline at end of file diff --git a/deploy_mandeye.bat b/deploy_mandeye.bat index a7e2d178..ab24d95c 100644 --- a/deploy_mandeye.bat +++ b/deploy_mandeye.bat @@ -60,7 +60,7 @@ rem This list has to be kept in sync by hand with the hdmapping_install_app() rem calls in apps/*/CMakeLists.txt. hd_mapper is deliberately absent: it is rem gated behind BUILD_WITH_HD_MAPPER_APPLICATION, which defaults to OFF, so rem listing it would warn on every normal build. -set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe mandeye_with_360_camera_manual_coloring.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe single_session_manual_coloring.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll" +set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe mandeye_with_360_camera_manual_coloring.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_session_registration_step_3_legacy.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe single_session_manual_coloring.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll" set "MISSING=" for %%F in (%EXPECTED%) do ( From d36aba2610cb2a048cb7238dd7964617dcb6e48b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 18:50:13 +0200 Subject: [PATCH 5/9] Port step 3 (multi_session_registration) from GLUT to raylib Same approach as step 2: camera/input/picking via raylib_widgets::OrbitCamera and step 2's helpers, points via core_raylib's ScanRenderer (one per session), rlImGui + ImGuizmo for the UI. Panels and menus are the GLUT code unchanged apart from renames. Intentional differences: - one point size (the loop closure window's value used to override the View menu and 1-9 keys every frame) - View > Points color: session color or step 2's intensity/height/distance shader gradients - index guards in loop closure rendering (GLUT could index with -1) - drag & drop of .mjp projects and .mjs/.json sessions, like step 2 Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- .../multi_session_registration/CMakeLists.txt | 30 +- .../multi_session_registration.cpp | 2176 +++++++++++------ 2 files changed, 1385 insertions(+), 821 deletions(-) diff --git a/apps/multi_session_registration/CMakeLists.txt b/apps/multi_session_registration/CMakeLists.txt index bd9be085..f4f898ce 100644 --- a/apps/multi_session_registration/CMakeLists.txt +++ b/apps/multi_session_registration/CMakeLists.txt @@ -2,11 +2,12 @@ cmake_minimum_required(VERSION 4.0.0) project(multi_session_registration_step_3) -# Source files +# raylib-based, like step 2 (apps/multi_view_tls_registration): no core/src/utils.cpp, +# GLUT or legacy OpenGL. The GLUT build is kept as apps/multi_session_registration_legacy. +# multi_session_registration_no_gui.cpp is not part of this app; pybind builds it. set(SOURCES multi_session_registration.cpp multi_session_factor_graph.cpp - "../../core/src/utils.cpp" ) # Windows: add resource file @@ -26,28 +27,25 @@ target_include_directories( ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${THIRDPARTY_DIRECTORY} - ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${THIRDPARTY_DIRECTORY}/imgui - ${THIRDPARTY_DIRECTORY}/imgui/backends - ${THIRDPARTY_DIRECTORY}/ImGuizmo ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master - ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/freeglut/include ${LASZIP_INCLUDE_DIR}/LASzip/include) target_link_libraries( multi_session_registration_step_3 - # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib - ${FREEGLUT_LIBRARY} - ${OPENGL_gl_LIBRARY} - OpenGL::GLU + PRIVATE + # core_raylib brings in core + raylib (and links FREEGLUT after core for libcore.a's + # remaining glutBitmap* references) -- see core/CMakeLists.txt. + core_raylib + raylib_widgets + imgui_raylib + rlimgui + imguizmo_raylib + spdlog::spdlog ${PLATFORM_LASZIP_LIB} - ${PLATFORM_MISCELLANEOUS_LIBS} - ${CORE_LIBRARIES} - ${GUI_LIBRARIES}) + ${PLATFORM_MISCELLANEOUS_LIBS}) if(WIN32) add_custom_command( @@ -64,4 +62,4 @@ if (MSVC) target_compile_options(multi_session_registration_step_3 PRIVATE /bigobj) endif() -hdmapping_install_app(multi_session_registration_step_3) \ No newline at end of file +hdmapping_install_app(multi_session_registration_step_3) diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index ca6a4bbf..b20977e6 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -1,18 +1,31 @@ +#include #include #include +#include +#include + +// Step 3 used to be built on GLUT + legacy immediate-mode OpenGL via +// core/src/utils.cpp (the GLUT build is kept as +// apps/multi_session_registration_legacy). Like step 2 +// (apps/multi_view_tls_registration), it now runs on raylib: the camera, +// input and picking helpers it used to get from are +// re-implemented below on top of raylib_widgets::OrbitCamera, and point +// clouds are drawn with Core/raylib_render.hpp's ScanRenderer (one per +// session) instead of core's legacy-GL PointCloud::render(). +#include "raylib.h" +#include "raymath.h" +#include "rlImGui.h" +#include "rlgl.h" #include -#include -#include #include #include -#include -#include -#include #include +#include + #include #include @@ -21,22 +34,70 @@ #include #include #include +#include #include #include -#include +#include +#include +#ifdef _WIN32 +// Same windows.h/raylib clash as step 2 (see multi_view_tls_registration_gui.cpp): +// rename windows.h's CloseWindow/ShowCursor so raylib's stay callable. +#define CloseWindow CloseWindow_win32 +#define ShowCursor ShowCursor_win32 +#endif #include +#ifdef _WIN32 +#undef CloseWindow +#undef ShowCursor +#endif #include #ifdef _WIN32 #include "resource.h" -#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef _WIN32 +// windows.h #defines DrawText as DrawTextA; restore raylib's DrawText. +#undef DrawText #endif #include "multi_session_factor_graph.h" +using raylib_widgets::ShortcutEntry; +using raylib_widgets::ShowMainDockSpace; + +const float DEG_TO_RAD = M_PI / 180.0f; +const float RAD_TO_DEG = 180.0f / M_PI; + +constexpr float ImGuiNumberWidth = 120.0f; +constexpr const char* xText = "Longitudinal (forward/backward)"; +constexpr const char* yText = "Lateral (left/right)"; +constexpr const char* zText = "Vertical (up/down)"; + +const uint32_t window_width = 1600; +const uint32_t window_height = 900; + +// GLUT/mouse-button codes kept so mouse() keeps its GLUT-callback shape (as in step 2). +constexpr int GLUT_LEFT_BUTTON = 0; +constexpr int GLUT_MIDDLE_BUTTON = 1; +constexpr int GLUT_RIGHT_BUTTON = 2; +constexpr int GLUT_DOWN = 0; +constexpr int GLUT_UP = 1; + +// Point downsampling default and camera-Reset value, as in the GLUT step 3 (utils.cpp). +constexpr int kDefaultDecimate = 1000; + std::string winTitle = std::string("Step 3 (Multi session registration) ") + HDMAPPING_VERSION_STRING; std::vector infoLines = { @@ -181,11 +242,988 @@ namespace fs = std::filesystem; int num_edge_extended_before = 0; int num_edge_extended_after = 0; -int gui_point_size = 2; - TaitBryanPose motion_model_weights = { 0.01, 0.01, 0.01, 0.1, 0.1, 0.1 }; /////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////// + +// Camera/view state that used to be globals. +struct AppStateBase +{ + int viewer_decimate_point_cloud = kDefaultDecimate; + + int mouse_old_x = 0, mouse_old_y = 0; + int mouse_buttons = 0; + bool show_axes = true; + ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); + // Single point size for all sessions. The GLUT app had two (View menu/1-9 keys and the + // loop closure window's gui_point_size), and the latter silently overrode the former every frame. + int point_size = 2; + + bool info_gui = false; + bool compass_ruler = true; + + // Rebuilt from `camera` every frame; used by the compass and the perspective modelview. + Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); + + raylib_widgets::OrbitCamera camera; +}; + +inline AppStateBase app_state; + +// Edge-triggered request to open the Center of rotation dialog (Shift+R). +bool cor_gui = false; + +bool scroll_hint_enabled = true; +bool scroll_hint_active = false; +int scroll_hint_count = 0; +float scroll_hint_accu = 0.0f; +double scroll_hint_lastT = 0.0; + +// One GPU renderer per session, parallel to `sessions` (ScanRenderer is indexed by a single +// std::vector). Rebuilt whenever the session list changes -- see syncSessionRenderers(). +std::vector> session_renderers; + +// Point coloring, using the same ScanRenderer shader modes as step 2's color schemes. Flat (each +// scan's render_color, i.e. the session color) is the default, since step 3 compares sessions. +ScanColorMode points_color_mode = ScanColorMode::Flat; + +// Bounds of all loaded sessions, for the height and distance gradients; updated with session_renderers. +PointClouds::PointCloudDimensions scene_dims{ 0, 0, 0, 0, 0, 1, 1, 1, 1 }; + +// This frame's 3D model-view-projection, captured before the matrix stack is switched to 2D, +// so the 2D label pass can project world points to the screen. +Matrix frame_mvp_3d{}; + +void display(); +void mouse(int glut_button, int state, int x, int y); + +/////////////////////////////////////////////////////////////////////////////////// + +// Camera/input/picking helpers, same as step 2 (apps/multi_view_tls_registration). + +std::string truncPath(const std::string& fullPath) +{ + namespace fspath = std::filesystem; + fspath::path path(fullPath); + + auto parent1 = path.parent_path().filename().string(); + auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder + auto filename = path.filename().string(); + + return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; +} + +void wheel(int button, int dir, int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MouseWheel += dir; // or direction * 1.0f depending on your setup + + if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) + { + // GetMouseWheelMove(), not `dir`: dir is already quantized to +-1 by + // main()'s caller (see its comment), which discards a trackpad's + // fractional per-frame scroll magnitude -- reading it again here + // (stable within the same frame, since raylib only updates it once + // per PollInputEvents()) lets zoom() scale the step by how much was + // actually scrolled instead of always taking a full step. + app_state.camera.zoom(GetMouseWheelMove(), io.KeyShift); + + if (scroll_hint_enabled) + { + if (!scroll_hint_active) + { + scroll_hint_accu += fabs(dir); + + if (scroll_hint_accu > 30.0f) // tweak threshold + { + scroll_hint_accu = 0.0f; + scroll_hint_active = true; + scroll_hint_count++; + } + } + + if (scroll_hint_active) + scroll_hint_lastT = ImGui::GetTime(); + + // Reset and disable hint if Shift is pressed while scrolling + if (io.KeyShift || scroll_hint_count > 3) + { + scroll_hint_active = false; + scroll_hint_enabled = false; + } + } + } +} + +void motion(int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + + if (!io.WantCaptureMouse) + { + float dx, dy; + dx = (float)(x - app_state.mouse_old_x); + dy = (float)(y - app_state.mouse_old_y); + + // Ctrl/Shift held: reserved for the discrete click actions and the + // keyboard shortcuts in view_kbd_shortcuts() -- mouse() sets + // mouse_buttons for *every* button-down, including a Ctrl/Shift+ + // click used to pick a new rotation center (which starts a camera + // transition -- see getClosestTrajectoryPoint()/ + // setNewRotationCenter()/the Center of rotation dialog). Without + // this guard, any stray sub-pixel movement on the same click + // (trackpads are far more prone to this than a physical mouse + // button) got read as an ordinary orbit/pan drag and immediately + // broke that transition via dragOrbit()/dragPanPerspective()'s + // breakEulerTransition() call. + if (!io.KeyCtrl && !io.KeyShift) + { + if (app_state.mouse_buttons & 1) // left button + { + app_state.camera.dragOrbit(dx, dy); + } + + if (app_state.mouse_buttons & 4) // right button + { + if (app_state.camera.isOrtho) + app_state.camera.dragPanOrtho(dx, dy, io.DisplaySize.x, io.DisplaySize.y); + else + app_state.camera.dragPanPerspective(dx, dy); + } + } + + app_state.mouse_old_x = x; + app_state.mouse_old_y = y; + } +} + +void showAxes() +{ + if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes + { + const auto& rc = app_state.camera.euler.rotationCenter; + rlBegin(RL_LINES); + rlColor3f(1.f, 1.f, 1.f); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x + 1.f, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x - 1.f, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y - 1.f, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y + 1.f, rc.z); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z - 1.f); + rlVertex3f(rc.x, rc.y, rc.z); + rlVertex3f(rc.x, rc.y, rc.z + 1.f); + rlEnd(); + } + + if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // origin axes + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(100, 0.0f, 0.0f); + + rlColor3f(0.0f, 1.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 100, 0.0f); + + rlColor3f(0.0f, 0.0f, 1.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 100); + rlEnd(); + } +} + +void camMenu() +{ + using raylib_widgets::OrbitCamera; + + if (ImGui::BeginMenu("Camera")) + { + if (ImGui::MenuItem("Front (yz view)", "key F")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Front); + if (ImGui::MenuItem("Back", "key B")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Back); + if (ImGui::MenuItem("Left (xz view)", "key L")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Left); + if (ImGui::MenuItem("Right", "key R")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Right); + if (ImGui::MenuItem("Top (xy view)", "key T")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Top); + if (ImGui::MenuItem("Bottom", "key U")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Bottom); + if (ImGui::MenuItem("Isometric", "key I")) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Iso); + ImGui::Separator(); + if (ImGui::MenuItem("Reset", "key Z")) + { + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Reset); + app_state.viewer_decimate_point_cloud = kDefaultDecimate; + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Change camera view to fixed positions"); + ImGui::Separator(); + ImGui::Text("Metrics:"); + if (ImGui::BeginTable("Metrics", 4)) + { + ImGui::TableSetupColumn("Coord"); + ImGui::TableSetupColumn("rotate"); + ImGui::TableSetupColumn("translate"); + ImGui::TableSetupColumn("rot center"); + ImGui::TableHeadersRow(); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + + std::string text = "X"; + float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("X"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", app_state.camera.euler.rotateX); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", app_state.camera.euler.translate.x); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.x); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Y"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", app_state.camera.euler.rotateY); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", app_state.camera.euler.translate.y); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.y); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Z"); + + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", app_state.camera.euler.translate.z); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.y); + + ImGui::EndTable(); + } + ImGui::Text("Mouse sensitivity: %.4f", app_state.camera.eulerMouseSensitivity); + + ImGui::EndTooltip(); + } + + if (scroll_hint_active) + { + ImVec2 mousePos = ImGui::GetMousePos(); + ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); + ImGui::SetNextWindowBgAlpha(0.7f); + ImGui::BeginTooltip(); + ImGui::Text("Tip: To accelerate hold Shift + scroll"); + ImGui::EndTooltip(); + + if (ImGui::GetTime() - scroll_hint_lastT > 1) + scroll_hint_active = false; + } +} + +void view_kbd_shortcuts() +{ + using raylib_widgets::OrbitCamera; + + ImGuiIO& io = ImGui::GetIO(); + + if (io.WantCaptureKeyboard) + return; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + app_state.camera.euler.translate.x += 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + app_state.camera.euler.translate.x -= 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + app_state.camera.euler.translate.y += 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + app_state.camera.euler.translate.y -= 0.5f * app_state.camera.eulerMouseSensitivity; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + app_state.camera.euler.rotateY -= 0.6f; + app_state.camera.breakEulerTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + app_state.camera.euler.rotateY += 0.6f; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + app_state.camera.euler.rotateX -= 0.6f; + app_state.camera.breakEulerTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + app_state.camera.euler.rotateX += 0.6f; + app_state.camera.breakEulerTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) + cor_gui = true; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !app_state.camera.isOrtho) + app_state.camera.lockZ = !app_state.camera.lockZ; + + if (io.KeyCtrl || io.KeyAlt || io.KeyShift) + return; + + if (ImGui::IsKeyPressed(ImGuiKey_B)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Back); + if (ImGui::IsKeyPressed(ImGuiKey_F)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Front); + if (ImGui::IsKeyPressed(ImGuiKey_I)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Iso); + if (ImGui::IsKeyPressed(ImGuiKey_L)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Left); + if (ImGui::IsKeyPressed(ImGuiKey_R)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Right); + if (ImGui::IsKeyPressed(ImGuiKey_T)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Top); + if (ImGui::IsKeyPressed(ImGuiKey_U)) + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Bottom); + if (ImGui::IsKeyPressed(ImGuiKey_Z)) + { + app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Reset); + app_state.viewer_decimate_point_cloud = kDefaultDecimate; + } + + if (ImGui::IsKeyPressed(ImGuiKey_C, false)) + app_state.compass_ruler = !app_state.compass_ruler; + if (ImGui::IsKeyPressed(ImGuiKey_O, false)) + app_state.camera.isOrtho = !app_state.camera.isOrtho; + if (ImGui::IsKeyPressed(ImGuiKey_X, false)) + app_state.show_axes = !app_state.show_axes; + + if (ImGui::IsKeyPressed(ImGuiKey_1)) + app_state.point_size = 1; + if (ImGui::IsKeyPressed(ImGuiKey_2)) + app_state.point_size = 2; + if (ImGui::IsKeyPressed(ImGuiKey_3)) + app_state.point_size = 3; + if (ImGui::IsKeyPressed(ImGuiKey_4)) + app_state.point_size = 4; + if (ImGui::IsKeyPressed(ImGuiKey_5)) + app_state.point_size = 5; + if (ImGui::IsKeyPressed(ImGuiKey_6)) + app_state.point_size = 6; + if (ImGui::IsKeyPressed(ImGuiKey_7)) + app_state.point_size = 7; + if (ImGui::IsKeyPressed(ImGuiKey_8)) + app_state.point_size = 8; + if (ImGui::IsKeyPressed(ImGuiKey_9)) + app_state.point_size = 9; +} + +void drawMiniCompassWithRuler() +{ + const Eigen::Matrix3f& R = app_state.viewLocal.rotation(); + Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; + Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; + Color rulerColor = + ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); + raylib_widgets::drawCompassRuler( + right, + up, + app_state.camera.euler.translate.z, + rulerColor, + raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" }); +} + +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) +{ + Eigen::Vector3d hit = laser_beam.position; + raylib_widgets::intersectPlane(laser_beam.position, laser_beam.direction, plane.a, plane.b, plane.c, plane.d, hit); + return hit; +} + +LaserBeam GetLaserBeam(int x, int y) +{ + Ray ray = app_state.camera.eulerScreenRay(x, y, GetScreenWidth(), GetScreenHeight()); + + LaserBeam laser_beam; + laser_beam.position = Eigen::Vector3d(ray.position.x, ray.position.y, ray.position.z); + laser_beam.direction = Eigen::Vector3d(ray.direction.x, ray.direction.y, ray.direction.z); + + return laser_beam; +} + +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) +{ + return raylib_widgets::distancePointToLine(point, line.position, line.direction); +} + +void setNewRotationCenter(int x, int y) +{ + const auto laser_beam = GetLaserBeam(x, y); + + RegistrationPlaneFeature::Plane pl; + + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = 0; + Eigen::Vector3f center_eigen = rayIntersection(laser_beam, pl).cast(); + + spdlog::info("Setting new rotation center to: {}, {}, {}", center_eigen.x(), center_eigen.y(), center_eigen.z()); + + app_state.camera.moveEulerRotationCenterTo(Vector3{ center_eigen.x(), center_eigen.y(), center_eigen.z() }); +} + +bool checkClHelp(int argc, char** argv) +{ + for (int i = 1; i < argc; ++i) + { + std::string arg(argv[i]); + + if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") + { + return true; + } + } + return false; +} + +// Was utils.cpp's getClosestTrajectoriesPoint(): picks the trajectory node nearest to the mouse ray +// and moves the rotation center there. Ctrl picks the loop-closure source (and time_stamp_offset), +// Shift the target; with more than two visible sessions it searches all visible ones. +void getClosestTrajectoriesPoint( + std::vector& sessions, + int x, + int y, + const int first_session_index, + const int second_session_index, + const int number_visible_sessions, + int& index_loop_closure_source, + int& index_loop_closure_target, + bool KeyShift, + double& time_stamp_offset) +{ + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + Vector3 center = app_state.camera.eulerGoal.rotationCenter; + + auto visit = [&](int s, bool update_source_target) + { + if (s < 0 || s >= static_cast(sessions.size())) + return; + const auto& pcs = sessions[s].point_clouds_container.point_clouds; + for (size_t i = 0; i < pcs.size(); i++) + { + for (size_t j = 0; j < pcs[i].local_trajectory.size(); j++) + { + Eigen::Vector3d vp = pcs[i].m_pose * pcs[i].local_trajectory[j].m_pose.translation(); + double dist = distance_point_to_line(vp, laser_beam); + if (dist >= min_distance) + continue; + min_distance = dist; + + if (!update_source_target) + { + center = Vector3{ static_cast(vp.x()), static_cast(vp.y()), static_cast(vp.z()) }; + time_stamp_offset = pcs[i].local_trajectory[j].timestamps.first; + } + else if (!KeyShift) // Ctrl + { + center = Vector3{ static_cast(vp.x()), static_cast(vp.y()), static_cast(vp.z()) }; + index_loop_closure_source = static_cast(i); + time_stamp_offset = pcs[i].local_trajectory[j].timestamps.first; + } + else // Shift + { + index_loop_closure_target = static_cast(i); + } + } + } + }; + + if (number_visible_sessions == 1) + visit(first_session_index, true); + else if (number_visible_sessions == 2) + visit(KeyShift ? second_session_index : first_session_index, true); + else + for (size_t s = 0; s < sessions.size(); s++) + if (sessions[s].visible) + visit(static_cast(s), false); + + app_state.camera.moveEulerRotationCenterTo(center); +} + +// Keeps session_renderers parallel to `sessions` and each renderer's GPU buffers in sync with its +// scans' poses. A size mismatch means the session list changed (load/remove), so everything is +// re-uploaded; otherwise only scans whose m_pose changed are rebuilt. +void syncSessionRenderers() +{ + if (session_renderers.size() != sessions.size()) + { + session_renderers.clear(); + bool first = true; + for (const auto& s : sessions) + { + auto renderer = std::make_unique(); + renderer->init(); + renderer->rebuildAll(s.point_clouds_container.point_clouds); + session_renderers.push_back(std::move(renderer)); + + if (s.point_clouds_container.point_clouds.empty()) + continue; + const auto d = s.point_clouds_container.compute_point_cloud_dimension(); + if (first) + scene_dims = d; + scene_dims.x_min = std::min(scene_dims.x_min, d.x_min); + scene_dims.x_max = std::max(scene_dims.x_max, d.x_max); + scene_dims.y_min = std::min(scene_dims.y_min, d.y_min); + scene_dims.y_max = std::max(scene_dims.y_max, d.y_max); + scene_dims.z_min = std::min(scene_dims.z_min, d.z_min); + scene_dims.z_max = std::max(scene_dims.z_max, d.z_max); + first = false; + } + scene_dims.length = scene_dims.x_max - scene_dims.x_min; + scene_dims.width = scene_dims.y_max - scene_dims.y_min; + scene_dims.height = scene_dims.z_max - scene_dims.z_min; + return; + } + + for (size_t i = 0; i < sessions.size(); i++) + session_renderers[i]->syncPoses(sessions[i].point_clouds_container.point_clouds); +} + +// Draws the given session's visible scans (points + trajectories), colored by points_color_mode. +// `only` restricts drawing to scans whose index it accepts (used by loop closure mode). +template +void drawSession(size_t session_index, Pred only) +{ + if (session_index >= sessions.size() || session_index >= session_renderers.size()) + return; + + auto& pcc = sessions[session_index].point_clouds_container; + auto& pcs = pcc.point_clouds; + + std::vector was_visible(pcs.size()); + for (size_t i = 0; i < pcs.size(); i++) + { + was_visible[i] = pcs[i].visible; + pcs[i].visible = pcs[i].visible && only(static_cast(i)); + } + + const auto& rc = app_state.camera.euler.rotationCenter; + session_renderers[session_index]->draw( + pcs, + static_cast(app_state.point_size), + points_color_mode, + static_cast(scene_dims.z_min), + static_cast(scene_dims.z_max), + Eigen::Vector3d(rc.x, rc.y, rc.z), + static_cast(std::max({ scene_dims.length, scene_dims.width, scene_dims.height, 1.0 })), + app_state.viewer_decimate_point_cloud, + pcc.xz_intersection, + pcc.yz_intersection, + pcc.xy_intersection, + static_cast(pcc.intersection_width), + pcc.show_with_initial_pose); + session_renderers[session_index]->drawTrajectories( + pcs, + viewer_reduce_rendered_trajectory, + pcc.show_imu_to_lio_diff, + pcc.xz_intersection, + pcc.yz_intersection, + pcc.xy_intersection, + pcc.show_with_initial_pose, + pcc.imu_to_lio_diff_scale); + + for (size_t i = 0; i < pcs.size(); i++) + pcs[i].visible = was_visible[i]; +} + +void drawSession(size_t session_index) +{ + drawSession( + session_index, + [](int) + { + return true; + }); +} + +// Was PointCloud::render(pose, ...): previews scan `index` of a session at `pose` (points only, from +// the cached GPU buffer), plus its trajectory at its real m_pose, as the GLUT version drew it. +void drawScanAtPose(size_t session_index, int index, const Eigen::Affine3d& pose, const float color[3]) +{ + if (session_index >= sessions.size() || session_index >= session_renderers.size()) + return; + const auto& pcs = sessions[session_index].point_clouds_container.point_clouds; + if (index < 0 || index >= static_cast(pcs.size()) || !pcs[index].visible) + return; + const auto& pc = pcs[index]; + + Color c = ColorFromNormalized(Vector4{ color[0], color[1], color[2], 1.f }); + session_renderers[session_index]->drawCachedWithTransform( + static_cast(index), pose * pc.m_pose.inverse(), c, static_cast(app_state.point_size), false); + + const int stride = std::max(1, viewer_reduce_rendered_trajectory); + rlBegin(RL_LINES); + rlColor3f(color[0], color[1], color[2]); + for (size_t i = stride; i < pc.local_trajectory.size(); i += stride) + { + Eigen::Vector3d a = (pc.m_pose * pc.local_trajectory[i - stride].m_pose).translation(); + Eigen::Vector3d b = (pc.m_pose * pc.local_trajectory[i].m_pose).translation(); + rlVertex3f(static_cast(a.x()), static_cast(a.y()), static_cast(a.z())); + rlVertex3f(static_cast(b.x()), static_cast(b.y()), static_cast(b.z())); + } + rlEnd(); +} + +void vertex(const Eigen::Vector3d& v) +{ + rlVertex3f(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); +} + +// Polyline through every scan pose of a session, colored per scan (was a GL_LINE_STRIP). +void drawPosePolyline(const Session& session) +{ + const auto& pcs = session.point_clouds_container.point_clouds; + rlBegin(RL_LINES); + for (size_t i = 1; i < pcs.size(); i++) + { + rlColor3f(pcs[i - 1].render_color[0], pcs[i - 1].render_color[1], pcs[i - 1].render_color[2]); + vertex(pcs[i - 1].m_pose.translation()); + rlColor3f(pcs[i].render_color[0], pcs[i].render_color[1], pcs[i].render_color[2]); + vertex(pcs[i].m_pose.translation()); + } + rlEnd(); +} + +// Edge line between two poses plus a 10 m vertical flagpole at its midpoint (label drawn in the 2D pass). +void drawEdge(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, float r, float g, float b) +{ + const Eigen::Vector3d mid = (v1 + v2) * 0.5; + rlBegin(RL_LINES); + rlColor3f(r, g, b); + vertex(v1); + vertex(v2); + vertex(mid); + vertex(mid + Eigen::Vector3d(0, 0, 10)); + rlEnd(); +} + +bool validScan(int session_index, int scan_index) +{ + return session_index >= 0 && session_index < static_cast(sessions.size()) && scan_index >= 0 && + scan_index < static_cast(sessions[session_index].point_clouds_container.point_clouds.size()); +} + +void drawUncertaintyEllipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Color color) +{ + Eigen::LLT> cholSolver(covar); + Eigen::Matrix3d transform = cholSolver.matrixL(); + + const double pi = 3.141592; + const double di = 0.02; + const double dj = 0.04; + const double du = di * 2 * pi; + const double dv = dj * pi; + + rlBegin(RL_LINES); + rlColor4ub(color.r, color.g, color.b, color.a); + for (double i = 0; i < 1.0; i += di) + { + for (double j = 0; j < 1.0; j += dj) + { + double u = i * 2 * pi; + double v = (j - 0.5) * pi; + + const Eigen::Vector3d tp0 = transform * Eigen::Vector3d(cos(v) * cos(u), cos(v) * sin(u), sin(v)) + mean; + const Eigen::Vector3d tp1 = transform * Eigen::Vector3d(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)) + mean; + const Eigen::Vector3d tp2 = + transform * Eigen::Vector3d(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)) + mean; + const Eigen::Vector3d tp3 = transform * Eigen::Vector3d(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)) + mean; + + vertex(tp0); + vertex(tp1); + vertex(tp1); + vertex(tp2); + vertex(tp2); + vertex(tp3); + vertex(tp3); + vertex(tp0); + } + } + rlEnd(); +} + +// Was GroundControlPoints::render() (legacy GL in core); same drawing as step 2's port. Labels are +// drawn in the 2D pass. +void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container) +{ + const Color markColor{ 179, 77, 128, 255 }; + const Color connectorColor{ 0, 77, 153, 255 }; + + for (const auto& gcp : ground_control_points.gpcs) + { + if (gcp.index_to_node_inner < 0 || static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) + continue; + const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner]; + if (gcp.index_to_node_outer < 0 || static_cast(gcp.index_to_node_outer) >= pc.local_trajectory.size()) + continue; + + Eigen::Vector3d c = pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation(); + float h = static_cast(gcp.lidar_height_above_ground); + Vector3 g{ static_cast(gcp.x), static_cast(gcp.y), static_cast(gcp.z) }; + + DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); + DrawLine3D(Vector3{ g.x - 0.01f, g.y, g.z + h }, Vector3{ g.x + 0.01f, g.y, g.z + h }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.01f, g.z + h }, Vector3{ g.x, g.y + 0.01f, g.z + h }, markColor); + DrawLine3D(g, Vector3{ g.x, g.y, g.z + h }, markColor); + DrawLine3D( + Vector3{ static_cast(c.x()), static_cast(c.y()), static_cast(c.z()) }, + Vector3{ g.x, g.y, g.z + h }, + connectorColor); + + if (ground_control_points.draw_uncertainty) + { + Eigen::Matrix3d covar = Eigen::Matrix3d::Zero(); + covar(0, 0) = gcp.sigma_x * gcp.sigma_x; + covar(1, 1) = gcp.sigma_y * gcp.sigma_y; + covar(2, 2) = gcp.sigma_z * gcp.sigma_z; + drawUncertaintyEllipse(covar, Eigen::Vector3d(gcp.x, gcp.y, gcp.z + h), GRAY); + } + } +} + +// Was ControlPoints::render(pcs, show_pc = false) (legacy GL in core): markers only; step 3 never +// opens the control points editor, so step 2's editor branch is not needed. +void renderControlPoints(const ControlPoints& control_points, const PointClouds& point_clouds_container) +{ + const Color markColor{ 179, 77, 128, 255 }; + const Color connectorColor{ 0, 77, 153, 255 }; + const auto& pcs = point_clouds_container.point_clouds; + + for (const auto& cp : control_points.cps) + { + if (cp.index_to_pose < 0 || static_cast(cp.index_to_pose) >= pcs.size()) + continue; + + Eigen::Vector3d c = pcs[cp.index_to_pose].m_pose * Eigen::Vector3d(cp.x_source_local, cp.y_source_local, cp.z_source_local); + Vector3 g{ static_cast(cp.x_target_global), static_cast(cp.y_target_global), static_cast(cp.z_target_global) }; + + DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); + DrawLine3D(Vector3{ g.x - 0.01f, g.y, g.z }, Vector3{ g.x + 0.01f, g.y, g.z }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.01f, g.z }, Vector3{ g.x, g.y + 0.01f, g.z }, markColor); + DrawLine3D(Vector3{ static_cast(c.x()), static_cast(c.y()), static_cast(c.z()) }, g, connectorColor); + + if (control_points.draw_uncertainty) + { + Eigen::Matrix3d covar = Eigen::Matrix3d::Zero(); + covar(0, 0) = cp.is_z_0 ? 0.01 * 0.01 : cp.sigma_x * cp.sigma_x; + covar(1, 1) = cp.is_z_0 ? 0.01 * 0.01 : cp.sigma_y * cp.sigma_y; + covar(2, 2) = cp.sigma_z * cp.sigma_z; + drawUncertaintyEllipse(covar, Eigen::Vector3d(cp.x_target_global, cp.y_target_global, cp.z_target_global), GRAY); + } + } +} + +namespace +{ + // Outlined so labels stay readable over same-colored geometry; `line` stacks labels above one anchor. + void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color, int line = 0) + { + int x = static_cast(anchor.x) + 6; + int y = static_cast(anchor.y) - fontSize - 6 - line * (fontSize + 4); + for (int dx = -1; dx <= 1; ++dx) + for (int dy = -1; dy <= 1; ++dy) + if (dx != 0 || dy != 0) + DrawText(text, x + dx, y + dy, fontSize, BLACK); + DrawText(text, x, y, fontSize, color); + } + + // Projects a world point with frame_mvp_3d (needs w for the perspective divide, so not Vector3Transform). + Vector2 worldToScreen(const Eigen::Vector3d& world) + { + const ImGuiIO& io = ImGui::GetIO(); + const Matrix& m = frame_mvp_3d; + float x = static_cast(world.x()); + float y = static_cast(world.y()); + float z = static_cast(world.z()); + float clipX = m.m0 * x + m.m4 * y + m.m8 * z + m.m12; + float clipY = m.m1 * x + m.m5 * y + m.m9 * z + m.m13; + float clipW = m.m3 * x + m.m7 * y + m.m11 * z + m.m15; + if (clipW < 1e-6f) // behind the camera, or degenerate + return Vector2{ -1000.f, -1000.f }; + float ndcX = clipX / clipW; + float ndcY = clipY / clipW; + return Vector2{ (ndcX * 0.5f + 0.5f) * io.DisplaySize.x, (1.0f - (ndcY * 0.5f + 0.5f)) * io.DisplaySize.y }; + } + + Color colorOf(const float c[3]) + { + return ColorFromNormalized(Vector4{ c[0], c[1], c[2], 1.f }); + } +} // namespace + +void renderGroundControlPointsLabels(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container) +{ + const Color markColor{ 179, 77, 128, 255 }; + const Color connectorColor{ 0, 77, 153, 255 }; + + for (size_t i = 0; i < ground_control_points.gpcs.size(); ++i) + { + const auto& gcp = ground_control_points.gpcs[i]; + Vector2 anchor = worldToScreen(Eigen::Vector3d(gcp.x, gcp.y, gcp.z)); + drawOutlinedText(gcp.name, anchor, 22, WHITE, 2); + drawOutlinedText(TextFormat("GCP_%d: LiDAR center", static_cast(i)), anchor, 14, markColor, 1); + drawOutlinedText(TextFormat("GCP_%d: 'plane on the ground'", static_cast(i)), anchor, 14, markColor, 0); + + if (gcp.index_to_node_inner < 0 || static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) + continue; + const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner]; + if (gcp.index_to_node_outer < 0 || static_cast(gcp.index_to_node_outer) >= pc.local_trajectory.size()) + continue; + + Eigen::Vector3d c = pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation(); + drawOutlinedText(TextFormat("GCP_%d: assigned trajectory node", static_cast(i)), worldToScreen(c), 14, connectorColor); + } +} + +void renderControlPointsLabels(const ControlPoints& control_points, const PointClouds& point_clouds_container) +{ + const Color markColor{ 179, 77, 128, 255 }; + + for (size_t i = 0; i < control_points.cps.size(); ++i) + { + const auto& cp = control_points.cps[i]; + Vector2 anchor = worldToScreen(Eigen::Vector3d(cp.x_target_global, cp.y_target_global, cp.z_target_global)); + drawOutlinedText(cp.name, anchor, 22, WHITE, 1); + drawOutlinedText(TextFormat("CP_%d", static_cast(i)), anchor, 14, WHITE, 0); + + if (cp.index_to_pose < 0 || static_cast(cp.index_to_pose) >= point_clouds_container.point_clouds.size()) + continue; + + Eigen::Vector3d c = point_clouds_container.point_clouds[cp.index_to_pose].m_pose * + Eigen::Vector3d(cp.x_source_local, cp.y_source_local, cp.z_source_local); + drawOutlinedText(TextFormat("CP_%d: initial location", static_cast(i)), worldToScreen(c), 14, markColor); + } +} + +// Was the glRasterPos3f + glutBitmapString labels of loop closure mode: scan indices of the first and +// second session (in scan color), per-session pose graph edges (blue) and inter-session edges +// (cyan if a ground truth session is involved, otherwise yellow), at the top of each edge's flagpole. +void renderLoopClosureLabels() +{ + for (int s : { first_session_index, second_session_index }) + { + if (s < 0 || s >= static_cast(sessions.size())) + continue; + const auto& pcs = sessions[s].point_clouds_container.point_clouds; + for (size_t i = 0; i < pcs.size(); i++) + drawOutlinedText( + TextFormat("%d", static_cast(i)), + worldToScreen(pcs[i].m_pose.translation() + Eigen::Vector3d(0, 0, 0.1)), + 20, + colorOf(pcs[i].render_color)); + if (first_session_index == second_session_index) + break; + } + + for (size_t i = 0; i < sessions.size(); i++) + { + const auto& pcs = sessions[i].point_clouds_container.point_clouds; + const auto& pg_edges = sessions[i].pose_graph_loop_closure.edges; + for (size_t j = 0; j < pg_edges.size(); j++) + { + if (!validScan(static_cast(i), pg_edges[j].index_from) || !validScan(static_cast(i), pg_edges[j].index_to)) + continue; + Eigen::Vector3d mid = (pcs[pg_edges[j].index_from].m_pose.translation() + pcs[pg_edges[j].index_to].m_pose.translation()) * 0.5; + drawOutlinedText(TextFormat("%d", static_cast(j)), worldToScreen(mid + Eigen::Vector3d(0, 0, 10.1)), 22, BLUE); + } + } + + for (size_t i = 0; i < edges.size(); i++) + { + const auto& e = edges[i]; + if (!validScan(e.index_session_from, e.index_from) || !validScan(e.index_session_to, e.index_to)) + continue; + Eigen::Vector3d v1 = sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.translation(); + Eigen::Vector3d v2 = sessions[e.index_session_to].point_clouds_container.point_clouds[e.index_to].m_pose.translation(); + bool gt = sessions[e.index_session_from].is_ground_truth || sessions[e.index_session_to].is_ground_truth; + drawOutlinedText( + TextFormat("%d", static_cast(i)), worldToScreen((v1 + v2) * 0.5 + Eigen::Vector3d(0, 0, 10.1)), 22, gt ? SKYBLUE : YELLOW); + } +} + +// Copies the current rlgl modelview/projection into column-major float[16] for ImGuizmo. +void currentGizmoMatrices(float modelview[16], float projection[16]) +{ + Matrix p = rlGetMatrixProjection(); + Matrix m = rlGetMatrixModelview(); + const float pv[16] = { p.m0, p.m1, p.m2, p.m3, p.m4, p.m5, p.m6, p.m7, p.m8, p.m9, p.m10, p.m11, p.m12, p.m13, p.m14, p.m15 }; + const float mv[16] = { m.m0, m.m1, m.m2, m.m3, m.m4, m.m5, m.m6, m.m7, m.m8, m.m9, m.m10, m.m11, m.m12, m.m13, m.m14, m.m15 }; + std::copy(pv, pv + 16, projection); + std::copy(mv, mv + 16, modelview); +} + +// ImGuizmo on m_gizmo with this app's usual operation sets: full 3D in perspective, planar in ortho. +void manipulateGizmo() +{ + if (!app_state.camera.isOrtho) + { + float modelview[16], projection[16]; + currentGizmoMatrices(modelview, projection); + ImGuizmo::Manipulate( + modelview, + projection, + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); + } + else + ImGuizmo::Manipulate( + app_state.camera.orthoGizmoView, + app_state.camera.orthoProjection, + ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, + ImGuizmo::WORLD, + m_gizmo, + NULL); +} + +/////////////////////////////////////////////////////////////////////////////////// + void ndt_gui() { static bool compute_mean_and_cov_for_bucket = false; @@ -383,9 +1421,9 @@ void loop_closure_gui() // auto point_cloud_upper = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1; - ImGui::InputInt("gui_point_size", &gui_point_size); - if (gui_point_size < 1) - gui_point_size = 1; + ImGui::InputInt("gui_point_size", &app_state.point_size); + if (app_state.point_size < 1) + app_state.point_size = 1; ImGui::Text("Num edge extended:"); @@ -1796,7 +2834,7 @@ bool loadProject(const std::string& file_name, ProjectSettings& _project_setting } std::string newTitle = winTitle + " - " + truncPath(file_name); - glutSetWindowTitle(newTitle.c_str()); + SetWindowTitle(newTitle.c_str()); loaded_sessions = false; time_stamp_offset = 0.0; @@ -1824,7 +2862,7 @@ void saveProject() if (save_project_settings(fs::path(output_file_name).string(), project_settings)) { std::string newTitle = winTitle + " - " + truncPath(output_file_name); - glutSetWindowTitle(newTitle.c_str()); + SetWindowTitle(newTitle.c_str()); } } @@ -2538,494 +3576,197 @@ void settings_gui() } void display() -{ - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - - glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); +{ + syncSessionRenderers(); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); + ImGuiIO& io = ImGui::GetIO(); + // Framebuffer pixels, not io.DisplaySize (they differ on HiDPI) -- see step 2's display(). + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); + + ClearBackground(ColorFromNormalized( + Vector4{ app_state.bg_color.x * app_state.bg_color.w, + app_state.bg_color.y * app_state.bg_color.w, + app_state.bg_color.z * app_state.bg_color.w, + app_state.bg_color.w })); + rlEnableDepthTest(); + + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - updateCameraTransition(); + auto& camera = app_state.camera; + camera.updateEulerTransition(io.DeltaTime); - viewLocal = Eigen::Affine3f::Identity(); - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - pc.point_size = gui_point_size; - } - } + app_state.viewLocal = Eigen::Affine3f::Identity(); - if (!is_ortho) + if (!camera.isOrtho) { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - glTranslatef(translate_x, translate_y, translate_z); + camera.applyPerspectiveProjection((int)io.DisplaySize.x, (int)io.DisplaySize.y); - // janusz - if (is_loop_closure_gui) + // In loop closure mode the rotation center follows the source scan / active edge (when enabled). + if (is_loop_closure_gui && update_rotation_center) { - // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, - // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, - // false, false, 100000); - // sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false, - // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, - // false, false, 100000); - - if (first_session_index < sessions[first_session_index].point_clouds_container.point_clouds.size()) - { - if (update_rotation_center) - { - rotation_center.x() = sessions[first_session_index] - .point_clouds_container.point_clouds[index_loop_closure_source] - .m_pose.translation() - .x(); - rotation_center.y() = sessions[first_session_index] - .point_clouds_container.point_clouds[index_loop_closure_source] - .m_pose.translation() - .y(); - rotation_center.z() = sessions[first_session_index] - .point_clouds_container.point_clouds[index_loop_closure_source] - .m_pose.translation() - .z(); - } - } - - if (manipulate_active_edge) + auto follow = [&](const Eigen::Vector3d& t) { - if (edges.size() > 0) - { - int index_src = edges[index_active_edge].index_from; - Eigen::Affine3d m_src = - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + camera.euler.rotationCenter = Vector3{ static_cast(t.x()), static_cast(t.y()), static_cast(t.z()) }; + camera.eulerGoal.rotationCenter = camera.euler.rotationCenter; + }; - if (update_rotation_center) - { - rotation_center.x() = m_src(0, 3); - rotation_center.y() = m_src(1, 3); - rotation_center.z() = m_src(2, 3); - } - } - } + if (validScan(first_session_index, index_loop_closure_source)) + follow(sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation()); - /*if (session.pose_graph_loop_closure.manipulate_active_edge) + if (manipulate_active_edge && index_active_edge >= 0 && index_active_edge < static_cast(edges.size())) { - if (session.pose_graph_loop_closure.edges.size() > 0) - { - if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) - { - rotation_center.x() = - session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(0, - 3); rotation_center.y() = - session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(1, - 3); rotation_center.z() = - session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(2, - 3); - } - } - }*/ + const auto& e = edges[index_active_edge]; + if (validScan(e.index_session_from, e.index_from)) + follow(sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.translation()); + } } - viewLocal.translate(rotation_center); - - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - if (!lock_z) - viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + Eigen::Vector3f rotationCenter(camera.euler.rotationCenter.x, camera.euler.rotationCenter.y, camera.euler.rotationCenter.z); + app_state.viewLocal.translate(rotationCenter); + app_state.viewLocal.translate(Eigen::Vector3f(camera.euler.translate.x, camera.euler.translate.y, camera.euler.translate.z)); + if (!camera.lockZ) + app_state.viewLocal.rotate(Eigen::AngleAxisf(camera.euler.rotateX * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else - viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + app_state.viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + app_state.viewLocal.rotate(Eigen::AngleAxisf(camera.euler.rotateY * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + app_state.viewLocal.translate(-rotationCenter); - viewLocal.translate(-rotation_center); - - glLoadMatrixf(viewLocal.matrix().data()); + rlMultMatrixf(app_state.viewLocal.matrix().data()); } else - updateOrthoView(); + { + app_state.viewLocal.rotate(Eigen::AngleAxisf((camera.euler.rotateX + camera.euler.rotateY) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + camera.updateOrtho(ratio); + } + + camera.captureFrameMatrices(); + frame_mvp_3d = MatrixMultiply(camera.frameView3D, camera.frameProj3D); showAxes(); if (is_loop_closure_gui) { - if (manipulate_active_edge) + // Scans within [index - before, index + after] of the loop closure source/target. + auto in_range = [](int center) { - if (edges.size() > 0) + return [center](int i) { - /*int index_src = edges[index_active_edge].index_from; - int index_trg = edges[index_active_edge].index_to; - - Eigen::Affine3d m_src = - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; - Eigen::Affine3d m_trg = m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render( - m_src, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render_color); - sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render( - m_trg, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render_color);*/ - - int index_src = edges[index_active_edge].index_from; - int index_trg = edges[index_active_edge].index_to; - - Eigen::Affine3d _m_src = - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; - Eigen::Affine3d _m_trg = _m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - Eigen::Affine3d m_src_0 = - sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).m_pose; // Todo - - for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; - i++) - { - if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && - sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) - { - // ObservationPicking observation_picking; - // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, - // false); - - Eigen::Affine3d m_src_curr = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo - Eigen::Affine3d m_src = _m_src * (m_src_0.inverse() * m_src_curr); - - // sessions[first_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; - - sessions[first_session_index].point_clouds_container.point_clouds.at(i).render( - m_src, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(i).render_color); - } - } + return i >= center - num_edge_extended_before && i <= center + num_edge_extended_after; + }; + }; - Eigen::Affine3d m_trg_0 = - sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).m_pose; // Todo + const bool edge_ok = manipulate_active_edge && index_active_edge >= 0 && index_active_edge < static_cast(edges.size()) && + validScan(edges[index_active_edge].index_session_from, edges[index_active_edge].index_from); - for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; - i++) - { - if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && - sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) - { - // ObservationPicking observation_picking; - // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, - // false); - Eigen::Affine3d m_trg_curr = - sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo - Eigen::Affine3d m_trg = _m_trg * (m_trg_0.inverse() * m_trg_curr); - - // sessions[second_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; - sessions[second_session_index].point_clouds_container.point_clouds.at(i).render( - m_trg, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(i).render_color); - } - } - } - } - else + if (edge_ok && validScan(first_session_index, index_loop_closure_source) && + validScan(second_session_index, index_loop_closure_target)) { - ObservationPicking observation_picking; - - /*sessions[first_session_index] - .point_clouds_container.point_clouds.at(index_loop_closure_source) - .render( - false, - observation_picking, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - false, - false, - false, - 100000, - false);*/ - + // Preview the active edge: the source range placed at the edge's source pose, the target range + // at source * relative_pose. Like the GLUT version, the ranges are taken around + // index_loop_closure_source/target of the first/second visible session. + const auto& e = edges[index_active_edge]; + Eigen::Affine3d edge_src = sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose; + Eigen::Affine3d edge_trg = edge_src * affine_matrix_from_pose_tait_bryan(e.relative_pose_tb); + + const auto& first_pcs = sessions[first_session_index].point_clouds_container.point_clouds; + Eigen::Affine3d src_0 = first_pcs[index_loop_closure_source].m_pose; for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; i++) - { - if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && - sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) - { - // ObservationPicking observation_picking; - // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, - // false); - Eigen::Affine3d m_src = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; - - sessions[first_session_index].point_clouds_container.point_clouds.at(i).render( - false, - observation_picking, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - false, - false, - false, - 100000, - false); - } - } - - /*sessions[second_session_index] - .point_clouds_container.point_clouds.at(index_loop_closure_target) - .render( - false, - observation_picking, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - false, - false, - false, - 100000, - false);*/ + if (i >= 0 && i < static_cast(first_pcs.size())) + drawScanAtPose(first_session_index, i, edge_src * (src_0.inverse() * first_pcs[i].m_pose), first_pcs[i].render_color); + const auto& second_pcs = sessions[second_session_index].point_clouds_container.point_clouds; + Eigen::Affine3d trg_0 = second_pcs[index_loop_closure_target].m_pose; for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; i++) - { - if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && - sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) - { - // ObservationPicking observation_picking; - // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, - // false); - Eigen::Affine3d m_src = sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; - - sessions[second_session_index].point_clouds_container.point_clouds.at(i).render( - false, - observation_picking, - viewer_decimate_point_cloud, - viewer_reduce_rendered_trajectory, - false, - false, - false, - 100000, - false); - } - } - } - - // sessions[first_session_index].point_clouds_container.render(); - - glBegin(GL_LINE_STRIP); - for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) - { - glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); - glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); - } - glEnd(); - - int i = 0; - for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) - { - glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); - glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); - i++; + if (i >= 0 && i < static_cast(second_pcs.size())) + drawScanAtPose( + second_session_index, i, edge_trg * (trg_0.inverse() * second_pcs[i].m_pose), second_pcs[i].render_color); } - - glBegin(GL_LINE_STRIP); - for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) + else if (!manipulate_active_edge) { - glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); - glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + if (first_session_index >= 0) + drawSession(first_session_index, in_range(index_loop_closure_source)); + if (second_session_index >= 0 && second_session_index != first_session_index) + drawSession(second_session_index, in_range(index_loop_closure_target)); + else if (second_session_index >= 0) + drawSession( + second_session_index, + [&](int i) + { + return in_range(index_loop_closure_source)(i) || in_range(index_loop_closure_target)(i); + }); } - glEnd(); - i = 0; - for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) - { - glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); - glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); - i++; - } + for (int s : { first_session_index, second_session_index }) + if (s >= 0 && s < static_cast(sessions.size())) + drawPosePolyline(sessions[s]); for (size_t i = 0; i < sessions.size(); i++) { - for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) - { - int index_src = sessions[i].pose_graph_loop_closure.edges[j].index_from; - int index_trg = sessions[i].pose_graph_loop_closure.edges[j].index_to; - - glColor3f(0.0f, 0.0f, 1.0f); - glBegin(GL_LINES); - auto v1 = sessions[i].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); - auto v2 = sessions[i].point_clouds_container.point_clouds.at(index_trg).m_pose.translation(); - glVertex3f(v1.x(), v1.y(), v1.z()); - glVertex3f(v2.x(), v2.y(), v2.z()); - - glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5); - glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10); - glEnd(); - - glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(j).c_str()); - } + const auto& pcs = sessions[i].point_clouds_container.point_clouds; + for (const auto& pg_edge : sessions[i].pose_graph_loop_closure.edges) + if (validScan(static_cast(i), pg_edge.index_from) && validScan(static_cast(i), pg_edge.index_to)) + drawEdge(pcs[pg_edge.index_from].m_pose.translation(), pcs[pg_edge.index_to].m_pose.translation(), 0.f, 0.f, 1.f); } - for (size_t i = 0; i < edges.size(); i++) + for (const auto& e : edges) { - int index_src = edges[i].index_from; - int index_trg = edges[i].index_to; - - int index_session_from = edges[i].index_session_from; - int index_session_to = edges[i].index_session_to; - - if (sessions[index_session_from].is_ground_truth || sessions[index_session_to].is_ground_truth) - glColor3f(0.0f, 1.0f, 1.0f); - else - glColor3f(1.0f, 1.0f, 0.0f); - - glBegin(GL_LINES); - auto v1 = sessions[index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); - auto v2 = sessions[index_session_to].point_clouds_container.point_clouds.at(index_trg).m_pose.translation(); - glVertex3f(v1.x(), v1.y(), v1.z()); - glVertex3f(v2.x(), v2.y(), v2.z()); - - glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5); - glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10); - glEnd(); - - glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); + if (!validScan(e.index_session_from, e.index_from) || !validScan(e.index_session_to, e.index_to)) + continue; + bool gt = sessions[e.index_session_from].is_ground_truth || sessions[e.index_session_to].is_ground_truth; + drawEdge( + sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.translation(), + sessions[e.index_session_to].point_clouds_container.point_clouds[e.index_to].m_pose.translation(), + gt ? 0.f : 1.f, // cyan with a ground truth session, otherwise yellow + 1.f, + gt ? 1.f : 0.f); } } else { - for (auto& session : sessions) + for (size_t s = 0; s < sessions.size(); s++) { - if (session.visible) + auto& session = sessions[s]; + if (!session.visible) + continue; + + drawSession(s); + renderGroundControlPoints(session.ground_control_points, session.point_clouds_container); + renderControlPoints(session.control_points, session.point_clouds_container); + + // +-5 m cross at the session's first trajectory node after time_stamp_offset. + const auto& pcs = session.point_clouds_container.point_clouds; + bool found = false; + for (size_t a = 0; a < pcs.size() && !found; a++) { - session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory); - session.ground_control_points.render(session.point_clouds_container); - session.control_points.render(session.point_clouds_container, false); - - //// - int index_point_clouds = -1; - int index_local_trajectory = -1; - bool found = false; - for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++) - { - for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) - { - if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) - { - if (!found) - { - found = true; - index_point_clouds = a; - index_local_trajectory = b; - break; - } - } - } - } - - if (index_point_clouds != -1 && index_local_trajectory != -1) + for (size_t b = 0; b < pcs[a].local_trajectory.size(); b++) { - if (index_local_trajectory < session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory.size()) + if (pcs[a].local_trajectory[b].timestamps.first > time_stamp_offset) { - glColor3f( - session.point_clouds_container.point_clouds[index_point_clouds].render_color[0], - session.point_clouds_container.point_clouds[index_point_clouds].render_color[1], - session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]); - glBegin(GL_LINES); - - auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose; - auto m2 = - session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; - - auto v1 = (m1 * m2).translation(); - - glVertex3f(v1.x() - 5.0, v1.y(), v1.z()); - glVertex3f(v1.x() + 5.0, v1.y(), v1.z()); - - glVertex3f(v1.x(), v1.y() - 5.0, v1.z()); - glVertex3f(v1.x(), v1.y() + 5.0, v1.z()); - - glVertex3f(v1.x(), v1.y(), v1.z() - 5.0); - glVertex3f(v1.x(), v1.y(), v1.z() + 5.0); - - glEnd(); + found = true; + Eigen::Vector3d v1 = (pcs[a].m_pose * pcs[a].local_trajectory[b].m_pose).translation(); + rlBegin(RL_LINES); + rlColor3f(pcs[a].render_color[0], pcs[a].render_color[1], pcs[a].render_color[2]); + vertex(v1 - Eigen::Vector3d(5, 0, 0)); + vertex(v1 + Eigen::Vector3d(5, 0, 0)); + vertex(v1 - Eigen::Vector3d(0, 5, 0)); + vertex(v1 + Eigen::Vector3d(0, 5, 0)); + vertex(v1 - Eigen::Vector3d(0, 0, 5)); + vertex(v1 + Eigen::Vector3d(0, 0, 5)); + rlEnd(); + break; } } } } } - /*if (is_loop_closure_gui) - { - session.manual_pose_graph_loop_closure.Render(session.point_clouds_container, index_loop_closure_source, index_loop_closure_target); - } - else - { - for (const auto &g : available_geo_points) - { - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - auto c = g.coordinates - session.point_clouds_container.offset; - glVertex3f(c.x() - 0.5, c.y(), c.z()); - glVertex3f(c.x() + 0.5, c.y(), c.z()); - - glVertex3f(c.x(), c.y() - 0.5, c.z()); - glVertex3f(c.x(), c.y() + 0.5, c.z()); - - glVertex3f(c.x(), c.y(), c.z() - 0.5); - glVertex3f(c.x(), c.y(), c.z() + 0.5); - glEnd(); - } - - // - for (const auto &pc : session.point_clouds_container.point_clouds) - { - for (const auto &gp : pc.available_geo_points) - { - if (gp.choosen) - { - auto c = pc.m_pose * gp.coordinates; - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(c.x() - 0.5, c.y(), c.z()); - glVertex3f(c.x() + 0.5, c.y(), c.z()); - - glVertex3f(c.x(), c.y() - 0.5, c.z()); - glVertex3f(c.x(), c.y() + 0.5, c.z()); - - glVertex3f(c.x(), c.y(), c.z() - 0.5); - glVertex3f(c.x(), c.y(), c.z() + 0.5); - glEnd(); - - glBegin(GL_LINES); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(c.x(), c.y(), c.z()); - glVertex3f(gp.coordinates.x(), gp.coordinates.y(), gp.coordinates.z()); - glEnd(); - - glColor3f(0.0f, 0.0f, 0.0f); - glBegin(GL_LINES); - glVertex3f(c.x(), c.y(), c.z()); - glVertex3f(c.x() + 10, c.y(), c.z()); - glEnd(); - - glRasterPos3f(c.x() + 10, c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str()); - } - } - } - }*/ - - // gnss.render(session.point_clouds_container); - - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + // rlImGuiBegin() only feeds input to ImGui and starts its frame; it leaves the rlgl 3D matrices + // active, so the gizmo code below still sees this frame's camera. + rlImGuiBegin(); ShowMainDockSpace(); @@ -3052,30 +3793,7 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate( - modelview, - projection, - ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, - ImGuizmo::WORLD, - m_gizmo, - NULL); - } - else - ImGuizmo::Manipulate( - m_ortho_gizmo_view, - m_ortho_projection, - ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, - ImGuizmo::WORLD, - m_gizmo, - NULL); + manipulateGizmo(); sessions[i].point_clouds_container.point_clouds[0].m_pose = Eigen::Map(m_gizmo).cast(); prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose; @@ -3190,30 +3908,7 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate( - &modelview[0], - &projection[0], - ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, - ImGuizmo::WORLD, - m_gizmo, - NULL); - } - else - ImGuizmo::Manipulate( - m_ortho_gizmo_view, - m_ortho_projection, - ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, - ImGuizmo::WORLD, - m_gizmo, - NULL); + manipulateGizmo(); Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); @@ -3227,215 +3922,6 @@ void display() } } - /*if (!is_loop_closure_gui) -{ - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - if (session.point_clouds_container.point_clouds[i].gizmo) - { - std::vector all_m_poses; - for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) - all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); - - ImGuiIO &io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | -ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | -ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - - session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; - session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; - session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; - session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; - session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; - session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; - session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; - session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; - session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; - session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; - session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; - session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; - session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; - session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; - session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; - session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; - session.point_clouds_container.point_clouds[i].pose = -pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); - - session.point_clouds_container.point_clouds[i].gui_translation[0] = -(float)session.point_clouds_container.point_clouds[i].pose.px; session.point_clouds_container.point_clouds[i].gui_translation[1] = -(float)session.point_clouds_container.point_clouds[i].pose.py; session.point_clouds_container.point_clouds[i].gui_translation[2] = -(float)session.point_clouds_container.point_clouds[i].pose.pz; - - session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om -* RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[1] = -(float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[2] -= (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); - - if (!manipulate_only_marked_gizmo) - { - Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; - for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - session.point_clouds_container.point_clouds[j].pose = -pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); - - session.point_clouds_container.point_clouds[j].gui_translation[0] = -(float)session.point_clouds_container.point_clouds[j].pose.px; session.point_clouds_container.point_clouds[j].gui_translation[1] = -(float)session.point_clouds_container.point_clouds[j].pose.py; session.point_clouds_container.point_clouds[j].gui_translation[2] = -(float)session.point_clouds_container.point_clouds[j].pose.pz; - - session.point_clouds_container.point_clouds[j].gui_rotation[0] = -(float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); session.point_clouds_container.point_clouds[j].gui_rotation[1] -= (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[j].gui_rotation[2] = -(float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); - } - } - } - } - - session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud); - observation_picking.render(); - - glPushAttrib(GL_ALL_ATTRIB_BITS); - glPointSize(5); - for (const auto &obs : observation_picking.observations) - { - for (const auto &[key1, value1] : obs) - { - for (const auto &[key2, value2] : obs) - { - if (key1 != key2) - { - Eigen::Vector3d p1, p2; - if (session.point_clouds_container.show_with_initial_pose) - { - p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; - } - else - { - p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; - } - glColor3f(0, 1, 0); - glBegin(GL_POINTS); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - glColor3f(1, 0, 0); - glBegin(GL_LINES); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - } - } - } - } - glPopAttrib(); - - for (const auto &obs : observation_picking.observations) - { - Eigen::Vector3d mean(0, 0, 0); - int counter = 0; - for (const auto &[key1, value1] : obs) - { - mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - counter++; - } - if (counter > 0) - { - mean /= counter; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glEnd();F - } - } - - glColor3f(1, 0, 1); - glBegin(GL_POINTS); - for (auto p : picked_points) - { - glVertex3f(p.x(), p.y(), p.z()); - } - glEnd(); -} -else -{ - // ImGuizmo ----------------------------------------------- - if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0) - { - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | -ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, -ImGuizmo::WORLD, m_gizmo, NULL); - - Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); - - m_g(0, 0) = m_gizmo[0]; - m_g(1, 0) = m_gizmo[1]; - m_g(2, 0) = m_gizmo[2]; - m_g(3, 0) = m_gizmo[3]; - m_g(0, 1) = m_gizmo[4]; - m_g(1, 1) = m_gizmo[5]; - m_g(2, 1) = m_gizmo[6]; - m_g(3, 1) = m_gizmo[7]; - m_g(0, 2) = m_gizmo[8]; - m_g(1, 2) = m_gizmo[9]; - m_g(2, 2) = m_gizmo[10]; - m_g(3, 2) = m_gizmo[11]; - m_g(0, 3) = m_gizmo[12]; - m_g(1, 3) = m_gizmo[13]; - m_g(2, 3) = m_gizmo[14]; - m_g(3, 3) = m_gizmo[15]; - - const int &index_src = -session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from; - - const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; - session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb = -pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); - } -}*/ - view_kbd_shortcuts(); if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) @@ -4041,45 +4527,38 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); { ImGui::BeginDisabled(!(sessions.size() > 0)); { - auto tmp = point_size; + auto tmp = app_state.point_size; ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points size", &point_size); + ImGui::InputInt("Points size", &app_state.point_size); if (ImGui::IsItemHovered()) ImGui::SetTooltip("keyboard 1-9 keys"); - if (point_size < 1) - point_size = 1; - else if (point_size > 10) - point_size = 10; + if (app_state.point_size < 1) + app_state.point_size = 1; + else if (app_state.point_size > 10) + app_state.point_size = 10; - if (tmp != point_size) + if (tmp != app_state.point_size) for (auto& session : sessions) for (auto& point_cloud : session.point_clouds_container.point_clouds) - point_cloud.point_size = point_size; + point_cloud.point_size = app_state.point_size; ImGui::Separator(); } ImGui::EndDisabled(); - if (ImGui::MenuItem("Orthographic", "key O", &is_ortho)) + if (ImGui::MenuItem("Orthographic", "key O", &app_state.camera.isOrtho)) { - if (is_ortho) - { - new_rotation_center = rotation_center; - new_rotate_x = 0.0; - new_rotate_y = 0.0; - new_translate_x = translate_x; - new_translate_y = translate_y; - new_translate_z = translate_z; - camera_transition_active = true; - } + if (app_state.camera.isOrtho) + app_state.camera.startEulerTransition( + 0.0f, 0.0f, app_state.camera.euler.translate, app_state.camera.euler.rotationCenter); } if (ImGui::IsItemHovered()) ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); - ImGui::MenuItem("Show axes", "key X", &show_axes); - ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); + ImGui::MenuItem("Show axes", "key X", &app_state.show_axes); + ImGui::MenuItem("Show compass/ruler", "key C", &app_state.compass_ruler); - ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); + ImGui::MenuItem("Lock Z", "Shift + Z", &app_state.camera.lockZ, !app_state.camera.isOrtho); // ImGui::MenuItem("show_covs", nullptr, &show_covs); @@ -4087,7 +4566,35 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float*)&app_state.bg_color, ImGuiColorEditFlags_NoInputs); + + // Same shader color modes as step 2's point cloud color schemes (ScanRenderer). + if (ImGui::BeginMenu("Points color")) + { + if (ImGui::MenuItem("> Session color", nullptr, points_color_mode == ScanColorMode::Flat)) + points_color_mode = ScanColorMode::Flat; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Each session in its own color (Settings window)"); + + ImGui::Separator(); + + if (ImGui::MenuItem("> By intensity (gradient)", nullptr, points_color_mode == ScanColorMode::Intensity)) + points_color_mode = ScanColorMode::Intensity; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Per-point jet colormap from LAS/LAZ intensity"); + + if (ImGui::MenuItem("> By height (gradient)", nullptr, points_color_mode == ScanColorMode::Elevation)) + points_color_mode = ScanColorMode::Elevation; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Per-point jet colormap from world Z, over all sessions' [z_min, z_max]"); + + if (ImGui::MenuItem("> By distance (gradient)", nullptr, points_color_mode == ScanColorMode::Distance)) + points_color_mode = ScanColorMode::Distance; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Per-point jet colormap from distance to the rotation center"); + + ImGui::EndMenu(); + } ImGui::Separator(); @@ -4109,13 +4616,13 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); + ImGui::InputInt("Points render downsampling", &app_state.viewer_decimate_point_cloud, 10, 100); if (ImGui::IsItemHovered()) ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); // ImGui::SameLine(); - if (viewer_decimate_point_cloud < 1) - viewer_decimate_point_cloud = 1; + if (app_state.viewer_decimate_point_cloud < 1) + app_state.viewer_decimate_point_cloud = 1; ImGui::SameLine(); @@ -4129,7 +4636,7 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); ImGui::SameLine(); - ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); + ImGui::Text("(%d FPS)", GetFPS()); } ImGui::EndDisabled(); @@ -4147,7 +4654,7 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - info_gui = !info_gui; + app_state.info_gui = !app_state.info_gui; ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -4224,63 +4731,43 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); if (is_loop_closure_gui) loop_closure_gui(); - cor_window(); - - info_window(infoLines, appShortcuts); - - if (compass_ruler) - drawMiniCompassWithRuler(); + raylib_widgets::showEulerCenterOfRotationWindow(cor_gui, app_state.camera, xText, yText, zText); - // my_display_code(); - /*if (is_ndt_gui) - ndt_gui(); - if (is_icp_gui) - icp_gui(); - if (is_pose_graph_slam) - pose_graph_slam_gui(); - if (is_registration_plane_feature) - registration_plane_feature_gui(); - if (is_manual_analisys) - observation_picking_gui();*/ - // if (is_loop_closure_gui) - // manual_pose_graph_loop_closure.Gui(); + raylib_widgets::ShowInfoWindow(app_state.info_gui, infoLines, appShortcuts, HDMAPPING_VERSION_STRING, __DATE__); if (is_settings_gui) settings_gui(); - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + // Switch to 2D screen space for text labels, the compass and ImGui's own draw pass. + raylib_widgets::end3DMatrixStack(io.DisplaySize.x, io.DisplaySize.y); + + if (is_loop_closure_gui) + renderLoopClosureLabels(); + else + for (const auto& session : sessions) + if (session.visible) + { + renderGroundControlPointsLabels(session.ground_control_points, session.point_clouds_container); + renderControlPointsLabels(session.control_points, session.point_clouds_container); + } + + if (app_state.compass_ruler) + drawMiniCompassWithRuler(); - glutSwapBuffers(); - glutPostRedisplay(); + rlImGuiEnd(); } void mouse(int glut_button, int state, int x, int y) { ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - int button = -1; - if (glut_button == GLUT_LEFT_BUTTON) - button = 0; - if (glut_button == GLUT_RIGHT_BUTTON) - button = 1; - if (glut_button == GLUT_MIDDLE_BUTTON) - button = 2; - if (button != -1 && state == GLUT_DOWN) - io.MouseDown[button] = true; - if (button != -1 && state == GLUT_UP) - io.MouseDown[button] = false; - - static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) - wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + + // GLUT's wheel-as-button-3/4 fallback is gone: main() polls GetMouseWheelMove() and calls wheel(). if (!io.WantCaptureMouse) { if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) && !manipulate_active_edge) { - // if (s_loop_closure_gui) if ((sessions.size() > 0) && (number_visible_sessions > 0) && update_rotation_center) { getClosestTrajectoriesPoint( @@ -4295,45 +4782,84 @@ void mouse(int glut_button, int state, int x, int y) io.KeyShift, time_stamp_offset); } - else + else if (update_rotation_center) { - if (update_rotation_center) - { - setNewRotationCenter(x, y); - } + setNewRotationCenter(x, y); } } if (state == GLUT_DOWN) - { - mouse_buttons |= 1 << glut_button; + app_state.mouse_buttons |= 1 << glut_button; + else if (state == GLUT_UP) + app_state.mouse_buttons = 0; - /*if (observation_picking.is_observation_picking_mode) - { - Eigen::Vector3d p = GLWidgetGetOGLPos(x, y, observation_picking); - int number_active_pcs = 0; - int index_picked = -1; - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - if (session.point_clouds_container.point_clouds[i].visible) - { - number_active_pcs++; - index_picked = i; - } - } - if (number_active_pcs == 1) - { - observation_picking.add_picked_to_current_observation(index_picked, p); - } - }*/ + app_state.mouse_old_x = x; + app_state.mouse_old_y = y; + } +} + +// Was utils.cpp's GLUT initGL(): raylib window + rlImGui, same setup as step 2. +bool initGL(const std::string& winTitleArg) +{ + // HiDPI breaks ImGui scaling on Windows, so it is only enabled on macOS and Linux (as in step 2). + unsigned int flags = FLAG_WINDOW_RESIZABLE; +#ifdef __APPLE__ + flags |= FLAG_WINDOW_HIGHDPI; +#endif +#if __LINUX__ + flags |= FLAG_WINDOW_HIGHDPI; +#endif + + SetConfigFlags(flags); + InitWindow(static_cast(window_width), static_cast(window_height), winTitleArg.c_str()); + SetExitKey(KEY_NULL); // Esc must not close the window (e.g. while cancelling a dialog) + SetTargetFPS(60); + raylib_widgets::fitWindowToScreen(/*marginW=*/100, /*marginH=*/100, /*centerVertically=*/true); + + rlImGuiSetup(true); + ImGuiIO& io = ImGui::GetIO(); + io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; + io.ConfigDockingWithShift = true; + + app_state.camera.applyPerspectiveProjection(static_cast(window_width), static_cast(window_height)); + + return true; +} + +// Drag & drop: a project (*.mjp) replaces the current one; session files (*.mjs/*.json) are added to it. +void loadDroppedFiles(const std::vector& paths) +{ + for (const auto& path : paths) + { + std::string ext = fs::path(path).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + if (ext == ".mjp") + { + loadProject(path, project_settings); + return; } - else if (state == GLUT_UP) + } + + bool added = false; + for (const auto& path : paths) + { + std::string ext = fs::path(path).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + if (ext == ".mjs" || ext == ".json") { - mouse_buttons = 0; + std::cout << "Adding session file: '" << path << "'" << std::endl; + project_settings.session_file_names.push_back(path); + added = true; } - mouse_old_x = x; - mouse_old_y = y; } + + if (added) + { + loaded_sessions = false; + time_stamp_offset = 0.0; + } + else + pfd::message("Unsupported file", "Drop a project (*.mjp) or session files (*.mjs, *.json).", pfd::choice::ok, pfd::icon::warning); } int main(int argc, char* argv[]) @@ -4352,7 +4878,7 @@ int main(int argc, char* argv[]) return 0; } - initGL(&argc, argv, winTitle, display, mouse); + initGL(winTitle); if (argc > 1) { @@ -4370,11 +4896,51 @@ int main(int argc, char* argv[]) } } - glutMainLoop(); + // Was glutMainLoop(): the GLUT callbacks are called directly, on raylib's input transitions. + while (!WindowShouldClose()) + { + int mx = static_cast(GetMouseX()); + int my = static_cast(GetMouseY()); + + if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) + mouse(GLUT_LEFT_BUTTON, GLUT_DOWN, mx, my); + if (IsMouseButtonReleased(MOUSE_BUTTON_LEFT)) + mouse(GLUT_LEFT_BUTTON, GLUT_UP, mx, my); + if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) + mouse(GLUT_RIGHT_BUTTON, GLUT_DOWN, mx, my); + if (IsMouseButtonReleased(MOUSE_BUTTON_RIGHT)) + mouse(GLUT_RIGHT_BUTTON, GLUT_UP, mx, my); + if (IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) + mouse(GLUT_MIDDLE_BUTTON, GLUT_DOWN, mx, my); + if (IsMouseButtonReleased(MOUSE_BUTTON_MIDDLE)) + mouse(GLUT_MIDDLE_BUTTON, GLUT_UP, mx, my); + + motion(mx, my); + + float wheelMove = GetMouseWheelMove(); + if (wheelMove != 0.0f) + wheel(0, wheelMove > 0.0f ? 1 : -1, mx, my); + + if (IsFileDropped()) + { + FilePathList dropped_files = LoadDroppedFiles(); + std::vector paths; + for (unsigned int i = 0; i < dropped_files.count; i++) + paths.emplace_back(dropped_files.paths[i]); + UnloadDroppedFiles(dropped_files); + if (!paths.empty()) + loadDroppedFiles(paths); + } + + BeginDrawing(); + display(); + EndDrawing(); + } - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplGLUT_Shutdown(); - ImGui::DestroyContext(); + // GPU buffers must be released while the GL context still exists. + session_renderers.clear(); + rlImGuiShutdown(); + CloseWindow(); } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; @@ -4388,4 +4954,4 @@ int main(int argc, char* argv[]) } return 0; -} \ No newline at end of file +} From e01a2c18249e9f44860a176030714c1a082c4d4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 20:34:49 +0200 Subject: [PATCH 6/9] Revert splitting NDT's Session overload into ndt_session.cpp Session's layout no longer depends on WITH_GUI (09c72a26), so core_math (WITH_GUI=0) and GUI apps already agree on sizeof(Session) and the split is not needed. NDT::optimize(std::vector&) walks sessions with the GUI apps' 0x250 stride again, and NDT on two forvia sessions solves as before. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- core/CMakeLists.txt | 3 +- core/include/Core/ndt.h | 27 -- core/src/ndt.cpp | 752 ++++++++++++++++++++++++++++++++++++++ core/src/ndt_session.cpp | 760 --------------------------------------- 4 files changed, 753 insertions(+), 789 deletions(-) delete mode 100644 core/src/ndt_session.cpp diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a91da772..3c353cbc 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -8,7 +8,6 @@ set(CORE_BASE_SOURCES src/gnss.cpp src/ground_control_points.cpp src/imu_preintegration.cpp - src/ndt_session.cpp src/nmea.cpp src/point_cloud.cpp src/point_clouds.cpp @@ -17,7 +16,7 @@ set(CORE_BASE_SOURCES # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... ) -# core_math holds the registration/optimization sources with auto-generated Jacobian headers (up to ~24k chars/line, expensive to compile); built once as a static lib shared by core and core_no_gui, so nothing here may touch a type whose layout depends on WITH_GUI (e.g. Session) -- that code goes in CORE_BASE_SOURCES (see ndt_session.cpp). +# core_math holds the registration/optimization sources with auto-generated Jacobian headers (up to ~24k chars/line, expensive to compile); built once as a static lib shared by core and core_no_gui since none of it branches on WITH_GUI. # hash_utils.cpp lives here (not CORE_BASE_SOURCES) because pair_wise_iterative_closest_point.cpp needs get_rgd_index_3d() from it -- keeping both in the same archive avoids a circular static-lib link dependency between core_math and core/core_no_gui. set(CORE_MATH_SOURCES src/hash_utils.cpp diff --git a/core/include/Core/ndt.h b/core/include/Core/ndt.h index b484fc07..5c081654 100644 --- a/core/include/Core/ndt.h +++ b/core/include/Core/ndt.h @@ -203,30 +203,3 @@ class NDT double sigma_azimuthal_angle = 0.0001; int num_extended_points = 10; }; - -//! Per-thread NDT worker (defined in ndt.cpp); declared here so ndt_session.cpp can dispatch it. -void ndt_job( - int i, - NDT::Job* job, - std::vector* buckets, - Eigen::SparseMatrix* AtPA, - Eigen::SparseMatrix* AtPB, - std::vector* index_pair_internal, - std::vector* pp, - std::vector* mposes, - std::vector* mposes_inv, - size_t trajectory_size, - NDT::PoseConvention pose_convention, - NDT::RotationMatrixParametrization rotation_matrix_parametrization, - int number_of_unknowns, - double* sumssr, - int* sums_obs, - bool is_generalized, - double sigma_r, - double sigma_polar_angle, - double sigma_azimuthal_angle, - int num_extended_points, - double* md_out, - double* md_count_out, - bool compute_only_mean_and_cov, - bool compute_mean_and_cov_for_bucket); diff --git a/core/src/ndt.cpp b/core/src/ndt.cpp index 8e5e4f76..3cca056a 100644 --- a/core/src/ndt.cpp +++ b/core/src/ndt.cpp @@ -2736,6 +2736,758 @@ bool NDT::optimize(std::vector& point_clouds, bool compute_only_maha return true; } +bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) +{ + std::cout << "optimize sessions" << std::endl; + + auto start = std::chrono::system_clock::now(); + + Session tmp_session; + + if (sessions.size() > 1) + { + if (sessions[0].is_ground_truth) + { + tmp_session = sessions[0]; + PointClouds point_clouds_container; + + std::vector point_clouds; + + std::vector points_local; + + for (const auto& pc : sessions[0].point_clouds_container.point_clouds) + { + for (const auto& p : pc.points_local) + { + Eigen::Vector3d pg = pc.m_pose * p; + points_local.push_back(pg); + } + } + + PointCloud pc; + pc.points_local = points_local; + pc.m_initial_pose = Eigen::Affine3d::Identity(); + pc.m_pose = Eigen::Affine3d::Identity(); + + point_clouds.push_back(pc); + point_clouds_container.point_clouds = point_clouds; + + sessions[0].point_clouds_container = point_clouds_container; + } + } + + OptimizationAlgorithm optimization_algorithm; + if (is_gauss_newton) + { + optimization_algorithm = OptimizationAlgorithm::gauss_newton; + } + if (is_levenberg_marguardt) + { + optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; + } + + PoseConvention pose_convention; + if (is_wc) + { + pose_convention = PoseConvention::wc; + } + if (is_cw) + { + pose_convention = PoseConvention::cw; + } + + RotationMatrixParametrization rotation_matrix_parametrization; + if (is_tait_bryan_angles) + { + rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; + } + else if (is_rodrigues) + { + rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; + } + else if (is_quaternion) + { + rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; + } + else if (is_lie_algebra_left_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; + } + else if (is_lie_algebra_right_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; + } + + if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + TaitBryanPose pose; + pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); + pc.m_pose = pc.m_pose * m; + } + } + } + + int number_of_unknowns; + if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + number_of_unknowns = 6; + } + if (is_quaternion) + { + number_of_unknowns = 7; + } + + double lm_lambda = 0.0001; + double previous_rms = std::numeric_limits::max(); + int number_of_lm_iterations = 0; + + std::vector m_poses_tmp; + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + // for (size_t i = 0; i < point_clouds.size(); i++) + //{ + // m_poses_tmp.push_back(point_clouds[i].m_pose); + // } + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + m_poses_tmp.push_back(pc.m_pose); + } + } + } + + for (int iter = 0; iter < number_of_iterations; iter++) + { + std::cout << "building points_global_external begin" << std::endl; + std::vector points_global_external; + size_t num_total_points = 0; + + // for (int i = 0; i < point_clouds.size(); i++) + //{ + // num_total_points += point_clouds[i].points_local.size(); + // } + + int num_point_clouds = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + num_total_points += pc.points_local.size(); + num_point_clouds++; + } + } + + points_global_external.reserve(num_total_points); + Eigen::Vector3d vt; + Point3D p; + + int index_pose = 0; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + // num_total_points += pc.points_local.size(); + std::cout << "processing point_cloud [" << index_pose + 1 << "] of " << num_point_clouds << std::endl; + + for (int j = 0; j < pc.points_local.size(); j++) + { + vt = pc.m_pose * pc.points_local[j]; + p.x = vt.x(); + p.y = vt.y(); + p.z = vt.z(); + p.index_pose = index_pose; + points_global_external.emplace_back(p); + } + index_pose++; + } + } + + std::cout << "building points_global_external end" << std::endl; + + std::vector index_pair_external; + std::vector buckets_external; + + GridParameters rgd_params_external; + rgd_params_external.resolution_X = this->bucket_size_external[0]; + rgd_params_external.resolution_Y = this->bucket_size_external[1]; + rgd_params_external.resolution_Z = this->bucket_size_external[2]; + + int bbext = this->bucket_size_external[0]; + if (this->bucket_size_external[1] > bbext) + bbext = this->bucket_size_external[1]; + if (this->bucket_size_external[2] > bbext) + bbext = this->bucket_size_external[2]; + rgd_params_external.bounding_box_extension = bbext; + + std::cout << "building external grid begin" << std::endl; + grid_calculate_params(points_global_external, rgd_params_external); + int num_threads = 1; + if (buckets_external.size() > this->number_of_threads) + { + num_threads = this->number_of_threads; + } + build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); + std::vector buckets_external_reduced; + + for (const auto& b : buckets_external) + { + if (b.number_of_points > 1000) + { + buckets_external_reduced.push_back(b); + } + } + buckets_external = buckets_external_reduced; + buckets_external_reduced.clear(); + + std::sort( + buckets_external.begin(), + buckets_external.end(), + [](const Bucket& a, const Bucket& b) + { + return (a.number_of_points > b.number_of_points); + }); + + std::cout << "building external grid end" << std::endl; + std::cout << "number active buckets external: " << buckets_external.size() << std::endl; + + bool init = false; + Eigen::SparseMatrix AtPA_ndt(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + Eigen::SparseMatrix AtPB_ndt(num_point_clouds * number_of_unknowns, 1); + double rms = 0.0; + int sum = 0; + double md = 0.0; + double md_sum = 0.0; + + for (int bi = 0; bi < buckets_external.size(); bi++) + { + if (compute_only_mahalanobis_distance) + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; + } + else + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " + << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; + } + std::vector points_global; + + for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) + { + points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); + } + + GridParameters rgd_params; + rgd_params.resolution_X = this->bucket_size[0]; + rgd_params.resolution_Y = this->bucket_size[1]; + rgd_params.resolution_Z = this->bucket_size[2]; + rgd_params.bounding_box_extension = 1.0; + + std::vector index_pair; + std::vector buckets; + + std::cout << "building rgd begin" << std::endl; + grid_calculate_params(points_global, rgd_params); + build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + std::cout << "building rgd end" << std::endl; + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + // double *md_out, double *md_count_out + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + mposes.push_back(pc.m_pose); + mposes_inv.push_back(pc.m_pose.inverse()); + } + } + + std::cout << "computing AtPA AtPB start" << std::endl; + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back( + std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + num_point_clouds, + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + false, + compute_mean_and_cov_for_bucket)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + std::cout << "computing AtPA AtPB finished" << std::endl; + + for (size_t k = 0; k < jobs.size(); k++) + { + rms += sumrmss[k]; + sum += sums[k]; + md += md_out[k]; + md_sum += md_count_out[k]; + } + + for (size_t k = 0; k < jobs.size(); k++) + { + if (!init) + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt = AtPAtmp[k]; + AtPB_ndt = AtPBtmp[k]; + init = true; + } + } + else + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt += AtPAtmp[k]; + AtPB_ndt += AtPBtmp[k]; + } + } + } + } + std::cout << "cleaning start" << std::endl; + points_global_external.clear(); + index_pair_external.clear(); + buckets_external.clear(); + std::cout << "cleaning finished" << std::endl; + + rms /= sum; + std::cout << "rms " << rms << std::endl; + + md /= md_sum; + std::cout << "mean mahalanobis distance: " << md << std::endl; + + if (compute_only_mahalanobis_distance) + { + return true; + } + + ////////////////////////////////////////////////////////////////// + + if (is_fix_first_node) + { + Eigen::SparseMatrix I(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + for (int ii = 0; ii < number_of_unknowns; ii++) + { + I.coeffRef(ii, ii) = 1000000; + } + AtPA_ndt += I; + } + + std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; + if (is_levenberg_marguardt) + { + if (rms < previous_rms) + { + if (lm_lambda < 1000000) + { + lm_lambda *= 10.0; + } + previous_rms = rms; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + } + else + { + lm_lambda /= 10.0; + number_of_lm_iterations++; + iter--; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + int index = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + pc.m_pose = m_poses_tmp[index++]; + } + } + + previous_rms = std::numeric_limits::max(); + continue; + } + } + else + { + previous_rms = rms; + } + + if (is_quaternion) + { + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + int index = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + // pc.m_pose = m_poses_tmp[index++]; + int ic = index * 7; + int ir = 0; + QuaternionPose pose; + if (is_wc) + { + pose = pose_quaternion_from_affine_matrix(pc.m_pose); + } + else + { + pose = pose_quaternion_from_affine_matrix(pc.m_pose.inverse()); + } + double delta; + quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); + + Eigen::Matrix jacobian; + quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); + + tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); + tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); + + tripletListP.emplace_back(ir, ir, 1000000.0); + + tripletListB.emplace_back(ir, 0, delta); + + index++; + } + } + + Eigen::SparseMatrix matA(tripletListB.size(), num_point_clouds * 7); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(num_point_clouds * 7, num_point_clouds * 7); + Eigen::SparseMatrix AtPB(num_point_clouds * 7, 1); + + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = AtP * matA; + AtPB = AtP * matB; + + AtPA_ndt += AtPA; + AtPB_ndt += AtPB; + } + + if (is_levenberg_marguardt) + { + Eigen::SparseMatrix LM(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + LM.setIdentity(); + LM *= lm_lambda; + AtPA_ndt += LM; + } + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA_ndt); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB_ndt); + + std::vector h_x; + std::cout << "redult: row,col,value" << std::endl; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + if (it.value() == it.value()) + { + h_x.push_back(it.value()); + std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; + } + } + } + + if (h_x.size() == num_point_clouds * number_of_unknowns) + { + std::cout << "AtPA=AtPB SOLVED" << std::endl; + int counter = 0; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + Eigen::Affine3d m_pose; + + if (is_wc) + { + m_pose = pc.m_pose; + } + else + { + m_pose = pc.m_pose.inverse(); + } + + if (is_tait_bryan_angles) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + m_pose = affine_matrix_from_pose_tait_bryan(pose); + } + else if (is_rodrigues) + { + RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.sx += h_x[counter++]; + pose.sy += h_x[counter++]; + pose.sz += h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose); + } + else if (is_quaternion) + { + QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); + + QuaternionPose poseq; + poseq.px = h_x[counter++]; + poseq.py = h_x[counter++]; + poseq.pz = h_x[counter++]; + poseq.q0 = h_x[counter++]; + poseq.q1 = h_x[counter++]; + poseq.q2 = h_x[counter++]; + poseq.q3 = h_x[counter++]; + + if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && + fabs(poseq.pz) < this->bucket_size[0] && fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && + fabs(poseq.q3) < 10) + { + pose.px += poseq.px; + pose.py += poseq.py; + pose.pz += poseq.pz; + pose.q0 += poseq.q0; + pose.q1 += poseq.q1; + pose.q2 += poseq.q2; + pose.q3 += poseq.q3; + m_pose = affine_matrix_from_pose_quaternion(pose); + } + } + else if (is_lie_algebra_left_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; + } + else if (is_lie_algebra_right_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); + } + + if (is_wc) + { + } + else + { + m_pose = m_pose.inverse(); + } + + auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); + auto pose_src = pose_tait_bryan_from_affine_matrix(pc.m_pose); + + if (!pc.fixed_x) + { + pose_src.px = pose_res.px; + } + if (!pc.fixed_y) + { + pose_src.py = pose_res.py; + } + if (!pc.fixed_z) + { + pose_src.pz = pose_res.pz; + } + if (!pc.fixed_om) + { + pose_src.om = pose_res.om; + } + if (!pc.fixed_fi) + { + pose_src.fi = pose_res.fi; + } + if (!pc.fixed_ka) + { + pose_src.ka = pose_res.ka; + } + + pc.pose = pose_src; + pc.gui_translation[0] = pose_src.px; + pc.gui_translation[1] = pose_src.py; + pc.gui_translation[2] = pose_src.pz; + pc.gui_rotation[0] = rad2deg(pose_src.om); + pc.gui_rotation[1] = rad2deg(pose_src.fi); + pc.gui_rotation[2] = rad2deg(pose_src.ka); + + /* + if (is_wc) + { + // if (!s.is_ground_truth) + //{ + pc.m_pose = m_pose; // ToDo check if !pc.fixed needed + //} + } + else + { + // if (!s.is_ground_truth) + //{ + pc.m_pose = m_pose.inverse(); // ToDo check if !pc.fixed needed + //} + } + + if (!pc.fixed) + { + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + }*/ + } + } + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + m_poses_tmp.push_back(pc.m_pose); + } + } + } + + std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; + } + else + { + std::cout << "AtPA=AtPB FAILED" << std::endl; + break; + } + } + + ////////// + + if (sessions.size() > 1) + { + if (sessions[0].is_ground_truth) + { + Eigen::Affine3d pose_inv0 = sessions[0].point_clouds_container.point_clouds[0].m_pose.inverse(); + + sessions[0].point_clouds_container = tmp_session.point_clouds_container; + + for (int i = 1; i < sessions.size(); i++) + { + for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + sessions[i].point_clouds_container.point_clouds[j].m_pose = + sessions[i].point_clouds_container.point_clouds[j].m_pose * pose_inv0; + sessions[i].point_clouds_container.point_clouds[j].pose = + pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); + sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = + sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = + sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = + sessions[i].point_clouds_container.point_clouds[j].pose.pz; + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.om); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.fi); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.ka); + } + } + } + } + ////////// + + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; + + return true; +} + std::vector> NDT::compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms) { OptimizationAlgorithm optimization_algorithm; diff --git a/core/src/ndt_session.cpp b/core/src/ndt_session.cpp deleted file mode 100644 index 52feba7e..00000000 --- a/core/src/ndt_session.cpp +++ /dev/null @@ -1,760 +0,0 @@ -#include - -// Split out of ndt.cpp: Session has a WITH_GUI-dependent layout, so this must be built per GUI flavour, not in core_math. -#include -#include -#include - -#include - -bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) -{ - std::cout << "optimize sessions" << std::endl; - - auto start = std::chrono::system_clock::now(); - - Session tmp_session; - - if (sessions.size() > 1) - { - if (sessions[0].is_ground_truth) - { - tmp_session = sessions[0]; - PointClouds point_clouds_container; - - std::vector point_clouds; - - std::vector points_local; - - for (const auto& pc : sessions[0].point_clouds_container.point_clouds) - { - for (const auto& p : pc.points_local) - { - Eigen::Vector3d pg = pc.m_pose * p; - points_local.push_back(pg); - } - } - - PointCloud pc; - pc.points_local = points_local; - pc.m_initial_pose = Eigen::Affine3d::Identity(); - pc.m_pose = Eigen::Affine3d::Identity(); - - point_clouds.push_back(pc); - point_clouds_container.point_clouds = point_clouds; - - sessions[0].point_clouds_container = point_clouds_container; - } - } - - OptimizationAlgorithm optimization_algorithm; - if (is_gauss_newton) - { - optimization_algorithm = OptimizationAlgorithm::gauss_newton; - } - if (is_levenberg_marguardt) - { - optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; - } - - PoseConvention pose_convention; - if (is_wc) - { - pose_convention = PoseConvention::wc; - } - if (is_cw) - { - pose_convention = PoseConvention::cw; - } - - RotationMatrixParametrization rotation_matrix_parametrization; - if (is_tait_bryan_angles) - { - rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; - } - else if (is_rodrigues) - { - rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; - } - else if (is_quaternion) - { - rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; - } - else if (is_lie_algebra_left_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; - } - else if (is_lie_algebra_right_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; - } - - if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - TaitBryanPose pose; - pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); - pc.m_pose = pc.m_pose * m; - } - } - } - - int number_of_unknowns; - if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - number_of_unknowns = 6; - } - if (is_quaternion) - { - number_of_unknowns = 7; - } - - double lm_lambda = 0.0001; - double previous_rms = std::numeric_limits::max(); - int number_of_lm_iterations = 0; - - std::vector m_poses_tmp; - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - // for (size_t i = 0; i < point_clouds.size(); i++) - //{ - // m_poses_tmp.push_back(point_clouds[i].m_pose); - // } - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - m_poses_tmp.push_back(pc.m_pose); - } - } - } - - for (int iter = 0; iter < number_of_iterations; iter++) - { - std::cout << "building points_global_external begin" << std::endl; - std::vector points_global_external; - size_t num_total_points = 0; - - // for (int i = 0; i < point_clouds.size(); i++) - //{ - // num_total_points += point_clouds[i].points_local.size(); - // } - - int num_point_clouds = 0; - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - num_total_points += pc.points_local.size(); - num_point_clouds++; - } - } - - points_global_external.reserve(num_total_points); - Eigen::Vector3d vt; - Point3D p; - - int index_pose = 0; - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - // num_total_points += pc.points_local.size(); - std::cout << "processing point_cloud [" << index_pose + 1 << "] of " << num_point_clouds << std::endl; - - for (int j = 0; j < pc.points_local.size(); j++) - { - vt = pc.m_pose * pc.points_local[j]; - p.x = vt.x(); - p.y = vt.y(); - p.z = vt.z(); - p.index_pose = index_pose; - points_global_external.emplace_back(p); - } - index_pose++; - } - } - - std::cout << "building points_global_external end" << std::endl; - - std::vector index_pair_external; - std::vector buckets_external; - - GridParameters rgd_params_external; - rgd_params_external.resolution_X = this->bucket_size_external[0]; - rgd_params_external.resolution_Y = this->bucket_size_external[1]; - rgd_params_external.resolution_Z = this->bucket_size_external[2]; - - int bbext = this->bucket_size_external[0]; - if (this->bucket_size_external[1] > bbext) - bbext = this->bucket_size_external[1]; - if (this->bucket_size_external[2] > bbext) - bbext = this->bucket_size_external[2]; - rgd_params_external.bounding_box_extension = bbext; - - std::cout << "building external grid begin" << std::endl; - grid_calculate_params(points_global_external, rgd_params_external); - int num_threads = 1; - if (buckets_external.size() > this->number_of_threads) - { - num_threads = this->number_of_threads; - } - build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); - std::vector buckets_external_reduced; - - for (const auto& b : buckets_external) - { - if (b.number_of_points > 1000) - { - buckets_external_reduced.push_back(b); - } - } - buckets_external = buckets_external_reduced; - buckets_external_reduced.clear(); - - std::sort( - buckets_external.begin(), - buckets_external.end(), - [](const Bucket& a, const Bucket& b) - { - return (a.number_of_points > b.number_of_points); - }); - - std::cout << "building external grid end" << std::endl; - std::cout << "number active buckets external: " << buckets_external.size() << std::endl; - - bool init = false; - Eigen::SparseMatrix AtPA_ndt(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - Eigen::SparseMatrix AtPB_ndt(num_point_clouds * number_of_unknowns, 1); - double rms = 0.0; - int sum = 0; - double md = 0.0; - double md_sum = 0.0; - - for (int bi = 0; bi < buckets_external.size(); bi++) - { - if (compute_only_mahalanobis_distance) - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; - } - else - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " - << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; - } - std::vector points_global; - - for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) - { - points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); - } - - GridParameters rgd_params; - rgd_params.resolution_X = this->bucket_size[0]; - rgd_params.resolution_Y = this->bucket_size[1]; - rgd_params.resolution_Z = this->bucket_size[2]; - rgd_params.bounding_box_extension = 1.0; - - std::vector index_pair; - std::vector buckets; - - std::cout << "building rgd begin" << std::endl; - grid_calculate_params(points_global, rgd_params); - build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); - std::cout << "building rgd end" << std::endl; - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); - // double *md_out, double *md_count_out - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } - - std::vector mposes; - std::vector mposes_inv; - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - mposes.push_back(pc.m_pose); - mposes_inv.push_back(pc.m_pose.inverse()); - } - } - - std::cout << "computing AtPA AtPB start" << std::endl; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back( - std::thread( - ndt_job, - k, - &jobs[k], - &buckets, - &(AtPAtmp[k]), - &(AtPBtmp[k]), - &index_pair, - &points_global, - &mposes, - &mposes_inv, - num_point_clouds, - pose_convention, - rotation_matrix_parametrization, - number_of_unknowns, - &(sumrmss[k]), - &(sums[k]), - is_generalized, - sigma_r, - sigma_polar_angle, - sigma_azimuthal_angle, - num_extended_points, - &(md_out[k]), - &(md_count_out[k]), - false, - compute_mean_and_cov_for_bucket)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - std::cout << "computing AtPA AtPB finished" << std::endl; - - for (size_t k = 0; k < jobs.size(); k++) - { - rms += sumrmss[k]; - sum += sums[k]; - md += md_out[k]; - md_sum += md_count_out[k]; - } - - for (size_t k = 0; k < jobs.size(); k++) - { - if (!init) - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt = AtPAtmp[k]; - AtPB_ndt = AtPBtmp[k]; - init = true; - } - } - else - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt += AtPAtmp[k]; - AtPB_ndt += AtPBtmp[k]; - } - } - } - } - std::cout << "cleaning start" << std::endl; - points_global_external.clear(); - index_pair_external.clear(); - buckets_external.clear(); - std::cout << "cleaning finished" << std::endl; - - rms /= sum; - std::cout << "rms " << rms << std::endl; - - md /= md_sum; - std::cout << "mean mahalanobis distance: " << md << std::endl; - - if (compute_only_mahalanobis_distance) - { - return true; - } - - ////////////////////////////////////////////////////////////////// - - if (is_fix_first_node) - { - Eigen::SparseMatrix I(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - for (int ii = 0; ii < number_of_unknowns; ii++) - { - I.coeffRef(ii, ii) = 1000000; - } - AtPA_ndt += I; - } - - std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; - if (is_levenberg_marguardt) - { - if (rms < previous_rms) - { - if (lm_lambda < 1000000) - { - lm_lambda *= 10.0; - } - previous_rms = rms; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - } - else - { - lm_lambda /= 10.0; - number_of_lm_iterations++; - iter--; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - int index = 0; - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - pc.m_pose = m_poses_tmp[index++]; - } - } - - previous_rms = std::numeric_limits::max(); - continue; - } - } - else - { - previous_rms = rms; - } - - if (is_quaternion) - { - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - int index = 0; - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - // pc.m_pose = m_poses_tmp[index++]; - int ic = index * 7; - int ir = 0; - QuaternionPose pose; - if (is_wc) - { - pose = pose_quaternion_from_affine_matrix(pc.m_pose); - } - else - { - pose = pose_quaternion_from_affine_matrix(pc.m_pose.inverse()); - } - double delta; - quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); - - Eigen::Matrix jacobian; - quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); - - tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); - tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); - - tripletListP.emplace_back(ir, ir, 1000000.0); - - tripletListB.emplace_back(ir, 0, delta); - - index++; - } - } - - Eigen::SparseMatrix matA(tripletListB.size(), num_point_clouds * 7); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(num_point_clouds * 7, num_point_clouds * 7); - Eigen::SparseMatrix AtPB(num_point_clouds * 7, 1); - - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = AtP * matA; - AtPB = AtP * matB; - - AtPA_ndt += AtPA; - AtPB_ndt += AtPB; - } - - if (is_levenberg_marguardt) - { - Eigen::SparseMatrix LM(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - LM.setIdentity(); - LM *= lm_lambda; - AtPA_ndt += LM; - } - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA_ndt); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB_ndt); - - std::vector h_x; - std::cout << "redult: row,col,value" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - if (it.value() == it.value()) - { - h_x.push_back(it.value()); - std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; - } - } - } - - if (h_x.size() == num_point_clouds * number_of_unknowns) - { - std::cout << "AtPA=AtPB SOLVED" << std::endl; - int counter = 0; - - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - Eigen::Affine3d m_pose; - - if (is_wc) - { - m_pose = pc.m_pose; - } - else - { - m_pose = pc.m_pose.inverse(); - } - - if (is_tait_bryan_angles) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; - m_pose = affine_matrix_from_pose_tait_bryan(pose); - } - else if (is_rodrigues) - { - RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.sx += h_x[counter++]; - pose.sy += h_x[counter++]; - pose.sz += h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose); - } - else if (is_quaternion) - { - QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); - - QuaternionPose poseq; - poseq.px = h_x[counter++]; - poseq.py = h_x[counter++]; - poseq.pz = h_x[counter++]; - poseq.q0 = h_x[counter++]; - poseq.q1 = h_x[counter++]; - poseq.q2 = h_x[counter++]; - poseq.q3 = h_x[counter++]; - - if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && - fabs(poseq.pz) < this->bucket_size[0] && fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && - fabs(poseq.q3) < 10) - { - pose.px += poseq.px; - pose.py += poseq.py; - pose.pz += poseq.pz; - pose.q0 += poseq.q0; - pose.q1 += poseq.q1; - pose.q2 += poseq.q2; - pose.q3 += poseq.q3; - m_pose = affine_matrix_from_pose_quaternion(pose); - } - } - else if (is_lie_algebra_left_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; - } - else if (is_lie_algebra_right_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); - } - - if (is_wc) - { - } - else - { - m_pose = m_pose.inverse(); - } - - auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); - auto pose_src = pose_tait_bryan_from_affine_matrix(pc.m_pose); - - if (!pc.fixed_x) - { - pose_src.px = pose_res.px; - } - if (!pc.fixed_y) - { - pose_src.py = pose_res.py; - } - if (!pc.fixed_z) - { - pose_src.pz = pose_res.pz; - } - if (!pc.fixed_om) - { - pose_src.om = pose_res.om; - } - if (!pc.fixed_fi) - { - pose_src.fi = pose_res.fi; - } - if (!pc.fixed_ka) - { - pose_src.ka = pose_res.ka; - } - - pc.pose = pose_src; - pc.gui_translation[0] = pose_src.px; - pc.gui_translation[1] = pose_src.py; - pc.gui_translation[2] = pose_src.pz; - pc.gui_rotation[0] = rad2deg(pose_src.om); - pc.gui_rotation[1] = rad2deg(pose_src.fi); - pc.gui_rotation[2] = rad2deg(pose_src.ka); - - /* - if (is_wc) - { - // if (!s.is_ground_truth) - //{ - pc.m_pose = m_pose; // ToDo check if !pc.fixed needed - //} - } - else - { - // if (!s.is_ground_truth) - //{ - pc.m_pose = m_pose.inverse(); // ToDo check if !pc.fixed needed - //} - } - - if (!pc.fixed) - { - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - }*/ - } - } - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - m_poses_tmp.push_back(pc.m_pose); - } - } - } - - std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; - } - else - { - std::cout << "AtPA=AtPB FAILED" << std::endl; - break; - } - } - - ////////// - - if (sessions.size() > 1) - { - if (sessions[0].is_ground_truth) - { - Eigen::Affine3d pose_inv0 = sessions[0].point_clouds_container.point_clouds[0].m_pose.inverse(); - - sessions[0].point_clouds_container = tmp_session.point_clouds_container; - - for (int i = 1; i < sessions.size(); i++) - { - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - { - sessions[i].point_clouds_container.point_clouds[j].m_pose = - sessions[i].point_clouds_container.point_clouds[j].m_pose * pose_inv0; - sessions[i].point_clouds_container.point_clouds[j].pose = - pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); - sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = - sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = - sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = - sessions[i].point_clouds_container.point_clouds[j].pose.pz; - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = - rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.om); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = - rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.fi); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = - rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.ka); - } - } - } - } - ////////// - - auto end = std::chrono::system_clock::now(); - auto elapsed = std::chrono::duration_cast(end - start); - - std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; - - return true; -} From 4d0fe4eec75b97ede9ca1cfa09f6eeee3538d2a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 20:34:49 +0200 Subject: [PATCH 7/9] ScanRenderer: add FlatIntensity color mode Each scan's flat render_color shaded by its normalized intensity (sqrt, with a 0.35 floor so dark points keep their hue). Keeps per-session colors apart while surfaces keep detail. Appended to ScanColorMode, so existing mode values and step 2 are unchanged. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- core/include/Core/raylib_render.hpp | 1 + core/src/raylib_render.cpp | 2 +- core/src/raylib_render_shaders.hpp | 6 ++++++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index b0d0c3f3..959db519 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -46,6 +46,7 @@ enum class ScanColorMode Intensity, // jet colormap by normalized LAS/LAZ intensity Elevation, // jet colormap by world-space Z, normalized over [elevationMin, elevationMax] Distance, // jet colormap by distance from distanceCenter, normalized over [0, distanceMax] + FlatIntensity, // pc.render_color shaded by normalized intensity (keeps per-scan colors distinguishable) }; class ScanRenderer diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 010c18e6..3413a7a7 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -317,7 +317,7 @@ void ScanRenderer::draw( color[2] = pc.render_color[2]; color[3] = 1.0f; // Matches the shader's colorMode branches (0=flat, 1=intensity, - // 2=elevation, 3=distance) exactly, since ScanColorMode's + // 2=elevation, 3=distance, 4=flat shaded by intensity) exactly, since ScanColorMode's // enumerator order was chosen to match. colorModeInt = static_cast(colorMode); } diff --git a/core/src/raylib_render_shaders.hpp b/core/src/raylib_render_shaders.hpp index b0995e97..b6ee6c29 100644 --- a/core/src/raylib_render_shaders.hpp +++ b/core/src/raylib_render_shaders.hpp @@ -85,6 +85,12 @@ void main() float d = length(fragWorldPos - distCenter); finalColor = vec4(jet(d / max(distMax, 1e-6)), pointColor.a); } + else if (colorMode == 4) + { + // Flat color shaded by intensity. sqrt lifts the typically low-skewed intensity distribution; + // the 0.35 floor keeps low-intensity points' hue visible. + finalColor = vec4(pointColor.rgb * mix(0.35, 1.0, sqrt(fragIntensity)), pointColor.a); + } else { finalColor = pointColor; From dbf3d1f3a5af8c106bb4369ff599776867f17094 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 23 Sep 2026 20:34:49 +0200 Subject: [PATCH 8/9] Step 3 raylib port: edit the GLUT code in place to minimize the diff Replaces the earlier rewrite. multi_session_registration.cpp is the GLUT file with only its GL calls changed (~100 lines). New raylib_utils.h/.cpp provide the names it used from Core/utils.hpp on top of raylib_widgets::OrbitCamera (the old camera globals are references into it) plus small GL-style helpers (line strips, 3D text labels) and per-session ScanRenderer drawing. Also: View > Points color (session color shaded by intensity by default, or step 2's intensity/height/distance gradients), render downsampling default 2 (GPU buffers; GLUT needed 1000), and session colors red, blue, orange, green, magenta, cyan, yellow, violet, then a random hue seeded by index. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_015fZ2YByu6eAyC3Rvi6huo1 --- .../multi_session_registration/CMakeLists.txt | 19 +- .../multi_session_registration.cpp | 2137 ++++++----------- .../raylib_utils.cpp | 759 ++++++ .../multi_session_registration/raylib_utils.h | 156 ++ 4 files changed, 1710 insertions(+), 1361 deletions(-) create mode 100644 apps/multi_session_registration/raylib_utils.cpp create mode 100644 apps/multi_session_registration/raylib_utils.h diff --git a/apps/multi_session_registration/CMakeLists.txt b/apps/multi_session_registration/CMakeLists.txt index f4f898ce..89be3fdf 100644 --- a/apps/multi_session_registration/CMakeLists.txt +++ b/apps/multi_session_registration/CMakeLists.txt @@ -2,12 +2,11 @@ cmake_minimum_required(VERSION 4.0.0) project(multi_session_registration_step_3) -# raylib-based, like step 2 (apps/multi_view_tls_registration): no core/src/utils.cpp, -# GLUT or legacy OpenGL. The GLUT build is kept as apps/multi_session_registration_legacy. -# multi_session_registration_no_gui.cpp is not part of this app; pybind builds it. +# Source files set(SOURCES multi_session_registration.cpp multi_session_factor_graph.cpp + raylib_utils.cpp ) # Windows: add resource file @@ -27,23 +26,27 @@ target_include_directories( ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/freeglut/include ${LASZIP_INCLUDE_DIR}/LASzip/include) target_link_libraries( multi_session_registration_step_3 - PRIVATE - # core_raylib brings in core + raylib (and links FREEGLUT after core for libcore.a's - # remaining glutBitmap* references) -- see core/CMakeLists.txt. + # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib + # raylib-based like step 2; core_raylib brings in core, raylib and ScanRenderer. core_raylib raylib_widgets imgui_raylib rlimgui imguizmo_raylib - spdlog::spdlog ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS}) @@ -62,4 +65,4 @@ if (MSVC) target_compile_options(multi_session_registration_step_3 PRIVATE /bigobj) endif() -hdmapping_install_app(multi_session_registration_step_3) +hdmapping_install_app(multi_session_registration_step_3) \ No newline at end of file diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index b20977e6..45cc55df 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -1,31 +1,16 @@ -#include #include #include -#include -#include - -// Step 3 used to be built on GLUT + legacy immediate-mode OpenGL via -// core/src/utils.cpp (the GLUT build is kept as -// apps/multi_session_registration_legacy). Like step 2 -// (apps/multi_view_tls_registration), it now runs on raylib: the camera, -// input and picking helpers it used to get from are -// re-implemented below on top of raylib_widgets::OrbitCamera, and point -// clouds are drawn with Core/raylib_render.hpp's ScanRenderer (one per -// session) instead of core's legacy-GL PointCloud::render(). -#include "raylib.h" -#include "raymath.h" -#include "rlImGui.h" -#include "rlgl.h" #include #include +#include #include +#include +#include #include -#include - #include #include @@ -34,15 +19,11 @@ #include #include #include -#include #include #include -#include -#include #ifdef _WIN32 -// Same windows.h/raylib clash as step 2 (see multi_view_tls_registration_gui.cpp): -// rename windows.h's CloseWindow/ShowCursor so raylib's stay callable. +// windows.h (pulled in by portable-file-dialogs.h) declares CloseWindow/ShowCursor like raylib.h does. #define CloseWindow CloseWindow_win32 #define ShowCursor ShowCursor_win32 #endif @@ -56,47 +37,11 @@ #ifdef _WIN32 #include "resource.h" -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef _WIN32 -// windows.h #defines DrawText as DrawTextA; restore raylib's DrawText. -#undef DrawText #endif #include "multi_session_factor_graph.h" - -using raylib_widgets::ShortcutEntry; -using raylib_widgets::ShowMainDockSpace; - -const float DEG_TO_RAD = M_PI / 180.0f; -const float RAD_TO_DEG = 180.0f / M_PI; - -constexpr float ImGuiNumberWidth = 120.0f; -constexpr const char* xText = "Longitudinal (forward/backward)"; -constexpr const char* yText = "Lateral (left/right)"; -constexpr const char* zText = "Vertical (up/down)"; - -const uint32_t window_width = 1600; -const uint32_t window_height = 900; - -// GLUT/mouse-button codes kept so mouse() keeps its GLUT-callback shape (as in step 2). -constexpr int GLUT_LEFT_BUTTON = 0; -constexpr int GLUT_MIDDLE_BUTTON = 1; -constexpr int GLUT_RIGHT_BUTTON = 2; -constexpr int GLUT_DOWN = 0; -constexpr int GLUT_UP = 1; - -// Point downsampling default and camera-Reset value, as in the GLUT step 3 (utils.cpp). -constexpr int kDefaultDecimate = 1000; +#include "raylib_utils.h" std::string winTitle = std::string("Step 3 (Multi session registration) ") + HDMAPPING_VERSION_STRING; @@ -239,989 +184,12 @@ std::vector sessions; int viewer_reduce_rendered_trajectory = 1; namespace fs = std::filesystem; -int num_edge_extended_before = 0; -int num_edge_extended_after = 0; - -TaitBryanPose motion_model_weights = { 0.01, 0.01, 0.01, 0.1, 0.1, 0.1 }; -/////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////// - -// Camera/view state that used to be globals. -struct AppStateBase -{ - int viewer_decimate_point_cloud = kDefaultDecimate; - - int mouse_old_x = 0, mouse_old_y = 0; - int mouse_buttons = 0; - bool show_axes = true; - ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); - // Single point size for all sessions. The GLUT app had two (View menu/1-9 keys and the - // loop closure window's gui_point_size), and the latter silently overrode the former every frame. - int point_size = 2; - - bool info_gui = false; - bool compass_ruler = true; - - // Rebuilt from `camera` every frame; used by the compass and the perspective modelview. - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - - raylib_widgets::OrbitCamera camera; -}; - -inline AppStateBase app_state; - -// Edge-triggered request to open the Center of rotation dialog (Shift+R). -bool cor_gui = false; - -bool scroll_hint_enabled = true; -bool scroll_hint_active = false; -int scroll_hint_count = 0; -float scroll_hint_accu = 0.0f; -double scroll_hint_lastT = 0.0; - -// One GPU renderer per session, parallel to `sessions` (ScanRenderer is indexed by a single -// std::vector). Rebuilt whenever the session list changes -- see syncSessionRenderers(). -std::vector> session_renderers; - -// Point coloring, using the same ScanRenderer shader modes as step 2's color schemes. Flat (each -// scan's render_color, i.e. the session color) is the default, since step 3 compares sessions. -ScanColorMode points_color_mode = ScanColorMode::Flat; - -// Bounds of all loaded sessions, for the height and distance gradients; updated with session_renderers. -PointClouds::PointCloudDimensions scene_dims{ 0, 0, 0, 0, 0, 1, 1, 1, 1 }; - -// This frame's 3D model-view-projection, captured before the matrix stack is switched to 2D, -// so the 2D label pass can project world points to the screen. -Matrix frame_mvp_3d{}; - -void display(); -void mouse(int glut_button, int state, int x, int y); - -/////////////////////////////////////////////////////////////////////////////////// - -// Camera/input/picking helpers, same as step 2 (apps/multi_view_tls_registration). - -std::string truncPath(const std::string& fullPath) -{ - namespace fspath = std::filesystem; - fspath::path path(fullPath); - - auto parent1 = path.parent_path().filename().string(); - auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder - auto filename = path.filename().string(); - - return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; -} - -void wheel(int button, int dir, int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MouseWheel += dir; // or direction * 1.0f depending on your setup - - if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) - { - // GetMouseWheelMove(), not `dir`: dir is already quantized to +-1 by - // main()'s caller (see its comment), which discards a trackpad's - // fractional per-frame scroll magnitude -- reading it again here - // (stable within the same frame, since raylib only updates it once - // per PollInputEvents()) lets zoom() scale the step by how much was - // actually scrolled instead of always taking a full step. - app_state.camera.zoom(GetMouseWheelMove(), io.KeyShift); - - if (scroll_hint_enabled) - { - if (!scroll_hint_active) - { - scroll_hint_accu += fabs(dir); - - if (scroll_hint_accu > 30.0f) // tweak threshold - { - scroll_hint_accu = 0.0f; - scroll_hint_active = true; - scroll_hint_count++; - } - } - - if (scroll_hint_active) - scroll_hint_lastT = ImGui::GetTime(); - - // Reset and disable hint if Shift is pressed while scrolling - if (io.KeyShift || scroll_hint_count > 3) - { - scroll_hint_active = false; - scroll_hint_enabled = false; - } - } - } -} - -void motion(int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - - if (!io.WantCaptureMouse) - { - float dx, dy; - dx = (float)(x - app_state.mouse_old_x); - dy = (float)(y - app_state.mouse_old_y); - - // Ctrl/Shift held: reserved for the discrete click actions and the - // keyboard shortcuts in view_kbd_shortcuts() -- mouse() sets - // mouse_buttons for *every* button-down, including a Ctrl/Shift+ - // click used to pick a new rotation center (which starts a camera - // transition -- see getClosestTrajectoryPoint()/ - // setNewRotationCenter()/the Center of rotation dialog). Without - // this guard, any stray sub-pixel movement on the same click - // (trackpads are far more prone to this than a physical mouse - // button) got read as an ordinary orbit/pan drag and immediately - // broke that transition via dragOrbit()/dragPanPerspective()'s - // breakEulerTransition() call. - if (!io.KeyCtrl && !io.KeyShift) - { - if (app_state.mouse_buttons & 1) // left button - { - app_state.camera.dragOrbit(dx, dy); - } - - if (app_state.mouse_buttons & 4) // right button - { - if (app_state.camera.isOrtho) - app_state.camera.dragPanOrtho(dx, dy, io.DisplaySize.x, io.DisplaySize.y); - else - app_state.camera.dragPanPerspective(dx, dy); - } - } - - app_state.mouse_old_x = x; - app_state.mouse_old_y = y; - } -} - -void showAxes() -{ - if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes - { - const auto& rc = app_state.camera.euler.rotationCenter; - rlBegin(RL_LINES); - rlColor3f(1.f, 1.f, 1.f); - rlVertex3f(rc.x, rc.y, rc.z); - rlVertex3f(rc.x + 1.f, rc.y, rc.z); - rlVertex3f(rc.x, rc.y, rc.z); - rlVertex3f(rc.x - 1.f, rc.y, rc.z); - rlVertex3f(rc.x, rc.y, rc.z); - rlVertex3f(rc.x, rc.y - 1.f, rc.z); - rlVertex3f(rc.x, rc.y, rc.z); - rlVertex3f(rc.x, rc.y + 1.f, rc.z); - rlVertex3f(rc.x, rc.y, rc.z); - rlVertex3f(rc.x, rc.y, rc.z - 1.f); - rlVertex3f(rc.x, rc.y, rc.z); - rlVertex3f(rc.x, rc.y, rc.z + 1.f); - rlEnd(); - } - - if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // origin axes - { - rlBegin(RL_LINES); - rlColor3f(1.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(100, 0.0f, 0.0f); - - rlColor3f(0.0f, 1.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 100, 0.0f); - - rlColor3f(0.0f, 0.0f, 1.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 100); - rlEnd(); - } -} - -void camMenu() -{ - using raylib_widgets::OrbitCamera; - - if (ImGui::BeginMenu("Camera")) - { - if (ImGui::MenuItem("Front (yz view)", "key F")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Front); - if (ImGui::MenuItem("Back", "key B")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Back); - if (ImGui::MenuItem("Left (xz view)", "key L")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Left); - if (ImGui::MenuItem("Right", "key R")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Right); - if (ImGui::MenuItem("Top (xy view)", "key T")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Top); - if (ImGui::MenuItem("Bottom", "key U")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Bottom); - if (ImGui::MenuItem("Isometric", "key I")) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Iso); - ImGui::Separator(); - if (ImGui::MenuItem("Reset", "key Z")) - { - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Reset); - app_state.viewer_decimate_point_cloud = kDefaultDecimate; - } - - ImGui::EndMenu(); - } - if (ImGui::IsItemHovered()) - { - ImGui::BeginTooltip(); - ImGui::Text("Change camera view to fixed positions"); - ImGui::Separator(); - ImGui::Text("Metrics:"); - if (ImGui::BeginTable("Metrics", 4)) - { - ImGui::TableSetupColumn("Coord"); - ImGui::TableSetupColumn("rotate"); - ImGui::TableSetupColumn("translate"); - ImGui::TableSetupColumn("rot center"); - ImGui::TableHeadersRow(); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - - std::string text = "X"; - float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("X"); - - ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", app_state.camera.euler.rotateX); - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", app_state.camera.euler.translate.x); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.x); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("Y"); - - ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", app_state.camera.euler.rotateY); - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", app_state.camera.euler.translate.y); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.y); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("Z"); - - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", app_state.camera.euler.translate.z); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", app_state.camera.euler.rotationCenter.y); - - ImGui::EndTable(); - } - ImGui::Text("Mouse sensitivity: %.4f", app_state.camera.eulerMouseSensitivity); - - ImGui::EndTooltip(); - } - - if (scroll_hint_active) - { - ImVec2 mousePos = ImGui::GetMousePos(); - ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); - ImGui::SetNextWindowBgAlpha(0.7f); - ImGui::BeginTooltip(); - ImGui::Text("Tip: To accelerate hold Shift + scroll"); - ImGui::EndTooltip(); - - if (ImGui::GetTime() - scroll_hint_lastT > 1) - scroll_hint_active = false; - } -} - -void view_kbd_shortcuts() -{ - using raylib_widgets::OrbitCamera; - - ImGuiIO& io = ImGui::GetIO(); - - if (io.WantCaptureKeyboard) - return; - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - { - app_state.camera.euler.translate.x += 0.5f * app_state.camera.eulerMouseSensitivity; - app_state.camera.breakEulerTransition(); - } - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - { - app_state.camera.euler.translate.x -= 0.5f * app_state.camera.eulerMouseSensitivity; - app_state.camera.breakEulerTransition(); - } - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) - { - app_state.camera.euler.translate.y += 0.5f * app_state.camera.eulerMouseSensitivity; - app_state.camera.breakEulerTransition(); - } - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) - { - app_state.camera.euler.translate.y -= 0.5f * app_state.camera.eulerMouseSensitivity; - app_state.camera.breakEulerTransition(); - } - - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - { - app_state.camera.euler.rotateY -= 0.6f; - app_state.camera.breakEulerTransition(); - } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - { - app_state.camera.euler.rotateY += 0.6f; - app_state.camera.breakEulerTransition(); - } - - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) - { - app_state.camera.euler.rotateX -= 0.6f; - app_state.camera.breakEulerTransition(); - } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) - { - app_state.camera.euler.rotateX += 0.6f; - app_state.camera.breakEulerTransition(); - } - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) - cor_gui = true; - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !app_state.camera.isOrtho) - app_state.camera.lockZ = !app_state.camera.lockZ; - - if (io.KeyCtrl || io.KeyAlt || io.KeyShift) - return; - - if (ImGui::IsKeyPressed(ImGuiKey_B)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Back); - if (ImGui::IsKeyPressed(ImGuiKey_F)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Front); - if (ImGui::IsKeyPressed(ImGuiKey_I)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Iso); - if (ImGui::IsKeyPressed(ImGuiKey_L)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Left); - if (ImGui::IsKeyPressed(ImGuiKey_R)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Right); - if (ImGui::IsKeyPressed(ImGuiKey_T)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Top); - if (ImGui::IsKeyPressed(ImGuiKey_U)) - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Bottom); - if (ImGui::IsKeyPressed(ImGuiKey_Z)) - { - app_state.camera.setEulerPreset(OrbitCamera::EulerPreset::Reset); - app_state.viewer_decimate_point_cloud = kDefaultDecimate; - } - - if (ImGui::IsKeyPressed(ImGuiKey_C, false)) - app_state.compass_ruler = !app_state.compass_ruler; - if (ImGui::IsKeyPressed(ImGuiKey_O, false)) - app_state.camera.isOrtho = !app_state.camera.isOrtho; - if (ImGui::IsKeyPressed(ImGuiKey_X, false)) - app_state.show_axes = !app_state.show_axes; - - if (ImGui::IsKeyPressed(ImGuiKey_1)) - app_state.point_size = 1; - if (ImGui::IsKeyPressed(ImGuiKey_2)) - app_state.point_size = 2; - if (ImGui::IsKeyPressed(ImGuiKey_3)) - app_state.point_size = 3; - if (ImGui::IsKeyPressed(ImGuiKey_4)) - app_state.point_size = 4; - if (ImGui::IsKeyPressed(ImGuiKey_5)) - app_state.point_size = 5; - if (ImGui::IsKeyPressed(ImGuiKey_6)) - app_state.point_size = 6; - if (ImGui::IsKeyPressed(ImGuiKey_7)) - app_state.point_size = 7; - if (ImGui::IsKeyPressed(ImGuiKey_8)) - app_state.point_size = 8; - if (ImGui::IsKeyPressed(ImGuiKey_9)) - app_state.point_size = 9; -} - -void drawMiniCompassWithRuler() -{ - const Eigen::Matrix3f& R = app_state.viewLocal.rotation(); - Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; - Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; - Color rulerColor = - ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); - raylib_widgets::drawCompassRuler( - right, - up, - app_state.camera.euler.translate.z, - rulerColor, - raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" }); -} - -Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) -{ - Eigen::Vector3d hit = laser_beam.position; - raylib_widgets::intersectPlane(laser_beam.position, laser_beam.direction, plane.a, plane.b, plane.c, plane.d, hit); - return hit; -} - -LaserBeam GetLaserBeam(int x, int y) -{ - Ray ray = app_state.camera.eulerScreenRay(x, y, GetScreenWidth(), GetScreenHeight()); - - LaserBeam laser_beam; - laser_beam.position = Eigen::Vector3d(ray.position.x, ray.position.y, ray.position.z); - laser_beam.direction = Eigen::Vector3d(ray.direction.x, ray.direction.y, ray.direction.z); - - return laser_beam; -} - -double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) -{ - return raylib_widgets::distancePointToLine(point, line.position, line.direction); -} - -void setNewRotationCenter(int x, int y) -{ - const auto laser_beam = GetLaserBeam(x, y); - - RegistrationPlaneFeature::Plane pl; - - pl.a = 0; - pl.b = 0; - pl.c = 1; - pl.d = 0; - Eigen::Vector3f center_eigen = rayIntersection(laser_beam, pl).cast(); - - spdlog::info("Setting new rotation center to: {}, {}, {}", center_eigen.x(), center_eigen.y(), center_eigen.z()); - - app_state.camera.moveEulerRotationCenterTo(Vector3{ center_eigen.x(), center_eigen.y(), center_eigen.z() }); -} - -bool checkClHelp(int argc, char** argv) -{ - for (int i = 1; i < argc; ++i) - { - std::string arg(argv[i]); - - if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") - { - return true; - } - } - return false; -} - -// Was utils.cpp's getClosestTrajectoriesPoint(): picks the trajectory node nearest to the mouse ray -// and moves the rotation center there. Ctrl picks the loop-closure source (and time_stamp_offset), -// Shift the target; with more than two visible sessions it searches all visible ones. -void getClosestTrajectoriesPoint( - std::vector& sessions, - int x, - int y, - const int first_session_index, - const int second_session_index, - const int number_visible_sessions, - int& index_loop_closure_source, - int& index_loop_closure_target, - bool KeyShift, - double& time_stamp_offset) -{ - const auto laser_beam = GetLaserBeam(x, y); - double min_distance = std::numeric_limits::max(); - Vector3 center = app_state.camera.eulerGoal.rotationCenter; - - auto visit = [&](int s, bool update_source_target) - { - if (s < 0 || s >= static_cast(sessions.size())) - return; - const auto& pcs = sessions[s].point_clouds_container.point_clouds; - for (size_t i = 0; i < pcs.size(); i++) - { - for (size_t j = 0; j < pcs[i].local_trajectory.size(); j++) - { - Eigen::Vector3d vp = pcs[i].m_pose * pcs[i].local_trajectory[j].m_pose.translation(); - double dist = distance_point_to_line(vp, laser_beam); - if (dist >= min_distance) - continue; - min_distance = dist; - - if (!update_source_target) - { - center = Vector3{ static_cast(vp.x()), static_cast(vp.y()), static_cast(vp.z()) }; - time_stamp_offset = pcs[i].local_trajectory[j].timestamps.first; - } - else if (!KeyShift) // Ctrl - { - center = Vector3{ static_cast(vp.x()), static_cast(vp.y()), static_cast(vp.z()) }; - index_loop_closure_source = static_cast(i); - time_stamp_offset = pcs[i].local_trajectory[j].timestamps.first; - } - else // Shift - { - index_loop_closure_target = static_cast(i); - } - } - } - }; - - if (number_visible_sessions == 1) - visit(first_session_index, true); - else if (number_visible_sessions == 2) - visit(KeyShift ? second_session_index : first_session_index, true); - else - for (size_t s = 0; s < sessions.size(); s++) - if (sessions[s].visible) - visit(static_cast(s), false); - - app_state.camera.moveEulerRotationCenterTo(center); -} - -// Keeps session_renderers parallel to `sessions` and each renderer's GPU buffers in sync with its -// scans' poses. A size mismatch means the session list changed (load/remove), so everything is -// re-uploaded; otherwise only scans whose m_pose changed are rebuilt. -void syncSessionRenderers() -{ - if (session_renderers.size() != sessions.size()) - { - session_renderers.clear(); - bool first = true; - for (const auto& s : sessions) - { - auto renderer = std::make_unique(); - renderer->init(); - renderer->rebuildAll(s.point_clouds_container.point_clouds); - session_renderers.push_back(std::move(renderer)); - - if (s.point_clouds_container.point_clouds.empty()) - continue; - const auto d = s.point_clouds_container.compute_point_cloud_dimension(); - if (first) - scene_dims = d; - scene_dims.x_min = std::min(scene_dims.x_min, d.x_min); - scene_dims.x_max = std::max(scene_dims.x_max, d.x_max); - scene_dims.y_min = std::min(scene_dims.y_min, d.y_min); - scene_dims.y_max = std::max(scene_dims.y_max, d.y_max); - scene_dims.z_min = std::min(scene_dims.z_min, d.z_min); - scene_dims.z_max = std::max(scene_dims.z_max, d.z_max); - first = false; - } - scene_dims.length = scene_dims.x_max - scene_dims.x_min; - scene_dims.width = scene_dims.y_max - scene_dims.y_min; - scene_dims.height = scene_dims.z_max - scene_dims.z_min; - return; - } - - for (size_t i = 0; i < sessions.size(); i++) - session_renderers[i]->syncPoses(sessions[i].point_clouds_container.point_clouds); -} - -// Draws the given session's visible scans (points + trajectories), colored by points_color_mode. -// `only` restricts drawing to scans whose index it accepts (used by loop closure mode). -template -void drawSession(size_t session_index, Pred only) -{ - if (session_index >= sessions.size() || session_index >= session_renderers.size()) - return; - - auto& pcc = sessions[session_index].point_clouds_container; - auto& pcs = pcc.point_clouds; - - std::vector was_visible(pcs.size()); - for (size_t i = 0; i < pcs.size(); i++) - { - was_visible[i] = pcs[i].visible; - pcs[i].visible = pcs[i].visible && only(static_cast(i)); - } - - const auto& rc = app_state.camera.euler.rotationCenter; - session_renderers[session_index]->draw( - pcs, - static_cast(app_state.point_size), - points_color_mode, - static_cast(scene_dims.z_min), - static_cast(scene_dims.z_max), - Eigen::Vector3d(rc.x, rc.y, rc.z), - static_cast(std::max({ scene_dims.length, scene_dims.width, scene_dims.height, 1.0 })), - app_state.viewer_decimate_point_cloud, - pcc.xz_intersection, - pcc.yz_intersection, - pcc.xy_intersection, - static_cast(pcc.intersection_width), - pcc.show_with_initial_pose); - session_renderers[session_index]->drawTrajectories( - pcs, - viewer_reduce_rendered_trajectory, - pcc.show_imu_to_lio_diff, - pcc.xz_intersection, - pcc.yz_intersection, - pcc.xy_intersection, - pcc.show_with_initial_pose, - pcc.imu_to_lio_diff_scale); - - for (size_t i = 0; i < pcs.size(); i++) - pcs[i].visible = was_visible[i]; -} - -void drawSession(size_t session_index) -{ - drawSession( - session_index, - [](int) - { - return true; - }); -} - -// Was PointCloud::render(pose, ...): previews scan `index` of a session at `pose` (points only, from -// the cached GPU buffer), plus its trajectory at its real m_pose, as the GLUT version drew it. -void drawScanAtPose(size_t session_index, int index, const Eigen::Affine3d& pose, const float color[3]) -{ - if (session_index >= sessions.size() || session_index >= session_renderers.size()) - return; - const auto& pcs = sessions[session_index].point_clouds_container.point_clouds; - if (index < 0 || index >= static_cast(pcs.size()) || !pcs[index].visible) - return; - const auto& pc = pcs[index]; - - Color c = ColorFromNormalized(Vector4{ color[0], color[1], color[2], 1.f }); - session_renderers[session_index]->drawCachedWithTransform( - static_cast(index), pose * pc.m_pose.inverse(), c, static_cast(app_state.point_size), false); - - const int stride = std::max(1, viewer_reduce_rendered_trajectory); - rlBegin(RL_LINES); - rlColor3f(color[0], color[1], color[2]); - for (size_t i = stride; i < pc.local_trajectory.size(); i += stride) - { - Eigen::Vector3d a = (pc.m_pose * pc.local_trajectory[i - stride].m_pose).translation(); - Eigen::Vector3d b = (pc.m_pose * pc.local_trajectory[i].m_pose).translation(); - rlVertex3f(static_cast(a.x()), static_cast(a.y()), static_cast(a.z())); - rlVertex3f(static_cast(b.x()), static_cast(b.y()), static_cast(b.z())); - } - rlEnd(); -} - -void vertex(const Eigen::Vector3d& v) -{ - rlVertex3f(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); -} - -// Polyline through every scan pose of a session, colored per scan (was a GL_LINE_STRIP). -void drawPosePolyline(const Session& session) -{ - const auto& pcs = session.point_clouds_container.point_clouds; - rlBegin(RL_LINES); - for (size_t i = 1; i < pcs.size(); i++) - { - rlColor3f(pcs[i - 1].render_color[0], pcs[i - 1].render_color[1], pcs[i - 1].render_color[2]); - vertex(pcs[i - 1].m_pose.translation()); - rlColor3f(pcs[i].render_color[0], pcs[i].render_color[1], pcs[i].render_color[2]); - vertex(pcs[i].m_pose.translation()); - } - rlEnd(); -} - -// Edge line between two poses plus a 10 m vertical flagpole at its midpoint (label drawn in the 2D pass). -void drawEdge(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, float r, float g, float b) -{ - const Eigen::Vector3d mid = (v1 + v2) * 0.5; - rlBegin(RL_LINES); - rlColor3f(r, g, b); - vertex(v1); - vertex(v2); - vertex(mid); - vertex(mid + Eigen::Vector3d(0, 0, 10)); - rlEnd(); -} - -bool validScan(int session_index, int scan_index) -{ - return session_index >= 0 && session_index < static_cast(sessions.size()) && scan_index >= 0 && - scan_index < static_cast(sessions[session_index].point_clouds_container.point_clouds.size()); -} - -void drawUncertaintyEllipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Color color) -{ - Eigen::LLT> cholSolver(covar); - Eigen::Matrix3d transform = cholSolver.matrixL(); - - const double pi = 3.141592; - const double di = 0.02; - const double dj = 0.04; - const double du = di * 2 * pi; - const double dv = dj * pi; - - rlBegin(RL_LINES); - rlColor4ub(color.r, color.g, color.b, color.a); - for (double i = 0; i < 1.0; i += di) - { - for (double j = 0; j < 1.0; j += dj) - { - double u = i * 2 * pi; - double v = (j - 0.5) * pi; - - const Eigen::Vector3d tp0 = transform * Eigen::Vector3d(cos(v) * cos(u), cos(v) * sin(u), sin(v)) + mean; - const Eigen::Vector3d tp1 = transform * Eigen::Vector3d(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)) + mean; - const Eigen::Vector3d tp2 = - transform * Eigen::Vector3d(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)) + mean; - const Eigen::Vector3d tp3 = transform * Eigen::Vector3d(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)) + mean; - - vertex(tp0); - vertex(tp1); - vertex(tp1); - vertex(tp2); - vertex(tp2); - vertex(tp3); - vertex(tp3); - vertex(tp0); - } - } - rlEnd(); -} - -// Was GroundControlPoints::render() (legacy GL in core); same drawing as step 2's port. Labels are -// drawn in the 2D pass. -void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container) -{ - const Color markColor{ 179, 77, 128, 255 }; - const Color connectorColor{ 0, 77, 153, 255 }; - - for (const auto& gcp : ground_control_points.gpcs) - { - if (gcp.index_to_node_inner < 0 || static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) - continue; - const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner]; - if (gcp.index_to_node_outer < 0 || static_cast(gcp.index_to_node_outer) >= pc.local_trajectory.size()) - continue; - - Eigen::Vector3d c = pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation(); - float h = static_cast(gcp.lidar_height_above_ground); - Vector3 g{ static_cast(gcp.x), static_cast(gcp.y), static_cast(gcp.z) }; - - DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); - DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); - DrawLine3D(Vector3{ g.x - 0.01f, g.y, g.z + h }, Vector3{ g.x + 0.01f, g.y, g.z + h }, markColor); - DrawLine3D(Vector3{ g.x, g.y - 0.01f, g.z + h }, Vector3{ g.x, g.y + 0.01f, g.z + h }, markColor); - DrawLine3D(g, Vector3{ g.x, g.y, g.z + h }, markColor); - DrawLine3D( - Vector3{ static_cast(c.x()), static_cast(c.y()), static_cast(c.z()) }, - Vector3{ g.x, g.y, g.z + h }, - connectorColor); - - if (ground_control_points.draw_uncertainty) - { - Eigen::Matrix3d covar = Eigen::Matrix3d::Zero(); - covar(0, 0) = gcp.sigma_x * gcp.sigma_x; - covar(1, 1) = gcp.sigma_y * gcp.sigma_y; - covar(2, 2) = gcp.sigma_z * gcp.sigma_z; - drawUncertaintyEllipse(covar, Eigen::Vector3d(gcp.x, gcp.y, gcp.z + h), GRAY); - } - } -} - -// Was ControlPoints::render(pcs, show_pc = false) (legacy GL in core): markers only; step 3 never -// opens the control points editor, so step 2's editor branch is not needed. -void renderControlPoints(const ControlPoints& control_points, const PointClouds& point_clouds_container) -{ - const Color markColor{ 179, 77, 128, 255 }; - const Color connectorColor{ 0, 77, 153, 255 }; - const auto& pcs = point_clouds_container.point_clouds; - - for (const auto& cp : control_points.cps) - { - if (cp.index_to_pose < 0 || static_cast(cp.index_to_pose) >= pcs.size()) - continue; - - Eigen::Vector3d c = pcs[cp.index_to_pose].m_pose * Eigen::Vector3d(cp.x_source_local, cp.y_source_local, cp.z_source_local); - Vector3 g{ static_cast(cp.x_target_global), static_cast(cp.y_target_global), static_cast(cp.z_target_global) }; - - DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); - DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); - DrawLine3D(Vector3{ g.x - 0.01f, g.y, g.z }, Vector3{ g.x + 0.01f, g.y, g.z }, markColor); - DrawLine3D(Vector3{ g.x, g.y - 0.01f, g.z }, Vector3{ g.x, g.y + 0.01f, g.z }, markColor); - DrawLine3D(Vector3{ static_cast(c.x()), static_cast(c.y()), static_cast(c.z()) }, g, connectorColor); - - if (control_points.draw_uncertainty) - { - Eigen::Matrix3d covar = Eigen::Matrix3d::Zero(); - covar(0, 0) = cp.is_z_0 ? 0.01 * 0.01 : cp.sigma_x * cp.sigma_x; - covar(1, 1) = cp.is_z_0 ? 0.01 * 0.01 : cp.sigma_y * cp.sigma_y; - covar(2, 2) = cp.sigma_z * cp.sigma_z; - drawUncertaintyEllipse(covar, Eigen::Vector3d(cp.x_target_global, cp.y_target_global, cp.z_target_global), GRAY); - } - } -} - -namespace -{ - // Outlined so labels stay readable over same-colored geometry; `line` stacks labels above one anchor. - void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color, int line = 0) - { - int x = static_cast(anchor.x) + 6; - int y = static_cast(anchor.y) - fontSize - 6 - line * (fontSize + 4); - for (int dx = -1; dx <= 1; ++dx) - for (int dy = -1; dy <= 1; ++dy) - if (dx != 0 || dy != 0) - DrawText(text, x + dx, y + dy, fontSize, BLACK); - DrawText(text, x, y, fontSize, color); - } - - // Projects a world point with frame_mvp_3d (needs w for the perspective divide, so not Vector3Transform). - Vector2 worldToScreen(const Eigen::Vector3d& world) - { - const ImGuiIO& io = ImGui::GetIO(); - const Matrix& m = frame_mvp_3d; - float x = static_cast(world.x()); - float y = static_cast(world.y()); - float z = static_cast(world.z()); - float clipX = m.m0 * x + m.m4 * y + m.m8 * z + m.m12; - float clipY = m.m1 * x + m.m5 * y + m.m9 * z + m.m13; - float clipW = m.m3 * x + m.m7 * y + m.m11 * z + m.m15; - if (clipW < 1e-6f) // behind the camera, or degenerate - return Vector2{ -1000.f, -1000.f }; - float ndcX = clipX / clipW; - float ndcY = clipY / clipW; - return Vector2{ (ndcX * 0.5f + 0.5f) * io.DisplaySize.x, (1.0f - (ndcY * 0.5f + 0.5f)) * io.DisplaySize.y }; - } - - Color colorOf(const float c[3]) - { - return ColorFromNormalized(Vector4{ c[0], c[1], c[2], 1.f }); - } -} // namespace - -void renderGroundControlPointsLabels(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container) -{ - const Color markColor{ 179, 77, 128, 255 }; - const Color connectorColor{ 0, 77, 153, 255 }; - - for (size_t i = 0; i < ground_control_points.gpcs.size(); ++i) - { - const auto& gcp = ground_control_points.gpcs[i]; - Vector2 anchor = worldToScreen(Eigen::Vector3d(gcp.x, gcp.y, gcp.z)); - drawOutlinedText(gcp.name, anchor, 22, WHITE, 2); - drawOutlinedText(TextFormat("GCP_%d: LiDAR center", static_cast(i)), anchor, 14, markColor, 1); - drawOutlinedText(TextFormat("GCP_%d: 'plane on the ground'", static_cast(i)), anchor, 14, markColor, 0); - - if (gcp.index_to_node_inner < 0 || static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) - continue; - const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner]; - if (gcp.index_to_node_outer < 0 || static_cast(gcp.index_to_node_outer) >= pc.local_trajectory.size()) - continue; - - Eigen::Vector3d c = pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation(); - drawOutlinedText(TextFormat("GCP_%d: assigned trajectory node", static_cast(i)), worldToScreen(c), 14, connectorColor); - } -} - -void renderControlPointsLabels(const ControlPoints& control_points, const PointClouds& point_clouds_container) -{ - const Color markColor{ 179, 77, 128, 255 }; - - for (size_t i = 0; i < control_points.cps.size(); ++i) - { - const auto& cp = control_points.cps[i]; - Vector2 anchor = worldToScreen(Eigen::Vector3d(cp.x_target_global, cp.y_target_global, cp.z_target_global)); - drawOutlinedText(cp.name, anchor, 22, WHITE, 1); - drawOutlinedText(TextFormat("CP_%d", static_cast(i)), anchor, 14, WHITE, 0); - - if (cp.index_to_pose < 0 || static_cast(cp.index_to_pose) >= point_clouds_container.point_clouds.size()) - continue; - - Eigen::Vector3d c = point_clouds_container.point_clouds[cp.index_to_pose].m_pose * - Eigen::Vector3d(cp.x_source_local, cp.y_source_local, cp.z_source_local); - drawOutlinedText(TextFormat("CP_%d: initial location", static_cast(i)), worldToScreen(c), 14, markColor); - } -} - -// Was the glRasterPos3f + glutBitmapString labels of loop closure mode: scan indices of the first and -// second session (in scan color), per-session pose graph edges (blue) and inter-session edges -// (cyan if a ground truth session is involved, otherwise yellow), at the top of each edge's flagpole. -void renderLoopClosureLabels() -{ - for (int s : { first_session_index, second_session_index }) - { - if (s < 0 || s >= static_cast(sessions.size())) - continue; - const auto& pcs = sessions[s].point_clouds_container.point_clouds; - for (size_t i = 0; i < pcs.size(); i++) - drawOutlinedText( - TextFormat("%d", static_cast(i)), - worldToScreen(pcs[i].m_pose.translation() + Eigen::Vector3d(0, 0, 0.1)), - 20, - colorOf(pcs[i].render_color)); - if (first_session_index == second_session_index) - break; - } - - for (size_t i = 0; i < sessions.size(); i++) - { - const auto& pcs = sessions[i].point_clouds_container.point_clouds; - const auto& pg_edges = sessions[i].pose_graph_loop_closure.edges; - for (size_t j = 0; j < pg_edges.size(); j++) - { - if (!validScan(static_cast(i), pg_edges[j].index_from) || !validScan(static_cast(i), pg_edges[j].index_to)) - continue; - Eigen::Vector3d mid = (pcs[pg_edges[j].index_from].m_pose.translation() + pcs[pg_edges[j].index_to].m_pose.translation()) * 0.5; - drawOutlinedText(TextFormat("%d", static_cast(j)), worldToScreen(mid + Eigen::Vector3d(0, 0, 10.1)), 22, BLUE); - } - } - - for (size_t i = 0; i < edges.size(); i++) - { - const auto& e = edges[i]; - if (!validScan(e.index_session_from, e.index_from) || !validScan(e.index_session_to, e.index_to)) - continue; - Eigen::Vector3d v1 = sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.translation(); - Eigen::Vector3d v2 = sessions[e.index_session_to].point_clouds_container.point_clouds[e.index_to].m_pose.translation(); - bool gt = sessions[e.index_session_from].is_ground_truth || sessions[e.index_session_to].is_ground_truth; - drawOutlinedText( - TextFormat("%d", static_cast(i)), worldToScreen((v1 + v2) * 0.5 + Eigen::Vector3d(0, 0, 10.1)), 22, gt ? SKYBLUE : YELLOW); - } -} - -// Copies the current rlgl modelview/projection into column-major float[16] for ImGuizmo. -void currentGizmoMatrices(float modelview[16], float projection[16]) -{ - Matrix p = rlGetMatrixProjection(); - Matrix m = rlGetMatrixModelview(); - const float pv[16] = { p.m0, p.m1, p.m2, p.m3, p.m4, p.m5, p.m6, p.m7, p.m8, p.m9, p.m10, p.m11, p.m12, p.m13, p.m14, p.m15 }; - const float mv[16] = { m.m0, m.m1, m.m2, m.m3, m.m4, m.m5, m.m6, m.m7, m.m8, m.m9, m.m10, m.m11, m.m12, m.m13, m.m14, m.m15 }; - std::copy(pv, pv + 16, projection); - std::copy(mv, mv + 16, modelview); -} - -// ImGuizmo on m_gizmo with this app's usual operation sets: full 3D in perspective, planar in ortho. -void manipulateGizmo() -{ - if (!app_state.camera.isOrtho) - { - float modelview[16], projection[16]; - currentGizmoMatrices(modelview, projection); - ImGuizmo::Manipulate( - modelview, - projection, - ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, - ImGuizmo::WORLD, - m_gizmo, - NULL); - } - else - ImGuizmo::Manipulate( - app_state.camera.orthoGizmoView, - app_state.camera.orthoProjection, - ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, - ImGuizmo::WORLD, - m_gizmo, - NULL); -} - +int num_edge_extended_before = 0; +int num_edge_extended_after = 0; + +int gui_point_size = 2; + +TaitBryanPose motion_model_weights = { 0.01, 0.01, 0.01, 0.1, 0.1, 0.1 }; /////////////////////////////////////////////////////////////////////////////////// void ndt_gui() @@ -1421,9 +389,9 @@ void loop_closure_gui() // auto point_cloud_upper = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1; - ImGui::InputInt("gui_point_size", &app_state.point_size); - if (app_state.point_size < 1) - app_state.point_size = 1; + ImGui::InputInt("gui_point_size", &gui_point_size); + if (gui_point_size < 1) + gui_point_size = 1; ImGui::Text("Num edge extended:"); @@ -2951,6 +1919,9 @@ void loadSessions() std::cout << "session: '" << s.session_file_name << "' ground truth [" << int(s.is_ground_truth) << "]" << std::endl; } + assignDistinctSessionColors(sessions); + invalidateSessionRenderers(); + // update time_stamp_offset std::cout << "update time_stamp_offset" << std::endl; for (const auto& s : sessions) @@ -3577,195 +2548,499 @@ void settings_gui() void display() { - syncSessionRenderers(); + syncSessionRenderers(sessions); ImGuiIO& io = ImGui::GetIO(); - // Framebuffer pixels, not io.DisplaySize (they differ on HiDPI) -- see step 2's display(). rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); - ClearBackground(ColorFromNormalized( - Vector4{ app_state.bg_color.x * app_state.bg_color.w, - app_state.bg_color.y * app_state.bg_color.w, - app_state.bg_color.z * app_state.bg_color.w, - app_state.bg_color.w })); + ClearBackground(ColorFromNormalized(Vector4{ bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w })); rlEnableDepthTest(); rlMatrixMode(RL_PROJECTION); rlLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - auto& camera = app_state.camera; - camera.updateEulerTransition(io.DeltaTime); + updateCameraTransition(); - app_state.viewLocal = Eigen::Affine3f::Identity(); + viewLocal = Eigen::Affine3f::Identity(); + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + pc.point_size = gui_point_size; + } + } - if (!camera.isOrtho) + if (!is_ortho) { - camera.applyPerspectiveProjection((int)io.DisplaySize.x, (int)io.DisplaySize.y); + reshape((int)io.DisplaySize.x, (int)io.DisplaySize.y); - // In loop closure mode the rotation center follows the source scan / active edge (when enabled). - if (is_loop_closure_gui && update_rotation_center) + // janusz + if (is_loop_closure_gui) { - auto follow = [&](const Eigen::Vector3d& t) + // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, + // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, + // false, false, 100000); + // sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false, + // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, + // false, false, 100000); + + if (first_session_index < sessions[first_session_index].point_clouds_container.point_clouds.size()) { - camera.euler.rotationCenter = Vector3{ static_cast(t.x()), static_cast(t.y()), static_cast(t.z()) }; - camera.eulerGoal.rotationCenter = camera.euler.rotationCenter; - }; - - if (validScan(first_session_index, index_loop_closure_source)) - follow(sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation()); + if (update_rotation_center) + { + rotation_center.x() = sessions[first_session_index] + .point_clouds_container.point_clouds[index_loop_closure_source] + .m_pose.translation() + .x(); + rotation_center.y() = sessions[first_session_index] + .point_clouds_container.point_clouds[index_loop_closure_source] + .m_pose.translation() + .y(); + rotation_center.z() = sessions[first_session_index] + .point_clouds_container.point_clouds[index_loop_closure_source] + .m_pose.translation() + .z(); + } + } - if (manipulate_active_edge && index_active_edge >= 0 && index_active_edge < static_cast(edges.size())) + if (manipulate_active_edge) { - const auto& e = edges[index_active_edge]; - if (validScan(e.index_session_from, e.index_from)) - follow(sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.translation()); + if (edges.size() > 0) + { + int index_src = edges[index_active_edge].index_from; + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + + if (update_rotation_center) + { + rotation_center.x() = m_src(0, 3); + rotation_center.y() = m_src(1, 3); + rotation_center.z() = m_src(2, 3); + } + } } + + /*if (session.pose_graph_loop_closure.manipulate_active_edge) + { + if (session.pose_graph_loop_closure.edges.size() > 0) + { + if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) + { + rotation_center.x() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(0, + 3); rotation_center.y() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(1, + 3); rotation_center.z() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(2, + 3); + } + } + }*/ } - Eigen::Vector3f rotationCenter(camera.euler.rotationCenter.x, camera.euler.rotationCenter.y, camera.euler.rotationCenter.z); - app_state.viewLocal.translate(rotationCenter); - app_state.viewLocal.translate(Eigen::Vector3f(camera.euler.translate.x, camera.euler.translate.y, camera.euler.translate.z)); - if (!camera.lockZ) - app_state.viewLocal.rotate(Eigen::AngleAxisf(camera.euler.rotateX * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.translate(rotation_center); + + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else - app_state.viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - app_state.viewLocal.rotate(Eigen::AngleAxisf(camera.euler.rotateY * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - app_state.viewLocal.translate(-rotationCenter); + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - rlMultMatrixf(app_state.viewLocal.matrix().data()); + viewLocal.translate(-rotation_center); + + rlMultMatrixf(viewLocal.matrix().data()); } else - { - app_state.viewLocal.rotate(Eigen::AngleAxisf((camera.euler.rotateX + camera.euler.rotateY) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - camera.updateOrtho(ratio); - } - - camera.captureFrameMatrices(); - frame_mvp_3d = MatrixMultiply(camera.frameView3D, camera.frameProj3D); + updateOrthoView(); + captureFrameMatrices(); showAxes(); if (is_loop_closure_gui) { - // Scans within [index - before, index + after] of the loop closure source/target. - auto in_range = [](int center) + if (manipulate_active_edge) { - return [center](int i) + if (edges.size() > 0) { - return i >= center - num_edge_extended_before && i <= center + num_edge_extended_after; - }; - }; + /*int index_src = edges[index_active_edge].index_from; + int index_trg = edges[index_active_edge].index_to; + + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + Eigen::Affine3d m_trg = m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render( + m_src, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render_color); + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render( + m_trg, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render_color);*/ + + int index_src = edges[index_active_edge].index_from; + int index_trg = edges[index_active_edge].index_to; + + Eigen::Affine3d _m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + Eigen::Affine3d _m_trg = _m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + Eigen::Affine3d m_src_0 = + sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).m_pose; // Todo + + for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; + i++) + { + if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && + sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + + Eigen::Affine3d m_src_curr = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo + Eigen::Affine3d m_src = _m_src * (m_src_0.inverse() * m_src_curr); + + // sessions[first_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; + + renderScanAtPose( + first_session_index, + i, + m_src, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(i).render_color); + } + } - const bool edge_ok = manipulate_active_edge && index_active_edge >= 0 && index_active_edge < static_cast(edges.size()) && - validScan(edges[index_active_edge].index_session_from, edges[index_active_edge].index_from); + Eigen::Affine3d m_trg_0 = + sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).m_pose; // Todo - if (edge_ok && validScan(first_session_index, index_loop_closure_source) && - validScan(second_session_index, index_loop_closure_target)) + for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; + i++) + { + if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && + sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + Eigen::Affine3d m_trg_curr = + sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo + Eigen::Affine3d m_trg = _m_trg * (m_trg_0.inverse() * m_trg_curr); + + // sessions[second_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; + renderScanAtPose( + second_session_index, + i, + m_trg, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(i).render_color); + } + } + } + } + else { - // Preview the active edge: the source range placed at the edge's source pose, the target range - // at source * relative_pose. Like the GLUT version, the ranges are taken around - // index_loop_closure_source/target of the first/second visible session. - const auto& e = edges[index_active_edge]; - Eigen::Affine3d edge_src = sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose; - Eigen::Affine3d edge_trg = edge_src * affine_matrix_from_pose_tait_bryan(e.relative_pose_tb); - - const auto& first_pcs = sessions[first_session_index].point_clouds_container.point_clouds; - Eigen::Affine3d src_0 = first_pcs[index_loop_closure_source].m_pose; + ObservationPicking observation_picking; + + /*sessions[first_session_index] + .point_clouds_container.point_clouds.at(index_loop_closure_source) + .render( + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false);*/ + for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; i++) - if (i >= 0 && i < static_cast(first_pcs.size())) - drawScanAtPose(first_session_index, i, edge_src * (src_0.inverse() * first_pcs[i].m_pose), first_pcs[i].render_color); + { + if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && + sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + Eigen::Affine3d m_src = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; + + renderScan( + first_session_index, + i, + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false); + } + } + + /*sessions[second_session_index] + .point_clouds_container.point_clouds.at(index_loop_closure_target) + .render( + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false);*/ - const auto& second_pcs = sessions[second_session_index].point_clouds_container.point_clouds; - Eigen::Affine3d trg_0 = second_pcs[index_loop_closure_target].m_pose; for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; i++) - if (i >= 0 && i < static_cast(second_pcs.size())) - drawScanAtPose( - second_session_index, i, edge_trg * (trg_0.inverse() * second_pcs[i].m_pose), second_pcs[i].render_color); + { + if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && + sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) + { + // ObservationPicking observation_picking; + // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, + // false); + Eigen::Affine3d m_src = sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; + + renderScan( + second_session_index, + i, + false, + observation_picking, + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + false, + false, + false, + 100000, + false); + } + } } - else if (!manipulate_active_edge) + + // sessions[first_session_index].point_clouds_container.render(); + + beginLineStrip(); + for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) { - if (first_session_index >= 0) - drawSession(first_session_index, in_range(index_loop_closure_source)); - if (second_session_index >= 0 && second_session_index != first_session_index) - drawSession(second_session_index, in_range(index_loop_closure_target)); - else if (second_session_index >= 0) - drawSession( - second_session_index, - [&](int i) - { - return in_range(index_loop_closure_source)(i) || in_range(index_loop_closure_target)(i); - }); + color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + lineStripVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + } + endLineStrip(); + + int i = 0; + for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) + { + color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + labelPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); + labelText(std::to_string(i).c_str()); + i++; + } + + beginLineStrip(); + for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) + { + color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + lineStripVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); } + endLineStrip(); - for (int s : { first_session_index, second_session_index }) - if (s >= 0 && s < static_cast(sessions.size())) - drawPosePolyline(sessions[s]); + i = 0; + for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) + { + color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); + labelPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); + labelText(std::to_string(i).c_str()); + i++; + } for (size_t i = 0; i < sessions.size(); i++) { - const auto& pcs = sessions[i].point_clouds_container.point_clouds; - for (const auto& pg_edge : sessions[i].pose_graph_loop_closure.edges) - if (validScan(static_cast(i), pg_edge.index_from) && validScan(static_cast(i), pg_edge.index_to)) - drawEdge(pcs[pg_edge.index_from].m_pose.translation(), pcs[pg_edge.index_to].m_pose.translation(), 0.f, 0.f, 1.f); + for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++) + { + int index_src = sessions[i].pose_graph_loop_closure.edges[j].index_from; + int index_trg = sessions[i].pose_graph_loop_closure.edges[j].index_to; + + color3f(0.0f, 0.0f, 1.0f); + rlBegin(RL_LINES); + auto v1 = sessions[i].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); + auto v2 = sessions[i].point_clouds_container.point_clouds.at(index_trg).m_pose.translation(); + rlVertex3f(v1.x(), v1.y(), v1.z()); + rlVertex3f(v2.x(), v2.y(), v2.z()); + + rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5); + rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10); + rlEnd(); + + labelPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); + labelText(std::to_string(j).c_str()); + } } - for (const auto& e : edges) + for (size_t i = 0; i < edges.size(); i++) { - if (!validScan(e.index_session_from, e.index_from) || !validScan(e.index_session_to, e.index_to)) - continue; - bool gt = sessions[e.index_session_from].is_ground_truth || sessions[e.index_session_to].is_ground_truth; - drawEdge( - sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.translation(), - sessions[e.index_session_to].point_clouds_container.point_clouds[e.index_to].m_pose.translation(), - gt ? 0.f : 1.f, // cyan with a ground truth session, otherwise yellow - 1.f, - gt ? 1.f : 0.f); + int index_src = edges[i].index_from; + int index_trg = edges[i].index_to; + + int index_session_from = edges[i].index_session_from; + int index_session_to = edges[i].index_session_to; + + if (sessions[index_session_from].is_ground_truth || sessions[index_session_to].is_ground_truth) + color3f(0.0f, 1.0f, 1.0f); + else + color3f(1.0f, 1.0f, 0.0f); + + rlBegin(RL_LINES); + auto v1 = sessions[index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); + auto v2 = sessions[index_session_to].point_clouds_container.point_clouds.at(index_trg).m_pose.translation(); + rlVertex3f(v1.x(), v1.y(), v1.z()); + rlVertex3f(v2.x(), v2.y(), v2.z()); + + rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5); + rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10); + rlEnd(); + + labelPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); + labelText(std::to_string(i).c_str()); } } else { - for (size_t s = 0; s < sessions.size(); s++) + for (auto& session : sessions) { - auto& session = sessions[s]; - if (!session.visible) - continue; - - drawSession(s); - renderGroundControlPoints(session.ground_control_points, session.point_clouds_container); - renderControlPoints(session.control_points, session.point_clouds_container); - - // +-5 m cross at the session's first trajectory node after time_stamp_offset. - const auto& pcs = session.point_clouds_container.point_clouds; - bool found = false; - for (size_t a = 0; a < pcs.size() && !found; a++) + if (session.visible) { - for (size_t b = 0; b < pcs[a].local_trajectory.size(); b++) + renderSession(session, observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory); + renderGroundControlPoints(session.ground_control_points, session.point_clouds_container); + renderControlPoints(session.control_points, session.point_clouds_container); + + //// + int index_point_clouds = -1; + int index_local_trajectory = -1; + bool found = false; + for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++) + { + for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++) + { + if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset) + { + if (!found) + { + found = true; + index_point_clouds = a; + index_local_trajectory = b; + break; + } + } + } + } + + if (index_point_clouds != -1 && index_local_trajectory != -1) { - if (pcs[a].local_trajectory[b].timestamps.first > time_stamp_offset) + if (index_local_trajectory < session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory.size()) { - found = true; - Eigen::Vector3d v1 = (pcs[a].m_pose * pcs[a].local_trajectory[b].m_pose).translation(); + color3f( + session.point_clouds_container.point_clouds[index_point_clouds].render_color[0], + session.point_clouds_container.point_clouds[index_point_clouds].render_color[1], + session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]); rlBegin(RL_LINES); - rlColor3f(pcs[a].render_color[0], pcs[a].render_color[1], pcs[a].render_color[2]); - vertex(v1 - Eigen::Vector3d(5, 0, 0)); - vertex(v1 + Eigen::Vector3d(5, 0, 0)); - vertex(v1 - Eigen::Vector3d(0, 5, 0)); - vertex(v1 + Eigen::Vector3d(0, 5, 0)); - vertex(v1 - Eigen::Vector3d(0, 0, 5)); - vertex(v1 + Eigen::Vector3d(0, 0, 5)); + + auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose; + auto m2 = + session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; + + auto v1 = (m1 * m2).translation(); + + rlVertex3f(v1.x() - 5.0, v1.y(), v1.z()); + rlVertex3f(v1.x() + 5.0, v1.y(), v1.z()); + + rlVertex3f(v1.x(), v1.y() - 5.0, v1.z()); + rlVertex3f(v1.x(), v1.y() + 5.0, v1.z()); + + rlVertex3f(v1.x(), v1.y(), v1.z() - 5.0); + rlVertex3f(v1.x(), v1.y(), v1.z() + 5.0); + rlEnd(); - break; } } } } } - // rlImGuiBegin() only feeds input to ImGui and starts its frame; it leaves the rlgl 3D matrices - // active, so the gizmo code below still sees this frame's camera. + /*if (is_loop_closure_gui) + { + session.manual_pose_graph_loop_closure.Render(session.point_clouds_container, index_loop_closure_source, index_loop_closure_target); + } + else + { + for (const auto &g : available_geo_points) + { + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + auto c = g.coordinates - session.point_clouds_container.offset; + glVertex3f(c.x() - 0.5, c.y(), c.z()); + glVertex3f(c.x() + 0.5, c.y(), c.z()); + + glVertex3f(c.x(), c.y() - 0.5, c.z()); + glVertex3f(c.x(), c.y() + 0.5, c.z()); + + glVertex3f(c.x(), c.y(), c.z() - 0.5); + glVertex3f(c.x(), c.y(), c.z() + 0.5); + glEnd(); + } + + // + for (const auto &pc : session.point_clouds_container.point_clouds) + { + for (const auto &gp : pc.available_geo_points) + { + if (gp.choosen) + { + auto c = pc.m_pose * gp.coordinates; + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(c.x() - 0.5, c.y(), c.z()); + glVertex3f(c.x() + 0.5, c.y(), c.z()); + + glVertex3f(c.x(), c.y() - 0.5, c.z()); + glVertex3f(c.x(), c.y() + 0.5, c.z()); + + glVertex3f(c.x(), c.y(), c.z() - 0.5); + glVertex3f(c.x(), c.y(), c.z() + 0.5); + glEnd(); + + glBegin(GL_LINES); + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(c.x(), c.y(), c.z()); + glVertex3f(gp.coordinates.x(), gp.coordinates.y(), gp.coordinates.z()); + glEnd(); + + glColor3f(0.0f, 0.0f, 0.0f); + glBegin(GL_LINES); + glVertex3f(c.x(), c.y(), c.z()); + glVertex3f(c.x() + 10, c.y(), c.z()); + glEnd(); + + glRasterPos3f(c.x() + 10, c.y(), c.z()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str()); + } + } + } + }*/ + + // gnss.render(session.point_clouds_container); + rlImGuiBegin(); ShowMainDockSpace(); @@ -3793,7 +3068,30 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - manipulateGizmo(); + if (!is_ortho) + { + float projection[16]; + getProjectionMatrix(projection); + + float modelview[16]; + getModelviewMatrix(modelview); + + ImGuizmo::Manipulate( + modelview, + projection, + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); + } + else + ImGuizmo::Manipulate( + m_ortho_gizmo_view, + m_ortho_projection, + ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, + ImGuizmo::WORLD, + m_gizmo, + NULL); sessions[i].point_clouds_container.point_clouds[0].m_pose = Eigen::Map(m_gizmo).cast(); prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose; @@ -3908,7 +3206,30 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - manipulateGizmo(); + if (!is_ortho) + { + float projection[16]; + getProjectionMatrix(projection); + + float modelview[16]; + getModelviewMatrix(modelview); + + ImGuizmo::Manipulate( + &modelview[0], + &projection[0], + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); + } + else + ImGuizmo::Manipulate( + m_ortho_gizmo_view, + m_ortho_projection, + ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, + ImGuizmo::WORLD, + m_gizmo, + NULL); Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); @@ -3922,6 +3243,215 @@ void display() } } + /*if (!is_loop_closure_gui) +{ + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].gizmo) + { + std::vector all_m_poses; + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); + + ImGuiIO &io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | +ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | +ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; + session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; + session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; + session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; + session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; + session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; + session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; + session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; + session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; + session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; + session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; + session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; + session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; + session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; + session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; + session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; + session.point_clouds_container.point_clouds[i].pose = +pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); + + session.point_clouds_container.point_clouds[i].gui_translation[0] = +(float)session.point_clouds_container.point_clouds[i].pose.px; session.point_clouds_container.point_clouds[i].gui_translation[1] = +(float)session.point_clouds_container.point_clouds[i].pose.py; session.point_clouds_container.point_clouds[i].gui_translation[2] = +(float)session.point_clouds_container.point_clouds[i].pose.pz; + + session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om +* RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[1] = +(float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[2] += (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); + + if (!manipulate_only_marked_gizmo) + { + Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; + for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + session.point_clouds_container.point_clouds[j].pose = +pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); + + session.point_clouds_container.point_clouds[j].gui_translation[0] = +(float)session.point_clouds_container.point_clouds[j].pose.px; session.point_clouds_container.point_clouds[j].gui_translation[1] = +(float)session.point_clouds_container.point_clouds[j].pose.py; session.point_clouds_container.point_clouds[j].gui_translation[2] = +(float)session.point_clouds_container.point_clouds[j].pose.pz; + + session.point_clouds_container.point_clouds[j].gui_rotation[0] = +(float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); session.point_clouds_container.point_clouds[j].gui_rotation[1] += (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[j].gui_rotation[2] = +(float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + } + } + } + + session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud); + observation_picking.render(); + + glPushAttrib(GL_ALL_ATTRIB_BITS); + glPointSize(5); + for (const auto &obs : observation_picking.observations) + { + for (const auto &[key1, value1] : obs) + { + for (const auto &[key2, value2] : obs) + { + if (key1 != key2) + { + Eigen::Vector3d p1, p2; + if (session.point_clouds_container.show_with_initial_pose) + { + p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; + } + else + { + p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; + } + glColor3f(0, 1, 0); + glBegin(GL_POINTS); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + glColor3f(1, 0, 0); + glBegin(GL_LINES); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + } + } + } + } + glPopAttrib(); + + for (const auto &obs : observation_picking.observations) + { + Eigen::Vector3d mean(0, 0, 0); + int counter = 0; + for (const auto &[key1, value1] : obs) + { + mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + counter++; + } + if (counter > 0) + { + mean /= counter; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glEnd();F + } + } + + glColor3f(1, 0, 1); + glBegin(GL_POINTS); + for (auto p : picked_points) + { + glVertex3f(p.x(), p.y(), p.z()); + } + glEnd(); +} +else +{ + // ImGuizmo ----------------------------------------------- + if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0) + { + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | +ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, +ImGuizmo::WORLD, m_gizmo, NULL); + + Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); + + m_g(0, 0) = m_gizmo[0]; + m_g(1, 0) = m_gizmo[1]; + m_g(2, 0) = m_gizmo[2]; + m_g(3, 0) = m_gizmo[3]; + m_g(0, 1) = m_gizmo[4]; + m_g(1, 1) = m_gizmo[5]; + m_g(2, 1) = m_gizmo[6]; + m_g(3, 1) = m_gizmo[7]; + m_g(0, 2) = m_gizmo[8]; + m_g(1, 2) = m_gizmo[9]; + m_g(2, 2) = m_gizmo[10]; + m_g(3, 2) = m_gizmo[11]; + m_g(0, 3) = m_gizmo[12]; + m_g(1, 3) = m_gizmo[13]; + m_g(2, 3) = m_gizmo[14]; + m_g(3, 3) = m_gizmo[15]; + + const int &index_src = +session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from; + + const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; + session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb = +pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); + } +}*/ + view_kbd_shortcuts(); if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) @@ -4527,38 +4057,45 @@ void display() { ImGui::BeginDisabled(!(sessions.size() > 0)); { - auto tmp = app_state.point_size; + auto tmp = point_size; ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points size", &app_state.point_size); + ImGui::InputInt("Points size", &point_size); if (ImGui::IsItemHovered()) ImGui::SetTooltip("keyboard 1-9 keys"); - if (app_state.point_size < 1) - app_state.point_size = 1; - else if (app_state.point_size > 10) - app_state.point_size = 10; + if (point_size < 1) + point_size = 1; + else if (point_size > 10) + point_size = 10; - if (tmp != app_state.point_size) + if (tmp != point_size) for (auto& session : sessions) for (auto& point_cloud : session.point_clouds_container.point_clouds) - point_cloud.point_size = app_state.point_size; + point_cloud.point_size = point_size; ImGui::Separator(); } ImGui::EndDisabled(); - if (ImGui::MenuItem("Orthographic", "key O", &app_state.camera.isOrtho)) + if (ImGui::MenuItem("Orthographic", "key O", &is_ortho)) { - if (app_state.camera.isOrtho) - app_state.camera.startEulerTransition( - 0.0f, 0.0f, app_state.camera.euler.translate, app_state.camera.euler.rotationCenter); + if (is_ortho) + { + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; + } } if (ImGui::IsItemHovered()) ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); - ImGui::MenuItem("Show axes", "key X", &app_state.show_axes); - ImGui::MenuItem("Show compass/ruler", "key C", &app_state.compass_ruler); + ImGui::MenuItem("Show axes", "key X", &show_axes); + ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); - ImGui::MenuItem("Lock Z", "Shift + Z", &app_state.camera.lockZ, !app_state.camera.isOrtho); + ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); // ImGui::MenuItem("show_covs", nullptr, &show_covs); @@ -4566,35 +4103,8 @@ void display() ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background", (float*)&app_state.bg_color, ImGuiColorEditFlags_NoInputs); - - // Same shader color modes as step 2's point cloud color schemes (ScanRenderer). - if (ImGui::BeginMenu("Points color")) - { - if (ImGui::MenuItem("> Session color", nullptr, points_color_mode == ScanColorMode::Flat)) - points_color_mode = ScanColorMode::Flat; - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Each session in its own color (Settings window)"); - - ImGui::Separator(); - - if (ImGui::MenuItem("> By intensity (gradient)", nullptr, points_color_mode == ScanColorMode::Intensity)) - points_color_mode = ScanColorMode::Intensity; - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Per-point jet colormap from LAS/LAZ intensity"); - - if (ImGui::MenuItem("> By height (gradient)", nullptr, points_color_mode == ScanColorMode::Elevation)) - points_color_mode = ScanColorMode::Elevation; - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Per-point jet colormap from world Z, over all sessions' [z_min, z_max]"); - - if (ImGui::MenuItem("> By distance (gradient)", nullptr, points_color_mode == ScanColorMode::Distance)) - points_color_mode = ScanColorMode::Distance; - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Per-point jet colormap from distance to the rotation center"); - - ImGui::EndMenu(); - } + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + pointsColorMenu(); ImGui::Separator(); @@ -4616,13 +4126,13 @@ void display() ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points render downsampling", &app_state.viewer_decimate_point_cloud, 10, 100); + ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); if (ImGui::IsItemHovered()) ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); // ImGui::SameLine(); - if (app_state.viewer_decimate_point_cloud < 1) - app_state.viewer_decimate_point_cloud = 1; + if (viewer_decimate_point_cloud < 1) + viewer_decimate_point_cloud = 1; ImGui::SameLine(); @@ -4636,7 +4146,7 @@ void display() ImGui::SameLine(); - ImGui::Text("(%d FPS)", GetFPS()); + ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); } ImGui::EndDisabled(); @@ -4654,7 +4164,7 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - app_state.info_gui = !app_state.info_gui; + info_gui = !info_gui; ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -4731,28 +4241,31 @@ void display() if (is_loop_closure_gui) loop_closure_gui(); - raylib_widgets::showEulerCenterOfRotationWindow(cor_gui, app_state.camera, xText, yText, zText); + cor_window(); - raylib_widgets::ShowInfoWindow(app_state.info_gui, infoLines, appShortcuts, HDMAPPING_VERSION_STRING, __DATE__); + info_window(infoLines, appShortcuts); - if (is_settings_gui) - settings_gui(); + end3DAndDrawLabels(); - // Switch to 2D screen space for text labels, the compass and ImGui's own draw pass. - raylib_widgets::end3DMatrixStack(io.DisplaySize.x, io.DisplaySize.y); + if (compass_ruler) + drawMiniCompassWithRuler(); - if (is_loop_closure_gui) - renderLoopClosureLabels(); - else - for (const auto& session : sessions) - if (session.visible) - { - renderGroundControlPointsLabels(session.ground_control_points, session.point_clouds_container); - renderControlPointsLabels(session.control_points, session.point_clouds_container); - } + // my_display_code(); + /*if (is_ndt_gui) + ndt_gui(); + if (is_icp_gui) + icp_gui(); + if (is_pose_graph_slam) + pose_graph_slam_gui(); + if (is_registration_plane_feature) + registration_plane_feature_gui(); + if (is_manual_analisys) + observation_picking_gui();*/ + // if (is_loop_closure_gui) + // manual_pose_graph_loop_closure.Gui(); - if (app_state.compass_ruler) - drawMiniCompassWithRuler(); + if (is_settings_gui) + settings_gui(); rlImGuiEnd(); } @@ -4761,13 +4274,12 @@ void mouse(int glut_button, int state, int x, int y) { ImGuiIO& io = ImGui::GetIO(); - // GLUT's wheel-as-button-3/4 fallback is gone: main() polls GetMouseWheelMove() and calls wheel(). - if (!io.WantCaptureMouse) { if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) && !manipulate_active_edge) { + // if (s_loop_closure_gui) if ((sessions.size() > 0) && (number_visible_sessions > 0) && update_rotation_center) { getClosestTrajectoriesPoint( @@ -4782,84 +4294,45 @@ void mouse(int glut_button, int state, int x, int y) io.KeyShift, time_stamp_offset); } - else if (update_rotation_center) + else { - setNewRotationCenter(x, y); + if (update_rotation_center) + { + setNewRotationCenter(x, y); + } } } if (state == GLUT_DOWN) - app_state.mouse_buttons |= 1 << glut_button; - else if (state == GLUT_UP) - app_state.mouse_buttons = 0; - - app_state.mouse_old_x = x; - app_state.mouse_old_y = y; - } -} - -// Was utils.cpp's GLUT initGL(): raylib window + rlImGui, same setup as step 2. -bool initGL(const std::string& winTitleArg) -{ - // HiDPI breaks ImGui scaling on Windows, so it is only enabled on macOS and Linux (as in step 2). - unsigned int flags = FLAG_WINDOW_RESIZABLE; -#ifdef __APPLE__ - flags |= FLAG_WINDOW_HIGHDPI; -#endif -#if __LINUX__ - flags |= FLAG_WINDOW_HIGHDPI; -#endif - - SetConfigFlags(flags); - InitWindow(static_cast(window_width), static_cast(window_height), winTitleArg.c_str()); - SetExitKey(KEY_NULL); // Esc must not close the window (e.g. while cancelling a dialog) - SetTargetFPS(60); - raylib_widgets::fitWindowToScreen(/*marginW=*/100, /*marginH=*/100, /*centerVertically=*/true); - - rlImGuiSetup(true); - ImGuiIO& io = ImGui::GetIO(); - io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; - io.ConfigDockingWithShift = true; - - app_state.camera.applyPerspectiveProjection(static_cast(window_width), static_cast(window_height)); - - return true; -} - -// Drag & drop: a project (*.mjp) replaces the current one; session files (*.mjs/*.json) are added to it. -void loadDroppedFiles(const std::vector& paths) -{ - for (const auto& path : paths) - { - std::string ext = fs::path(path).extension().string(); - std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); - if (ext == ".mjp") { - loadProject(path, project_settings); - return; - } - } + mouse_buttons |= 1 << glut_button; - bool added = false; - for (const auto& path : paths) - { - std::string ext = fs::path(path).extension().string(); - std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); - if (ext == ".mjs" || ext == ".json") + /*if (observation_picking.is_observation_picking_mode) + { + Eigen::Vector3d p = GLWidgetGetOGLPos(x, y, observation_picking); + int number_active_pcs = 0; + int index_picked = -1; + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].visible) + { + number_active_pcs++; + index_picked = i; + } + } + if (number_active_pcs == 1) + { + observation_picking.add_picked_to_current_observation(index_picked, p); + } + }*/ + } + else if (state == GLUT_UP) { - std::cout << "Adding session file: '" << path << "'" << std::endl; - project_settings.session_file_names.push_back(path); - added = true; + mouse_buttons = 0; } + mouse_old_x = x; + mouse_old_y = y; } - - if (added) - { - loaded_sessions = false; - time_stamp_offset = 0.0; - } - else - pfd::message("Unsupported file", "Drop a project (*.mjp) or session files (*.mjs, *.json).", pfd::choice::ok, pfd::icon::warning); } int main(int argc, char* argv[]) @@ -4878,7 +4351,7 @@ int main(int argc, char* argv[]) return 0; } - initGL(winTitle); + initGL(&argc, argv, winTitle, display, mouse); if (argc > 1) { @@ -4896,51 +4369,9 @@ int main(int argc, char* argv[]) } } - // Was glutMainLoop(): the GLUT callbacks are called directly, on raylib's input transitions. - while (!WindowShouldClose()) - { - int mx = static_cast(GetMouseX()); - int my = static_cast(GetMouseY()); - - if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) - mouse(GLUT_LEFT_BUTTON, GLUT_DOWN, mx, my); - if (IsMouseButtonReleased(MOUSE_BUTTON_LEFT)) - mouse(GLUT_LEFT_BUTTON, GLUT_UP, mx, my); - if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) - mouse(GLUT_RIGHT_BUTTON, GLUT_DOWN, mx, my); - if (IsMouseButtonReleased(MOUSE_BUTTON_RIGHT)) - mouse(GLUT_RIGHT_BUTTON, GLUT_UP, mx, my); - if (IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) - mouse(GLUT_MIDDLE_BUTTON, GLUT_DOWN, mx, my); - if (IsMouseButtonReleased(MOUSE_BUTTON_MIDDLE)) - mouse(GLUT_MIDDLE_BUTTON, GLUT_UP, mx, my); - - motion(mx, my); - - float wheelMove = GetMouseWheelMove(); - if (wheelMove != 0.0f) - wheel(0, wheelMove > 0.0f ? 1 : -1, mx, my); - - if (IsFileDropped()) - { - FilePathList dropped_files = LoadDroppedFiles(); - std::vector paths; - for (unsigned int i = 0; i < dropped_files.count; i++) - paths.emplace_back(dropped_files.paths[i]); - UnloadDroppedFiles(dropped_files); - if (!paths.empty()) - loadDroppedFiles(paths); - } - - BeginDrawing(); - display(); - EndDrawing(); - } + mainLoop(); - // GPU buffers must be released while the GL context still exists. - session_renderers.clear(); - rlImGuiShutdown(); - CloseWindow(); + shutdownGL(); } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; @@ -4954,4 +4385,4 @@ int main(int argc, char* argv[]) } return 0; -} +} \ No newline at end of file diff --git a/apps/multi_session_registration/raylib_utils.cpp b/apps/multi_session_registration/raylib_utils.cpp new file mode 100644 index 00000000..c7b8a55c --- /dev/null +++ b/apps/multi_session_registration/raylib_utils.cpp @@ -0,0 +1,759 @@ +#include "raylib_utils.h" + +#include "raymath.h" +#include "rlImGui.h" + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +raylib_widgets::OrbitCamera camera; + +// GLUT step 3 used 1000 (every point was a glVertex call); GPU buffers draw full density, as in step 2. +int viewer_decimate_point_cloud = 2; +int mouse_old_x = 0, mouse_old_y = 0; +int mouse_buttons = 0; +bool& is_ortho = camera.isOrtho; +bool& lock_z = camera.lockZ; +bool show_axes = true; +ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); +int point_size = 1; +bool info_gui = false; +bool compass_ruler = true; +bool cor_gui = false; +Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); + +Eigen::Map rotation_center(&camera.euler.rotationCenter.x); +float& rotate_x = camera.euler.rotateX; +float& rotate_y = camera.euler.rotateY; +float& translate_x = camera.euler.translate.x; +float& translate_y = camera.euler.translate.y; +float& translate_z = camera.euler.translate.z; +Eigen::Map new_rotation_center(&camera.eulerGoal.rotationCenter.x); +float& new_rotate_x = camera.eulerGoal.rotateX; +float& new_rotate_y = camera.eulerGoal.rotateY; +float& new_translate_x = camera.eulerGoal.translate.x; +float& new_translate_y = camera.eulerGoal.translate.y; +float& new_translate_z = camera.eulerGoal.translate.z; +bool& camera_transition_active = camera.eulerTransitionActive; +float* const m_ortho_projection = camera.orthoProjection; +float* const m_ortho_gizmo_view = camera.orthoGizmoView; + +namespace +{ + void (*display_cb)() = nullptr; + void (*mouse_cb)(int, int, int, int) = nullptr; + + Matrix frame_mvp{}; + float current_color[3] = { 1.f, 1.f, 1.f }; + + struct StripVertex + { + Vector3 p; + float c[3]; + }; + std::vector strip; + + struct Label + { + Vector3 p; + std::string text; + Color color; + }; + Vector3 label_pos{}; + float label_color[3] = { 1.f, 1.f, 1.f }; + std::vector