diff --git a/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp b/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp index 707e0d724..064508edd 100644 --- a/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/two_view_bundle_adjustment_example.cpp @@ -215,7 +215,7 @@ GRAPHICS_MAIN() // 3. Recalculate the feature tracks that are still alive. const auto [tracks, track_visibility_count] = - feature_tracker.calculate_alive_feature_tracks(1); + feature_tracker.find_feature_tracks_at_pose(1); auto point_cloud = sara::PointCloudGenerator::PointCloud{}; auto point_cloud_generator = sara::PointCloudGenerator{ diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 4e375e112..1b5713bae 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -104,9 +104,8 @@ class SingleWindowApp { // Current projection matrix _projection = _video_viewport.orthographic_projection(); - _point_cloud_projection = _point_cloud_viewport.orthographic_projection(); - // _point_cloud_projection = - // _point_cloud_viewport.perspective(120.f, 1e-6f, 1e3f); + // _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); @@ -466,15 +465,20 @@ class SingleWindowApp Eigen::Matrix4f _point_cloud_projection; //! @brief Camera of the point cloud scene. k::Camera _point_cloud_camera; + //! @{ + //! @brief Viewing parameters. 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; + //! @} }; bool SingleWindowApp::_glfw_initialized = false; diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp index 5fd2ec4f3..dfd9e66a4 100644 --- a/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp +++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/PointCloudGenerator.cpp @@ -93,8 +93,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) { @@ -392,7 +392,8 @@ auto PointCloudGenerator::grow_point_cloud( // Calculate the scene point. const Eigen::Vector3d coords = X.col(j).hnormalized(); - if (coords.squaredNorm() > distance_max_squared()) // 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, // @@ -474,11 +475,16 @@ auto PointCloudGenerator::write_ply(const std::filesystem::path& out_ply) const -> void { auto coords = std::vector(_point_cloud.size()); - auto colors = std::vector(_point_cloud.size()); + auto colors = std::vector>(_point_cloud.size()); std::transform(_point_cloud.begin(), _point_cloud.end(), coords.begin(), [](const ScenePoint& x) { return x.coords(); }); std::transform(_point_cloud.begin(), _point_cloud.end(), colors.begin(), - [](const ScenePoint& x) { return x.color(); }); + [](const ScenePoint& x) -> Eigen::Vector3 { + Eigen::Vector3d x255 = x.color() * 255; + for (auto i = 0; i < 3; ++i) + x255(i) = std::clamp(x255(i), 0., 255.); + return x255.cast(); + }); auto fb = std::filebuf{}; fb.open(out_ply, std::ios::out); @@ -493,7 +499,7 @@ auto PointCloudGenerator::write_ply(const std::filesystem::path& out_ply) const tinyply::Type::INVALID, 0); ply.add_properties_to_element("vertex", {"red", "green", "blue"}, - tinyply::Type::FLOAT64, colors.size(), + tinyply::Type::UINT8, colors.size(), reinterpret_cast(colors.data()), tinyply::Type::INVALID, 0); diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp index 611a8c298..7259ee629 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.cpp @@ -129,8 +129,8 @@ auto FeatureTracker::update_feature_tracks( _feature_tracks.size()); } -auto FeatureTracker::calculate_alive_feature_tracks( - const CameraPoseGraph::Vertex camera_vertex_curr) const +auto FeatureTracker::find_feature_tracks_at_pose( + const CameraPoseGraph::Vertex pose_vertex) const -> std::tuple { auto& logger = Logger::get(); @@ -139,7 +139,7 @@ auto FeatureTracker::calculate_alive_feature_tracks( const FeatureGraph::Impl& fgraph = _feature_graph; const auto& ftracks = _feature_tracks; - auto tracks_alive = TrackArray{}; + auto tracks_filtered = TrackArray{}; auto track_visibility_count = TrackVisibilityCountArray{}; for (const auto& ftrack : ftracks) @@ -147,15 +147,15 @@ auto FeatureTracker::calculate_alive_feature_tracks( // Do we still see the track in the image. const auto is_alive = std::find_if(ftrack.begin(), ftrack.end(), - [&fgraph, camera_vertex_curr](const auto& v) { - return fgraph[v].pose_vertex == camera_vertex_curr; + [&fgraph, pose_vertex](const auto& v) { + return fgraph[v].pose_vertex == pose_vertex; }) != ftrack.end(); if (!is_alive) continue; // Add the newly found alive track. - tracks_alive.push_back(ftrack); + tracks_filtered.push_back(ftrack); // Carefully count the track life, it's not the number of vertices, but // the number of camera views in which the feature reappears. @@ -167,7 +167,7 @@ auto FeatureTracker::calculate_alive_feature_tracks( [&fgraph](const auto& v) { return fgraph[v].pose_vertex; }); track_visibility_count.push_back(camera_vertices_where_present.size()); } - SARA_LOGD(logger, "Num tracks alive: {}", tracks_alive.size()); + SARA_LOGD(logger, "Num tracks alive: {}", tracks_filtered.size()); const auto longest_track_alive = std::max_element( track_visibility_count.begin(), track_visibility_count.end()); @@ -186,5 +186,5 @@ auto FeatureTracker::calculate_alive_feature_tracks( #endif } - return std::make_tuple(tracks_alive, track_visibility_count); + return std::make_tuple(tracks_filtered, track_visibility_count); } diff --git a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp index 9ad4da9be..0b9df2684 100644 --- a/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp +++ b/cpp/src/DO/Sara/SfM/Graph/FeatureTracker.hpp @@ -44,7 +44,7 @@ namespace DO::Sara { auto update_feature_tracks(const CameraPoseGraph&, const CameraPoseGraph::Edge) -> void; - auto calculate_alive_feature_tracks( + auto find_feature_tracks_at_pose( const CameraPoseGraph::Vertex last_pose_vertex) const -> std::tuple; }; diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp index caacfec58..4398f6f3e 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp @@ -19,6 +19,7 @@ #include +// #define USE_ABSOLUTE_ROTATION // #define DEBUG_ABSOLUTE_POSE_RECOVERY @@ -218,7 +219,7 @@ auto OdometryPipeline::grow_geometry() -> bool // 3. Recalculate the feature tracks that are still alive. std::tie(_tracks_alive, _track_visibility_count) = - _feature_tracker.calculate_alive_feature_tracks(_pose_curr); + _feature_tracker.find_feature_tracks_at_pose(_pose_curr); #if defined(DEBUG_ABSOLUTE_POSE_RECOVERY) const auto corr_csv_fp = fmt::format("corr_{}.csv", _pose_curr); @@ -254,7 +255,6 @@ auto OdometryPipeline::grow_geometry() -> bool _tracks_alive_without_scene_point) = _point_cloud_generator->split_by_scene_point_knowledge(_tracks_alive); -// #define USE_ABSOLUTE_ROTATION #if defined(USE_ABSOLUTE_ROTATION) const auto [abs_pose_mat, abs_pose_est_successful] = _abs_pose_estimator.estimate_pose( @@ -282,16 +282,10 @@ auto OdometryPipeline::grow_geometry() -> bool } // 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); -#if defined(DEBUG_ABSOLUTE_POSE_RECOVERY) - const auto scene_csv_fp = fmt::format("scene_{}.csv", _pose_curr); - _point_cloud_generator->write_point_cloud(_tracks_alive, scene_csv_fp); -#endif adjust_bundles(); @@ -308,7 +302,11 @@ auto OdometryPipeline::grow_geometry() -> bool // // --------------------------------------------------------------------------- - _point_cloud_generator->write_ply(fmt::format("scene_{:04d}.ply", _pose_curr)); +#if defined(DEBUG_ABSOLUTE_POSE_RECOVERY) + const auto scene_ply = fmt::format("scene_{:04d}.ply", _pose_curr); + SARA_LOGI(logger, "Saving PLY {}", scene_ply); + _point_cloud_generator->write_ply(scene_ply); +#endif return true; }