From 8c8dca9b6a6e784b4e5495b030704a28e2599f96 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sat, 25 May 2024 19:00:16 +0100 Subject: [PATCH] MAINT: refactor code. --- cpp/src/DO/Sara/SfM/Helpers/Utilities.cpp | 100 ++++++++++++++++++ cpp/src/DO/Sara/SfM/Helpers/Utilities.hpp | 36 +++++++ .../DO/Sara/SfM/Odometry/OdometryPipeline.cpp | 78 +------------- 3 files changed, 137 insertions(+), 77 deletions(-) create mode 100644 cpp/src/DO/Sara/SfM/Helpers/Utilities.cpp create mode 100644 cpp/src/DO/Sara/SfM/Helpers/Utilities.hpp diff --git a/cpp/src/DO/Sara/SfM/Helpers/Utilities.cpp b/cpp/src/DO/Sara/SfM/Helpers/Utilities.cpp new file mode 100644 index 000000000..2b0db98dd --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Helpers/Utilities.cpp @@ -0,0 +1,100 @@ +// ========================================================================== // +// 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 + + +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, + [[maybe_unused]] 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 write_point_correspondences( + const CameraPoseGraph& pgraph, // + const FeatureGraph& fgraph, // + const std::vector& tracks_alive, + const std::filesystem::path& out_csv_fp) -> void + { + std::ofstream out{out_csv_fp.string()}; + if (!out) + throw std::runtime_error{"Cannot create output CSV!"}; + + for (auto t = 0u; t < tracks_alive.size(); ++t) + { + const auto& track = tracks_alive[t]; + + auto p = std::array{}; + + const auto& [pose_v0, k0] = fgraph[track.front()]; + const auto& f0 = features(pgraph[pose_v0].keypoints)[k0]; + p[0] = f0.center(); + + const auto& [pose_v1, k1] = fgraph[track.back()]; + const auto& f1 = features(pgraph[pose_v1].keypoints)[k1]; + p[1] = f1.center(); + + out << fmt::format("{},{},{},{}\n", p[0].x(), p[0].y(), p[1].x(), + p[1].y()); + } + } + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Helpers/Utilities.hpp b/cpp/src/DO/Sara/SfM/Helpers/Utilities.hpp new file mode 100644 index 000000000..499ce36e6 --- /dev/null +++ b/cpp/src/DO/Sara/SfM/Helpers/Utilities.hpp @@ -0,0 +1,36 @@ +// ========================================================================== // +// 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 + + +namespace DO::Sara { + + auto + draw_feature_tracks(ImageView&, // + const CameraPoseGraph&, // + const FeatureGraph&, // + const CameraPoseGraph::Vertex pose_vertex, + const std::vector& tracks, + const std::vector& track_visibility_count, + const float scale = 1.f) -> void; + + + auto + write_point_correspondences(const CameraPoseGraph&, const FeatureGraph&, + const std::vector& tracks, + const std::filesystem::path& out_csv_fp) -> void; + +} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp index bc4568b5e..d7bd5b6ab 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp @@ -14,10 +14,9 @@ #include #include #include -#include #include #include -#include +#include // #define DEBUG_ABSOLUTE_POSE_RECOVERY @@ -26,81 +25,6 @@ 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, - [[maybe_unused]] 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 write_point_correspondences( - const CameraPoseGraph& pgraph, // - const FeatureGraph& fgraph, // - const std::vector& tracks_alive, - const std::filesystem::path& out_csv_fp) -> void -{ - std::ofstream out{out_csv_fp.string()}; - if (!out) - throw std::runtime_error{"Cannot create output CSV!"}; - - for (auto t = 0u; t < tracks_alive.size(); ++t) - { - const auto& track = tracks_alive[t]; - - auto p = std::array{}; - - const auto& [pose_v0, k0] = fgraph[track.front()]; - const auto& f0 = features(pgraph[pose_v0].keypoints)[k0]; - p[0] = f0.center(); - - const auto& [pose_v1, k1] = fgraph[track.back()]; - const auto& f1 = features(pgraph[pose_v1].keypoints)[k1]; - p[1] = f1.center(); - - out << fmt::format("{},{},{},{}\n", p[0].x(), p[0].y(), p[1].x(), p[1].y()); - } -} - auto OdometryPipeline::set_config( const std::filesystem::path& video_path, const v2::BrownConradyDistortionModel& camera) -> void