From bfe8e3fa90c54d5ed6d6f40524ff99b764f9a9cb Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 26 May 2024 02:28:51 +0100 Subject: [PATCH 1/9] ENH: add movable camera. --- .../visual_odometry_example.cpp | 54 +++++++++++++++++-- 1 file changed, 49 insertions(+), 5 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index eb8956e84..bb51e434c 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -10,6 +10,7 @@ // ========================================================================== // #include +#include #include #include #include @@ -234,13 +235,10 @@ class SingleWindowApp 0, 0, -1; // clang-format on - // Update the model view matrix. - const Eigen::Matrix4f model_view = Eigen::Matrix4f::Identity(); - // Render the point cloud. _point_cloud_renderer.render(_point_cloud, _point_size, from_cam_to_gl.matrix(), // - model_view, _point_cloud_projection); + _model_view, _point_cloud_projection); } auto get_framebuffer_sizes() const -> Eigen::Vector2i @@ -302,6 +300,49 @@ class SingleWindowApp std::cout << "RESUME" << std::endl; return; } + + if (action == GLFW_RELEASE || action == GLFW_REPEAT) + app.move_point_cloud_camera_with_keyboard(key); + } + + auto move_point_cloud_camera_with_keyboard(const int key) -> void + { + using sara::operator""_m; + using sara::operator""_deg; + static constexpr auto delta = (5._m).value; + static constexpr auto angle_delta = (40._deg).value; + + if (key == GLFW_KEY_W) + _point_cloud_camera.move_forward(delta); + if (key == GLFW_KEY_S) + _point_cloud_camera.move_backward(delta); + if (key == GLFW_KEY_A) + _point_cloud_camera.move_left(delta); + if (key == GLFW_KEY_D) + _point_cloud_camera.move_right(delta); + + if (key == GLFW_KEY_DELETE) + _point_cloud_camera.no_head_movement(-angle_delta); // CCW + if (key == GLFW_KEY_PAGE_DOWN) + _point_cloud_camera.no_head_movement(+angle_delta); // CW + + if (key == GLFW_KEY_HOME) + _point_cloud_camera.yes_head_movement(+angle_delta); + if (key == GLFW_KEY_END) + _point_cloud_camera.yes_head_movement(-angle_delta); + + if (key == GLFW_KEY_R) + _point_cloud_camera.move_up(delta); + if (key == GLFW_KEY_F) + _point_cloud_camera.move_down(delta); + + if (key == GLFW_KEY_INSERT) + _point_cloud_camera.maybe_head_movement(-angle_delta); + if (key == GLFW_KEY_PAGE_UP) + _point_cloud_camera.maybe_head_movement(+angle_delta); + + _point_cloud_camera.update(); + _model_view = _point_cloud_camera.view_matrix(); } private: @@ -373,8 +414,11 @@ class SingleWindowApp kgl::ColoredPointCloudRenderer _point_cloud_renderer; //! @brief Point cloud rendering options. Eigen::Matrix4f _point_cloud_projection; + //! @brief Camera of the point cloud scene. + k::Camera _point_cloud_camera; + Eigen::Matrix4f _model_view = Eigen::Matrix4f::Identity(); // kgl::Camera _point_cloud_camera; - float _point_size = 3.f; + float _point_size = 1.5f; //! @brief User interaction. bool _pause = false; From 6ca7ed063f7be05b3ac3557b7085fa3c5bfa4225 Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 26 May 2024 10:55:06 +0100 Subject: [PATCH 2/9] ENH: tweak OpenGL program. --- .../visual_odometry_example.cpp | 16 +++++++++++++++- cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp | 2 +- cpp/src/DO/Kalpana/EasyGL/Shader.cpp | 2 +- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index bb51e434c..4fe3abcec 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -119,6 +119,9 @@ class SingleWindowApp { if (!_pause) { + if (_quit) + break; + if (!_pipeline.read()) break; @@ -136,6 +139,9 @@ class SingleWindowApp } } + if (_quit) + break; + // Clear the color buffer and the buffer testing. glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); @@ -222,6 +228,9 @@ class SingleWindowApp auto render_point_cloud() -> void { + if (_pipeline.point_cloud().empty()) + return; + glViewport(_point_cloud_viewport.x(), _point_cloud_viewport.y(), _point_cloud_viewport.width(), _point_cloud_viewport.height()); @@ -303,6 +312,10 @@ class SingleWindowApp if (action == GLFW_RELEASE || action == GLFW_REPEAT) app.move_point_cloud_camera_with_keyboard(key); + + if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && + key == GLFW_KEY_ESCAPE) + app._quit = true; } auto move_point_cloud_camera_with_keyboard(const int key) -> void @@ -310,7 +323,7 @@ class SingleWindowApp using sara::operator""_m; using sara::operator""_deg; static constexpr auto delta = (5._m).value; - static constexpr auto angle_delta = (40._deg).value; + static constexpr auto angle_delta = (10._deg).value; if (key == GLFW_KEY_W) _point_cloud_camera.move_forward(delta); @@ -422,6 +435,7 @@ class SingleWindowApp //! @brief User interaction. bool _pause = false; + bool _quit = false; }; bool SingleWindowApp::_glfw_initialized = false; diff --git a/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp b/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp index 8ebebb6ba..fe7cb3c1a 100644 --- a/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp +++ b/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp @@ -25,7 +25,7 @@ namespace DO::Kalpana { static constexpr auto YAW = -90.0f; static constexpr auto PITCH = 0.0f; static constexpr auto SPEED = 1e-1f; - static constexpr auto SENSITIVITY = 1e-1f; + static constexpr auto SENSITIVITY = 5e-1f; static constexpr auto ZOOM = 45.0f; Eigen::Vector3f position{10.f * Eigen::Vector3f::UnitY()}; diff --git a/cpp/src/DO/Kalpana/EasyGL/Shader.cpp b/cpp/src/DO/Kalpana/EasyGL/Shader.cpp index aa8fc6cdd..bba965379 100644 --- a/cpp/src/DO/Kalpana/EasyGL/Shader.cpp +++ b/cpp/src/DO/Kalpana/EasyGL/Shader.cpp @@ -219,7 +219,7 @@ namespace DO::Kalpana::GL { program_object = 0; - throw std::runtime_error{fmt::format("Failed to delete shader program: %d." + throw std::runtime_error{fmt::format("Failed to delete shader program: {}." "Delete log:\n{}", success, log)}; } From 94d8ce02eacddd6ed131b46fe5d64df7df0976b8 Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 26 May 2024 11:38:39 +0100 Subject: [PATCH 3/9] ENH: filter 3D points that are deemed to be at infinity. --- .../visual_odometry_example.cpp | 17 ++++++++++++++ cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp | 2 +- .../BuildingBlocks/CameraPoseEstimator.cpp | 11 ---------- .../Sara/SfM/BuildingBlocks/FeatureParams.hpp | 2 +- .../BuildingBlocks/PointCloudGenerator.cpp | 18 +++++++++++---- .../BuildingBlocks/PointCloudGenerator.hpp | 22 +++++++++++++------ .../DO/Sara/SfM/Odometry/OdometryPipeline.cpp | 14 +++++++----- 7 files changed, 56 insertions(+), 30 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 4fe3abcec..43920db4b 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -313,9 +313,15 @@ class SingleWindowApp if (action == GLFW_RELEASE || action == GLFW_REPEAT) app.move_point_cloud_camera_with_keyboard(key); + // Use the escape key to smoothly exit the OpenGL app. if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && key == GLFW_KEY_ESCAPE) + { app._quit = true; + return; + } + + app.resize_point_size(action, key); } auto move_point_cloud_camera_with_keyboard(const int key) -> void @@ -358,6 +364,17 @@ class SingleWindowApp _model_view = _point_cloud_camera.view_matrix(); } + auto resize_point_size(const int action, const int key) -> void + { + if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && + key == GLFW_KEY_MINUS) + _point_size /= 1.1f; + + if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && + key == GLFW_KEY_EQUAL) + _point_size *= 1.1f; + } + private: static auto init_glfw() -> void { diff --git a/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp b/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp index fe7cb3c1a..1b71688c9 100644 --- a/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp +++ b/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp @@ -25,7 +25,7 @@ namespace DO::Kalpana { static constexpr auto YAW = -90.0f; static constexpr auto PITCH = 0.0f; static constexpr auto SPEED = 1e-1f; - static constexpr auto SENSITIVITY = 5e-1f; + static constexpr auto SENSITIVITY = 8e-1f; static constexpr auto ZOOM = 45.0f; Eigen::Vector3f position{10.f * Eigen::Vector3f::UnitY()}; diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp index 819627b97..e26b04bc0 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp @@ -172,19 +172,8 @@ auto CameraPoseEstimator::estimate_pose( const auto pixel_coords = pcg.pixel_coords(*fv).cast(); // Normalize the rays. This is important for Lambda-Twist P3P method. rays.col(ti) = camera.backproject(pixel_coords).normalized(); -#if 0 - if (ti < 10) - SARA_LOGD(logger, "Backproject point {}:\n{} -> {}", ti, - pixel_coords.transpose().eval(), - rays.col(ti).transpose().eval()); -#endif } -#if 0 - SARA_LOGD(logger, "Scene points:\n{}", scene_coords.leftCols(10).eval()); - SARA_LOGD(logger, "Rays:\n{}", rays.leftCols(10).eval()); -#endif - // 3. solve the PnP problem with RANSAC. const auto [pose, inliers, sample_best] = estimate_pose(point_ray_pairs, camera); diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp index d93670df5..17303cc6b 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/FeatureParams.hpp @@ -7,7 +7,7 @@ namespace DO::Sara { struct FeatureParams { - ImagePyramidParams image_pyr_params = ImagePyramidParams(-1); + ImagePyramidParams image_pyr_params = ImagePyramidParams(0); float sift_nn_ratio = 0.6f; std::size_t num_matches_max = 1000u; int num_inliers_min = 100; diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 574287e0a..c0f834f5d 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -91,8 +91,8 @@ auto PointCloudGenerator::filter_by_non_max_suppression( } auto PointCloudGenerator::find_feature_vertex_at_pose( - const FeatureTrack& track, - const PoseVertex pose_vertex) const -> std::optional + const FeatureTrack& track, const PoseVertex pose_vertex) const + -> std::optional { auto v = std::find_if(track.begin(), track.end(), [this, pose_vertex](const auto& v) { @@ -251,6 +251,8 @@ auto PointCloudGenerator::compress_point_cloud( auto point_cloud_compressed = std::vector{}; point_cloud_compressed.reserve(tracks.size()); + auto from_vertex_to_scene_point_index_new = FeatureToScenePointMap{}; + // Reset the scene point index for each feature track. for (auto t = ScenePointIndex{}; t < tracks.size(); ++t) { @@ -263,9 +265,13 @@ auto PointCloudGenerator::compress_point_cloud( // Recalculate the scene point index as a barycenter. const auto scene_point = barycenter(scene_point_indices); + // Discard point at infinity. + if (std::abs(scene_point.coords().z()) > _zmax) + continue; + // Reassign the scene point index for the given feature track. for (const auto& v : track) - _from_vertex_to_scene_point_index[v] = point_cloud_compressed.size(); + from_vertex_to_scene_point_index_new[v] = point_cloud_compressed.size(); // Only then store the new point coordinates. Otherwise the index is wrong! point_cloud_compressed.emplace_back(scene_point); @@ -273,6 +279,7 @@ auto PointCloudGenerator::compress_point_cloud( // Swap the point cloud with the set of barycenters. std::swap(_point_cloud, point_cloud_compressed); + _from_vertex_to_scene_point_index.swap(from_vertex_to_scene_point_index_new); return true; } @@ -383,6 +390,9 @@ auto PointCloudGenerator::grow_point_cloud( // Calculate the scene point. const Eigen::Vector3d coords = X.col(j).hnormalized(); + if (coords.z() > _zmax) // We deem it to be a point at infinity. + continue; + const auto color = retrieve_scene_point_color(coords, image, // tsfm_v, camera); @@ -433,7 +443,7 @@ auto PointCloudGenerator::write_point_cloud( const std::vector& ftracks, const std::filesystem::path& out_csv) const -> void { - std::ofstream out{out_csv.string()}; + auto out = std::ofstream{out_csv.string()}; for (const auto& ftrack : ftracks) { diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 134966305..7b0586662 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -48,6 +48,11 @@ namespace DO::Sara { { } + auto zmax() const -> double + { + return _zmax; + } + public: /* helper feature retrieval methods */ auto gid(const FeatureVertex u) const -> const FeatureGID& { @@ -91,11 +96,12 @@ namespace DO::Sara { const PoseVertex) const -> std::optional; - auto retrieve_scene_point_color( - const Eigen::Vector3d& scene_point, // - const ImageView& image, // - const QuaternionBasedPose& pose, - const v2::PinholeCamera& camera) const -> Rgb64f; + auto + retrieve_scene_point_color(const Eigen::Vector3d& scene_point, // + const ImageView& image, // + const QuaternionBasedPose& pose, + const v2::PinholeCamera& camera) const + -> Rgb64f; public: /* data transformation methods */ //! @brief Calculate the barycentric scene point. @@ -121,8 +127,8 @@ namespace DO::Sara { //! - The scene point is recalculated as a the barycenter of the //! possibly multiple scene points we have found after recalculating the //! feature tracks. - auto - propagate_scene_point_indices(const std::vector&) -> void; + auto propagate_scene_point_indices(const std::vector&) + -> void; //! - The point cloud compression reassigns a unique scene point cloud to //! each feature tracks. @@ -152,6 +158,8 @@ namespace DO::Sara { PointCloud& _point_cloud; FeatureToScenePointMap _from_vertex_to_scene_point_index; + + double _zmax = +1e3; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp index 853e52a09..ec8f1a9d3 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp @@ -221,8 +221,7 @@ auto OdometryPipeline::grow_geometry() -> bool _feature_tracker.calculate_alive_feature_tracks(_pose_curr); #if defined(DEBUG_ABSOLUTE_POSE_RECOVERY) - const auto corr_csv_fp = - fmt::format("/Users/oddkiva/Desktop/corr_{}.csv", _pose_curr); + const auto corr_csv_fp = fmt::format("corr_{}.csv", _pose_curr); write_point_correspondences(_pose_graph, _feature_tracker._feature_graph, _tracks_alive, corr_csv_fp); #endif @@ -290,8 +289,7 @@ auto OdometryPipeline::grow_geometry() -> bool frame_corrected, pose_edge, _camera_corrected); #if defined(DEBUG_ABSOLUTE_POSE_RECOVERY) - const auto scene_csv_fp = - fmt::format("/Users/oddkiva/Desktop/scene_{}.csv", _pose_curr); + const auto scene_csv_fp = fmt::format("scene_{}.csv", _pose_curr); _point_cloud_generator->write_point_cloud(_tracks_alive, scene_csv_fp); #endif @@ -324,8 +322,12 @@ auto OdometryPipeline::adjust_bundles() -> void for (const auto& ftrack : _feature_tracker._feature_tracks) { // Does a track have a 3D point? If not, discard it. - const auto idx = _point_cloud_generator->scene_point_index(ftrack.front()); - if (idx == std::nullopt) + const auto p = _point_cloud_generator->scene_point(ftrack.front()); + if (p == std::nullopt) + continue; + + // Discard point at infinity. + if (std::abs(p->coords().z()) > _point_cloud_generator->zmax()) continue; // Filter the feature track by NMS: there should be only 1 feature per From f7e7d42e888d8d8807a8ca7339bbf9607b74e85c Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 26 May 2024 11:53:48 +0100 Subject: [PATCH 4/9] BUG: fix definition of point at infinity. --- .../Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp | 4 ++-- .../Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp | 12 +++++++++--- cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp | 3 ++- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index c0f834f5d..feaf3a0af 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -266,7 +266,7 @@ auto PointCloudGenerator::compress_point_cloud( const auto scene_point = barycenter(scene_point_indices); // Discard point at infinity. - if (std::abs(scene_point.coords().z()) > _zmax) + if (scene_point.coords().squaredNorm() > distance_max_squared()) continue; // Reassign the scene point index for the given feature track. @@ -390,7 +390,7 @@ auto PointCloudGenerator::grow_point_cloud( // Calculate the scene point. const Eigen::Vector3d coords = X.col(j).hnormalized(); - if (coords.z() > _zmax) // We deem it to be a point at infinity. + if (coords.squaredNorm() > distance_max_squared()) // We deem it to be a point at infinity. continue; const auto color = retrieve_scene_point_color(coords, image, // diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index 7b0586662..b8331923e 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -11,6 +11,7 @@ #pragma once +#include #include #include #include @@ -48,9 +49,14 @@ namespace DO::Sara { { } - auto zmax() const -> double + auto distance_max() const -> double { - return _zmax; + return _distance_max; + } + + auto distance_max_squared() const -> double + { + return square(_distance_max); } public: /* helper feature retrieval methods */ @@ -159,7 +165,7 @@ namespace DO::Sara { FeatureToScenePointMap _from_vertex_to_scene_point_index; - double _zmax = +1e3; + double _distance_max = +1e3; }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp index ec8f1a9d3..72c6ea97f 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp @@ -327,7 +327,8 @@ auto OdometryPipeline::adjust_bundles() -> void continue; // Discard point at infinity. - if (std::abs(p->coords().z()) > _point_cloud_generator->zmax()) + if (p->coords().squaredNorm() > + _point_cloud_generator->distance_max_squared()) continue; // Filter the feature track by NMS: there should be only 1 feature per From 4c4b21e477377b9510cf89e45be2bcc5282e1e4b Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 26 May 2024 11:56:28 +0100 Subject: [PATCH 5/9] MAINT: change keybindings. --- .../MultiViewGeometry/visual_odometry_example.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index bb51e434c..8f089d02f 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -321,14 +321,14 @@ class SingleWindowApp if (key == GLFW_KEY_D) _point_cloud_camera.move_right(delta); - if (key == GLFW_KEY_DELETE) + if (key == GLFW_KEY_J) _point_cloud_camera.no_head_movement(-angle_delta); // CCW - if (key == GLFW_KEY_PAGE_DOWN) + if (key == GLFW_KEY_L) _point_cloud_camera.no_head_movement(+angle_delta); // CW - if (key == GLFW_KEY_HOME) + if (key == GLFW_KEY_I) _point_cloud_camera.yes_head_movement(+angle_delta); - if (key == GLFW_KEY_END) + if (key == GLFW_KEY_K) _point_cloud_camera.yes_head_movement(-angle_delta); if (key == GLFW_KEY_R) @@ -336,9 +336,9 @@ class SingleWindowApp if (key == GLFW_KEY_F) _point_cloud_camera.move_down(delta); - if (key == GLFW_KEY_INSERT) + if (key == GLFW_KEY_U) _point_cloud_camera.maybe_head_movement(-angle_delta); - if (key == GLFW_KEY_PAGE_UP) + if (key == GLFW_KEY_O) _point_cloud_camera.maybe_head_movement(+angle_delta); _point_cloud_camera.update(); @@ -439,7 +439,7 @@ auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) #define USE_HARDCODED_VIDEO_PATH #if defined(USE_HARDCODED_VIDEO_PATH) && defined(__APPLE__) const auto video_path = - fs::path{"/Users/oddkiva/Desktop/datasets/odometry/turn_bikes.mp4"}; + fs::path{"/Users/oddkiva/Desktop/datasets/odometry/pass_bikes.mp4"}; if (!fs::exists(video_path)) { fmt::print("Video {} does not exist", video_path.string()); From 1409e89b023294bb2b2ab3559667d026d4287354 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 26 May 2024 12:01:28 +0100 Subject: [PATCH 6/9] MAINT: tweaks. --- .../visual_odometry_example.cpp | 31 +++++++++++-------- .../BuildingBlocks/PointCloudGenerator.hpp | 2 ++ 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index a0247f4f4..733dc7c74 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -268,8 +268,8 @@ class SingleWindowApp return *app_ptr; } - static auto window_size_callback(GLFWwindow* window, const int, const int) - -> void + static auto window_size_callback(GLFWwindow* window, const int, + const int) -> void { auto& self = get_self(window); @@ -310,9 +310,6 @@ class SingleWindowApp return; } - if (action == GLFW_RELEASE || action == GLFW_REPEAT) - app.move_point_cloud_camera_with_keyboard(key); - // Use the escape key to smoothly exit the OpenGL app. if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && key == GLFW_KEY_ESCAPE) @@ -321,7 +318,17 @@ class SingleWindowApp return; } - app.resize_point_size(action, key); + if (action == GLFW_RELEASE || action == GLFW_REPEAT) + { + app.move_point_cloud_camera_with_keyboard(key); + return; + } + + if (action == GLFW_RELEASE || action == GLFW_REPEAT) + { + app.resize_point_size(key); + return; + } } auto move_point_cloud_camera_with_keyboard(const int key) -> void @@ -364,14 +371,12 @@ class SingleWindowApp _model_view = _point_cloud_camera.view_matrix(); } - auto resize_point_size(const int action, const int key) -> void + auto resize_point_size(const int key) -> void { - if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && - key == GLFW_KEY_MINUS) + if (key == GLFW_KEY_MINUS) _point_size /= 1.1f; - if ((action == GLFW_RELEASE || action == GLFW_REPEAT) && - key == GLFW_KEY_EQUAL) + if (key == GLFW_KEY_EQUAL) _point_size *= 1.1f; } @@ -458,8 +463,8 @@ class SingleWindowApp bool SingleWindowApp::_glfw_initialized = false; -auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) - -> int +auto main([[maybe_unused]] int const argc, + [[maybe_unused]] char** const argv) -> int { #if defined(_OPENMP) const auto num_threads = omp_get_max_threads(); diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp index b8331923e..853b7cd7e 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.hpp @@ -165,6 +165,8 @@ namespace DO::Sara { FeatureToScenePointMap _from_vertex_to_scene_point_index; + //! @brief Even if we don't have a metric reconstruction, this default value + //! works well in practice. double _distance_max = +1e3; }; From 18dc3c39ad1388f56109d5ef3587cede745aee53 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 26 May 2024 12:17:31 +0100 Subject: [PATCH 7/9] MAINT: tweak keybindings. --- .../visual_odometry_example.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 733dc7c74..327cf588c 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -321,11 +321,6 @@ class SingleWindowApp if (action == GLFW_RELEASE || action == GLFW_REPEAT) { app.move_point_cloud_camera_with_keyboard(key); - return; - } - - if (action == GLFW_RELEASE || action == GLFW_REPEAT) - { app.resize_point_size(key); return; } @@ -347,14 +342,14 @@ class SingleWindowApp if (key == GLFW_KEY_D) _point_cloud_camera.move_right(delta); - if (key == GLFW_KEY_J) + if (key == GLFW_KEY_H) _point_cloud_camera.no_head_movement(-angle_delta); // CCW - if (key == GLFW_KEY_L) + if (key == GLFW_KEY_K) _point_cloud_camera.no_head_movement(+angle_delta); // CW - if (key == GLFW_KEY_I) + if (key == GLFW_KEY_U) _point_cloud_camera.yes_head_movement(+angle_delta); - if (key == GLFW_KEY_K) + if (key == GLFW_KEY_J) _point_cloud_camera.yes_head_movement(-angle_delta); if (key == GLFW_KEY_R) @@ -362,9 +357,9 @@ class SingleWindowApp if (key == GLFW_KEY_F) _point_cloud_camera.move_down(delta); - if (key == GLFW_KEY_U) + if (key == GLFW_KEY_Y) _point_cloud_camera.maybe_head_movement(-angle_delta); - if (key == GLFW_KEY_O) + if (key == GLFW_KEY_I) _point_cloud_camera.maybe_head_movement(+angle_delta); _point_cloud_camera.update(); From c7aa19b0d0549e2bd9a59fad3a75c5ac1d28f8cc Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 26 May 2024 12:50:52 +0100 Subject: [PATCH 8/9] MAINT: tweaks. --- .../visual_odometry_example.cpp | 59 +++++++++++-------- cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp | 8 +-- 2 files changed, 40 insertions(+), 27 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 327cf588c..5d0648cdc 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -42,6 +42,9 @@ namespace sara = DO::Sara; namespace k = DO::Kalpana; namespace kgl = DO::Kalpana::GL; +using sara::operator""_m; +using sara::operator""_deg; + class SingleWindowApp { @@ -101,7 +104,10 @@ class SingleWindowApp { // Current projection matrix _projection = _video_viewport.orthographic_projection(); - _point_cloud_projection = _point_cloud_viewport.orthographic_projection(); + // _point_cloud_projection = + _point_cloud_viewport.orthographic_projection(); + // _point_cloud_projection = + // _point_cloud_viewport.perspective(120.f, 1e-6f, 1e3f); // Background color. glClearColor(0.2f, 0.3f, 0.3f, 1.0f); @@ -268,8 +274,8 @@ class SingleWindowApp return *app_ptr; } - static auto window_size_callback(GLFWwindow* window, const int, - const int) -> void + static auto window_size_callback(GLFWwindow* window, const int, const int) + -> void { auto& self = get_self(window); @@ -322,45 +328,41 @@ class SingleWindowApp { app.move_point_cloud_camera_with_keyboard(key); app.resize_point_size(key); + app.change_camera_step_size(key); return; } } auto move_point_cloud_camera_with_keyboard(const int key) -> void { - using sara::operator""_m; - using sara::operator""_deg; - static constexpr auto delta = (5._m).value; - static constexpr auto angle_delta = (10._deg).value; - if (key == GLFW_KEY_W) - _point_cloud_camera.move_forward(delta); + _point_cloud_camera.move_forward(_delta); if (key == GLFW_KEY_S) - _point_cloud_camera.move_backward(delta); + _point_cloud_camera.move_backward(_delta); if (key == GLFW_KEY_A) - _point_cloud_camera.move_left(delta); + _point_cloud_camera.move_left(_delta); if (key == GLFW_KEY_D) - _point_cloud_camera.move_right(delta); + _point_cloud_camera.move_right(_delta); if (key == GLFW_KEY_H) - _point_cloud_camera.no_head_movement(-angle_delta); // CCW + _point_cloud_camera.no_head_movement(-_angle_delta); // CCW if (key == GLFW_KEY_K) - _point_cloud_camera.no_head_movement(+angle_delta); // CW + _point_cloud_camera.no_head_movement(+_angle_delta); // CW if (key == GLFW_KEY_U) - _point_cloud_camera.yes_head_movement(+angle_delta); + _point_cloud_camera.yes_head_movement(+_angle_delta); if (key == GLFW_KEY_J) - _point_cloud_camera.yes_head_movement(-angle_delta); + _point_cloud_camera.yes_head_movement(-_angle_delta); if (key == GLFW_KEY_R) - _point_cloud_camera.move_up(delta); + _point_cloud_camera.move_up(_delta); if (key == GLFW_KEY_F) - _point_cloud_camera.move_down(delta); + _point_cloud_camera.move_down(_delta); if (key == GLFW_KEY_Y) - _point_cloud_camera.maybe_head_movement(-angle_delta); + _point_cloud_camera.maybe_head_movement(-_angle_delta); if (key == GLFW_KEY_I) - _point_cloud_camera.maybe_head_movement(+angle_delta); + _point_cloud_camera.maybe_head_movement(+_angle_delta); _point_cloud_camera.update(); _model_view = _point_cloud_camera.view_matrix(); @@ -375,6 +377,15 @@ class SingleWindowApp _point_size *= 1.1f; } + auto change_camera_step_size(const int key) -> void + { + if (key == GLFW_KEY_1) + _delta /= 1.1f; + + if (key == GLFW_KEY_2) + _delta *= 1.1f; + } + private: static auto init_glfw() -> void { @@ -447,8 +458,10 @@ class SingleWindowApp //! @brief Camera of the point cloud scene. k::Camera _point_cloud_camera; Eigen::Matrix4f _model_view = Eigen::Matrix4f::Identity(); - // kgl::Camera _point_cloud_camera; float _point_size = 1.5f; + double _delta = (5._m).value; + double _angle_delta = (10._deg).value; + //! @brief User interaction. bool _pause = false; @@ -458,8 +471,8 @@ class SingleWindowApp bool SingleWindowApp::_glfw_initialized = false; -auto main([[maybe_unused]] int const argc, - [[maybe_unused]] char** const argv) -> int +auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) + -> int { #if defined(_OPENMP) const auto num_threads = omp_get_max_threads(); diff --git a/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp b/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp index 1b71688c9..698fe1b95 100644 --- a/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp +++ b/cpp/src/DO/Kalpana/EasyGL/Objects/Camera.hpp @@ -28,11 +28,11 @@ namespace DO::Kalpana { static constexpr auto SENSITIVITY = 8e-1f; static constexpr auto ZOOM = 45.0f; - Eigen::Vector3f position{10.f * Eigen::Vector3f::UnitY()}; - Eigen::Vector3f front{-Eigen::Vector3f::UnitZ()}; - Eigen::Vector3f up{Eigen::Vector3f::UnitY()}; + Eigen::Vector3f position = Eigen::Vector3f::Zero(); + Eigen::Vector3f front = -Eigen::Vector3f::UnitZ(); + Eigen::Vector3f up = Eigen::Vector3f::UnitY(); Eigen::Vector3f right; - Eigen::Vector3f world_up{Eigen::Vector3f::UnitY()}; + Eigen::Vector3f world_up = Eigen::Vector3f::UnitY(); float yaw{YAW}; float pitch{PITCH}; From 073bb3fd2afa5e0900c50af4343085d603eabdef Mon Sep 17 00:00:00 2001 From: David OK Date: Sun, 26 May 2024 13:44:15 +0100 Subject: [PATCH 9/9] MAINT: interactive model rescaling. --- .../visual_odometry_example.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 5d0648cdc..47f124ff8 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -104,8 +104,7 @@ class SingleWindowApp { // Current projection matrix _projection = _video_viewport.orthographic_projection(); - // _point_cloud_projection = - _point_cloud_viewport.orthographic_projection(); + _point_cloud_projection = _point_cloud_viewport.orthographic_projection(); // _point_cloud_projection = // _point_cloud_viewport.perspective(120.f, 1e-6f, 1e3f); @@ -251,9 +250,10 @@ class SingleWindowApp // clang-format on // Render the point cloud. + const Eigen::Matrix4f model_view_final = _model_view * _scale_mat; _point_cloud_renderer.render(_point_cloud, _point_size, from_cam_to_gl.matrix(), // - _model_view, _point_cloud_projection); + model_view_final, _point_cloud_projection); } auto get_framebuffer_sizes() const -> Eigen::Vector2i @@ -329,6 +329,7 @@ class SingleWindowApp app.move_point_cloud_camera_with_keyboard(key); app.resize_point_size(key); app.change_camera_step_size(key); + app.change_model_scale(key); return; } } @@ -386,6 +387,15 @@ class SingleWindowApp _delta *= 1.1f; } + auto change_model_scale(const int key) -> void + { + if (key == GLFW_KEY_Z) + _scale_mat.topLeftCorner<3, 3>() /= 1.1f; + + if (key == GLFW_KEY_X) + _scale_mat.topLeftCorner<3, 3>() *= 1.1f; + } + private: static auto init_glfw() -> void { @@ -458,11 +468,11 @@ class SingleWindowApp //! @brief Camera of the point cloud scene. k::Camera _point_cloud_camera; Eigen::Matrix4f _model_view = Eigen::Matrix4f::Identity(); + Eigen::Matrix4f _scale_mat = Eigen::Matrix4f::Identity(); float _point_size = 1.5f; double _delta = (5._m).value; double _angle_delta = (10._deg).value; - //! @brief User interaction. bool _pause = false; bool _quit = false;