diff --git a/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt b/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt index 258c5278a..9993ff73f 100644 --- a/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt +++ b/cpp/examples/Sara/MultiViewGeometry/CMakeLists.txt @@ -48,12 +48,11 @@ target_link_libraries(essential_5_point_example PRIVATE tinyply) # Visual odometry. sara_add_example(visual_odometry_example) -target_link_libraries(visual_odometry_example PRIVATE DO::Kalpana::EasyGL # - fmt::fmt glfw) - -sara_add_example(visual_odometry_example_v2) -target_link_libraries(visual_odometry_example_v2 PRIVATE DO::Kalpana::EasyGL # - fmt::fmt glfw) +target_link_libraries( + visual_odometry_example + PRIVATE DO::Kalpana::EasyGL # + fmt::fmt # + glfw) # Bundle adjustment. sara_add_example(two_view_bundle_adjustment_example) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 365e59c04..8da362a9b 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #if defined(_WIN32) @@ -71,6 +72,7 @@ class SingleWindowApp glfwSetWindowUserPointer(_window, this); // Register callbacks. glfwSetWindowSizeCallback(_window, window_size_callback); + glfwSetKeyCallback(_window, key_callback); } ~SingleWindowApp() @@ -114,12 +116,24 @@ class SingleWindowApp glfwSwapInterval(1); while (!glfwWindowShouldClose(_window)) { - if (!_pipeline.read()) - break; + if (!_pause) + { + if (!_pipeline.read()) + break; - _pipeline.process(); - // Load data to OpenGL. - upload_point_cloud_data_to_opengl(); + if (!_pipeline._video_streamer.skip()) + { + _pipeline.process(); + + // Load data to OpenGL. + // + // TODO: upload only if we have a new image frame to process and only + // if the absolute pose estimation is successful. + upload_point_cloud_data_to_opengl(); + + _pause = true; + } + } // Clear the color buffer and the buffer testing. glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); @@ -170,8 +184,24 @@ class SingleWindowApp auto upload_point_cloud_data_to_opengl() -> void { - _point_cloud.upload_host_data_to_gl( - _pipeline._triangulator->_colored_point_cloud); + const auto& point_cloud = _pipeline.point_cloud(); + + static constexpr auto dim = 6; + const auto num_points = static_cast(point_cloud.size()); + if (num_points == 0) + return; + + const auto ptr = + const_cast(point_cloud.data()); + const auto ptrd = reinterpret_cast(ptr); + const auto pc_tview = sara::TensorView_{ + ptrd, // + {num_points, dim} // + }; + + auto& logger = sara::Logger::get(); + SARA_LOGI(logger, "point cloud dimensions: {} ", pc_tview.sizes()); + _point_cloud.upload_host_data_to_gl(pc_tview.cast()); } auto render_video() -> void @@ -257,6 +287,22 @@ class SingleWindowApp self._point_cloud_projection = self._point_cloud_viewport.perspective(); } + static auto key_callback(GLFWwindow* window, // + int key, // + int /* scancode */, // + int action, // + int /* mods */) -> void + { + auto& app = get_self(window); + if (app._pause && key == GLFW_KEY_SPACE && + (action == GLFW_RELEASE || action == GLFW_REPEAT)) + { + app._pause = false; + std::cout << "RESUME" << std::endl; + return; + } + } + private: static auto init_glfw() -> void { @@ -328,12 +374,16 @@ class SingleWindowApp Eigen::Matrix4f _point_cloud_projection; // kgl::Camera _point_cloud_camera; float _point_size = 5.f; + + //! @brief User interaction. + bool _pause = false; }; bool SingleWindowApp::_glfw_initialized = false; -auto main(int const argc, 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(); @@ -341,6 +391,16 @@ auto main(int const argc, char** const argv) -> int Eigen::setNbThreads(num_threads); #endif +#define USE_HARDCODED_VIDEO_PATH +#if defined(USE_HARDCODED_VIDEO_PATH) && defined(__APPLE__) + const auto video_path = + fs::path{"/Users/oddkiva/Desktop/datasets/sample-1.mp4"}; + if (!fs::exists(video_path)) + { + fmt::print("Video {} does not exist", video_path.string()); + return EXIT_FAILURE; + } +#else if (argc < 2) { std::cout << fmt::format("Usage: {} VIDEO_PATH\n", @@ -349,6 +409,7 @@ auto main(int const argc, char** const argv) -> int } const auto video_path = fs::path{argv[1]}; +#endif auto camera = sara::v2::BrownConradyDistortionModel{}; { camera.fx() = 917.2878392016245; @@ -359,7 +420,7 @@ auto main(int const argc, char** const argv) -> int // clang-format off camera.k() << -0.2338367557617234, - 0.05952465745165465, + +0.05952465745165465, -0.007947847982157091; // clang-format on camera.p() << -0.0003137658969742134, 0.00021943576376532096; diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp deleted file mode 100644 index ec22a2a23..000000000 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example_v2.cpp +++ /dev/null @@ -1,441 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2023 David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#if defined(_WIN32) -# include -#endif - -#include - -#include - -#include - -#if defined(_OPENMP) -# include -#endif - - -namespace fs = std::filesystem; -namespace sara = DO::Sara; -namespace k = DO::Kalpana; -namespace kgl = DO::Kalpana::GL; - - -class SingleWindowApp -{ -public: - SingleWindowApp(const Eigen::Vector2i& sizes, const std::string& title) - { - // Init GLFW. - init_glfw(); - - // Create a window. - _window = glfwCreateWindow(sizes.x(), sizes.y(), // - title.c_str(), // - nullptr, nullptr); - - _fb_sizes = get_framebuffer_sizes(); - - // Initialize the point cloud viewport. - _point_cloud_viewport.top_left().setZero(); - _point_cloud_viewport.sizes() << _fb_sizes.x() / 2, _fb_sizes.y(); - - // Initialize the video viewport. - _video_viewport.top_left() << _fb_sizes.x() / 2, 0; - _video_viewport.sizes() << _fb_sizes.x() / 2, _fb_sizes.y(); - - // Prepare OpenGL first before any OpenGL calls. - init_opengl(); - - // The magic function. - glfwSetWindowUserPointer(_window, this); - // Register callbacks. - glfwSetWindowSizeCallback(_window, window_size_callback); - glfwSetKeyCallback(_window, key_callback); - } - - ~SingleWindowApp() - { - // Destroy GL objects. - deinit_gl_resources(); - - // Destroy GLFW. - if (_window != nullptr) - glfwDestroyWindow(_window); - - if (_glfw_initialized) - glfwTerminate(); - } - - auto set_config(const fs::path& video_path, - const sara::v2::BrownConradyDistortionModel& camera) - -> void - { - _pipeline.set_config(video_path, camera); - init_gl_resources(); - } - - auto run() -> void - { - // Current projection matrix - _projection = _video_viewport.orthographic_projection(); - _point_cloud_projection = _point_cloud_viewport.orthographic_projection(); - - // Background color. - glClearColor(0.2f, 0.3f, 0.3f, 1.0f); - - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - glEnable(GL_PROGRAM_POINT_SIZE); - - // You absolutely need this for 3D objects! - glEnable(GL_DEPTH_TEST); - - // Display image. - glfwSwapInterval(1); - while (!glfwWindowShouldClose(_window)) - { - if (!_pause) - { - if (!_pipeline.read()) - break; - - if (!_pipeline._video_streamer.skip()) - { - _pipeline.process(); - - // Load data to OpenGL. - // - // TODO: upload only if we have a new image frame to process and only - // if the absolute pose estimation is successful. - upload_point_cloud_data_to_opengl(); - - _pause = true; - } - } - - // Clear the color buffer and the buffer testing. - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - - render_video(); - render_point_cloud(); - - glfwSwapBuffers(_window); - glfwPollEvents(); - } - } - -private: - auto init_opengl() -> void - { - glfwMakeContextCurrent(_window); - init_glew(); - } - - auto init_gl_resources() -> void - { - // Video texture rendering - _texture.initialize(_pipeline._video_streamer.frame_rgb8(), 0); - - const auto& w = _pipeline._video_streamer.width(); - const auto& h = _pipeline._video_streamer.height(); - const auto aspect_ratio = static_cast(w) / h; - auto vertices = _quad.host_vertices().matrix(); - vertices.col(0) *= aspect_ratio; - _quad.initialize(); - - _texture_renderer.initialize(); - - // Point cloud rendering - _point_cloud.initialize(); - _point_cloud_renderer.initialize(); - } - - auto deinit_gl_resources() -> void - { - _texture.destroy(); - _quad.destroy(); - _texture_renderer.destroy(); - - _point_cloud.destroy(); - _point_cloud_renderer.destroy(); - } - - auto upload_point_cloud_data_to_opengl() -> void - { - const auto& point_cloud = _pipeline.point_cloud(); - - static constexpr auto dim = 6; - const auto num_points = static_cast(point_cloud.size()); - if (num_points == 0) - return; - - const auto ptr = - const_cast(point_cloud.data()); - const auto ptrd = reinterpret_cast(ptr); - const auto pc_tview = sara::TensorView_{ - ptrd, // - {num_points, dim} // - }; - - auto& logger = sara::Logger::get(); - SARA_LOGI(logger, "point cloud dimensions: {} ", pc_tview.sizes()); - _point_cloud.upload_host_data_to_gl(pc_tview.cast()); - } - - auto render_video() -> void - { - // Render on the right half of the window surface. - glViewport(_video_viewport.x(), _video_viewport.y(), // - _video_viewport.width(), _video_viewport.height()); - // Transfer the CPU image frame data to the OpenGL texture. - // _texture.reset(_pipeline._video_stream.frame_rgb8()); - _texture.reset(_pipeline.make_display_frame()); - // Render the texture on the quad. - auto model_view = Eigen::Transform{}; - model_view.setIdentity(); - _texture_renderer.render(_texture, _quad, model_view.matrix(), _projection); - } - - auto render_point_cloud() -> void - { - glViewport(_point_cloud_viewport.x(), _point_cloud_viewport.y(), - _point_cloud_viewport.width(), _point_cloud_viewport.height()); - - // CAVEAT: re-express the point cloud in OpenGL axis convention. - auto from_cam_to_gl = Eigen::Transform{}; - from_cam_to_gl.setIdentity(); - // clang-format off - from_cam_to_gl.matrix().topLeftCorner<3, 3>() << - 1, 0, 0, - 0, -1, 0, - 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); - } - - auto get_framebuffer_sizes() const -> Eigen::Vector2i - { - auto sizes = Eigen::Vector2i{}; - glfwGetFramebufferSize(_window, &sizes.x(), &sizes.y()); - return sizes; - } - -private: - static auto get_self(GLFWwindow* const window) -> SingleWindowApp& - { - const auto app_void_ptr = glfwGetWindowUserPointer(window); - if (app_void_ptr == nullptr) - throw std::runtime_error{ - "Please call glfwSetWindowUserPointer to register this window!"}; - const auto app_ptr = reinterpret_cast(app_void_ptr); - return *app_ptr; - } - - static auto window_size_callback(GLFWwindow* window, const int, const int) - -> void - { - auto& self = get_self(window); - - auto& fb_sizes = self._fb_sizes; - fb_sizes = self.get_framebuffer_sizes(); - - // Point cloud viewport rectangle geometry. - self._point_cloud_viewport.top_left().setZero(); - self._point_cloud_viewport.sizes() << fb_sizes.x() / 2, fb_sizes.y(); - - // Video viewport rectangle geometry. - self._video_viewport.top_left() << fb_sizes.x() / 2, 0; - self._video_viewport.sizes() << fb_sizes.x() / 2, fb_sizes.y(); - - // Update the current projection matrices. - auto scale = 0.5f; - if (self._video_viewport.width() < self._pipeline._video_streamer.width()) - scale *= static_cast(self._pipeline._video_streamer.width()) / - self._video_viewport.width(); - self._projection = self._video_viewport.orthographic_projection(scale); - - // Point cloud projection matrix. - self._point_cloud_projection = self._point_cloud_viewport.perspective(); - } - - static auto key_callback(GLFWwindow* window, // - int key, // - int /* scancode */, // - int action, // - int /* mods */) -> void - { - auto& app = get_self(window); - if (app._pause && key == GLFW_KEY_SPACE && - (action == GLFW_RELEASE || action == GLFW_REPEAT)) - { - app._pause = false; - std::cout << "RESUME" << std::endl; - return; - } - } - -private: - static auto init_glfw() -> void - { - if (_glfw_initialized) - throw std::runtime_error{ - "Error: cannot instantiate more than one GLFW application!"}; - - // Initialize the windows manager. - _glfw_initialized = glfwInit(); - if (!_glfw_initialized) - throw std::runtime_error{"Error: failed to initialize GLFW!"}; - - glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); - glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); - glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); -#if defined(__APPLE__) - glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); -#endif - } - - static auto init_glew() -> void - { -#if !defined(__APPLE__) - // Initialize GLEW. - const auto err = glewInit(); - if (err != GLEW_OK) - { - const auto err_str = - reinterpret_cast(glewGetErrorString(err)); - throw std::runtime_error{fmt::format( - "Error: failed to initialize GLEW: {}", std::string_view{err_str})}; - } -#endif - } - -private: - static bool _glfw_initialized; - - GLFWwindow* _window = nullptr; - //! @brief Framebuffer sizes - //! We want to use this and not the window sizes because of MacOS retina - //! display. - Eigen::Vector2i _fb_sizes = -Eigen::Vector2i::Ones(); - - sara::v2::OdometryPipeline _pipeline; - - //! Video rendering - //! - //! The viewport - k::Viewport _video_viewport; - //! @brief the video texture. - kgl::TexturedImage2D _texture; - //! @brief the video quad. - kgl::TexturedQuad _quad; - //! @brief Texture renderer. - kgl::TextureRenderer _texture_renderer; - //! @brief Model-view-projection matrices. - Eigen::Matrix4f _projection; - - //! Point cloud rendering - //! - //! @brief The viewport. - k::Viewport _point_cloud_viewport; - //! @brief Point cloud GPU data. - kgl::ColoredPointCloud _point_cloud; - //! @brief Point cloud GPU renderer. - kgl::ColoredPointCloudRenderer _point_cloud_renderer; - //! @brief Point cloud rendering options. - Eigen::Matrix4f _point_cloud_projection; - // kgl::Camera _point_cloud_camera; - float _point_size = 5.f; - - //! @brief User interaction. - bool _pause = false; -}; - -bool SingleWindowApp::_glfw_initialized = false; - - -auto main([[maybe_unused]] int const argc, - [[maybe_unused]] char** const argv) -> int -{ -#if defined(_OPENMP) - const auto num_threads = omp_get_max_threads(); - omp_set_num_threads(num_threads); - Eigen::setNbThreads(num_threads); -#endif - -#define USE_HARDCODED_VIDEO_PATH -#if defined(USE_HARDCODED_VIDEO_PATH) && defined(__APPLE__) - const auto video_path = fs::path{"/Users/oddkiva/Desktop/datasets/sample-1.mp4"}; - if (!fs::exists(video_path)) - { - fmt::print("Video {} does not exist", video_path.string()); - return EXIT_FAILURE; - } -#else - if (argc < 2) - { - std::cout << fmt::format("Usage: {} VIDEO_PATH\n", - std::string_view{argv[0]}); - return EXIT_FAILURE; - } - - const auto video_path = fs::path{argv[1]}; -#endif - auto camera = sara::v2::BrownConradyDistortionModel{}; - { - camera.fx() = 917.2878392016245; - camera.fy() = 917.2878392016245; - camera.shear() = 0.; - camera.u0() = 960.; - camera.v0() = 540.; - // clang-format off - camera.k() << - -0.2338367557617234, - +0.05952465745165465, - -0.007947847982157091; - // clang-format on - camera.p() << -0.0003137658969742134, 0.00021943576376532096; - } - - try - { - auto app = SingleWindowApp{{800, 600}, "Odometry: " + video_path.string()}; - app.set_config(video_path, camera); - app.run(); - } - catch (std::exception& e) - { - std::cerr << e.what() << std::endl; - return EXIT_FAILURE; - } - - return EXIT_SUCCESS; -} diff --git a/cpp/src/DO/Sara/SfM/Odometry/ImageDistortionCorrector.hpp b/cpp/src/DO/Sara/SfM/Odometry/ImageDistortionCorrector.hpp index e5a744bca..2fe3ba325 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/ImageDistortionCorrector.hpp +++ b/cpp/src/DO/Sara/SfM/Odometry/ImageDistortionCorrector.hpp @@ -7,7 +7,6 @@ namespace DO::Sara { - class ImageDistortionCorrector { public: diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.hpp index 145361f36..86c92f301 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.hpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.hpp @@ -1,98 +1,90 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// Copyright (C) 2024-present David Ok +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License v. 2.0. If a copy of the MPL was not distributed with this file, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + #pragma once -#include -#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include #include -#include - namespace DO::Sara { - struct OdometryPipeline + class OdometryPipeline { + public: auto set_config(const std::filesystem::path& video_path, const v2::BrownConradyDistortionModel& camera) - -> void - { - // Build the dependency graph. - _video_streamer.open(video_path); - _camera = camera; - - _distortion_corrector = std::make_unique( - _video_streamer.frame_rgb8(), // - _video_streamer.frame_gray32f(), // - _camera // - ); - - _feature_tracker = std::make_unique( - _distortion_corrector->frame_gray32f()); - - _relative_pose_estimator = std::make_unique( - _feature_tracker->keys, _feature_tracker->matches, _camera); - - _triangulator = std::make_unique( - _relative_pose_estimator->_geometry.C1, - _relative_pose_estimator->_geometry.C2, // - _relative_pose_estimator->_K, _relative_pose_estimator->_K_inv, - _relative_pose_estimator->_X, _relative_pose_estimator->_inliers); - } + -> void; - auto read() -> bool - { - return _video_streamer.read(); - } + auto read() -> bool; - auto process() -> void - { - if (_video_streamer.skip()) - return; + auto process() -> void; - _distortion_corrector->undistort(); + auto make_display_frame() const -> Image; - // N.B.: detect the features on the **undistorted** image. - _feature_tracker->detect_features(); - _feature_tracker->match_features(); + auto point_cloud() const -> const PointCloudGenerator::PointCloud& + { + return _point_cloud; + } - const auto success = _relative_pose_estimator->estimate_relative_pose(); - if (!success) - return; + private: /* computer vision tasks */ + auto detect_keypoints(const ImageView&) const + -> KeypointList; - _triangulator->triangulate(); - _triangulator->extract_colors(_distortion_corrector->frame_rgb8(0), - _distortion_corrector->frame_rgb8(1)); - _triangulator->update_colored_point_cloud(); - } + auto + estimate_relative_pose(const KeypointList& keys_src, + const KeypointList& keys_dst) const + -> std::pair; - auto make_display_frame() const -> Image - { - Image display = _distortion_corrector->frame_rgb8(); - const auto& matches = _feature_tracker->matches; - const auto& inliers = _relative_pose_estimator->_inliers; - const auto num_matches = static_cast(matches.size()); -#pragma omp parallel for - for (auto m = 0; m < num_matches; ++m) - { - if (!inliers(m)) - continue; - const auto& match = matches[m]; - draw(display, match.x(), Blue8); - draw(display, match.y(), Cyan8); - draw_arrow(display, match.x_pos(), match.y_pos(), Yellow8); - } - - return display; - } + private: /* graph update tasks */ + auto grow_geometry() -> bool; + public: /* data members */ VideoStreamer _video_streamer; v2::BrownConradyDistortionModel _camera; + v2::PinholeCamera _camera_corrected; + //! @brief Data mutators. + //! @{ std::unique_ptr _distortion_corrector; - std::unique_ptr _feature_tracker; - std::unique_ptr _relative_pose_estimator; - std::unique_ptr _triangulator; + v2::RelativePoseEstimator _rel_pose_estimator; + CameraPoseEstimator _abs_pose_estimator; + std::unique_ptr _point_cloud_generator; + //! @} + + //! @brief SfM data. + //! @{ + FeatureParams _feature_params; + FeatureTracker _feature_tracker; + CameraPoseGraph _pose_graph; + PointCloudGenerator::PointCloud _point_cloud; + //! @} + + //! @brief SfM state. + //! @{ + CameraPoseGraph::Vertex _pose_prev; + CameraPoseGraph::Vertex _pose_curr; + CameraPoseGraph::Edge _relative_pose_edge; + FeatureTracker::TrackArray _tracks_alive; + FeatureTracker::TrackArray _tracks_alive_with_known_scene_point; + FeatureTracker::TrackArray _tracks_alive_without_scene_point; + FeatureTracker::TrackVisibilityCountArray _track_visibility_count; + Eigen::Matrix3d _current_global_rotation = Eigen::Matrix3d::Identity(); + //! @} }; } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp deleted file mode 100644 index ca4251e25..000000000 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp +++ /dev/null @@ -1,323 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2024-present David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#include - -#include - -#include -#include -#include -#include -#include - - -using namespace DO::Sara; - - -auto draw_feature_tracks(DO::Sara::ImageView& display, // - const CameraPoseGraph& pgraph, // - const FeatureGraph& fgraph, // - const CameraPoseGraph::Vertex pose_u, - const std::vector& tracks_alive, - const std::vector& track_visibility_count, - const float scale) -> void -{ - for (auto t = 0u; t < tracks_alive.size(); ++t) - { - const auto& track = tracks_alive[t]; - - // Just to double check. - if (track_visibility_count[t] < 3u) - continue; - - auto p = std::array{}; - auto r = std::array{}; - { - const auto& [pose_v0, k] = fgraph[track[0]]; - const auto& f0 = features(pgraph[pose_v0].keypoints)[k]; - p[0] = f0.center(); - r[0] = std::max(f0.radius() * scale, 1.f); - const auto color = - pose_v0 == pose_u ? Rgb8{0, 255, 255} : Rgb8{0, 127, 127}; - draw_circle(display, p[0], r[0], color, 3); - } - - for (auto v1 = 1u; v1 < track.size(); ++v1) - { - const auto& [pose_v1, k] = fgraph[track[v1]]; - const auto& f1 = features(pgraph[pose_v1].keypoints)[k]; - p[1] = f1.center(); - r[1] = std::max(f1.radius() * scale, 1.f); - const auto color = - pose_v1 == pose_u ? Rgb8{255, 0, 0} : Rgb8{0, 127, 127}; - draw_circle(display, p[1], r[1], color, 3); - - draw_arrow(display, p[0], p[1], color, 3); - - p[0] = p[1]; - r[0] = r[1]; - } - } -} - - -auto v2::OdometryPipeline::set_config( - const std::filesystem::path& video_path, - const v2::BrownConradyDistortionModel& camera) -> void -{ - // Build the dependency graph. - _video_streamer.open(video_path); - // The original camera. - _camera = camera; - // The virtual camera for the undistorted image. - _camera_corrected.focal_lengths() << camera.fx(), camera.fy(); - _camera_corrected.shear() = camera.shear(); - _camera_corrected.principal_point() << camera.u0(), camera.v0(); - - // Computer vision tasks. - _distortion_corrector = std::make_unique( - _video_streamer.frame_rgb8(), // - _video_streamer.frame_gray32f(), // - _camera // - ); - _rel_pose_estimator.configure(_camera); - _point_cloud_generator = std::make_unique( - _pose_graph, _feature_tracker._feature_graph, _point_cloud); -} - -auto v2::OdometryPipeline::read() -> bool -{ - return _video_streamer.read(); -} - -auto v2::OdometryPipeline::process() -> void -{ - if (_video_streamer.skip()) - return; - - auto& logger = Logger::get(); - SARA_LOGI(logger, "[Video Stream] Processing image frame {}", - _video_streamer.frame_number()); - - SARA_LOGI(logger, "[Image Distortion] Undistort image frame {}", - _video_streamer.frame_number()); - _distortion_corrector->undistort(); - - grow_geometry(); -} - -auto v2::OdometryPipeline::make_display_frame() const -> Image -{ - auto frame = Image{_distortion_corrector->frame_rgb8()}; - draw_feature_tracks(frame, _pose_graph, _feature_tracker._feature_graph, - _pose_curr, _tracks_alive, _track_visibility_count, 1.f); - return frame; -} - -auto v2::OdometryPipeline::detect_keypoints(const ImageView& image) const - -> KeypointList -{ - auto& logger = Logger::get(); - const auto keys = compute_sift_keypoints(image, // - _feature_params.image_pyr_params); - SARA_LOGI(logger, "[Keypoint Detection] {} keypoints", features(keys).size()); - return keys; -} - -auto v2::OdometryPipeline::estimate_relative_pose( - const KeypointList& keys_src, - const KeypointList& keys_dst) const - -> std::pair -{ - auto& logger = Logger::get(); - - if (features(keys_src).empty() || features(keys_dst).empty()) - { - SARA_LOGI(logger, "[Relative Pose] Skipped image matching..."); - return {}; - } - - auto matches = match(keys_src, keys_dst, _feature_params.sift_nn_ratio); - SARA_LOGI(logger, "[Relative Pose] Matched image keypoints..."); - if (matches.empty()) - return {}; - if (matches.size() > _feature_params.num_matches_max) - matches.resize(_feature_params.num_matches_max); - - auto [two_view_geometry, inliers, sample_best] = - _rel_pose_estimator.estimate_relative_pose(keys_src, keys_dst, matches); - SARA_LOGI(logger, "[Relative Pose] Estimated relative pose..."); - - const auto res = std::make_pair( // - RelativePoseData{.matches = std::move(matches), - .inliers = std::move(inliers), - .motion = - { - .R = two_view_geometry.C2.R, // - .t = two_view_geometry.C2.t // - } - - }, - std::move(two_view_geometry) // - ); - - return res; -} - -auto v2::OdometryPipeline::grow_geometry() -> bool -{ - auto& logger = Logger::get(); - - // Detect and describe the local features. - _pose_prev = _pose_curr; - - const auto frame_gray32f = _distortion_corrector->frame_gray32f(); - const auto frame_number = _video_streamer.frame_number(); - auto keys_curr = detect_keypoints(frame_gray32f); - - // Boundary case: the graphs are empty. - if (_pose_graph.num_vertices() == 0) - { - // Initialize the new camera pose from the latest image frame. - auto abs_pose_curr = QuaternionBasedPose::identity(); - auto abs_pose_data = AbsolutePoseData{ - frame_number, // - std::move(keys_curr), // - std::move(abs_pose_curr) // - }; - _pose_curr = _pose_graph.add_absolute_pose(std::move(abs_pose_data)); - - return true; - } - - const auto& keys_prev = _pose_graph[_pose_prev].keypoints; - auto [rel_pose_data, two_view_geometry] = - estimate_relative_pose(keys_prev, keys_curr); - const auto num_inliers = rel_pose_data.inliers.flat_array().count(); - SARA_LOGI(logger, "[SfM] Relative pose inliers: {} 3D points", num_inliers); - if (num_inliers < _feature_params.num_inliers_min) - { - SARA_LOGI(logger, "[SfM] Relative pose failed!"); - return false; - } - SARA_LOGI(logger, "[SfM] Relative pose succeeded!"); - - // 1. Add the absolute pose vertex. - auto abs_pose_curr = QuaternionBasedPose::nan(); - if (_pose_graph.num_vertices() == 1) - abs_pose_curr = { - .q = Eigen::Quaterniond{rel_pose_data.motion.R}, // - .t = rel_pose_data.motion.t // - }; - auto abs_pose_data = AbsolutePoseData{ - frame_number, // - std::move(keys_curr), // - std::move(abs_pose_curr) // - }; - _pose_curr = _pose_graph.add_absolute_pose(std::move(abs_pose_data)); - - // 2. Update the feature tracks by adding the feature matches that are - // verified by the relative pose estimation. - // Notice move semantics which will the relative pose data after this call. - // - // Note that in the case of only two views, feature tracks are "compressed" - // feature matches. - const auto pose_edge = _pose_graph.add_relative_pose( // - _pose_prev, _pose_curr, // - std::move(rel_pose_data)); - _feature_tracker.update_feature_tracks(_pose_graph, pose_edge); - - // 3. Recalculate the feature tracks that are still alive. - std::tie(_tracks_alive, _track_visibility_count) = - _feature_tracker.calculate_alive_feature_tracks(_pose_curr); - - // Extra steps for when the pose graph contains 2 poses. - if (_pose_graph.num_vertices() == 2) - { - _tracks_alive_without_scene_point = _tracks_alive; - } - // Extra steps for when the pose graph contains 3 poses or more. - else - { - // 4. Propagate the scene point to the feature tracks that grew longer. - // The feature tracks that grew longer can only be those among the - // tracks that are still alive. - SARA_LOGI( - logger, - "Propagating the scene points to feature tracks that grew longer..."); - _point_cloud_generator->propagate_scene_point_indices(_tracks_alive); - - // 5. Reassign a unique scene point cloud to each feature tracks by - // compressing the point cloud. - SARA_LOGI(logger, "Compressing the point cloud..."); - _point_cloud_generator->compress_point_cloud( - _feature_tracker._feature_tracks); - - // 6. Determine the current absolute pose from the alive tracks using a PnP - // approach. - std::tie(_tracks_alive_with_known_scene_point, - _tracks_alive_without_scene_point) = - _point_cloud_generator->split_by_scene_point_knowledge(_tracks_alive); - const auto [abs_pose_mat, abs_pose_est_successful] = - _abs_pose_estimator.estimate_pose(_tracks_alive_with_known_scene_point, - _pose_curr, _camera_corrected, - *_point_cloud_generator); - if (!abs_pose_est_successful) - { - SARA_LOGI(logger, "Failed to estimate the absolute pose!"); - return false; - } - - // 7. Update the current absolute pose, which was initialized dummily. - abs_pose_curr = QuaternionBasedPose{ - Eigen::Quaterniond{abs_pose_mat.leftCols<3>()}, // - abs_pose_mat.col(3) // - }; - _pose_graph[_pose_curr].pose = abs_pose_curr; - } - - // 8. Grow point cloud by triangulation. - // - // TODO: don't add 3D scene points that are too far, like points in the sky - const auto frame_corrected = _distortion_corrector->frame_rgb8(); - _point_cloud_generator->grow_point_cloud(_tracks_alive_without_scene_point, - frame_corrected, pose_edge, - _camera_corrected); - - // The rotation is expressed in the camera coordinates. - // But the calculation is done in the automotive/aeronautics coordinate - // system. - // - // The z-coordinate of the camera coordinates is the x-axis of the - // automotive coordinates - // - // clang-format off - static const auto P = (Eigen::Matrix3d{} << - 0, 0, 1, - -1, 0, 0, - 0, -1, 0 - ).finished(); - // clang-format on - - const auto& R = _pose_graph[pose_edge].motion.R; - const Eigen::Matrix3d R_delta_abs = P * R.transpose() * P.transpose(); - _current_global_rotation = R_delta_abs * _current_global_rotation; - - const auto q_global = Eigen::Quaterniond{_current_global_rotation}; - auto angles = calculate_yaw_pitch_roll(q_global); - static constexpr auto degrees = 180. / M_PI; - SARA_LOGI(logger, "Global yaw = {:0.3f} deg", angles(0) * degrees); - SARA_LOGI(logger, "Global pitch = {:0.3f} deg", angles(1) * degrees); - SARA_LOGI(logger, "Global roll = {:0.3f} deg", angles(2) * degrees); - - return true; -} diff --git a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp b/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp deleted file mode 100644 index 5b4a96bab..000000000 --- a/cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2024-present David Ok -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License v. 2.0. If a copy of the MPL was not distributed with this file, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -#pragma once - -#include "DO/Sara/MultiViewGeometry/Camera/v2/PinholeCamera.hpp" -#include "DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp" -#include -#include -#include -#include -#include -#include -#include - -namespace DO::Sara::v2 { - - class OdometryPipeline - { - public: - auto - set_config(const std::filesystem::path& video_path, - const v2::BrownConradyDistortionModel& camera) -> void; - - auto read() -> bool; - - auto process() -> void; - - auto make_display_frame() const -> Image; - - auto point_cloud() const -> const PointCloudGenerator::PointCloud& - { - return _point_cloud; - } - - private: /* computer vision tasks */ - auto detect_keypoints(const ImageView&) const - -> KeypointList; - - auto estimate_relative_pose(const KeypointList& keys_src, - const KeypointList& keys_dst) - const -> std::pair; - - private: /* graph update tasks */ - auto grow_geometry() -> bool; - - public: /* data members */ - VideoStreamer _video_streamer; - v2::BrownConradyDistortionModel _camera; - v2::PinholeCamera _camera_corrected; - - //! @brief Data mutators. - //! @{ - std::unique_ptr _distortion_corrector; - v2::RelativePoseEstimator _rel_pose_estimator; - CameraPoseEstimator _abs_pose_estimator; - std::unique_ptr _point_cloud_generator; - //! @} - - //! @brief SfM data. - //! @{ - FeatureParams _feature_params; - FeatureTracker _feature_tracker; - CameraPoseGraph _pose_graph; - PointCloudGenerator::PointCloud _point_cloud; - //! @} - - //! @brief SfM state. - //! @{ - CameraPoseGraph::Vertex _pose_prev; - CameraPoseGraph::Vertex _pose_curr; - CameraPoseGraph::Edge _relative_pose_edge; - FeatureTracker::TrackArray _tracks_alive; - FeatureTracker::TrackArray _tracks_alive_with_known_scene_point; - FeatureTracker::TrackArray _tracks_alive_without_scene_point; - FeatureTracker::TrackVisibilityCountArray _track_visibility_count; - Eigen::Matrix3d _current_global_rotation = Eigen::Matrix3d::Identity(); - //! @} - }; - -} // namespace DO::Sara::v2