From 432d88c9d9925f61ac59540a8b85692952ce646a Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sun, 23 Feb 2025 15:23:47 +0000 Subject: [PATCH 01/37] [34] First working version of observer design --- feature_tracker/CMakeLists.txt | 7 +- .../include/feature_tracker/feature_tracker.h | 53 +++- .../feature_tracker_observer.hpp | 33 +++ .../feature_tracker_observer_spdlog_rerun.hpp | 25 ++ feature_tracker/src/feature_tracker.cpp | 237 +++++++++++++----- .../src/feature_tracker_observer.cpp | 15 ++ .../feature_tracker_observer_spdlog_rerun.cpp | 44 ++++ feature_tracker/src/main.cpp | 105 ++++++++ .../src/feature_tracker_node.cpp | 20 +- .../pose_graph_ros/pose_graph_node.hpp | 16 +- 10 files changed, 459 insertions(+), 96 deletions(-) create mode 100644 feature_tracker/include/feature_tracker/feature_tracker_observer.hpp create mode 100644 feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp create mode 100644 feature_tracker/src/feature_tracker_observer.cpp create mode 100644 feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp create mode 100644 feature_tracker/src/main.cpp diff --git a/feature_tracker/CMakeLists.txt b/feature_tracker/CMakeLists.txt index e085e4e37..b3152bd60 100644 --- a/feature_tracker/CMakeLists.txt +++ b/feature_tracker/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) # Define your library target -add_library(feature_tracker STATIC src/feature_tracker.cpp) +add_library(feature_tracker STATIC src/feature_tracker.cpp src/feature_tracker_observer.cpp src/feature_tracker_observer_spdlog_rerun.cpp) target_link_libraries(feature_tracker PRIVATE ${OpenCV_LIBS} ${CERES_LIBRARIES} spdlog::spdlog_header_only camodocal::camodocal ) # Set include directories @@ -18,6 +18,11 @@ target_include_directories(feature_tracker $ ) +add_executable(feature_tracker_run + src/main.cpp + + ) +target_link_libraries(feature_tracker_run feature_tracker ${OpenCV_LIBS} spdlog::spdlog_header_only) # Install the library include(GNUInstallDirs) install(TARGETS feature_tracker diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index ca5687d4a..ddbd4977f 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -1,18 +1,18 @@ #pragma once -#include -#include -#include #include -#include -#include +#include +#include #include +#include +#include +#include #include "camodocal/camera_models/CameraFactory.h" #include "camodocal/camera_models/CataCamera.h" #include "camodocal/camera_models/PinholeCamera.h" - +#include "feature_tracker/feature_tracker_observer.hpp" #include "feature_tracker/parameters.h" #include "feature_tracker/tic_toc.h" @@ -27,8 +27,17 @@ void reduceVector(vector &v, vector status); class FeatureTracker { public: - FeatureTracker(); - + FeatureTracker(std::string camera_config_file, bool fisheye, + bool run_histogram_equilisation, + uint max_feature_count_per_image, + uint min_distance_between_features, + double fundemental_matrix_ransac_threshold, double fx, + double fy, double feature_pruning_frequency,double max_time_difference); + void RegisterEventObserver( + std::shared_ptr event_observer); + void ProcessNewFrame(cv::Mat new_frame, double time_s); + + private: void readImage(const cv::Mat &_img, double _cur_time); void setMask(); @@ -45,20 +54,40 @@ class FeatureTracker { void undistortedPoints(); + void RestartTracker(); + cv::Mat mask; cv::Mat fisheye_mask; - cv::Mat prev_img, cur_img, forw_img; + cv::Mat prev_img_; + cv::Mat cur_img_; vector n_pts; - vector prev_pts, cur_pts, forw_pts; - vector prev_un_pts, cur_un_pts; + vector prev_pts, curr_pts; + vector pts_velocity; vector ids; vector track_cnt; - map cur_un_pts_map; + vector cur_un_pts; map prev_un_pts_map; + + double fx_; + double fy_; camodocal::CameraPtr m_camera; double cur_time; double prev_time; + double prev_prune_time; static int n_id; + + bool fisheye_; + bool run_histogram_equilisation_; + uint max_feature_count_per_image_; + uint min_distance_between_features_; + double fundemental_matrix_ransac_threshold_; + bool is_first_frame_; + double first_frame_time_; + double previous_frame_time_; + double max_time_difference_; + double feature_pruning_frequency_; + double feature_pruning_period_; + std::shared_ptr event_observer_; }; diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp new file mode 100644 index 000000000..2f1615c0f --- /dev/null +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp @@ -0,0 +1,33 @@ +#ifndef FEATURE_TRACKER_OBSERVER_HPP +#define FEATURE_TRACKER_OBSERVER_HPP + +#include +#include + +class FeatureTrackerObserver + : public std::enable_shared_from_this { + public: + FeatureTrackerObserver() = default; + virtual ~FeatureTrackerObserver() = default; + + virtual void OnRegistered() = 0; + virtual void OnRestart() = 0; + virtual void OnDurationBetweenFrameTooLarge(double current_image_time_s, + double previous_image_time_s) = 0; + virtual void OnImageTimeMovingBackwards(double current_image_time_s, + double previous_image_time_s)=0; + virtual void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, + std::vector features, + std::vector undistorted_features, + + std::vector ids, + std::vector track_count, + std::vector points_velocity) = 0; + + private: + void CreateTrackedFeatureImage(cv::Mat image, + std::vector features, + std::vector track_cnt, + uint max_track_count); +}; +#endif /* FEATURE_TRACKER_OBSERVER_HPP */ diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp b/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp new file mode 100644 index 000000000..1a6af0de4 --- /dev/null +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp @@ -0,0 +1,25 @@ +#ifndef FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP +#define FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP +#include "feature_tracker/feature_tracker_observer.hpp" +class FeatureTrackerObserverSPDRerun : public FeatureTrackerObserver { + public: + // FeatureTrackerObserverSPDRerun() = default; + ~FeatureTrackerObserverSPDRerun(); + + private: + void OnRegistered() final; + + void OnRestart() final; + void OnDurationBetweenFrameTooLarge(double current_image_time_s, + double previous_image_time_s) final; + void OnImageTimeMovingBackwards(double current_image_time_s, + double previous_image_time_s) final; + void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, + std::vector features, + std::vector undistorted_features, + + std::vector ids, std::vector track_count, + std::vector points_velocity) final; +}; + +#endif /* FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP */ diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 47b6aafec..9a03814c6 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -1,14 +1,17 @@ #include "feature_tracker/feature_tracker.h" + +#include + #include "spdlog/spdlog.h" int FeatureTracker::n_id = 0; -bool inBorder(const cv::Point2f &pt) { +bool inBorder(const cv::Point2f &pt, uint col, uint row) { const int BORDER_SIZE = 1; int img_x = cvRound(pt.x); int img_y = cvRound(pt.y); - return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && - BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE; + return BORDER_SIZE <= img_x && img_x < col - BORDER_SIZE && + BORDER_SIZE <= img_y && img_y < row - BORDER_SIZE; } void reduceVector(vector &v, vector status) { @@ -25,20 +28,101 @@ void reduceVector(vector &v, vector status) { v.resize(j); } -FeatureTracker::FeatureTracker() {} +FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, + bool run_histogram_equilisation, + uint max_feature_count_per_image, + uint min_distance_between_features, + double fundemental_matrix_threshold, double fx, + double fy, double feature_pruning_frequency, + double max_time_difference) + : fisheye_(fisheye), + run_histogram_equilisation_(run_histogram_equilisation), + max_feature_count_per_image_(max_feature_count_per_image), + min_distance_between_features_(min_distance_between_features), + fundemental_matrix_ransac_threshold_(fundemental_matrix_threshold), + fx_(fx), + fy_(fy), + feature_pruning_frequency_(feature_pruning_frequency), + max_time_difference_(max_time_difference) { + readIntrinsicParameter(camera_config_file); + feature_pruning_period_ = 1.0 / feature_pruning_frequency; +} + +void FeatureTracker::RegisterEventObserver( + std::shared_ptr event_observer) { + event_observer_ = event_observer; + std::cout << "test" << std::endl; + event_observer_->OnRegistered(); + std::cout << "test2" << std::endl; +} + +void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, + double current_image_time_s) { + std::cout << "Got a frame" << std::endl; + if (is_first_frame_) { + std::cout << "First frame" << std::endl; + is_first_frame_ = false; + first_frame_time_ = current_image_time_s; + previous_frame_time_ = current_image_time_s; + prev_prune_time = current_image_time_s; + return; + } + + if (current_image_time_s > previous_frame_time_ + max_time_difference_) { + std::cout << "Time diff too large" << std::endl; + RestartTracker(); + if (event_observer_) { + event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, + previous_frame_time_); + } + return; + } + + if (current_image_time_s < previous_frame_time_) { + std::cout << "went back in time" << std::endl; + RestartTracker(); + if (event_observer_) { + event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, + previous_frame_time_); + } + return; + } + + std::cout << "reading image" << std::endl; + readImage(new_frame, current_image_time_s); + + for (unsigned int i = 0;; i++) { + bool completed = false; + // std::cout << "updating id" << std::endl; + completed |= updateID(i); + if (!completed) break; + } +} + +void FeatureTracker::RestartTracker() { + is_first_frame_ = true; + first_frame_time_ = 0; + previous_frame_time_ = 0; + + if (event_observer_) { + event_observer_->OnRestart(); + } + return; +} void FeatureTracker::setMask() { - if (FISHEYE) + if (fisheye_) mask = fisheye_mask.clone(); else - mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255)); + mask = cv::Mat(m_camera->imageHeight(), m_camera->imageWidth(), CV_8UC1, + cv::Scalar(255)); // prefer to keep features that are tracked for long time vector>> cnt_pts_id; - for (unsigned int i = 0; i < forw_pts.size(); i++) + for (unsigned int i = 0; i < curr_pts.size(); i++) cnt_pts_id.push_back( - make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i]))); + make_pair(track_cnt[i], make_pair(curr_pts[i], ids[i]))); sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, @@ -46,23 +130,23 @@ void FeatureTracker::setMask() { return a.first > b.first; }); - forw_pts.clear(); + curr_pts.clear(); ids.clear(); track_cnt.clear(); for (auto &it : cnt_pts_id) { if (mask.at(it.second.first) == 255) { - forw_pts.push_back(it.second.first); + curr_pts.push_back(it.second.first); ids.push_back(it.second.second); track_cnt.push_back(it.first); - cv::circle(mask, it.second.first, MIN_DIST, 0, -1); + cv::circle(mask, it.second.first, min_distance_between_features_, 0, -1); } } } void FeatureTracker::addPoints() { for (auto &p : n_pts) { - forw_pts.push_back(p); + curr_pts.push_back(p); ids.push_back(-1); track_cnt.push_back(1); } @@ -72,45 +156,54 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { cv::Mat img; TicToc t_r; cur_time = _cur_time; - - if (EQUALIZE) { + std::cout << "test1" << std::endl; + if (run_histogram_equilisation_) { + std::cout << "Doing histogram equailsation" << std::endl; cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); TicToc t_c; clahe->apply(_img, img); + std::cout << "Applied histogram equailsation" << std::endl; spdlog::debug("CLAHE costs: {}ms", t_c.toc()); } else img = _img; - if (forw_img.empty()) { - prev_img = cur_img = forw_img = img; + if (cur_img_.empty()) { + std::cout << "First iamge to setting data" << std::endl; + prev_img_ = cur_img_ = img; } else { - forw_img = img; + std::cout << "set curr img" << std::endl; + cur_img_ = img; } - forw_pts.clear(); + curr_pts.clear(); - if (cur_pts.size() > 0) { + if (prev_pts.size() > 0) { TicToc t_o; vector status; vector err; - cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, - cv::Size(21, 21), 3); + cv::calcOpticalFlowPyrLK(prev_img_, cur_img_, prev_pts, curr_pts, status, + err, cv::Size(21, 21), 3); - for (int i = 0; i < int(forw_pts.size()); i++) - if (status[i] && !inBorder(forw_pts[i])) status[i] = 0; + for (int i = 0; i < int(curr_pts.size()); i++) + if (status[i] && !inBorder(curr_pts[i], m_camera->imageWidth(), + m_camera->imageHeight())) + status[i] = 0; reduceVector(prev_pts, status); - reduceVector(cur_pts, status); - reduceVector(forw_pts, status); + reduceVector(curr_pts, status); reduceVector(ids, status); - reduceVector(cur_un_pts, status); reduceVector(track_cnt, status); spdlog::debug("temporal optical flow costs: {}ms", t_o.toc()); } for (auto &n : track_cnt) n++; - if (PUB_THIS_FRAME) { + // Prune and detect new points + bool is_prune_and_detect_new_points = + _cur_time > prev_prune_time + feature_pruning_period_; + if (is_prune_and_detect_new_points) { + std::cout << "Time to prune and find new" << std::endl; rejectWithF(); + spdlog::debug("set mask begins"); TicToc t_m; setMask(); @@ -118,13 +211,14 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { spdlog::debug("detect feature begins"); TicToc t_t; - int n_max_cnt = MAX_CNT - static_cast(forw_pts.size()); + int n_max_cnt = + max_feature_count_per_image_ - static_cast(curr_pts.size()); if (n_max_cnt > 0) { if (mask.empty()) cout << "mask is empty " << endl; if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; - if (mask.size() != forw_img.size()) cout << "wrong size " << endl; - cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, - MIN_DIST, mask); + if (mask.size() != cur_img_.size()) cout << "wrong size " << endl; + cv::goodFeaturesToTrack(cur_img_, n_pts, n_max_cnt, 0.01, + min_distance_between_features_, mask); } else n_pts.clear(); spdlog::debug("detect feature costs: {}ms", t_t.toc()); @@ -133,49 +227,54 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { TicToc t_a; addPoints(); spdlog::debug("selectFeature costs: {}ms", t_a.toc()); + prev_prune_time = _cur_time; + + // event_observer_->OnProcessedImage(_img,_cur_time,); } - prev_img = cur_img; - prev_pts = cur_pts; - prev_un_pts = cur_un_pts; - cur_img = forw_img; - cur_pts = forw_pts; + undistortedPoints(); + if (is_prune_and_detect_new_points) { + if (event_observer_) { + event_observer_->OnProcessedImage(_img, _cur_time, curr_pts, cur_un_pts, + ids, track_cnt, pts_velocity); + } + } + prev_img_ = cur_img_; + prev_pts = curr_pts; prev_time = cur_time; } void FeatureTracker::rejectWithF() { - if (forw_pts.size() >= 8) { + if (curr_pts.size() >= 8) { spdlog::debug("FM ransac begins"); TicToc t_f; - vector un_cur_pts(cur_pts.size()), - un_forw_pts(forw_pts.size()); - for (unsigned int i = 0; i < cur_pts.size(); i++) { + vector un_cur_pts(prev_pts.size()), + un_forw_pts(curr_pts.size()); + for (unsigned int i = 0; i < prev_pts.size(); i++) { Eigen::Vector3d tmp_p; - m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), + m_camera->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_pts[i].y), tmp_p); - tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; - tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; + tmp_p.x() = fx_ * tmp_p.x() / tmp_p.z() + m_camera->imageWidth() / 2.0; + tmp_p.y() = fy_ * tmp_p.y() / tmp_p.z() + m_camera->imageHeight() / 2.0; un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); - m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), + m_camera->liftProjective(Eigen::Vector2d(curr_pts[i].x, curr_pts[i].y), tmp_p); - tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; - tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0; + tmp_p.x() = fx_ * tmp_p.x() / tmp_p.z() + m_camera->imageWidth() / 2.0; + tmp_p.y() = fy_ * tmp_p.y() / tmp_p.z() + m_camera->imageHeight() / 2.0; un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); } vector status; - cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, - 0.99, status); - int size_a = cur_pts.size(); + cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, + fundemental_matrix_ransac_threshold_, 0.99, status); + int size_a = prev_pts.size(); reduceVector(prev_pts, status); - reduceVector(cur_pts, status); - reduceVector(forw_pts, status); - reduceVector(cur_un_pts, status); + reduceVector(curr_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); - spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, forw_pts.size(), - 1.0 * forw_pts.size() / size_a); + spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, curr_pts.size(), + 1.0 * curr_pts.size() / size_a); spdlog::debug("FM ransac costs: {}ms", t_f.toc()); } } @@ -194,10 +293,11 @@ void FeatureTracker::readIntrinsicParameter(const string &calib_file) { } void FeatureTracker::showUndistortion(const string &name) { - cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0)); + cv::Mat undistortedImg(m_camera->imageHeight() + 600, + m_camera->imageWidth() + 600, CV_8UC1, cv::Scalar(0)); vector distortedp, undistortedp; - for (int i = 0; i < COL; i++) - for (int j = 0; j < ROW; j++) { + for (int i = 0; i < m_camera->imageWidth(); i++) + for (int j = 0; j < m_camera->imageHeight(); j++) { Eigen::Vector2d a(i, j); Eigen::Vector3d b; m_camera->liftProjective(a, b); @@ -207,17 +307,20 @@ void FeatureTracker::showUndistortion(const string &name) { } for (int i = 0; i < int(undistortedp.size()); i++) { cv::Mat pp(3, 1, CV_32FC1); - pp.at(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2; - pp.at(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2; + pp.at(0, 0) = undistortedp[i].x() * fx_ + m_camera->imageWidth() / 2; + pp.at(1, 0) = + undistortedp[i].y() * fy_ + m_camera->imageHeight() / 2; pp.at(2, 0) = 1.0; // cout << trackerData[0].K << endl; // printf("%lf %lf\n", p.at(1, 0), p.at(0, 0)); // printf("%lf %lf\n", pp.at(1, 0), pp.at(0, 0)); - if (pp.at(1, 0) + 300 >= 0 && pp.at(1, 0) + 300 < ROW + 600 && - pp.at(0, 0) + 300 >= 0 && pp.at(0, 0) + 300 < COL + 600) { + if (pp.at(1, 0) + 300 >= 0 && + pp.at(1, 0) + 300 < m_camera->imageHeight() + 600 && + pp.at(0, 0) + 300 >= 0 && + pp.at(0, 0) + 300 < m_camera->imageWidth() + 600) { undistortedImg.at(pp.at(1, 0) + 300, pp.at(0, 0) + 300) = - cur_img.at(distortedp[i].y(), distortedp[i].x()); + prev_img_.at(distortedp[i].y(), distortedp[i].x()); } else { // spdlog::error("({0} {1}) -> ({2} {3})", distortedp[i].y, // distortedp[i].x, pp.at(1, 0), pp.at(0, 0)); @@ -228,11 +331,11 @@ void FeatureTracker::showUndistortion(const string &name) { } void FeatureTracker::undistortedPoints() { + map cur_un_pts_map; cur_un_pts.clear(); - cur_un_pts_map.clear(); - // cv::undistortPoints(cur_pts, un_pts, K, cv::Mat()); - for (unsigned int i = 0; i < cur_pts.size(); i++) { - Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y); + + for (unsigned int i = 0; i < curr_pts.size(); i++) { + Eigen::Vector2d a(curr_pts[i].x, curr_pts[i].y); Eigen::Vector3d b; m_camera->liftProjective(a, b); cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); @@ -259,7 +362,7 @@ void FeatureTracker::undistortedPoints() { } } } else { - for (unsigned int i = 0; i < cur_pts.size(); i++) { + for (unsigned int i = 0; i < prev_pts.size(); i++) { pts_velocity.push_back(cv::Point2f(0, 0)); } } diff --git a/feature_tracker/src/feature_tracker_observer.cpp b/feature_tracker/src/feature_tracker_observer.cpp new file mode 100644 index 000000000..36924c078 --- /dev/null +++ b/feature_tracker/src/feature_tracker_observer.cpp @@ -0,0 +1,15 @@ +#include "feature_tracker/feature_tracker_observer.hpp" + +#include +void FeatureTrackerObserver::CreateTrackedFeatureImage( + cv::Mat image, std::vector features, + std::vector track_cnt, uint max_track_count) { + cv::Mat show_img; + cv::cvtColor(image, show_img, cv::COLOR_GRAY2RGB); + + for (unsigned int i = 0; i < features.size(); i++) { + double len = std::min(1.0, 1.0 * track_cnt[i] / max_track_count); + cv::circle(show_img, features[i], 2, + cv::Scalar(255 * (1 - len), 0, 255 * len), 2); + } +} \ No newline at end of file diff --git a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp new file mode 100644 index 000000000..c38aa7f4c --- /dev/null +++ b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp @@ -0,0 +1,44 @@ +#include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" + +#include "spdlog/spdlog.h" +// FeatureTrackerObserverSPDRerun::FeatureTrackerObserverSPDRerun() { +// spdlog::info("FeatureTracker observer created"); +// } + + +FeatureTrackerObserverSPDRerun:: ~FeatureTrackerObserverSPDRerun(){ + spdlog::info( + "Destroying Observer"); + +} +void FeatureTrackerObserverSPDRerun::OnRegistered() { + + spdlog::set_level(spdlog::level::debug); + spdlog::info("FeatureTracker observer Registered"); + +} + +void FeatureTrackerObserverSPDRerun::OnRestart() { + spdlog::info( + "FeatureTracker Restarted. Likely due to discontinous image frames"); +} + +void FeatureTrackerObserverSPDRerun::OnDurationBetweenFrameTooLarge( + double current_image_time_s, double previous_image_time_s) { + spdlog::info("Large time diff between prev and curr image."); +} + +void FeatureTrackerObserverSPDRerun::OnImageTimeMovingBackwards( + double current_image_time_s, double previous_image_time_s) { + spdlog::info("Timestamp for current image is before previous image"); +} + +void FeatureTrackerObserverSPDRerun::OnProcessedImage( + cv::Mat new_frame, double current_image_time_s, + std::vector features, + std::vector undistorted_features, + + std::vector ids, std::vector track_count, + std::vector points_velocity) { + spdlog::info("Features have been pruned and new features added"); +} \ No newline at end of file diff --git a/feature_tracker/src/main.cpp b/feature_tracker/src/main.cpp new file mode 100644 index 000000000..ec74b4cee --- /dev/null +++ b/feature_tracker/src/main.cpp @@ -0,0 +1,105 @@ +#include +#include // For file existence check +#include +#include +#include +#include +#include + +#include "feature_tracker/feature_tracker.h" +#include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" +#include "spdlog/spdlog.h" +int main(int argc, char** argv) { + FeatureTracker feat( + "/home/rosdev/workspace/ros_ws/src/VINS-Mono/config/euroc/" + "euroc_config.yaml", + false, true, 150, 30, 1.0, 460, 460, 10,1.0); + std::shared_ptr observer = + std::make_shared(); + + // FeatureTrackerObserverSPDRerun observer = FeatureTrackerObserverSPDRerun(); + feat.RegisterEventObserver(observer); + std::cout << "HI" << std::endl; + + // Path to your dataset + std::string folderPath = "/home/rosdev/workspace/data/MH_01_easy"; + std::string imageFolder = folderPath + "/mav0/cam0/data"; + std::string timestampFile = folderPath + "/mav0/cam0/data.csv"; + + // Open the timestamp file + std::ifstream tsFile(timestampFile); + if (!tsFile.is_open()) { + std::cerr << "Error opening timestamp file: " << timestampFile << std::endl; + return 1; + } + + std::string line; + double time_covnersion = pow(10.0, 9); + // Read the CSV file line by line + while (std::getline(tsFile, line)) { + std::stringstream ss(line); + double timestamp; + std::string imageFilename; + + // Read the timestamp + ss >> timestamp; + + // Skip the comma after the timestamp + ss.ignore(1, ','); + + // Read the filename (after the comma) + std::getline(ss, imageFilename); + + // Trim leading/trailing spaces from filename + imageFilename.erase(0, imageFilename.find_first_not_of(" \t\r\n")); + imageFilename.erase(imageFilename.find_last_not_of(" \t\r\n") + 1); + + // Strip out any potential newline or carriage return characters + imageFilename.erase( + std::remove(imageFilename.begin(), imageFilename.end(), '\n'), + imageFilename.end()); + imageFilename.erase( + std::remove(imageFilename.begin(), imageFilename.end(), '\r'), + imageFilename.end()); + + // Construct the full image path + std::string imagePath = imageFolder + "/" + imageFilename; + + // Debug: Print the image path + + // Check if the file exists + if (!std::filesystem::exists(imagePath)) { + std::cerr << "File does not exist: " << imagePath << std::endl; + continue; + } + + // Read the image + cv::Mat image = + cv::imread(imagePath, cv::IMREAD_GRAYSCALE); // Read as color image + + // Check if image was loaded successfully + if (image.empty()) { + std::cerr << "Error loading image: " << imagePath << std::endl; + continue; + } + + // Display the image + + feat.ProcessNewFrame(image, timestamp / time_covnersion); + // cv::imshow("Image", image); + + // Wait for 100 milliseconds before moving to the next image + // if (cv::waitKey(1) == 'q') { // Press 'q' to quit + // break; + // } + + // Optionally, print the timestamp to the console (or use it for + // synchronization) + } + + // Close the file and cleanup + tsFile.close(); + cv::destroyAllWindows(); + + return 0; +} diff --git a/feature_tracker_ros/src/feature_tracker_node.cpp b/feature_tracker_ros/src/feature_tracker_node.cpp index 9863183a0..ef6a85661 100644 --- a/feature_tracker_ros/src/feature_tracker_node.cpp +++ b/feature_tracker_ros/src/feature_tracker_node.cpp @@ -18,14 +18,18 @@ queue img_buf; ros::Publisher pub_img, pub_match; ros::Publisher pub_restart; -FeatureTracker trackerData[NUM_OF_CAM]; +FeatureTracker trackerData(); double first_image_time; int pub_count = 1; bool first_image_flag = true; double last_image_time = 0; bool init_pub = 0; + + + void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { + trackerData.ProcessNewFrame() if (first_image_flag) { first_image_flag = false; first_image_time = img_msg->header.stamp.toSec(); @@ -116,7 +120,7 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { vector> hash_ids(NUM_OF_CAM); for (int i = 0; i < NUM_OF_CAM; i++) { auto &un_pts = trackerData[i].cur_un_pts; - auto &cur_pts = trackerData[i].cur_pts; + auto &prev_pts = trackerData[i].prev_pts; auto &ids = trackerData[i].ids; auto &pts_velocity = trackerData[i].pts_velocity; for (unsigned int j = 0; j < ids.size(); j++) { @@ -130,8 +134,8 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { feature_points->points.push_back(p); id_of_point.values.push_back(p_id * NUM_OF_CAM + i); - u_of_point.values.push_back(cur_pts[j].x); - v_of_point.values.push_back(cur_pts[j].y); + u_of_point.values.push_back(prev_pts[j].x); + v_of_point.values.push_back(prev_pts[j].y); velocity_x_of_point.values.push_back(pts_velocity[j].x); velocity_y_of_point.values.push_back(pts_velocity[j].y); } @@ -159,10 +163,10 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW); cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); - for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++) { + for (unsigned int j = 0; j < trackerData[i].prev_pts.size(); j++) { double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE); - cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, + cv::circle(tmp_img, trackerData[i].prev_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2); // draw speed line /* @@ -174,13 +178,13 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { tmp_prev_un_pts.z() = 1; Vector2d tmp_prev_uv; trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv); - cv::line(tmp_img, trackerData[i].cur_pts[j], + cv::line(tmp_img, trackerData[i].prev_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0); */ // char name[10]; // sprintf(name, "%d", trackerData[i].ids[j]); - // cv::putText(tmp_img, name, trackerData[i].cur_pts[j], + // cv::putText(tmp_img, name, trackerData[i].prev_pts[j], // cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); } } diff --git a/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp b/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp index 8413c285f..359365b59 100644 --- a/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp +++ b/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp @@ -9,22 +9,22 @@ #ifndef POSE_GRAPH_ROS_POSE_GRAPH_NODE_HPP #define POSE_GRAPH_ROS_POSE_GRAPH_NODE_HPP -#include -#include -#include -#include - #include #include -#include #include #include -#include #include +#include + +#include +#include +#include +#include +#include #include "camodocal/camera_models/CameraFactory.h" -#include "pose_graph/pose_graph.hpp" #include "pose_graph/details/pose_graph_event_observer.hpp" +#include "pose_graph/pose_graph.hpp" #include "pose_graph_ros/utility/CameraPoseVisualization.h" namespace pose_graph { From 3bd7995d73707df80fb4135af6eb2bc7855bd1b4 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sun, 23 Feb 2025 23:22:55 +0000 Subject: [PATCH 02/37] [34] Minor cleanup --- .../feature_tracker/feature_tracker_observer.hpp | 11 +++++------ feature_tracker/src/feature_tracker.cpp | 13 ------------- feature_tracker/src/feature_tracker_observer.cpp | 3 ++- .../src/feature_tracker_observer_spdlog_rerun.cpp | 14 +++++++------- 4 files changed, 14 insertions(+), 27 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp index 2f1615c0f..a4ac3b4d1 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp @@ -15,7 +15,7 @@ class FeatureTrackerObserver virtual void OnDurationBetweenFrameTooLarge(double current_image_time_s, double previous_image_time_s) = 0; virtual void OnImageTimeMovingBackwards(double current_image_time_s, - double previous_image_time_s)=0; + double previous_image_time_s) = 0; virtual void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, std::vector features, std::vector undistorted_features, @@ -24,10 +24,9 @@ class FeatureTrackerObserver std::vector track_count, std::vector points_velocity) = 0; - private: - void CreateTrackedFeatureImage(cv::Mat image, - std::vector features, - std::vector track_cnt, - uint max_track_count); + cv::Mat CreateTrackedFeatureImage(cv::Mat image, + std::vector features, + std::vector track_cnt, + uint max_track_count); }; #endif /* FEATURE_TRACKER_OBSERVER_HPP */ diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 9a03814c6..67d7eaefb 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -51,16 +51,12 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, void FeatureTracker::RegisterEventObserver( std::shared_ptr event_observer) { event_observer_ = event_observer; - std::cout << "test" << std::endl; event_observer_->OnRegistered(); - std::cout << "test2" << std::endl; } void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, double current_image_time_s) { - std::cout << "Got a frame" << std::endl; if (is_first_frame_) { - std::cout << "First frame" << std::endl; is_first_frame_ = false; first_frame_time_ = current_image_time_s; previous_frame_time_ = current_image_time_s; @@ -69,7 +65,6 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, } if (current_image_time_s > previous_frame_time_ + max_time_difference_) { - std::cout << "Time diff too large" << std::endl; RestartTracker(); if (event_observer_) { event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, @@ -79,7 +74,6 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, } if (current_image_time_s < previous_frame_time_) { - std::cout << "went back in time" << std::endl; RestartTracker(); if (event_observer_) { event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, @@ -88,12 +82,10 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, return; } - std::cout << "reading image" << std::endl; readImage(new_frame, current_image_time_s); for (unsigned int i = 0;; i++) { bool completed = false; - // std::cout << "updating id" << std::endl; completed |= updateID(i); if (!completed) break; } @@ -156,22 +148,17 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { cv::Mat img; TicToc t_r; cur_time = _cur_time; - std::cout << "test1" << std::endl; if (run_histogram_equilisation_) { - std::cout << "Doing histogram equailsation" << std::endl; cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); TicToc t_c; clahe->apply(_img, img); - std::cout << "Applied histogram equailsation" << std::endl; spdlog::debug("CLAHE costs: {}ms", t_c.toc()); } else img = _img; if (cur_img_.empty()) { - std::cout << "First iamge to setting data" << std::endl; prev_img_ = cur_img_ = img; } else { - std::cout << "set curr img" << std::endl; cur_img_ = img; } diff --git a/feature_tracker/src/feature_tracker_observer.cpp b/feature_tracker/src/feature_tracker_observer.cpp index 36924c078..533fdc770 100644 --- a/feature_tracker/src/feature_tracker_observer.cpp +++ b/feature_tracker/src/feature_tracker_observer.cpp @@ -1,7 +1,7 @@ #include "feature_tracker/feature_tracker_observer.hpp" #include -void FeatureTrackerObserver::CreateTrackedFeatureImage( +cv::Mat FeatureTrackerObserver::CreateTrackedFeatureImage( cv::Mat image, std::vector features, std::vector track_cnt, uint max_track_count) { cv::Mat show_img; @@ -12,4 +12,5 @@ void FeatureTrackerObserver::CreateTrackedFeatureImage( cv::circle(show_img, features[i], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2); } + return show_img; } \ No newline at end of file diff --git a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp index c38aa7f4c..d60377358 100644 --- a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp +++ b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp @@ -5,17 +5,12 @@ // spdlog::info("FeatureTracker observer created"); // } - -FeatureTrackerObserverSPDRerun:: ~FeatureTrackerObserverSPDRerun(){ - spdlog::info( - "Destroying Observer"); - +FeatureTrackerObserverSPDRerun::~FeatureTrackerObserverSPDRerun() { + spdlog::info("Destroying Observer"); } void FeatureTrackerObserverSPDRerun::OnRegistered() { - spdlog::set_level(spdlog::level::debug); spdlog::info("FeatureTracker observer Registered"); - } void FeatureTrackerObserverSPDRerun::OnRestart() { @@ -41,4 +36,9 @@ void FeatureTrackerObserverSPDRerun::OnProcessedImage( std::vector ids, std::vector track_count, std::vector points_velocity) { spdlog::info("Features have been pruned and new features added"); + cv::Mat img = CreateTrackedFeatureImage(new_frame, features, track_count, 20); + cv::imshow("Image", img); + + // Wait for 100 milliseconds before moving to the next image + cv::waitKey(1); } \ No newline at end of file From 9b3990b8e1d9d52fba96174d39d0f3dcff750f89 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sun, 23 Feb 2025 23:32:58 +0000 Subject: [PATCH 03/37] [34] Remove cur_img variable --- .../include/feature_tracker/feature_tracker.h | 1 - feature_tracker/src/feature_tracker.cpp | 17 +++++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index ddbd4977f..107df8c8d 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -59,7 +59,6 @@ class FeatureTracker { cv::Mat mask; cv::Mat fisheye_mask; cv::Mat prev_img_; - cv::Mat cur_img_; vector n_pts; vector prev_pts, curr_pts; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 67d7eaefb..11bb6db7e 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -156,20 +156,17 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { } else img = _img; - if (cur_img_.empty()) { - prev_img_ = cur_img_ = img; - } else { - cur_img_ = img; + if (prev_img_.empty()) { + prev_img_ = img; } - curr_pts.clear(); if (prev_pts.size() > 0) { TicToc t_o; vector status; vector err; - cv::calcOpticalFlowPyrLK(prev_img_, cur_img_, prev_pts, curr_pts, status, - err, cv::Size(21, 21), 3); + cv::calcOpticalFlowPyrLK(prev_img_, img, prev_pts, curr_pts, status, err, + cv::Size(21, 21), 3); for (int i = 0; i < int(curr_pts.size()); i++) if (status[i] && !inBorder(curr_pts[i], m_camera->imageWidth(), @@ -203,8 +200,8 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { if (n_max_cnt > 0) { if (mask.empty()) cout << "mask is empty " << endl; if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; - if (mask.size() != cur_img_.size()) cout << "wrong size " << endl; - cv::goodFeaturesToTrack(cur_img_, n_pts, n_max_cnt, 0.01, + if (mask.size() != img.size()) cout << "wrong size " << endl; + cv::goodFeaturesToTrack(img, n_pts, n_max_cnt, 0.01, min_distance_between_features_, mask); } else n_pts.clear(); @@ -226,7 +223,7 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { ids, track_cnt, pts_velocity); } } - prev_img_ = cur_img_; + prev_img_ = img; prev_pts = curr_pts; prev_time = cur_time; } From 5b79588f2f61fce038eeed1a12a486a5271e9191 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sun, 23 Feb 2025 23:41:32 +0000 Subject: [PATCH 04/37] [34] Remove cur_time private varaible --- .../include/feature_tracker/feature_tracker.h | 3 +-- feature_tracker/src/feature_tracker.cpp | 10 ++++------ 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 107df8c8d..7ae413995 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -52,7 +52,7 @@ class FeatureTracker { void rejectWithF(); - void undistortedPoints(); + void undistortedPoints(double dt); void RestartTracker(); @@ -71,7 +71,6 @@ class FeatureTracker { double fx_; double fy_; camodocal::CameraPtr m_camera; - double cur_time; double prev_time; double prev_prune_time; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 11bb6db7e..3f4c4984f 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -147,7 +147,6 @@ void FeatureTracker::addPoints() { void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { cv::Mat img; TicToc t_r; - cur_time = _cur_time; if (run_histogram_equilisation_) { cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); TicToc t_c; @@ -213,10 +212,9 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { spdlog::debug("selectFeature costs: {}ms", t_a.toc()); prev_prune_time = _cur_time; - // event_observer_->OnProcessedImage(_img,_cur_time,); } - undistortedPoints(); + undistortedPoints(_cur_time - prev_time); if (is_prune_and_detect_new_points) { if (event_observer_) { event_observer_->OnProcessedImage(_img, _cur_time, curr_pts, cur_un_pts, @@ -225,7 +223,7 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { } prev_img_ = img; prev_pts = curr_pts; - prev_time = cur_time; + prev_time = _cur_time; } void FeatureTracker::rejectWithF() { @@ -314,7 +312,7 @@ void FeatureTracker::showUndistortion(const string &name) { cv::waitKey(0); } -void FeatureTracker::undistortedPoints() { +void FeatureTracker::undistortedPoints(double dt) { map cur_un_pts_map; cur_un_pts.clear(); @@ -329,7 +327,7 @@ void FeatureTracker::undistortedPoints() { } // caculate points velocity if (!prev_un_pts_map.empty()) { - double dt = cur_time - prev_time; + pts_velocity.clear(); for (unsigned int i = 0; i < cur_un_pts.size(); i++) { if (ids[i] != -1) { From 806a43830abb6fd81aefa6e82882a4064112f16b Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 24 Feb 2025 00:04:31 +0000 Subject: [PATCH 05/37] [34] Remove curr_pts private variable --- .../include/feature_tracker/feature_tracker.h | 17 ++--- feature_tracker/src/feature_tracker.cpp | 64 ++++++++++--------- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 7ae413995..4bd352b03 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -32,17 +32,18 @@ class FeatureTracker { uint max_feature_count_per_image, uint min_distance_between_features, double fundemental_matrix_ransac_threshold, double fx, - double fy, double feature_pruning_frequency,double max_time_difference); + double fy, double feature_pruning_frequency, + double max_time_difference); void RegisterEventObserver( std::shared_ptr event_observer); void ProcessNewFrame(cv::Mat new_frame, double time_s); private: - void readImage(const cv::Mat &_img, double _cur_time); + void readImage(const cv::Mat &_img, double current_time); - void setMask(); + void setMask(vector &curr_pts); - void addPoints(); + void addPoints(vector &curr_pts); bool updateID(unsigned int i); @@ -50,9 +51,9 @@ class FeatureTracker { void showUndistortion(const string &name); - void rejectWithF(); + void rejectWithF(vector &curr_pts); - void undistortedPoints(double dt); + void undistortedPoints(double dt, const vector &curr_pts); void RestartTracker(); @@ -60,12 +61,12 @@ class FeatureTracker { cv::Mat fisheye_mask; cv::Mat prev_img_; vector n_pts; - vector prev_pts, curr_pts; + vector prev_pts; vector pts_velocity; vector ids; vector track_cnt; - vector cur_un_pts; + vector cur_un_pts; map prev_un_pts_map; double fx_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 3f4c4984f..c6e97e736 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -102,7 +102,7 @@ void FeatureTracker::RestartTracker() { return; } -void FeatureTracker::setMask() { +void FeatureTracker::setMask(vector &curr_pts) { if (fisheye_) mask = fisheye_mask.clone(); else @@ -136,7 +136,7 @@ void FeatureTracker::setMask() { } } -void FeatureTracker::addPoints() { +void FeatureTracker::addPoints(vector &curr_pts) { for (auto &p : n_pts) { curr_pts.push_back(p); ids.push_back(-1); @@ -144,35 +144,36 @@ void FeatureTracker::addPoints() { } } -void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { - cv::Mat img; +void FeatureTracker::readImage(const cv::Mat &img, double current_time) { + cv::Mat pre_processed_img; + vector current_points; + TicToc t_r; if (run_histogram_equilisation_) { cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); TicToc t_c; - clahe->apply(_img, img); + clahe->apply(img, pre_processed_img); spdlog::debug("CLAHE costs: {}ms", t_c.toc()); } else - img = _img; + pre_processed_img = img; if (prev_img_.empty()) { - prev_img_ = img; + prev_img_ = pre_processed_img; } - curr_pts.clear(); if (prev_pts.size() > 0) { TicToc t_o; vector status; vector err; - cv::calcOpticalFlowPyrLK(prev_img_, img, prev_pts, curr_pts, status, err, - cv::Size(21, 21), 3); + cv::calcOpticalFlowPyrLK(prev_img_, pre_processed_img, prev_pts, + current_points, status, err, cv::Size(21, 21), 3); - for (int i = 0; i < int(curr_pts.size()); i++) - if (status[i] && !inBorder(curr_pts[i], m_camera->imageWidth(), + for (int i = 0; i < int(current_points.size()); i++) + if (status[i] && !inBorder(current_points[i], m_camera->imageWidth(), m_camera->imageHeight())) status[i] = 0; reduceVector(prev_pts, status); - reduceVector(curr_pts, status); + reduceVector(current_points, status); reduceVector(ids, status); reduceVector(track_cnt, status); spdlog::debug("temporal optical flow costs: {}ms", t_o.toc()); @@ -182,25 +183,26 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { // Prune and detect new points bool is_prune_and_detect_new_points = - _cur_time > prev_prune_time + feature_pruning_period_; + current_time > prev_prune_time + feature_pruning_period_; if (is_prune_and_detect_new_points) { std::cout << "Time to prune and find new" << std::endl; - rejectWithF(); + rejectWithF(current_points); spdlog::debug("set mask begins"); TicToc t_m; - setMask(); + setMask(current_points); spdlog::debug("set mask costs {}ms", t_m.toc()); spdlog::debug("detect feature begins"); TicToc t_t; int n_max_cnt = - max_feature_count_per_image_ - static_cast(curr_pts.size()); + max_feature_count_per_image_ - static_cast(current_points.size()); if (n_max_cnt > 0) { if (mask.empty()) cout << "mask is empty " << endl; if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; - if (mask.size() != img.size()) cout << "wrong size " << endl; - cv::goodFeaturesToTrack(img, n_pts, n_max_cnt, 0.01, + if (mask.size() != pre_processed_img.size()) + cout << "wrong size " << endl; + cv::goodFeaturesToTrack(pre_processed_img, n_pts, n_max_cnt, 0.01, min_distance_between_features_, mask); } else n_pts.clear(); @@ -208,25 +210,25 @@ void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { spdlog::debug("add feature begins"); TicToc t_a; - addPoints(); + addPoints(current_points); spdlog::debug("selectFeature costs: {}ms", t_a.toc()); - prev_prune_time = _cur_time; - + prev_prune_time = current_time; } - undistortedPoints(_cur_time - prev_time); + undistortedPoints(current_time - prev_time, current_points); if (is_prune_and_detect_new_points) { if (event_observer_) { - event_observer_->OnProcessedImage(_img, _cur_time, curr_pts, cur_un_pts, - ids, track_cnt, pts_velocity); + event_observer_->OnProcessedImage(img, current_time, current_points, + cur_un_pts, ids, track_cnt, + pts_velocity); } } - prev_img_ = img; - prev_pts = curr_pts; - prev_time = _cur_time; + prev_img_ = pre_processed_img; + prev_pts = current_points; + prev_time = current_time; } -void FeatureTracker::rejectWithF() { +void FeatureTracker::rejectWithF(vector &curr_pts) { if (curr_pts.size() >= 8) { spdlog::debug("FM ransac begins"); TicToc t_f; @@ -312,7 +314,8 @@ void FeatureTracker::showUndistortion(const string &name) { cv::waitKey(0); } -void FeatureTracker::undistortedPoints(double dt) { +void FeatureTracker::undistortedPoints(double dt, + const vector &curr_pts) { map cur_un_pts_map; cur_un_pts.clear(); @@ -327,7 +330,6 @@ void FeatureTracker::undistortedPoints(double dt) { } // caculate points velocity if (!prev_un_pts_map.empty()) { - pts_velocity.clear(); for (unsigned int i = 0; i < cur_un_pts.size(); i++) { if (ids[i] != -1) { From 7d6231b6a887be4716f7fecb1fb64cdbd7a278be Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 24 Feb 2025 00:08:49 +0000 Subject: [PATCH 06/37] [34] Remove n-pts private variable --- .../include/feature_tracker/feature_tracker.h | 4 ++-- feature_tracker/src/feature_tracker.cpp | 17 ++++++++++------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 4bd352b03..568100616 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -43,7 +43,8 @@ class FeatureTracker { void setMask(vector &curr_pts); - void addPoints(vector &curr_pts); + void addPoints(vector &curr_pts, + const vector &newly_generated_points); bool updateID(unsigned int i); @@ -60,7 +61,6 @@ class FeatureTracker { cv::Mat mask; cv::Mat fisheye_mask; cv::Mat prev_img_; - vector n_pts; vector prev_pts; vector pts_velocity; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index c6e97e736..9985ebcc9 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -136,8 +136,10 @@ void FeatureTracker::setMask(vector &curr_pts) { } } -void FeatureTracker::addPoints(vector &curr_pts) { - for (auto &p : n_pts) { +void FeatureTracker::addPoints( + vector &curr_pts, + const vector &newly_generated_points) { + for (auto &p : newly_generated_points) { curr_pts.push_back(p); ids.push_back(-1); track_cnt.push_back(1); @@ -147,6 +149,7 @@ void FeatureTracker::addPoints(vector &curr_pts) { void FeatureTracker::readImage(const cv::Mat &img, double current_time) { cv::Mat pre_processed_img; vector current_points; + vector newly_generated_points; TicToc t_r; if (run_histogram_equilisation_) { @@ -202,15 +205,15 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; if (mask.size() != pre_processed_img.size()) cout << "wrong size " << endl; - cv::goodFeaturesToTrack(pre_processed_img, n_pts, n_max_cnt, 0.01, - min_distance_between_features_, mask); - } else - n_pts.clear(); + cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, + n_max_cnt, 0.01, min_distance_between_features_, + mask); + } spdlog::debug("detect feature costs: {}ms", t_t.toc()); spdlog::debug("add feature begins"); TicToc t_a; - addPoints(current_points); + addPoints(current_points, newly_generated_points); spdlog::debug("selectFeature costs: {}ms", t_a.toc()); prev_prune_time = current_time; } From 700c13d6de1e3cd2edcff4e98e50913c9d0bb369 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 24 Feb 2025 00:15:56 +0000 Subject: [PATCH 07/37] [34] Remove pts-Velocity private variable --- .../include/feature_tracker/feature_tracker.h | 4 ++-- feature_tracker/src/feature_tracker.cpp | 17 +++++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 568100616..675572e21 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -54,7 +54,8 @@ class FeatureTracker { void rejectWithF(vector &curr_pts); - void undistortedPoints(double dt, const vector &curr_pts); + void undistortedPoints(double dt, const vector &curr_pts, + vector &pts_velocity_out); void RestartTracker(); @@ -63,7 +64,6 @@ class FeatureTracker { cv::Mat prev_img_; vector prev_pts; - vector pts_velocity; vector ids; vector track_cnt; vector cur_un_pts; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 9985ebcc9..711417c8c 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -217,8 +217,8 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { spdlog::debug("selectFeature costs: {}ms", t_a.toc()); prev_prune_time = current_time; } - - undistortedPoints(current_time - prev_time, current_points); + vector pts_velocity; + undistortedPoints(current_time - prev_time, current_points, pts_velocity); if (is_prune_and_detect_new_points) { if (event_observer_) { event_observer_->OnProcessedImage(img, current_time, current_points, @@ -318,7 +318,8 @@ void FeatureTracker::showUndistortion(const string &name) { } void FeatureTracker::undistortedPoints(double dt, - const vector &curr_pts) { + const vector &curr_pts, + vector &pts_velocity_out) { map cur_un_pts_map; cur_un_pts.clear(); @@ -333,7 +334,7 @@ void FeatureTracker::undistortedPoints(double dt, } // caculate points velocity if (!prev_un_pts_map.empty()) { - pts_velocity.clear(); + pts_velocity_out.clear(); for (unsigned int i = 0; i < cur_un_pts.size(); i++) { if (ids[i] != -1) { std::map::iterator it; @@ -341,16 +342,16 @@ void FeatureTracker::undistortedPoints(double dt, if (it != prev_un_pts_map.end()) { double v_x = (cur_un_pts[i].x - it->second.x) / dt; double v_y = (cur_un_pts[i].y - it->second.y) / dt; - pts_velocity.push_back(cv::Point2f(v_x, v_y)); + pts_velocity_out.push_back(cv::Point2f(v_x, v_y)); } else - pts_velocity.push_back(cv::Point2f(0, 0)); + pts_velocity_out.push_back(cv::Point2f(0, 0)); } else { - pts_velocity.push_back(cv::Point2f(0, 0)); + pts_velocity_out.push_back(cv::Point2f(0, 0)); } } } else { for (unsigned int i = 0; i < prev_pts.size(); i++) { - pts_velocity.push_back(cv::Point2f(0, 0)); + pts_velocity_out.push_back(cv::Point2f(0, 0)); } } prev_un_pts_map = cur_un_pts_map; From afa854c9030e9d4200599b384da76cdece5cb1e3 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 24 Feb 2025 12:25:37 +0000 Subject: [PATCH 08/37] [34] Remove all the tic toc --- .../include/feature_tracker/feature_tracker.h | 2 ++ feature_tracker/src/feature_tracker.cpp | 24 ++++++++----------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 675572e21..4b8635a6e 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -77,6 +77,8 @@ class FeatureTracker { static int n_id; + cv::Ptr clahe; + bool fisheye_; bool run_histogram_equilisation_; uint max_feature_count_per_image_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 711417c8c..b90785c52 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -46,6 +46,9 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, max_time_difference_(max_time_difference) { readIntrinsicParameter(camera_config_file); feature_pruning_period_ = 1.0 / feature_pruning_frequency; + if (run_histogram_equilisation) { + clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + } } void FeatureTracker::RegisterEventObserver( @@ -151,12 +154,8 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { vector current_points; vector newly_generated_points; - TicToc t_r; if (run_histogram_equilisation_) { - cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); - TicToc t_c; clahe->apply(img, pre_processed_img); - spdlog::debug("CLAHE costs: {}ms", t_c.toc()); } else pre_processed_img = img; @@ -165,7 +164,6 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { } if (prev_pts.size() > 0) { - TicToc t_o; vector status; vector err; cv::calcOpticalFlowPyrLK(prev_img_, pre_processed_img, prev_pts, @@ -179,7 +177,7 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { reduceVector(current_points, status); reduceVector(ids, status); reduceVector(track_cnt, status); - spdlog::debug("temporal optical flow costs: {}ms", t_o.toc()); + } for (auto &n : track_cnt) n++; @@ -187,17 +185,16 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { // Prune and detect new points bool is_prune_and_detect_new_points = current_time > prev_prune_time + feature_pruning_period_; + if (is_prune_and_detect_new_points) { std::cout << "Time to prune and find new" << std::endl; rejectWithF(current_points); spdlog::debug("set mask begins"); - TicToc t_m; + setMask(current_points); - spdlog::debug("set mask costs {}ms", t_m.toc()); spdlog::debug("detect feature begins"); - TicToc t_t; int n_max_cnt = max_feature_count_per_image_ - static_cast(current_points.size()); if (n_max_cnt > 0) { @@ -209,16 +206,17 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { n_max_cnt, 0.01, min_distance_between_features_, mask); } - spdlog::debug("detect feature costs: {}ms", t_t.toc()); spdlog::debug("add feature begins"); - TicToc t_a; + addPoints(current_points, newly_generated_points); - spdlog::debug("selectFeature costs: {}ms", t_a.toc()); + prev_prune_time = current_time; } + vector pts_velocity; undistortedPoints(current_time - prev_time, current_points, pts_velocity); + if (is_prune_and_detect_new_points) { if (event_observer_) { event_observer_->OnProcessedImage(img, current_time, current_points, @@ -234,7 +232,6 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { void FeatureTracker::rejectWithF(vector &curr_pts) { if (curr_pts.size() >= 8) { spdlog::debug("FM ransac begins"); - TicToc t_f; vector un_cur_pts(prev_pts.size()), un_forw_pts(curr_pts.size()); for (unsigned int i = 0; i < prev_pts.size(); i++) { @@ -262,7 +259,6 @@ void FeatureTracker::rejectWithF(vector &curr_pts) { reduceVector(track_cnt, status); spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, curr_pts.size(), 1.0 * curr_pts.size() / size_a); - spdlog::debug("FM ransac costs: {}ms", t_f.toc()); } } From 59fd7bc51833e3b43cc6f1751c4037cda7f00136 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 24 Feb 2025 12:31:21 +0000 Subject: [PATCH 09/37] [34] Remove cur_un_pts private variable --- .../include/feature_tracker/feature_tracker.h | 3 ++- feature_tracker/src/feature_tracker.cpp | 22 ++++++++++--------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 4b8635a6e..fec8dda44 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -55,6 +55,7 @@ class FeatureTracker { void rejectWithF(vector &curr_pts); void undistortedPoints(double dt, const vector &curr_pts, + vector &cur_un_pts_out, vector &pts_velocity_out); void RestartTracker(); @@ -66,7 +67,7 @@ class FeatureTracker { vector ids; vector track_cnt; - vector cur_un_pts; + map prev_un_pts_map; double fx_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index b90785c52..f9120aa84 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -177,7 +177,6 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { reduceVector(current_points, status); reduceVector(ids, status); reduceVector(track_cnt, status); - } for (auto &n : track_cnt) n++; @@ -185,7 +184,7 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { // Prune and detect new points bool is_prune_and_detect_new_points = current_time > prev_prune_time + feature_pruning_period_; - + if (is_prune_and_detect_new_points) { std::cout << "Time to prune and find new" << std::endl; rejectWithF(current_points); @@ -213,10 +212,12 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { prev_prune_time = current_time; } - + vector pts_velocity; - undistortedPoints(current_time - prev_time, current_points, pts_velocity); - + vector cur_un_pts; + undistortedPoints(current_time - prev_time, current_points, cur_un_pts, + pts_velocity); + if (is_prune_and_detect_new_points) { if (event_observer_) { event_observer_->OnProcessedImage(img, current_time, current_points, @@ -315,15 +316,16 @@ void FeatureTracker::showUndistortion(const string &name) { void FeatureTracker::undistortedPoints(double dt, const vector &curr_pts, + vector &cur_un_pts_out, vector &pts_velocity_out) { map cur_un_pts_map; - cur_un_pts.clear(); + cur_un_pts_out.clear(); for (unsigned int i = 0; i < curr_pts.size(); i++) { Eigen::Vector2d a(curr_pts[i].x, curr_pts[i].y); Eigen::Vector3d b; m_camera->liftProjective(a, b); - cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); + cur_un_pts_out.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); cur_un_pts_map.insert( make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z()))); // printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); @@ -331,13 +333,13 @@ void FeatureTracker::undistortedPoints(double dt, // caculate points velocity if (!prev_un_pts_map.empty()) { pts_velocity_out.clear(); - for (unsigned int i = 0; i < cur_un_pts.size(); i++) { + for (unsigned int i = 0; i < cur_un_pts_out.size(); i++) { if (ids[i] != -1) { std::map::iterator it; it = prev_un_pts_map.find(ids[i]); if (it != prev_un_pts_map.end()) { - double v_x = (cur_un_pts[i].x - it->second.x) / dt; - double v_y = (cur_un_pts[i].y - it->second.y) / dt; + double v_x = (cur_un_pts_out[i].x - it->second.x) / dt; + double v_y = (cur_un_pts_out[i].y - it->second.y) / dt; pts_velocity_out.push_back(cv::Point2f(v_x, v_y)); } else pts_velocity_out.push_back(cv::Point2f(0, 0)); From b829ff64006707d6a206b11d95b2b35b329e7764 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 24 Feb 2025 13:43:50 +0000 Subject: [PATCH 10/37] [34] Remove repeated lift projection --- .../include/feature_tracker/feature_tracker.h | 10 +- feature_tracker/src/feature_tracker.cpp | 154 +++++++++--------- 2 files changed, 86 insertions(+), 78 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index fec8dda44..5486ea476 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -52,17 +52,19 @@ class FeatureTracker { void showUndistortion(const string &name); - void rejectWithF(vector &curr_pts); + vector rejectWithF(const vector &cur_un_pts, + const vector &prev_un_pts); - void undistortedPoints(double dt, const vector &curr_pts, - vector &cur_un_pts_out, - vector &pts_velocity_out); + void GetPointVelocty(double dt, const map &cur_un_pts_map, + const map &prev_un_pts_map, + vector &pts_velocity_out); void RestartTracker(); cv::Mat mask; cv::Mat fisheye_mask; cv::Mat prev_img_; + vector prev_un_pts; vector prev_pts; vector ids; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index f9120aa84..c96ff518c 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -174,6 +174,7 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { m_camera->imageHeight())) status[i] = 0; reduceVector(prev_pts, status); + reduceVector(prev_un_pts, status); reduceVector(current_points, status); reduceVector(ids, status); reduceVector(track_cnt, status); @@ -185,9 +186,29 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { bool is_prune_and_detect_new_points = current_time > prev_prune_time + feature_pruning_period_; + vector cur_un_pts; + map cur_un_pts_map; + + for (unsigned int i = 0; i < current_points.size(); i++) { + Eigen::Vector2d a(current_points[i].x, current_points[i].y); + Eigen::Vector3d b; + m_camera->liftProjective(a, b); + cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); + cur_un_pts_map.insert( + make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z()))); + // printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); + } + if (is_prune_and_detect_new_points) { std::cout << "Time to prune and find new" << std::endl; - rejectWithF(current_points); + vector status = rejectWithF(cur_un_pts, prev_un_pts); + int size_a = prev_un_pts.size(); + reduceVector(prev_pts, status); + reduceVector(current_points, status); + reduceVector(ids, status); + reduceVector(track_cnt, status); + spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, current_points.size(), + 1.0 * current_points.size() / size_a); spdlog::debug("set mask begins"); @@ -209,58 +230,54 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { spdlog::debug("add feature begins"); addPoints(current_points, newly_generated_points); - - prev_prune_time = current_time; - } - - vector pts_velocity; - vector cur_un_pts; - undistortedPoints(current_time - prev_time, current_points, cur_un_pts, + vector pts_velocity; + GetPointVelocty(current_time - prev_time, cur_un_pts_map, prev_un_pts_map, pts_velocity); - - if (is_prune_and_detect_new_points) { if (event_observer_) { event_observer_->OnProcessedImage(img, current_time, current_points, cur_un_pts, ids, track_cnt, pts_velocity); } + prev_prune_time = current_time; + } + + if (is_prune_and_detect_new_points) { } + prev_un_pts = cur_un_pts; + prev_un_pts_map = cur_un_pts_map; prev_img_ = pre_processed_img; prev_pts = current_points; prev_time = current_time; } +vector FeatureTracker::rejectWithF( + const vector &cur_un_pts, + const vector &prev_un_pts) { + vector status; + if (cur_un_pts.size() < 8) { + return status; + } -void FeatureTracker::rejectWithF(vector &curr_pts) { - if (curr_pts.size() >= 8) { - spdlog::debug("FM ransac begins"); - vector un_cur_pts(prev_pts.size()), - un_forw_pts(curr_pts.size()); - for (unsigned int i = 0; i < prev_pts.size(); i++) { - Eigen::Vector3d tmp_p; - m_camera->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_pts[i].y), - tmp_p); - tmp_p.x() = fx_ * tmp_p.x() / tmp_p.z() + m_camera->imageWidth() / 2.0; - tmp_p.y() = fy_ * tmp_p.y() / tmp_p.z() + m_camera->imageHeight() / 2.0; - un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); - - m_camera->liftProjective(Eigen::Vector2d(curr_pts[i].x, curr_pts[i].y), - tmp_p); - tmp_p.x() = fx_ * tmp_p.x() / tmp_p.z() + m_camera->imageWidth() / 2.0; - tmp_p.y() = fy_ * tmp_p.y() / tmp_p.z() + m_camera->imageHeight() / 2.0; - un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); - } + spdlog::debug("FM ransac begins"); + vector curr_scaled_undistorted_pts(cur_un_pts.size()), + prev_scaled_undistorted_pts(prev_un_pts.size()); - vector status; - cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, - fundemental_matrix_ransac_threshold_, 0.99, status); - int size_a = prev_pts.size(); - reduceVector(prev_pts, status); - reduceVector(curr_pts, status); - reduceVector(ids, status); - reduceVector(track_cnt, status); - spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, curr_pts.size(), - 1.0 * curr_pts.size() / size_a); + std::cout << cur_un_pts.size() << " , " << prev_un_pts.size() << std::endl; + + for (unsigned int i = 0; i < prev_un_pts.size(); i++) { + curr_scaled_undistorted_pts[i] = + cv::Point2f(fx_ * cur_un_pts[i].x + m_camera->imageWidth() / 2.0, + fy_ * cur_un_pts[i].y + m_camera->imageHeight() / 2.0); + + prev_scaled_undistorted_pts[i] = + cv::Point2f(fx_ * prev_un_pts[i].x + m_camera->imageWidth() / 2.0, + fy_ * prev_un_pts[i].y + m_camera->imageHeight() / 2.0); } + + cv::findFundamentalMat(prev_scaled_undistorted_pts, + curr_scaled_undistorted_pts, cv::FM_RANSAC, + fundemental_matrix_ransac_threshold_, 0.99, status); + + return status; } bool FeatureTracker::updateID(unsigned int i) { @@ -314,43 +331,32 @@ void FeatureTracker::showUndistortion(const string &name) { cv::waitKey(0); } -void FeatureTracker::undistortedPoints(double dt, - const vector &curr_pts, - vector &cur_un_pts_out, - vector &pts_velocity_out) { - map cur_un_pts_map; - cur_un_pts_out.clear(); - - for (unsigned int i = 0; i < curr_pts.size(); i++) { - Eigen::Vector2d a(curr_pts[i].x, curr_pts[i].y); - Eigen::Vector3d b; - m_camera->liftProjective(a, b); - cur_un_pts_out.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); - cur_un_pts_map.insert( - make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z()))); - // printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); - } - // caculate points velocity - if (!prev_un_pts_map.empty()) { - pts_velocity_out.clear(); - for (unsigned int i = 0; i < cur_un_pts_out.size(); i++) { - if (ids[i] != -1) { - std::map::iterator it; - it = prev_un_pts_map.find(ids[i]); - if (it != prev_un_pts_map.end()) { - double v_x = (cur_un_pts_out[i].x - it->second.x) / dt; - double v_y = (cur_un_pts_out[i].y - it->second.y) / dt; - pts_velocity_out.push_back(cv::Point2f(v_x, v_y)); - } else - pts_velocity_out.push_back(cv::Point2f(0, 0)); - } else { - pts_velocity_out.push_back(cv::Point2f(0, 0)); - } - } - } else { +void FeatureTracker::GetPointVelocty( + double dt, const map &cur_un_pts_map, + const map &prev_un_pts_map, + vector &pts_velocity_out) { + if (prev_un_pts_map.empty()) { for (unsigned int i = 0; i < prev_pts.size(); i++) { pts_velocity_out.push_back(cv::Point2f(0, 0)); } + return; + } + + pts_velocity_out.clear(); + std::map::const_iterator prev_it; + std::map::const_iterator curr_it; + for (unsigned int i = 0; i < cur_un_pts_map.size(); i++) { + cv::Point2f point_velocity(0, 0); + if (ids[i] != -1) { + prev_it = prev_un_pts_map.find(ids[i]); + + curr_it = cur_un_pts_map.find(ids[i]); + if (prev_it != prev_un_pts_map.end() && curr_it != cur_un_pts_map.end()) { + double v_x = (curr_it->second.x - prev_it->second.x) / dt; + double v_y = (curr_it->second.y - prev_it->second.y) / dt; + point_velocity = cv::Point2f(v_x, v_y); + } + } + pts_velocity_out.push_back(point_velocity); } - prev_un_pts_map = cur_un_pts_map; } From f23b22102e53cb4c7afeb809d0741672c85f7894 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Tue, 25 Feb 2025 15:20:24 +0000 Subject: [PATCH 11/37] [34] Remove the need for maps between id and points. Remove UpdateID function --- .../include/feature_tracker/feature_tracker.h | 12 +-- feature_tracker/src/feature_tracker.cpp | 99 +++++++------------ 2 files changed, 42 insertions(+), 69 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 5486ea476..5d172a0c8 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -43,10 +43,12 @@ class FeatureTracker { void setMask(vector &curr_pts); - void addPoints(vector &curr_pts, + void addPoints(vector &curr_pts, vector &cur_un_pts, + const camodocal::CameraPtr m_camera, const vector &newly_generated_points); - bool updateID(unsigned int i); + cv::Point2f UndistortPoint(const cv::Point2f point, + const camodocal::CameraPtr camera) const; void readIntrinsicParameter(const string &calib_file); @@ -55,8 +57,8 @@ class FeatureTracker { vector rejectWithF(const vector &cur_un_pts, const vector &prev_un_pts); - void GetPointVelocty(double dt, const map &cur_un_pts_map, - const map &prev_un_pts_map, + void GetPointVelocty(double dt, const vector &cur_un_pts, + const vector &prev_un_pts, vector &pts_velocity_out); void RestartTracker(); @@ -70,8 +72,6 @@ class FeatureTracker { vector ids; vector track_cnt; - map prev_un_pts_map; - double fx_; double fy_; camodocal::CameraPtr m_camera; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index c96ff518c..a29d25df2 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -84,14 +84,8 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, } return; } - readImage(new_frame, current_image_time_s); - for (unsigned int i = 0;; i++) { - bool completed = false; - completed |= updateID(i); - if (!completed) break; - } } void FeatureTracker::RestartTracker() { @@ -140,11 +134,14 @@ void FeatureTracker::setMask(vector &curr_pts) { } void FeatureTracker::addPoints( - vector &curr_pts, + vector &curr_pts, vector &cur_un_pts, + const camodocal::CameraPtr m_camera, const vector &newly_generated_points) { for (auto &p : newly_generated_points) { + int new_point_id = ++n_id; curr_pts.push_back(p); - ids.push_back(-1); + cur_un_pts.push_back(UndistortPoint(p, m_camera)); + ids.push_back(new_point_id); track_cnt.push_back(1); } } @@ -152,7 +149,6 @@ void FeatureTracker::addPoints( void FeatureTracker::readImage(const cv::Mat &img, double current_time) { cv::Mat pre_processed_img; vector current_points; - vector newly_generated_points; if (run_histogram_equilisation_) { clahe->apply(img, pre_processed_img); @@ -179,34 +175,29 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { reduceVector(ids, status); reduceVector(track_cnt, status); } - for (auto &n : track_cnt) n++; - // Prune and detect new points - bool is_prune_and_detect_new_points = - current_time > prev_prune_time + feature_pruning_period_; - vector cur_un_pts; - map cur_un_pts_map; for (unsigned int i = 0; i < current_points.size(); i++) { - Eigen::Vector2d a(current_points[i].x, current_points[i].y); - Eigen::Vector3d b; - m_camera->liftProjective(a, b); - cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); - cur_un_pts_map.insert( - make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z()))); + cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); + // printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); } + // Prune and detect new points + bool is_prune_and_detect_new_points = + current_time > prev_prune_time + feature_pruning_period_; if (is_prune_and_detect_new_points) { - std::cout << "Time to prune and find new" << std::endl; vector status = rejectWithF(cur_un_pts, prev_un_pts); int size_a = prev_un_pts.size(); reduceVector(prev_pts, status); + reduceVector(prev_un_pts, status); reduceVector(current_points, status); + reduceVector(cur_un_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); + spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, current_points.size(), 1.0 * current_points.size() / size_a); @@ -222,17 +213,19 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; if (mask.size() != pre_processed_img.size()) cout << "wrong size " << endl; + + vector newly_generated_points; cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, n_max_cnt, 0.01, min_distance_between_features_, mask); + addPoints(current_points, cur_un_pts, m_camera, newly_generated_points); } - spdlog::debug("add feature begins"); - - addPoints(current_points, newly_generated_points); vector pts_velocity; - GetPointVelocty(current_time - prev_time, cur_un_pts_map, prev_un_pts_map, + + GetPointVelocty(current_time - prev_time, cur_un_pts, prev_un_pts, pts_velocity); + if (event_observer_) { event_observer_->OnProcessedImage(img, current_time, current_points, cur_un_pts, ids, track_cnt, @@ -241,10 +234,7 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { prev_prune_time = current_time; } - if (is_prune_and_detect_new_points) { - } prev_un_pts = cur_un_pts; - prev_un_pts_map = cur_un_pts_map; prev_img_ = pre_processed_img; prev_pts = current_points; prev_time = current_time; @@ -261,8 +251,6 @@ vector FeatureTracker::rejectWithF( vector curr_scaled_undistorted_pts(cur_un_pts.size()), prev_scaled_undistorted_pts(prev_un_pts.size()); - std::cout << cur_un_pts.size() << " , " << prev_un_pts.size() << std::endl; - for (unsigned int i = 0; i < prev_un_pts.size(); i++) { curr_scaled_undistorted_pts[i] = cv::Point2f(fx_ * cur_un_pts[i].x + m_camera->imageWidth() / 2.0, @@ -280,13 +268,7 @@ vector FeatureTracker::rejectWithF( return status; } -bool FeatureTracker::updateID(unsigned int i) { - if (i < ids.size()) { - if (ids[i] == -1) ids[i] = n_id++; - return true; - } else - return false; -} + void FeatureTracker::readIntrinsicParameter(const string &calib_file) { spdlog::info("reading paramerter of camera {}", calib_file.c_str()); @@ -331,32 +313,23 @@ void FeatureTracker::showUndistortion(const string &name) { cv::waitKey(0); } -void FeatureTracker::GetPointVelocty( - double dt, const map &cur_un_pts_map, - const map &prev_un_pts_map, - vector &pts_velocity_out) { - if (prev_un_pts_map.empty()) { - for (unsigned int i = 0; i < prev_pts.size(); i++) { - pts_velocity_out.push_back(cv::Point2f(0, 0)); - } - return; - } - +void FeatureTracker::GetPointVelocty(double dt, + const vector &cur_un_pts, + const vector &prev_un_pts, + vector &pts_velocity_out) { pts_velocity_out.clear(); - std::map::const_iterator prev_it; - std::map::const_iterator curr_it; - for (unsigned int i = 0; i < cur_un_pts_map.size(); i++) { - cv::Point2f point_velocity(0, 0); - if (ids[i] != -1) { - prev_it = prev_un_pts_map.find(ids[i]); - - curr_it = cur_un_pts_map.find(ids[i]); - if (prev_it != prev_un_pts_map.end() && curr_it != cur_un_pts_map.end()) { - double v_x = (curr_it->second.x - prev_it->second.x) / dt; - double v_y = (curr_it->second.y - prev_it->second.y) / dt; - point_velocity = cv::Point2f(v_x, v_y); - } - } + + for (unsigned int i = 0; i < prev_un_pts.size(); i++) { + cv::Point2f point_velocity((cur_un_pts[i].x - prev_un_pts[i].x) / dt, + (cur_un_pts[i].y - prev_un_pts[i].y) / dt); pts_velocity_out.push_back(point_velocity); } } + +cv::Point2f FeatureTracker::UndistortPoint( + const cv::Point2f point, const camodocal::CameraPtr camera) const { + Eigen::Vector2d a(point.x, point.y); + Eigen::Vector3d b; + m_camera->liftProjective(a, b); + return cv::Point2f(b.x() / b.z(), b.y() / b.z()); +} From 7982a9db612e28f373bb6d909d74f15237e2a190 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Tue, 25 Feb 2025 15:21:15 +0000 Subject: [PATCH 12/37] [34] Add arrows for point velocity --- .../feature_tracker_observer.hpp | 6 ++++++ .../src/feature_tracker_observer.cpp | 20 +++++++++++++++++++ .../feature_tracker_observer_spdlog_rerun.cpp | 5 +++-- 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp index a4ac3b4d1..48e0b42be 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp @@ -28,5 +28,11 @@ class FeatureTrackerObserver std::vector features, std::vector track_cnt, uint max_track_count); + + cv::Mat CreateOpticalFlowImage(cv::Mat image, + std::vector features, + std::vector track_cnt, + uint max_track_count, + std::vector points_velocity); }; #endif /* FEATURE_TRACKER_OBSERVER_HPP */ diff --git a/feature_tracker/src/feature_tracker_observer.cpp b/feature_tracker/src/feature_tracker_observer.cpp index 533fdc770..6879d9855 100644 --- a/feature_tracker/src/feature_tracker_observer.cpp +++ b/feature_tracker/src/feature_tracker_observer.cpp @@ -13,4 +13,24 @@ cv::Mat FeatureTrackerObserver::CreateTrackedFeatureImage( cv::Scalar(255 * (1 - len), 0, 255 * len), 2); } return show_img; +} + +cv::Mat FeatureTrackerObserver::CreateOpticalFlowImage( + cv::Mat image, std::vector features, + std::vector track_cnt, uint max_track_count, + std::vector points_velocity) { + cv::Mat show_img = + CreateTrackedFeatureImage(image, features, track_cnt, max_track_count); + + for (size_t i = 0; i < points_velocity.size(); i++) { + cv::Point2f start = features[i]; + // std::cout << "velocty: " << points_velocity[i].x << ", " + // << points_velocity[i].y << std::endl; + cv::Point2f end = + start + (points_velocity[i])*10; // Add velocity to get end point + + cv::arrowedLine(show_img, start, end, cv::Scalar(0, 255, 0), 1, + cv::LINE_AA); + } + return show_img; } \ No newline at end of file diff --git a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp index d60377358..8273aca0e 100644 --- a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp +++ b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp @@ -36,9 +36,10 @@ void FeatureTrackerObserverSPDRerun::OnProcessedImage( std::vector ids, std::vector track_count, std::vector points_velocity) { spdlog::info("Features have been pruned and new features added"); - cv::Mat img = CreateTrackedFeatureImage(new_frame, features, track_count, 20); + cv::Mat img = CreateOpticalFlowImage(new_frame, features, track_count, 20, + points_velocity); cv::imshow("Image", img); // Wait for 100 milliseconds before moving to the next image - cv::waitKey(1); + cv::waitKey(100); } \ No newline at end of file From 1a5f1292961f18edf29f46c6799e1e4d05f1d469 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Thu, 27 Feb 2025 15:25:52 +0000 Subject: [PATCH 13/37] [34] WIP create feature tracker node --- .../feature_tracker_node.hpp | 57 ++++ .../src/feature_tracker_node.cpp | 310 +++++------------- 2 files changed, 145 insertions(+), 222 deletions(-) create mode 100644 feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp diff --git a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp new file mode 100644 index 000000000..11037132e --- /dev/null +++ b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp @@ -0,0 +1,57 @@ +#ifndef FEATURE_TRACKER_NODE_HPP +#define FEATURE_TRACKER_NODE_HPP +#include +#include +#include +#include +#include +#include + +#include "feature_tracker/feature_tracker.h" +#include "feature_tracker/feature_tracker_observer.hpp" + +class FeatureTrackerNode : public FeatureTrackerObserver { + ~FeatureTrackerNode(); + bool Start(); + + private: + void OnRegistered() final; + + void OnRestart() final; + void OnDurationBetweenFrameTooLarge(double current_image_time_s, + double previous_image_time_s) final; + void OnImageTimeMovingBackwards(double current_image_time_s, + double previous_image_time_s) final; + void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, + std::vector features, + std::vector undistorted_features, + + std::vector ids, std::vector track_count, + std::vector points_velocity) final; + + void img_callback(const sensor_msgs::ImageConstPtr &img_msg); + + FeatureTracker feature_tracker; + + int min_track_count_to_publish_; + ros::NodeHandle nh_{"~"}; + ros::Subscriber image_subscriber_; + ros::Publisher optical_flow_image_publisher_; + ros::Publisher feature_point_cloud_publisher_; + ros::Publisher pub_restart_; + + sensor_msgs::PointCloud feature_points_msg_; + sensor_msgs::ChannelFloat32 feature_id_channel_; + sensor_msgs::ChannelFloat32 feature_u_pixel_channel_; + sensor_msgs::ChannelFloat32 feature_v_pixel_channel_; + sensor_msgs::ChannelFloat32 feature_x_velocity_channel_; + sensor_msgs::ChannelFloat32 feature_y_velocity_channel_; + ros::Time current_image_time_; + + std_msgs::Bool restart_flag_; + cv_bridge::CvImage optical_flow_img_; + sensor_msgs::Image optical_flow_img_msg_; + +}; + +#endif /* FEATURE_TRACKER_NODE_HPP */ diff --git a/feature_tracker_ros/src/feature_tracker_node.cpp b/feature_tracker_ros/src/feature_tracker_node.cpp index ef6a85661..f4ca722b5 100644 --- a/feature_tracker_ros/src/feature_tracker_node.cpp +++ b/feature_tracker_ros/src/feature_tracker_node.cpp @@ -1,234 +1,100 @@ +#include "feature_tracker_ros/feature_tracker_node.hpp" + +#include #include -#include -#include #include -#include -#include -#include -#include -#include "feature_tracker/feature_tracker.h" -#include "ros_parameter_reader.h" - -#define SHOW_UNDISTORTION 0 - -vector r_status; -vector r_err; -queue img_buf; - -ros::Publisher pub_img, pub_match; -ros::Publisher pub_restart; - -FeatureTracker trackerData(); -double first_image_time; -int pub_count = 1; -bool first_image_flag = true; -double last_image_time = 0; -bool init_pub = 0; - - - - -void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { - trackerData.ProcessNewFrame() - if (first_image_flag) { - first_image_flag = false; - first_image_time = img_msg->header.stamp.toSec(); - last_image_time = img_msg->header.stamp.toSec(); - return; - } - // detect unstable camera stream - if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || - img_msg->header.stamp.toSec() < last_image_time) { - ROS_WARN("image discontinue! reset the feature tracker!"); - first_image_flag = true; - last_image_time = 0; - pub_count = 1; - std_msgs::Bool restart_flag; - restart_flag.data = true; - pub_restart.publish(restart_flag); - return; - } - last_image_time = img_msg->header.stamp.toSec(); - // frequency control - if (round(1.0 * pub_count / - (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ) { - PUB_THIS_FRAME = true; - // reset the frequency control - if (abs(1.0 * pub_count / - (img_msg->header.stamp.toSec() - first_image_time) - - FREQ) < 0.01 * FREQ) { - first_image_time = img_msg->header.stamp.toSec(); - pub_count = 0; - } - } else - PUB_THIS_FRAME = false; - - cv_bridge::CvImageConstPtr ptr; - if (img_msg->encoding == "8UC1") { - sensor_msgs::Image img; - img.header = img_msg->header; - img.height = img_msg->height; - img.width = img_msg->width; - img.is_bigendian = img_msg->is_bigendian; - img.step = img_msg->step; - img.data = img_msg->data; - img.encoding = "mono8"; - ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); - } else - ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); - - cv::Mat show_img = ptr->image; - TicToc t_r; - for (int i = 0; i < NUM_OF_CAM; i++) { - ROS_DEBUG("processing camera %d", i); - if (i != 1 || !STEREO_TRACK) - trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), - img_msg->header.stamp.toSec()); - else { - if (EQUALIZE) { - cv::Ptr clahe = cv::createCLAHE(); - clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), - trackerData[i].cur_img); - } else - trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1)); - } - -#if SHOW_UNDISTORTION - trackerData[i].showUndistortion("undistrotion_" + std::to_string(i)); -#endif - } - - for (unsigned int i = 0;; i++) { - bool completed = false; - for (int j = 0; j < NUM_OF_CAM; j++) - if (j != 1 || !STEREO_TRACK) completed |= trackerData[j].updateID(i); - if (!completed) break; - } - - if (PUB_THIS_FRAME) { - pub_count++; - sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud); - sensor_msgs::ChannelFloat32 id_of_point; - sensor_msgs::ChannelFloat32 u_of_point; - sensor_msgs::ChannelFloat32 v_of_point; - sensor_msgs::ChannelFloat32 velocity_x_of_point; - sensor_msgs::ChannelFloat32 velocity_y_of_point; - - feature_points->header = img_msg->header; - feature_points->header.frame_id = "world"; - - vector> hash_ids(NUM_OF_CAM); - for (int i = 0; i < NUM_OF_CAM; i++) { - auto &un_pts = trackerData[i].cur_un_pts; - auto &prev_pts = trackerData[i].prev_pts; - auto &ids = trackerData[i].ids; - auto &pts_velocity = trackerData[i].pts_velocity; - for (unsigned int j = 0; j < ids.size(); j++) { - if (trackerData[i].track_cnt[j] > 1) { - int p_id = ids[j]; - hash_ids[i].insert(p_id); - geometry_msgs::Point32 p; - p.x = un_pts[j].x; - p.y = un_pts[j].y; - p.z = 1; - - feature_points->points.push_back(p); - id_of_point.values.push_back(p_id * NUM_OF_CAM + i); - u_of_point.values.push_back(prev_pts[j].x); - v_of_point.values.push_back(prev_pts[j].y); - velocity_x_of_point.values.push_back(pts_velocity[j].x); - velocity_y_of_point.values.push_back(pts_velocity[j].y); - } - } - } - feature_points->channels.push_back(id_of_point); - feature_points->channels.push_back(u_of_point); - feature_points->channels.push_back(v_of_point); - feature_points->channels.push_back(velocity_x_of_point); - feature_points->channels.push_back(velocity_y_of_point); - ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), - ros::Time::now().toSec()); - // skip the first image; since no optical speed on frist image - if (!init_pub) { - init_pub = 1; - } else - pub_img.publish(feature_points); - - if (SHOW_TRACK) { - ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8); - // cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3); - cv::Mat stereo_img = ptr->image; - - for (int i = 0; i < NUM_OF_CAM; i++) { - cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW); - cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); +#include +#include + +bool FeatureTrackerNode::Start() { + restart_flag_.data = true; + + feature_points_msg_.channels.push_back(feature_id_channel_); + feature_points_msg_.channels.push_back(feature_u_pixel_channel_); + feature_points_msg_.channels.push_back(feature_v_pixel_channel_); + feature_points_msg_.channels.push_back(feature_x_velocity_channel_); + feature_points_msg_.channels.push_back(feature_y_velocity_channel_); + + feature_id_channel_.name = "Feature Ids"; + feature_u_pixel_channel_.name = "Feature U Pixel"; + feature_v_pixel_channel_.name = "Feature V Pixel"; + feature_x_velocity_channel_.name = "Feature X Velocity"; + feature_y_velocity_channel_.name = "Feature Y Velocity"; + + // Set this upon first camera message received + optical_flow_img_.header.frame_id = "TODO"; + optical_flow_img_.encoding = sensor_msgs::image_encodings::BGR8; + return true; +} - for (unsigned int j = 0; j < trackerData[i].prev_pts.size(); j++) { - double len = - std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE); - cv::circle(tmp_img, trackerData[i].prev_pts[j], 2, - cv::Scalar(255 * (1 - len), 0, 255 * len), 2); - // draw speed line - /* - Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, - trackerData[i].cur_un_pts[j].y); Vector2d tmp_pts_velocity - (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y); - Vector3d tmp_prev_un_pts; - tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity; - tmp_prev_un_pts.z() = 1; - Vector2d tmp_prev_uv; - trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv); - cv::line(tmp_img, trackerData[i].prev_pts[j], - cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), - 1 , 8, 0); - */ - // char name[10]; - // sprintf(name, "%d", trackerData[i].ids[j]); - // cv::putText(tmp_img, name, trackerData[i].prev_pts[j], - // cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); - } - } - // cv::imshow("vis", stereo_img); - // cv::waitKey(5); - pub_match.publish(ptr->toImageMsg()); - } - } - ROS_INFO("whole feature tracker processing costs: %f", t_r.toc()); +void FeatureTrackerNode::OnRegistered() { + ROS_INFO("FeatureTrackerNode has been registered by feature tracker"); } -int main(int argc, char **argv) { - ros::init(argc, argv, "feature_tracker"); - ros::NodeHandle n("~"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Info); - readParameters(n); +void FeatureTrackerNode::OnRestart() { + ROS_INFO("FeatureTracker has been restarted"); + pub_restart_.publish(restart_flag_); +} - for (int i = 0; i < NUM_OF_CAM; i++) - trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); +void FeatureTrackerNode::OnDurationBetweenFrameTooLarge( + double current_image_time_s, double previous_image_time_s) { + ROS_INFO_STREAM("Too much time has elapsed between images." + << "\n\t Current image received had timestamp: " + << std::setprecision(2) << current_image_time_s + << "\n\t Previous image received had timestamp: " + << std::setprecision(2) << previous_image_time_s); +} - if (FISHEYE) { - for (int i = 0; i < NUM_OF_CAM; i++) { - trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0); - if (!trackerData[i].fisheye_mask.data) { - ROS_INFO("load mask fail"); - ROS_BREAK(); - } else - ROS_INFO("load mask success"); +void FeatureTrackerNode::OnImageTimeMovingBackwards( + double current_image_time_s, double previous_image_time_s) { + ROS_WARN_STREAM("Timestamp went backwards/" + << "\n\t Current image received had timestamp: " + << std::setprecision(2) << current_image_time_s + << "\n\t Previous image received had timestamp: " + << std::setprecision(2) << previous_image_time_s); +} +void FeatureTrackerNode::OnProcessedImage( + cv::Mat new_frame, double current_image_time_s, + std::vector features, + std::vector undistorted_features, std::vector ids, + std::vector track_count, std::vector points_velocity) { + feature_points_msg_.points.clear(); + feature_id_channel_.values.clear(); + feature_u_pixel_channel_.values.clear(); + feature_v_pixel_channel_.values.clear(); + feature_x_velocity_channel_.values.clear(); + feature_y_velocity_channel_.values.clear(); + + feature_points_msg_.header.stamp = current_image_time_; + for (size_t i = 0; i < ids.size(); i++) { + if (track_count[i] > min_track_count_to_publish_) { + geometry_msgs::Point32 p; + p.x = undistorted_features[i].x; + p.y = undistorted_features[i].y; + p.z = 1; + + feature_points_msg_.points.push_back(p); + feature_id_channel_.values.push_back(ids[i]); + feature_u_pixel_channel_.values.push_back(features[i].x); + feature_v_pixel_channel_.values.push_back(features[i].y); + feature_x_velocity_channel_.values.push_back(points_velocity[i].x); + feature_y_velocity_channel_.values.push_back(points_velocity[i].y); } } + optical_flow_img_.header.stamp = current_image_time_; + optical_flow_img_.image = CreateOpticalFlowImage( + new_frame, features, track_count, 20, points_velocity); + optical_flow_img_.toImageMsg(optical_flow_img_msg_); + optical_flow_image_publisher_.publish(optical_flow_img_msg_); + feature_point_cloud_publisher_.publish(feature_points_msg_); +} - ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback); - - pub_img = n.advertise("feature", 1000); - pub_match = n.advertise("feature_img", 1000); - pub_restart = n.advertise("restart", 1000); - /* - if (SHOW_TRACK) - cv::namedWindow("vis", cv::WINDOW_NORMAL); - */ - ros::spin(); - return 0; +void FeatureTrackerNode::img_callback( + const sensor_msgs::ImageConstPtr& img_msg) { + current_image_time_ = img_msg->header.stamp; + cv_bridge::CvImagePtr cv_ptr = + cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8); +// feature_tracker.ProcessNewFrame(cv_ptr->image); } -// new points velocity is 0, pub or not? -// track cnt > 1 pub? \ No newline at end of file +int main(int argc, char** argv) {} \ No newline at end of file From 0edd08383eed6d93724d6cbc1139eaab5939f796 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sat, 1 Mar 2025 03:20:48 +0000 Subject: [PATCH 14/37] [34] WIP Adding param reading --- .../feature_tracker_node.hpp | 8 ++- .../src/feature_tracker_node.cpp | 49 +++++++++++++++++-- 2 files changed, 50 insertions(+), 7 deletions(-) diff --git a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp index 11037132e..aa43ee2c9 100644 --- a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp +++ b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp @@ -29,11 +29,16 @@ class FeatureTrackerNode : public FeatureTrackerObserver { std::vector ids, std::vector track_count, std::vector points_velocity) final; - void img_callback(const sensor_msgs::ImageConstPtr &img_msg); + bool ReadParameters(); + + void ImageCallback(const sensor_msgs::ImageConstPtr &img_msg); + + void StartPublishersAndSubscribers(); FeatureTracker feature_tracker; int min_track_count_to_publish_; + bool first_image_; ros::NodeHandle nh_{"~"}; ros::Subscriber image_subscriber_; ros::Publisher optical_flow_image_publisher_; @@ -51,7 +56,6 @@ class FeatureTrackerNode : public FeatureTrackerObserver { std_msgs::Bool restart_flag_; cv_bridge::CvImage optical_flow_img_; sensor_msgs::Image optical_flow_img_msg_; - }; #endif /* FEATURE_TRACKER_NODE_HPP */ diff --git a/feature_tracker_ros/src/feature_tracker_node.cpp b/feature_tracker_ros/src/feature_tracker_node.cpp index f4ca722b5..c2fe4b2d9 100644 --- a/feature_tracker_ros/src/feature_tracker_node.cpp +++ b/feature_tracker_ros/src/feature_tracker_node.cpp @@ -3,8 +3,9 @@ #include #include #include -#include + #include +#include bool FeatureTrackerNode::Start() { restart_flag_.data = true; @@ -21,12 +22,23 @@ bool FeatureTrackerNode::Start() { feature_x_velocity_channel_.name = "Feature X Velocity"; feature_y_velocity_channel_.name = "Feature Y Velocity"; + first_image_ = false; + // Set this upon first camera message received optical_flow_img_.header.frame_id = "TODO"; optical_flow_img_.encoding = sensor_msgs::image_encodings::BGR8; return true; } +void FeatureTrackerNode::StartPublishersAndSubscribers() { + image_subscriber_ = n.subscribe(IMAGE_TOPIC, 100, img_callback); + feature_point_cloud_publisher_ = + n.advertise("feature", 1000); + optical_flow_image_publisher_ = + n.advertise("feature_img", 1000); + pub_restart_ = n.advertise("restart", 1000); +} + void FeatureTrackerNode::OnRegistered() { ROS_INFO("FeatureTrackerNode has been registered by feature tracker"); } @@ -89,12 +101,39 @@ void FeatureTrackerNode::OnProcessedImage( feature_point_cloud_publisher_.publish(feature_points_msg_); } -void FeatureTrackerNode::img_callback( +void FeatureTrackerNode::ImageCallback( const sensor_msgs::ImageConstPtr& img_msg) { current_image_time_ = img_msg->header.stamp; - cv_bridge::CvImagePtr cv_ptr = - cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8); -// feature_tracker.ProcessNewFrame(cv_ptr->image); + feature_tracker.ProcessNewFrame( + cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8)->image, + img_msg->header.stamp.toSec()); + // feature_tracker.ProcessNewFrame(cv_ptr->image); } +bool FeatureTrackerNode::ReadParameters() { + if (!nh_.getParam("config_file", config_file_path_)) { + ROS_ERROR("Failed to read \"config_file\" parameter"); + return false; + } + + nh_.getParam("fisheye", fisheye_); + nh_.getParam("max_cnt", max_number_of_features_); + nh_.getParam("min_dist", minimum_distance_between_features_); + nh_.getParam("image_height", image_height_); + nh_.getParam("image_width", image_width_); + nh_.getParam("freq", pruning_frequency_); + nh_.getParam("F_threshold", ransac_threshold_); + + nh_.getParam("equalize", run_histogram_equalisation_); + nh_.getParam("F_threshold", ransac_threshold_); + nh_.getParam("F_threshold", ransac_threshold_); + + ROS_INFO( + "Loaded parameters: config_file: %s, visualization_shift_x: %d, " + "visualization_shift_y: %d, skip_cnt: %d, skip_dis: %f", + config_file_path_.c_str(), visualization_shift_x_, visualization_shift_y_, + skip_cnt_threshold_, skip_distance_); + + return true; +} int main(int argc, char** argv) {} \ No newline at end of file From a22caf000a86be829a5eae407761b69f4d5ba2d8 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sat, 1 Mar 2025 14:38:33 +0000 Subject: [PATCH 15/37] [34] WIP ros node basic function --- .../feature_tracker_node.hpp | 13 +- .../launch/feature_tracker.launch | 7 + .../src/feature_tracker_node.cpp | 145 ++++++++++++------ 3 files changed, 114 insertions(+), 51 deletions(-) create mode 100644 feature_tracker_ros/launch/feature_tracker.launch diff --git a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp index aa43ee2c9..64cd28523 100644 --- a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp +++ b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp @@ -11,6 +11,7 @@ #include "feature_tracker/feature_tracker_observer.hpp" class FeatureTrackerNode : public FeatureTrackerObserver { + public: ~FeatureTrackerNode(); bool Start(); @@ -29,13 +30,17 @@ class FeatureTrackerNode : public FeatureTrackerObserver { std::vector ids, std::vector track_count, std::vector points_velocity) final; - bool ReadParameters(); + bool ReadParameters(std::string& configFilePath, int& maxFeatureCount, + int& minFeatureDistance, int& pruningFrequency, + double& ransacThreshold, + bool& enableHistogramEqualization, bool& useFisheye); - void ImageCallback(const sensor_msgs::ImageConstPtr &img_msg); + void ImageCallback(const sensor_msgs::ImageConstPtr& img_msg); void StartPublishersAndSubscribers(); - FeatureTracker feature_tracker; + std::unique_ptr feature_tracker_; + cv_bridge::CvImage optical_flow_img_; int min_track_count_to_publish_; bool first_image_; @@ -52,9 +57,7 @@ class FeatureTrackerNode : public FeatureTrackerObserver { sensor_msgs::ChannelFloat32 feature_x_velocity_channel_; sensor_msgs::ChannelFloat32 feature_y_velocity_channel_; ros::Time current_image_time_; - std_msgs::Bool restart_flag_; - cv_bridge::CvImage optical_flow_img_; sensor_msgs::Image optical_flow_img_msg_; }; diff --git a/feature_tracker_ros/launch/feature_tracker.launch b/feature_tracker_ros/launch/feature_tracker.launch new file mode 100644 index 000000000..a9f49e52c --- /dev/null +++ b/feature_tracker_ros/launch/feature_tracker.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/feature_tracker_ros/src/feature_tracker_node.cpp b/feature_tracker_ros/src/feature_tracker_node.cpp index c2fe4b2d9..8194380f7 100644 --- a/feature_tracker_ros/src/feature_tracker_node.cpp +++ b/feature_tracker_ros/src/feature_tracker_node.cpp @@ -3,40 +3,115 @@ #include #include #include +#include #include #include +FeatureTrackerNode::~FeatureTrackerNode() { ros::shutdown(); } +void FeatureTrackerNode::ImageCallback( + const sensor_msgs::ImageConstPtr& img_msg) { + if (first_image_) { + optical_flow_img_msg_ = *img_msg; + first_image_ = false; + } + current_image_time_ = img_msg->header.stamp; + ROS_INFO_STREAM("" << current_image_time_); + feature_tracker_->ProcessNewFrame( + cv_bridge::toCvShare(img_msg, img_msg->encoding)->image, + img_msg->header.stamp.toSec()); + // feature_tracker.ProcessNewFrame(cv_ptr->image); +} + bool FeatureTrackerNode::Start() { restart_flag_.data = true; - feature_points_msg_.channels.push_back(feature_id_channel_); - feature_points_msg_.channels.push_back(feature_u_pixel_channel_); - feature_points_msg_.channels.push_back(feature_v_pixel_channel_); - feature_points_msg_.channels.push_back(feature_x_velocity_channel_); - feature_points_msg_.channels.push_back(feature_y_velocity_channel_); - feature_id_channel_.name = "Feature Ids"; feature_u_pixel_channel_.name = "Feature U Pixel"; feature_v_pixel_channel_.name = "Feature V Pixel"; feature_x_velocity_channel_.name = "Feature X Velocity"; feature_y_velocity_channel_.name = "Feature Y Velocity"; + feature_points_msg_.channels.push_back(feature_id_channel_); + feature_points_msg_.channels.push_back(feature_u_pixel_channel_); + feature_points_msg_.channels.push_back(feature_v_pixel_channel_); + feature_points_msg_.channels.push_back(feature_x_velocity_channel_); + feature_points_msg_.channels.push_back(feature_y_velocity_channel_); + first_image_ = false; // Set this upon first camera message received optical_flow_img_.header.frame_id = "TODO"; optical_flow_img_.encoding = sensor_msgs::image_encodings::BGR8; + + // Read parameters + std::string config_file_path; + int max_feature_count, min_feature_distance, pruning_frequency; + double ransac_threshold; + bool enable_histogram_equalization, use_fisheye; + + if (!ReadParameters(config_file_path, max_feature_count, min_feature_distance, + pruning_frequency, ransac_threshold, + enable_histogram_equalization, use_fisheye)) { + ROS_ERROR("Failed to read parameters in Start()"); + return false; + } + + // Initialize FeatureTracker with parameters + feature_tracker_ = std::make_unique( + config_file_path, use_fisheye, enable_histogram_equalization, + max_feature_count, min_feature_distance, ransac_threshold, + /*fx=*/460.0, /*fy=*/460.0, pruning_frequency, + /*max_time_difference=*/1.0); + feature_tracker_->RegisterEventObserver( + FeatureTrackerNode::shared_from_this()); + StartPublishersAndSubscribers(); + return true; +} + +bool FeatureTrackerNode::ReadParameters( + std::string& config_file_path, int& max_feature_count, + int& min_feature_distance, int& pruning_frequency, double& ransac_threshold, + bool& enable_histogram_equalization, bool& use_fisheye) { + if (!nh_.getParam("config_file", config_file_path)) { + ROS_ERROR("Failed to read \"config_file\" parameter"); + return false; + } + + nh_.getParam("/max_cnt", max_feature_count); + nh_.getParam("/min_dist", min_feature_distance); + nh_.getParam("/freq", pruning_frequency); + nh_.getParam("/F_threshold", ransac_threshold); + nh_.getParam("/equalize", enable_histogram_equalization); + nh_.getParam("/fisheye", use_fisheye); + + ROS_INFO( + "Loaded parameters:\n" + "\tconfig_file: %s\n" + "\tmax_cnt: %u\n" + "\tmin_dist: %u\n" + + "\tfreq: %u\n" + "\tF_threshold: %f\n" + "\tequalize: %s\n" + "\tfisheye: %s", + config_file_path.c_str(), max_feature_count, min_feature_distance, + pruning_frequency, ransac_threshold, + enable_histogram_equalization ? "true" : "false", + use_fisheye ? "true" : "false"); + return true; } void FeatureTrackerNode::StartPublishersAndSubscribers() { - image_subscriber_ = n.subscribe(IMAGE_TOPIC, 100, img_callback); + ROS_INFO("Creating Subscribers and Publishers"); feature_point_cloud_publisher_ = - n.advertise("feature", 1000); + nh_.advertise("feature", 1000); optical_flow_image_publisher_ = - n.advertise("feature_img", 1000); - pub_restart_ = n.advertise("restart", 1000); + nh_.advertise("feature_img", 1000); + pub_restart_ = nh_.advertise("restart", 1000); + image_subscriber_ = nh_.subscribe("/cam0/image_raw", 100, + &FeatureTrackerNode::ImageCallback, this); } void FeatureTrackerNode::OnRegistered() { @@ -78,7 +153,8 @@ void FeatureTrackerNode::OnProcessedImage( feature_y_velocity_channel_.values.clear(); feature_points_msg_.header.stamp = current_image_time_; - for (size_t i = 0; i < ids.size(); i++) { + + for (size_t i = 0; i < points_velocity.size(); i++) { if (track_count[i] > min_track_count_to_publish_) { geometry_msgs::Point32 p; p.x = undistorted_features[i].x; @@ -93,6 +169,7 @@ void FeatureTrackerNode::OnProcessedImage( feature_y_velocity_channel_.values.push_back(points_velocity[i].y); } } + optical_flow_img_.header.stamp = current_image_time_; optical_flow_img_.image = CreateOpticalFlowImage( new_frame, features, track_count, 20, points_velocity); @@ -101,39 +178,15 @@ void FeatureTrackerNode::OnProcessedImage( feature_point_cloud_publisher_.publish(feature_points_msg_); } -void FeatureTrackerNode::ImageCallback( - const sensor_msgs::ImageConstPtr& img_msg) { - current_image_time_ = img_msg->header.stamp; - feature_tracker.ProcessNewFrame( - cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8)->image, - img_msg->header.stamp.toSec()); - // feature_tracker.ProcessNewFrame(cv_ptr->image); -} - -bool FeatureTrackerNode::ReadParameters() { - if (!nh_.getParam("config_file", config_file_path_)) { - ROS_ERROR("Failed to read \"config_file\" parameter"); - return false; +int main(int argc, char** argv) { + ros::init(argc, argv, "feature_tracker_node"); + std::shared_ptr feature_tracker_node = + std::make_shared(); + if (!feature_tracker_node->Start()) { + ROS_ERROR("Failed to start FeatureTrackerNode"); + ros::shutdown(); } - - nh_.getParam("fisheye", fisheye_); - nh_.getParam("max_cnt", max_number_of_features_); - nh_.getParam("min_dist", minimum_distance_between_features_); - nh_.getParam("image_height", image_height_); - nh_.getParam("image_width", image_width_); - nh_.getParam("freq", pruning_frequency_); - nh_.getParam("F_threshold", ransac_threshold_); - - nh_.getParam("equalize", run_histogram_equalisation_); - nh_.getParam("F_threshold", ransac_threshold_); - nh_.getParam("F_threshold", ransac_threshold_); - - ROS_INFO( - "Loaded parameters: config_file: %s, visualization_shift_x: %d, " - "visualization_shift_y: %d, skip_cnt: %d, skip_dis: %f", - config_file_path_.c_str(), visualization_shift_x_, visualization_shift_y_, - skip_cnt_threshold_, skip_distance_); - - return true; -} -int main(int argc, char** argv) {} \ No newline at end of file + ROS_INFO("FeatureTrackerNode started."); + ros::spin(); + return 0; +} \ No newline at end of file From bf0245c330b7f62842c3e71c2e204d6a0e2fb2ca Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sat, 1 Mar 2025 17:59:19 +0000 Subject: [PATCH 16/37] [34] Update to use first frame --- .../include/feature_tracker/feature_tracker.h | 34 ++-- feature_tracker/src/feature_tracker.cpp | 164 ++++++++++-------- 2 files changed, 108 insertions(+), 90 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 5d172a0c8..a55ef7360 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -36,12 +36,12 @@ class FeatureTracker { double max_time_difference); void RegisterEventObserver( std::shared_ptr event_observer); - void ProcessNewFrame(cv::Mat new_frame, double time_s); + void ProcessNewFrame(cv::Mat img, double time_s); private: - void readImage(const cv::Mat &_img, double current_time); - - void setMask(vector &curr_pts); + void readImage(const cv::Mat &pre_processed_img, double current_time); + void RestartTracker(const cv::Mat &pre_processed_img, double current_time); + cv::Mat setMask(vector &curr_pts); void addPoints(vector &curr_pts, vector &cur_un_pts, const camodocal::CameraPtr m_camera, @@ -49,7 +49,16 @@ class FeatureTracker { cv::Point2f UndistortPoint(const cv::Point2f point, const camodocal::CameraPtr camera) const; - + void PrunePointsUsingRansac(vector &curr_points, + vector &curr_un_points, + vector &prev_points, + vector &prev_un_points, + vector &ids, vector &track_counts); + + void DetectNewFeaturePoints(vector ¤t_points, + vector ¤t_undistorted_points, + const cv::Mat &pre_processed_img, + int n_max_point_to_detect); void readIntrinsicParameter(const string &calib_file); void showUndistortion(const string &name); @@ -61,22 +70,21 @@ class FeatureTracker { const vector &prev_un_pts, vector &pts_velocity_out); - void RestartTracker(); + std::string GenerateStateString(); - cv::Mat mask; cv::Mat fisheye_mask; + cv::Mat prev_img_; + double previous_frame_time_; vector prev_un_pts; vector prev_pts; - - vector ids; + vector feature_ids_; vector track_cnt; + double prev_prune_time_; double fx_; double fy_; camodocal::CameraPtr m_camera; - double prev_time; - double prev_prune_time; static int n_id; @@ -87,9 +95,7 @@ class FeatureTracker { uint max_feature_count_per_image_; uint min_distance_between_features_; double fundemental_matrix_ransac_threshold_; - bool is_first_frame_; - double first_frame_time_; - double previous_frame_time_; + double max_time_difference_; double feature_pruning_frequency_; double feature_pruning_period_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index a29d25df2..271d1dd72 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -1,9 +1,9 @@ #include "feature_tracker/feature_tracker.h" #include +#include #include "spdlog/spdlog.h" - int FeatureTracker::n_id = 0; bool inBorder(const cv::Point2f &pt, uint col, uint row) { @@ -43,7 +43,9 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, fx_(fx), fy_(fy), feature_pruning_frequency_(feature_pruning_frequency), - max_time_difference_(max_time_difference) { + max_time_difference_(max_time_difference), + previous_frame_time_(0.0), + prev_prune_time_(0.0) { readIntrinsicParameter(camera_config_file); feature_pruning_period_ = 1.0 / feature_pruning_frequency; if (run_histogram_equilisation) { @@ -59,47 +61,55 @@ void FeatureTracker::RegisterEventObserver( void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, double current_image_time_s) { - if (is_first_frame_) { - is_first_frame_ = false; - first_frame_time_ = current_image_time_s; - previous_frame_time_ = current_image_time_s; - prev_prune_time = current_image_time_s; - return; - } + cv::Mat pre_processed_img; + if (run_histogram_equilisation_) { + std::cout << "Running eq" << std::endl; + clahe->apply(new_frame, pre_processed_img); + } else + pre_processed_img = new_frame; if (current_image_time_s > previous_frame_time_ + max_time_difference_) { - RestartTracker(); if (event_observer_) { event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, previous_frame_time_); } + RestartTracker(pre_processed_img, current_image_time_s); return; } if (current_image_time_s < previous_frame_time_) { - RestartTracker(); if (event_observer_) { event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, previous_frame_time_); } + RestartTracker(pre_processed_img, current_image_time_s); return; } - readImage(new_frame, current_image_time_s); + readImage(pre_processed_img, current_image_time_s); } -void FeatureTracker::RestartTracker() { - is_first_frame_ = true; - first_frame_time_ = 0; - previous_frame_time_ = 0; - +void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, + double current_time) { if (event_observer_) { event_observer_->OnRestart(); } + prev_pts.clear(); + prev_un_pts.clear(); + feature_ids_.clear(); + track_cnt.clear(); + DetectNewFeaturePoints(prev_pts, prev_un_pts, pre_processed_img, + max_feature_count_per_image_); + + previous_frame_time_ = current_time; + prev_prune_time_ = current_time; + prev_img_ = pre_processed_img; + std::cout << GenerateStateString() << std::endl; return; } -void FeatureTracker::setMask(vector &curr_pts) { +cv::Mat FeatureTracker::setMask(vector &curr_pts) { + cv::Mat mask; if (fisheye_) mask = fisheye_mask.clone(); else @@ -111,7 +121,7 @@ void FeatureTracker::setMask(vector &curr_pts) { for (unsigned int i = 0; i < curr_pts.size(); i++) cnt_pts_id.push_back( - make_pair(track_cnt[i], make_pair(curr_pts[i], ids[i]))); + make_pair(track_cnt[i], make_pair(curr_pts[i], feature_ids_[i]))); sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, @@ -120,17 +130,18 @@ void FeatureTracker::setMask(vector &curr_pts) { }); curr_pts.clear(); - ids.clear(); + feature_ids_.clear(); track_cnt.clear(); for (auto &it : cnt_pts_id) { if (mask.at(it.second.first) == 255) { curr_pts.push_back(it.second.first); - ids.push_back(it.second.second); + feature_ids_.push_back(it.second.second); track_cnt.push_back(it.first); cv::circle(mask, it.second.first, min_distance_between_features_, 0, -1); } } + return mask; } void FeatureTracker::addPoints( @@ -141,23 +152,28 @@ void FeatureTracker::addPoints( int new_point_id = ++n_id; curr_pts.push_back(p); cur_un_pts.push_back(UndistortPoint(p, m_camera)); - ids.push_back(new_point_id); + feature_ids_.push_back(new_point_id); track_cnt.push_back(1); } } -void FeatureTracker::readImage(const cv::Mat &img, double current_time) { - cv::Mat pre_processed_img; - vector current_points; - - if (run_histogram_equilisation_) { - clahe->apply(img, pre_processed_img); - } else - pre_processed_img = img; - - if (prev_img_.empty()) { - prev_img_ = pre_processed_img; +void FeatureTracker::DetectNewFeaturePoints( + vector ¤t_points, + vector ¤t_undistorted_points, + const cv::Mat &pre_processed_img, int n_max_point_to_detect) { + cv::Mat mask = setMask(current_points); + if (n_max_point_to_detect > 0) { + vector newly_generated_points; + cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, + n_max_point_to_detect, 0.01, + min_distance_between_features_, mask); + addPoints(current_points, current_undistorted_points, m_camera, + newly_generated_points); } +} +void FeatureTracker::readImage(const cv::Mat &pre_processed_img, + double current_time) { + vector current_points; if (prev_pts.size() > 0) { vector status; @@ -172,7 +188,7 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { reduceVector(prev_pts, status); reduceVector(prev_un_pts, status); reduceVector(current_points, status); - reduceVector(ids, status); + reduceVector(feature_ids_, status); reduceVector(track_cnt, status); } for (auto &n : track_cnt) n++; @@ -181,64 +197,37 @@ void FeatureTracker::readImage(const cv::Mat &img, double current_time) { for (unsigned int i = 0; i < current_points.size(); i++) { cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); - - // printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y); } // Prune and detect new points bool is_prune_and_detect_new_points = - current_time > prev_prune_time + feature_pruning_period_; + current_time > prev_prune_time_ + feature_pruning_period_; if (is_prune_and_detect_new_points) { - vector status = rejectWithF(cur_un_pts, prev_un_pts); - int size_a = prev_un_pts.size(); - reduceVector(prev_pts, status); - reduceVector(prev_un_pts, status); - reduceVector(current_points, status); - reduceVector(cur_un_pts, status); - reduceVector(ids, status); - reduceVector(track_cnt, status); - - spdlog::debug("FM ransac: {0} -> {1}: {2}", size_a, current_points.size(), - 1.0 * current_points.size() / size_a); - - spdlog::debug("set mask begins"); - - setMask(current_points); - - spdlog::debug("detect feature begins"); + PrunePointsUsingRansac(current_points, cur_un_pts, prev_pts, prev_un_pts, + feature_ids_, track_cnt); int n_max_cnt = max_feature_count_per_image_ - static_cast(current_points.size()); - if (n_max_cnt > 0) { - if (mask.empty()) cout << "mask is empty " << endl; - if (mask.type() != CV_8UC1) cout << "mask type wrong " << endl; - if (mask.size() != pre_processed_img.size()) - cout << "wrong size " << endl; - - vector newly_generated_points; - cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, - n_max_cnt, 0.01, min_distance_between_features_, - mask); - addPoints(current_points, cur_un_pts, m_camera, newly_generated_points); - } - + DetectNewFeaturePoints(current_points, cur_un_pts, pre_processed_img, + n_max_cnt); vector pts_velocity; - GetPointVelocty(current_time - prev_time, cur_un_pts, prev_un_pts, - pts_velocity); + GetPointVelocty(current_time - previous_frame_time_, cur_un_pts, + prev_un_pts, pts_velocity); if (event_observer_) { - event_observer_->OnProcessedImage(img, current_time, current_points, - cur_un_pts, ids, track_cnt, - pts_velocity); + event_observer_->OnProcessedImage(pre_processed_img, current_time, + current_points, cur_un_pts, + feature_ids_, track_cnt, pts_velocity); } - prev_prune_time = current_time; + prev_prune_time_ = current_time; } prev_un_pts = cur_un_pts; prev_img_ = pre_processed_img; prev_pts = current_points; - prev_time = current_time; + previous_frame_time_ = current_time; } + vector FeatureTracker::rejectWithF( const vector &cur_un_pts, const vector &prev_un_pts) { @@ -268,8 +257,21 @@ vector FeatureTracker::rejectWithF( return status; } - - +void FeatureTracker::PrunePointsUsingRansac( + vector ¤t_points, + vector ¤t_undistorted_points, + vector &previous_points, + vector &previous_undistorted_points, vector &ids, + vector &track_counts) { + vector status = + rejectWithF(current_undistorted_points, previous_undistorted_points); + reduceVector(current_points, status); + reduceVector(current_undistorted_points, status); + reduceVector(previous_points, status); + reduceVector(previous_undistorted_points, status); + reduceVector(ids, status); + reduceVector(track_counts, status); +} void FeatureTracker::readIntrinsicParameter(const string &calib_file) { spdlog::info("reading paramerter of camera {}", calib_file.c_str()); m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); @@ -333,3 +335,13 @@ cv::Point2f FeatureTracker::UndistortPoint( m_camera->liftProjective(a, b); return cv::Point2f(b.x() / b.z(), b.y() / b.z()); } + +std::string FeatureTracker::GenerateStateString() { + std::stringstream ss; + ss << "REPORTING CURRENT STATE" + << "\n\t Previous time is:" << previous_frame_time_ + << "\n\t Previous prune time is:" << prev_prune_time_ + << "\n\t Previous number of points is: " << prev_pts.size() + << "\n\t Previous number of undistorted points is: " << prev_un_pts.size(); + return ss.str(); +} \ No newline at end of file From 7a1d353a8234afd5c2b25a9b5f6167161bdd8a3a Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sat, 1 Mar 2025 18:06:42 +0000 Subject: [PATCH 17/37] [34] Rename pravite variables --- .../include/feature_tracker/feature_tracker.h | 6 ++-- feature_tracker/src/feature_tracker.cpp | 32 +++++++++---------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index a55ef7360..d82682d34 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -74,10 +74,10 @@ class FeatureTracker { cv::Mat fisheye_mask; - cv::Mat prev_img_; + cv::Mat previous_pre_processed_image_; double previous_frame_time_; - vector prev_un_pts; - vector prev_pts; + vector previous_undistorted_pts_; + vector previous_points_; vector feature_ids_; vector track_cnt; double prev_prune_time_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 271d1dd72..e9a938045 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -94,16 +94,16 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, if (event_observer_) { event_observer_->OnRestart(); } - prev_pts.clear(); - prev_un_pts.clear(); + previous_points_.clear(); + previous_undistorted_pts_.clear(); feature_ids_.clear(); track_cnt.clear(); - DetectNewFeaturePoints(prev_pts, prev_un_pts, pre_processed_img, + DetectNewFeaturePoints(previous_points_, previous_undistorted_pts_, pre_processed_img, max_feature_count_per_image_); previous_frame_time_ = current_time; prev_prune_time_ = current_time; - prev_img_ = pre_processed_img; + previous_pre_processed_image_ = pre_processed_img; std::cout << GenerateStateString() << std::endl; return; } @@ -175,18 +175,18 @@ void FeatureTracker::readImage(const cv::Mat &pre_processed_img, double current_time) { vector current_points; - if (prev_pts.size() > 0) { + if (previous_points_.size() > 0) { vector status; vector err; - cv::calcOpticalFlowPyrLK(prev_img_, pre_processed_img, prev_pts, + cv::calcOpticalFlowPyrLK(previous_pre_processed_image_, pre_processed_img, previous_points_, current_points, status, err, cv::Size(21, 21), 3); for (int i = 0; i < int(current_points.size()); i++) if (status[i] && !inBorder(current_points[i], m_camera->imageWidth(), m_camera->imageHeight())) status[i] = 0; - reduceVector(prev_pts, status); - reduceVector(prev_un_pts, status); + reduceVector(previous_points_, status); + reduceVector(previous_undistorted_pts_, status); reduceVector(current_points, status); reduceVector(feature_ids_, status); reduceVector(track_cnt, status); @@ -203,7 +203,7 @@ void FeatureTracker::readImage(const cv::Mat &pre_processed_img, current_time > prev_prune_time_ + feature_pruning_period_; if (is_prune_and_detect_new_points) { - PrunePointsUsingRansac(current_points, cur_un_pts, prev_pts, prev_un_pts, + PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, previous_undistorted_pts_, feature_ids_, track_cnt); int n_max_cnt = max_feature_count_per_image_ - static_cast(current_points.size()); @@ -212,7 +212,7 @@ void FeatureTracker::readImage(const cv::Mat &pre_processed_img, vector pts_velocity; GetPointVelocty(current_time - previous_frame_time_, cur_un_pts, - prev_un_pts, pts_velocity); + previous_undistorted_pts_, pts_velocity); if (event_observer_) { event_observer_->OnProcessedImage(pre_processed_img, current_time, @@ -222,9 +222,9 @@ void FeatureTracker::readImage(const cv::Mat &pre_processed_img, prev_prune_time_ = current_time; } - prev_un_pts = cur_un_pts; - prev_img_ = pre_processed_img; - prev_pts = current_points; + previous_undistorted_pts_ = cur_un_pts; + previous_pre_processed_image_ = pre_processed_img; + previous_points_ = current_points; previous_frame_time_ = current_time; } @@ -305,7 +305,7 @@ void FeatureTracker::showUndistortion(const string &name) { pp.at(0, 0) + 300 < m_camera->imageWidth() + 600) { undistortedImg.at(pp.at(1, 0) + 300, pp.at(0, 0) + 300) = - prev_img_.at(distortedp[i].y(), distortedp[i].x()); + previous_pre_processed_image_.at(distortedp[i].y(), distortedp[i].x()); } else { // spdlog::error("({0} {1}) -> ({2} {3})", distortedp[i].y, // distortedp[i].x, pp.at(1, 0), pp.at(0, 0)); @@ -341,7 +341,7 @@ std::string FeatureTracker::GenerateStateString() { ss << "REPORTING CURRENT STATE" << "\n\t Previous time is:" << previous_frame_time_ << "\n\t Previous prune time is:" << prev_prune_time_ - << "\n\t Previous number of points is: " << prev_pts.size() - << "\n\t Previous number of undistorted points is: " << prev_un_pts.size(); + << "\n\t Previous number of points is: " << previous_points_.size() + << "\n\t Previous number of undistorted points is: " << previous_undistorted_pts_.size(); return ss.str(); } \ No newline at end of file From bf47ab3ab8ef870f6b694df70f8ac8c902013b0b Mon Sep 17 00:00:00 2001 From: ptan044 Date: Sat, 1 Mar 2025 18:21:26 +0000 Subject: [PATCH 18/37] [34] Minor cleanup --- .../include/feature_tracker/feature_tracker.h | 21 +-- feature_tracker/src/feature_tracker.cpp | 132 +++++++++--------- 2 files changed, 75 insertions(+), 78 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index d82682d34..1f6e4860b 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -39,22 +39,23 @@ class FeatureTracker { void ProcessNewFrame(cv::Mat img, double time_s); private: - void readImage(const cv::Mat &pre_processed_img, double current_time); void RestartTracker(const cv::Mat &pre_processed_img, double current_time); + cv::Mat setMask(vector &curr_pts); - void addPoints(vector &curr_pts, vector &cur_un_pts, + void AddPoints(vector &curr_pts, vector &cur_un_pts, const camodocal::CameraPtr m_camera, const vector &newly_generated_points); - cv::Point2f UndistortPoint(const cv::Point2f point, - const camodocal::CameraPtr camera) const; void PrunePointsUsingRansac(vector &curr_points, vector &curr_un_points, vector &prev_points, vector &prev_un_points, - vector &ids, vector &track_counts); + vector &ids, + vector &track_counts) const; + vector rejectWithF(const vector &cur_un_pts, + const vector &prev_un_pts) const; void DetectNewFeaturePoints(vector ¤t_points, vector ¤t_undistorted_points, const cv::Mat &pre_processed_img, @@ -63,14 +64,14 @@ class FeatureTracker { void showUndistortion(const string &name); - vector rejectWithF(const vector &cur_un_pts, - const vector &prev_un_pts); - void GetPointVelocty(double dt, const vector &cur_un_pts, const vector &prev_un_pts, - vector &pts_velocity_out); + vector &pts_velocity_out) const; + + cv::Point2f UndistortPoint(const cv::Point2f point, + const camodocal::CameraPtr camera) const; - std::string GenerateStateString(); + std::string GenerateStateString() const; cv::Mat fisheye_mask; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index e9a938045..b7885a1ce 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -63,7 +63,6 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, double current_image_time_s) { cv::Mat pre_processed_img; if (run_histogram_equilisation_) { - std::cout << "Running eq" << std::endl; clahe->apply(new_frame, pre_processed_img); } else pre_processed_img = new_frame; @@ -86,7 +85,58 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, return; } - readImage(pre_processed_img, current_image_time_s); + vector current_points; + if (previous_points_.size() > 0) { + vector status; + vector err; + cv::calcOpticalFlowPyrLK(previous_pre_processed_image_, pre_processed_img, + previous_points_, current_points, status, err, + cv::Size(21, 21), 3); + + for (int i = 0; i < int(current_points.size()); i++) + if (status[i] && !inBorder(current_points[i], m_camera->imageWidth(), + m_camera->imageHeight())) + status[i] = 0; + reduceVector(previous_points_, status); + reduceVector(previous_undistorted_pts_, status); + reduceVector(current_points, status); + reduceVector(feature_ids_, status); + reduceVector(track_cnt, status); + } + for (auto &n : track_cnt) n++; + + vector cur_un_pts; + for (unsigned int i = 0; i < current_points.size(); i++) { + cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); + } + // Prune and detect new points + bool is_prune_and_detect_new_points = + current_image_time_s > prev_prune_time_ + feature_pruning_period_; + + if (is_prune_and_detect_new_points) { + PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, + previous_undistorted_pts_, feature_ids_, track_cnt); + int n_max_cnt = + max_feature_count_per_image_ - static_cast(current_points.size()); + DetectNewFeaturePoints(current_points, cur_un_pts, pre_processed_img, + n_max_cnt); + vector pts_velocity; + + GetPointVelocty(current_image_time_s - previous_frame_time_, cur_un_pts, + previous_undistorted_pts_, pts_velocity); + + if (event_observer_) { + event_observer_->OnProcessedImage(pre_processed_img, current_image_time_s, + current_points, cur_un_pts, + feature_ids_, track_cnt, pts_velocity); + } + prev_prune_time_ = current_image_time_s; + } + + previous_undistorted_pts_ = cur_un_pts; + previous_pre_processed_image_ = pre_processed_img; + previous_points_ = current_points; + previous_frame_time_ = current_image_time_s; } void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, @@ -98,8 +148,8 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, previous_undistorted_pts_.clear(); feature_ids_.clear(); track_cnt.clear(); - DetectNewFeaturePoints(previous_points_, previous_undistorted_pts_, pre_processed_img, - max_feature_count_per_image_); + DetectNewFeaturePoints(previous_points_, previous_undistorted_pts_, + pre_processed_img, max_feature_count_per_image_); previous_frame_time_ = current_time; prev_prune_time_ = current_time; @@ -144,7 +194,7 @@ cv::Mat FeatureTracker::setMask(vector &curr_pts) { return mask; } -void FeatureTracker::addPoints( +void FeatureTracker::AddPoints( vector &curr_pts, vector &cur_un_pts, const camodocal::CameraPtr m_camera, const vector &newly_generated_points) { @@ -167,70 +217,14 @@ void FeatureTracker::DetectNewFeaturePoints( cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, n_max_point_to_detect, 0.01, min_distance_between_features_, mask); - addPoints(current_points, current_undistorted_points, m_camera, + AddPoints(current_points, current_undistorted_points, m_camera, newly_generated_points); } } -void FeatureTracker::readImage(const cv::Mat &pre_processed_img, - double current_time) { - vector current_points; - - if (previous_points_.size() > 0) { - vector status; - vector err; - cv::calcOpticalFlowPyrLK(previous_pre_processed_image_, pre_processed_img, previous_points_, - current_points, status, err, cv::Size(21, 21), 3); - - for (int i = 0; i < int(current_points.size()); i++) - if (status[i] && !inBorder(current_points[i], m_camera->imageWidth(), - m_camera->imageHeight())) - status[i] = 0; - reduceVector(previous_points_, status); - reduceVector(previous_undistorted_pts_, status); - reduceVector(current_points, status); - reduceVector(feature_ids_, status); - reduceVector(track_cnt, status); - } - for (auto &n : track_cnt) n++; - - vector cur_un_pts; - - for (unsigned int i = 0; i < current_points.size(); i++) { - cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); - } - // Prune and detect new points - bool is_prune_and_detect_new_points = - current_time > prev_prune_time_ + feature_pruning_period_; - - if (is_prune_and_detect_new_points) { - PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, previous_undistorted_pts_, - feature_ids_, track_cnt); - int n_max_cnt = - max_feature_count_per_image_ - static_cast(current_points.size()); - DetectNewFeaturePoints(current_points, cur_un_pts, pre_processed_img, - n_max_cnt); - vector pts_velocity; - - GetPointVelocty(current_time - previous_frame_time_, cur_un_pts, - previous_undistorted_pts_, pts_velocity); - - if (event_observer_) { - event_observer_->OnProcessedImage(pre_processed_img, current_time, - current_points, cur_un_pts, - feature_ids_, track_cnt, pts_velocity); - } - prev_prune_time_ = current_time; - } - - previous_undistorted_pts_ = cur_un_pts; - previous_pre_processed_image_ = pre_processed_img; - previous_points_ = current_points; - previous_frame_time_ = current_time; -} vector FeatureTracker::rejectWithF( const vector &cur_un_pts, - const vector &prev_un_pts) { + const vector &prev_un_pts) const { vector status; if (cur_un_pts.size() < 8) { return status; @@ -262,7 +256,7 @@ void FeatureTracker::PrunePointsUsingRansac( vector ¤t_undistorted_points, vector &previous_points, vector &previous_undistorted_points, vector &ids, - vector &track_counts) { + vector &track_counts) const { vector status = rejectWithF(current_undistorted_points, previous_undistorted_points); reduceVector(current_points, status); @@ -305,7 +299,8 @@ void FeatureTracker::showUndistortion(const string &name) { pp.at(0, 0) + 300 < m_camera->imageWidth() + 600) { undistortedImg.at(pp.at(1, 0) + 300, pp.at(0, 0) + 300) = - previous_pre_processed_image_.at(distortedp[i].y(), distortedp[i].x()); + previous_pre_processed_image_.at(distortedp[i].y(), + distortedp[i].x()); } else { // spdlog::error("({0} {1}) -> ({2} {3})", distortedp[i].y, // distortedp[i].x, pp.at(1, 0), pp.at(0, 0)); @@ -318,7 +313,7 @@ void FeatureTracker::showUndistortion(const string &name) { void FeatureTracker::GetPointVelocty(double dt, const vector &cur_un_pts, const vector &prev_un_pts, - vector &pts_velocity_out) { + vector &pts_velocity_out) const { pts_velocity_out.clear(); for (unsigned int i = 0; i < prev_un_pts.size(); i++) { @@ -336,12 +331,13 @@ cv::Point2f FeatureTracker::UndistortPoint( return cv::Point2f(b.x() / b.z(), b.y() / b.z()); } -std::string FeatureTracker::GenerateStateString() { +std::string FeatureTracker::GenerateStateString() const { std::stringstream ss; ss << "REPORTING CURRENT STATE" << "\n\t Previous time is:" << previous_frame_time_ << "\n\t Previous prune time is:" << prev_prune_time_ << "\n\t Previous number of points is: " << previous_points_.size() - << "\n\t Previous number of undistorted points is: " << previous_undistorted_pts_.size(); + << "\n\t Previous number of undistorted points is: " + << previous_undistorted_pts_.size(); return ss.str(); } \ No newline at end of file From d69add7666785bcddaf18a2ab30bf1ba8090f279 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 3 Mar 2025 13:00:18 +0000 Subject: [PATCH 19/37] [34] Minor Clean up --- .../include/feature_tracker/feature_tracker.h | 12 +- feature_tracker/src/feature_tracker.cpp | 121 ++++++++---------- 2 files changed, 60 insertions(+), 73 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 1f6e4860b..65430c03d 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -20,11 +20,6 @@ using namespace std; using namespace camodocal; using namespace Eigen; -bool inBorder(const cv::Point2f &pt); - -void reduceVector(vector &v, vector status); -void reduceVector(vector &v, vector status); - class FeatureTracker { public: FeatureTracker(std::string camera_config_file, bool fisheye, @@ -80,16 +75,17 @@ class FeatureTracker { vector previous_undistorted_pts_; vector previous_points_; vector feature_ids_; - vector track_cnt; + vector feature_track_lengh_; double prev_prune_time_; double fx_; double fy_; camodocal::CameraPtr m_camera; - static int n_id; + static int feature_counter_; // Static to ensure unique id between different + // instances - cv::Ptr clahe; + cv::Ptr clahe_; bool fisheye_; bool run_histogram_equilisation_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index b7885a1ce..2e72b94f1 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -4,26 +4,19 @@ #include #include "spdlog/spdlog.h" -int FeatureTracker::n_id = 0; - -bool inBorder(const cv::Point2f &pt, uint col, uint row) { - const int BORDER_SIZE = 1; - int img_x = cvRound(pt.x); - int img_y = cvRound(pt.y); - return BORDER_SIZE <= img_x && img_x < col - BORDER_SIZE && - BORDER_SIZE <= img_y && img_y < row - BORDER_SIZE; -} +int FeatureTracker::feature_counter_ = 0; -void reduceVector(vector &v, vector status) { - int j = 0; - for (int i = 0; i < int(v.size()); i++) - if (status[i]) v[j++] = v[i]; - v.resize(j); +bool inBorder(const cv::Point2f &pt, const uint col, const uint row, + const uint border_size = 1) { + int x = cvRound(pt.x), y = cvRound(pt.y); + return (x >= border_size && x < col - border_size) && + (y >= border_size && y < row - border_size); } -void reduceVector(vector &v, vector status) { +template +void reduceVector(vector &v, const vector status) { int j = 0; - for (int i = 0; i < int(v.size()); i++) + for (size_t i = 0; i < int(v.size()); i++) if (status[i]) v[j++] = v[i]; v.resize(j); } @@ -49,7 +42,7 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, readIntrinsicParameter(camera_config_file); feature_pruning_period_ = 1.0 / feature_pruning_frequency; if (run_histogram_equilisation) { - clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + clahe_ = cv::createCLAHE(3.0, cv::Size(8, 8)); } } @@ -63,7 +56,7 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, double current_image_time_s) { cv::Mat pre_processed_img; if (run_histogram_equilisation_) { - clahe->apply(new_frame, pre_processed_img); + clahe_->apply(new_frame, pre_processed_img); } else pre_processed_img = new_frame; @@ -84,8 +77,10 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, RestartTracker(pre_processed_img, current_image_time_s); return; } - + // Find new feature points by optical flow vector current_points; + vector cur_un_pts; + if (previous_points_.size() > 0) { vector status; vector err; @@ -93,7 +88,7 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, previous_points_, current_points, status, err, cv::Size(21, 21), 3); - for (int i = 0; i < int(current_points.size()); i++) + for (size_t i = 0; i < current_points.size(); i++) if (status[i] && !inBorder(current_points[i], m_camera->imageWidth(), m_camera->imageHeight())) status[i] = 0; @@ -101,34 +96,29 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, reduceVector(previous_undistorted_pts_, status); reduceVector(current_points, status); reduceVector(feature_ids_, status); - reduceVector(track_cnt, status); - } - for (auto &n : track_cnt) n++; + reduceVector(feature_track_lengh_, status); - vector cur_un_pts; - for (unsigned int i = 0; i < current_points.size(); i++) { - cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); - } - // Prune and detect new points - bool is_prune_and_detect_new_points = - current_image_time_s > prev_prune_time_ + feature_pruning_period_; - - if (is_prune_and_detect_new_points) { + for (size_t i = 0; i < current_points.size(); i++) { + cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); + } + for (int &n : feature_track_lengh_) n++; + } // Prune and detect new points + if (current_image_time_s > prev_prune_time_ + feature_pruning_period_) { PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, - previous_undistorted_pts_, feature_ids_, track_cnt); - int n_max_cnt = - max_feature_count_per_image_ - static_cast(current_points.size()); - DetectNewFeaturePoints(current_points, cur_un_pts, pre_processed_img, - n_max_cnt); - vector pts_velocity; + previous_undistorted_pts_, feature_ids_, + feature_track_lengh_); + DetectNewFeaturePoints( + current_points, cur_un_pts, pre_processed_img, + max_feature_count_per_image_ - current_points.size()); + vector pts_velocity; GetPointVelocty(current_image_time_s - previous_frame_time_, cur_un_pts, previous_undistorted_pts_, pts_velocity); if (event_observer_) { - event_observer_->OnProcessedImage(pre_processed_img, current_image_time_s, - current_points, cur_un_pts, - feature_ids_, track_cnt, pts_velocity); + event_observer_->OnProcessedImage( + pre_processed_img, current_image_time_s, current_points, cur_un_pts, + feature_ids_, feature_track_lengh_, pts_velocity); } prev_prune_time_ = current_image_time_s; } @@ -147,14 +137,13 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, previous_points_.clear(); previous_undistorted_pts_.clear(); feature_ids_.clear(); - track_cnt.clear(); + feature_track_lengh_.clear(); DetectNewFeaturePoints(previous_points_, previous_undistorted_pts_, pre_processed_img, max_feature_count_per_image_); previous_frame_time_ = current_time; prev_prune_time_ = current_time; previous_pre_processed_image_ = pre_processed_img; - std::cout << GenerateStateString() << std::endl; return; } @@ -169,9 +158,9 @@ cv::Mat FeatureTracker::setMask(vector &curr_pts) { // prefer to keep features that are tracked for long time vector>> cnt_pts_id; - for (unsigned int i = 0; i < curr_pts.size(); i++) - cnt_pts_id.push_back( - make_pair(track_cnt[i], make_pair(curr_pts[i], feature_ids_[i]))); + for (size_t i = 0; i < curr_pts.size(); i++) + cnt_pts_id.push_back(make_pair(feature_track_lengh_[i], + make_pair(curr_pts[i], feature_ids_[i]))); sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, @@ -181,13 +170,13 @@ cv::Mat FeatureTracker::setMask(vector &curr_pts) { curr_pts.clear(); feature_ids_.clear(); - track_cnt.clear(); + feature_track_lengh_.clear(); for (auto &it : cnt_pts_id) { if (mask.at(it.second.first) == 255) { curr_pts.push_back(it.second.first); feature_ids_.push_back(it.second.second); - track_cnt.push_back(it.first); + feature_track_lengh_.push_back(it.first); cv::circle(mask, it.second.first, min_distance_between_features_, 0, -1); } } @@ -198,12 +187,11 @@ void FeatureTracker::AddPoints( vector &curr_pts, vector &cur_un_pts, const camodocal::CameraPtr m_camera, const vector &newly_generated_points) { - for (auto &p : newly_generated_points) { - int new_point_id = ++n_id; + for (const cv::Point2f &p : newly_generated_points) { curr_pts.push_back(p); cur_un_pts.push_back(UndistortPoint(p, m_camera)); - feature_ids_.push_back(new_point_id); - track_cnt.push_back(1); + feature_ids_.push_back(feature_counter_++); + feature_track_lengh_.push_back(1); } } @@ -230,11 +218,10 @@ vector FeatureTracker::rejectWithF( return status; } - spdlog::debug("FM ransac begins"); vector curr_scaled_undistorted_pts(cur_un_pts.size()), prev_scaled_undistorted_pts(prev_un_pts.size()); - for (unsigned int i = 0; i < prev_un_pts.size(); i++) { + for (size_t i = 0; i < prev_un_pts.size(); i++) { curr_scaled_undistorted_pts[i] = cv::Point2f(fx_ * cur_un_pts[i].x + m_camera->imageWidth() / 2.0, fy_ * cur_un_pts[i].y + m_camera->imageHeight() / 2.0); @@ -275,8 +262,8 @@ void FeatureTracker::showUndistortion(const string &name) { cv::Mat undistortedImg(m_camera->imageHeight() + 600, m_camera->imageWidth() + 600, CV_8UC1, cv::Scalar(0)); vector distortedp, undistortedp; - for (int i = 0; i < m_camera->imageWidth(); i++) - for (int j = 0; j < m_camera->imageHeight(); j++) { + for (size_t i = 0; i < m_camera->imageWidth(); i++) + for (size_t j = 0; j < m_camera->imageHeight(); j++) { Eigen::Vector2d a(i, j); Eigen::Vector3d b; m_camera->liftProjective(a, b); @@ -284,7 +271,7 @@ void FeatureTracker::showUndistortion(const string &name) { undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z())); // printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z()); } - for (int i = 0; i < int(undistortedp.size()); i++) { + for (size_t i = 0; i < undistortedp.size(); i++) { cv::Mat pp(3, 1, CV_32FC1); pp.at(0, 0) = undistortedp[i].x() * fx_ + m_camera->imageWidth() / 2; pp.at(1, 0) = @@ -310,16 +297,20 @@ void FeatureTracker::showUndistortion(const string &name) { cv::waitKey(0); } -void FeatureTracker::GetPointVelocty(double dt, - const vector &cur_un_pts, - const vector &prev_un_pts, - vector &pts_velocity_out) const { +void FeatureTracker::GetPointVelocty( + double dt, const vector &cur_un_pts, + const vector &prev_un_pts, + vector &pts_velocity_out) const { pts_velocity_out.clear(); - for (unsigned int i = 0; i < prev_un_pts.size(); i++) { - cv::Point2f point_velocity((cur_un_pts[i].x - prev_un_pts[i].x) / dt, - (cur_un_pts[i].y - prev_un_pts[i].y) / dt); - pts_velocity_out.push_back(point_velocity); + for (size_t i = 0; i < cur_un_pts.size(); i++) { + if (i < prev_un_pts.size()) { + cv::Point2f point_velocity((cur_un_pts[i].x - prev_un_pts[i].x) / dt, + (cur_un_pts[i].y - prev_un_pts[i].y) / dt); + pts_velocity_out.push_back(point_velocity); + } else { + pts_velocity_out.push_back(cv::Point2f(0, 0)); + } } } From 3bb873aed032173b692c676af66b11bcee2e0bef Mon Sep 17 00:00:00 2001 From: ptan044 Date: Mon, 3 Mar 2025 13:02:15 +0000 Subject: [PATCH 20/37] [34] Remove unused function --- .../include/feature_tracker/feature_tracker.h | 2 - feature_tracker/src/feature_tracker.cpp | 39 ------------------- 2 files changed, 41 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 65430c03d..f683c7faa 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -57,8 +57,6 @@ class FeatureTracker { int n_max_point_to_detect); void readIntrinsicParameter(const string &calib_file); - void showUndistortion(const string &name); - void GetPointVelocty(double dt, const vector &cur_un_pts, const vector &prev_un_pts, vector &pts_velocity_out) const; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 2e72b94f1..1020fc199 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -258,45 +258,6 @@ void FeatureTracker::readIntrinsicParameter(const string &calib_file) { m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); } -void FeatureTracker::showUndistortion(const string &name) { - cv::Mat undistortedImg(m_camera->imageHeight() + 600, - m_camera->imageWidth() + 600, CV_8UC1, cv::Scalar(0)); - vector distortedp, undistortedp; - for (size_t i = 0; i < m_camera->imageWidth(); i++) - for (size_t j = 0; j < m_camera->imageHeight(); j++) { - Eigen::Vector2d a(i, j); - Eigen::Vector3d b; - m_camera->liftProjective(a, b); - distortedp.push_back(a); - undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z())); - // printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z()); - } - for (size_t i = 0; i < undistortedp.size(); i++) { - cv::Mat pp(3, 1, CV_32FC1); - pp.at(0, 0) = undistortedp[i].x() * fx_ + m_camera->imageWidth() / 2; - pp.at(1, 0) = - undistortedp[i].y() * fy_ + m_camera->imageHeight() / 2; - pp.at(2, 0) = 1.0; - // cout << trackerData[0].K << endl; - // printf("%lf %lf\n", p.at(1, 0), p.at(0, 0)); - // printf("%lf %lf\n", pp.at(1, 0), pp.at(0, 0)); - if (pp.at(1, 0) + 300 >= 0 && - pp.at(1, 0) + 300 < m_camera->imageHeight() + 600 && - pp.at(0, 0) + 300 >= 0 && - pp.at(0, 0) + 300 < m_camera->imageWidth() + 600) { - undistortedImg.at(pp.at(1, 0) + 300, - pp.at(0, 0) + 300) = - previous_pre_processed_image_.at(distortedp[i].y(), - distortedp[i].x()); - } else { - // spdlog::error("({0} {1}) -> ({2} {3})", distortedp[i].y, - // distortedp[i].x, pp.at(1, 0), pp.at(0, 0)); - } - } - cv::imshow(name, undistortedImg); - cv::waitKey(0); -} - void FeatureTracker::GetPointVelocty( double dt, const vector &cur_un_pts, const vector &prev_un_pts, From 921d16de9def151817c0d4250c9f72d8972bc83c Mon Sep 17 00:00:00 2001 From: ptan044 Date: Tue, 4 Mar 2025 14:00:06 +0000 Subject: [PATCH 21/37] [34] Simplify mask generation --- .../include/feature_tracker/feature_tracker.h | 10 ++++++---- feature_tracker/src/feature_tracker_observer.cpp | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index f683c7faa..7b95bf5e3 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -36,7 +36,9 @@ class FeatureTracker { private: void RestartTracker(const cv::Mat &pre_processed_img, double current_time); - cv::Mat setMask(vector &curr_pts); + cv::Mat CreateMask(vector &curr_pts, + vector &track_length, + vector &status_out); void AddPoints(vector &curr_pts, vector &cur_un_pts, const camodocal::CameraPtr m_camera, @@ -53,9 +55,9 @@ class FeatureTracker { const vector &prev_un_pts) const; void DetectNewFeaturePoints(vector ¤t_points, vector ¤t_undistorted_points, + const vector &feature_track_length, const cv::Mat &pre_processed_img, int n_max_point_to_detect); - void readIntrinsicParameter(const string &calib_file); void GetPointVelocty(double dt, const vector &cur_un_pts, const vector &prev_un_pts, @@ -66,8 +68,7 @@ class FeatureTracker { std::string GenerateStateString() const; - cv::Mat fisheye_mask; - + cv::Mat base_mask_; cv::Mat previous_pre_processed_image_; double previous_frame_time_; vector previous_undistorted_pts_; @@ -85,6 +86,7 @@ class FeatureTracker { cv::Ptr clahe_; + bool fisheye_; bool run_histogram_equilisation_; uint max_feature_count_per_image_; diff --git a/feature_tracker/src/feature_tracker_observer.cpp b/feature_tracker/src/feature_tracker_observer.cpp index 6879d9855..e193dd6ea 100644 --- a/feature_tracker/src/feature_tracker_observer.cpp +++ b/feature_tracker/src/feature_tracker_observer.cpp @@ -27,7 +27,7 @@ cv::Mat FeatureTrackerObserver::CreateOpticalFlowImage( // std::cout << "velocty: " << points_velocity[i].x << ", " // << points_velocity[i].y << std::endl; cv::Point2f end = - start + (points_velocity[i])*10; // Add velocity to get end point + start + (points_velocity[i])*50; // Add velocity to get end point cv::arrowedLine(show_img, start, end, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); From 8e9460326dc6bb3bdbe5ac3cfa0b07e166eff474 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Tue, 4 Mar 2025 14:00:35 +0000 Subject: [PATCH 22/37] [34]Simplify mask generation --- feature_tracker/src/feature_tracker.cpp | 133 +++++++++++++----------- 1 file changed, 70 insertions(+), 63 deletions(-) diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 1020fc199..5f5624401 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -1,5 +1,6 @@ #include "feature_tracker/feature_tracker.h" +#include #include #include @@ -21,6 +22,14 @@ void reduceVector(vector &v, const vector status) { v.resize(j); } +template +void reduceVector(vector &v, const vector status) { + int j = 0; + for (size_t i = 0; i < int(v.size()); i++) + if (status[i]) v[j++] = v[i]; + v.resize(j); +} + FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, bool run_histogram_equilisation, uint max_feature_count_per_image, @@ -28,8 +37,7 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, double fundemental_matrix_threshold, double fx, double fy, double feature_pruning_frequency, double max_time_difference) - : fisheye_(fisheye), - run_histogram_equilisation_(run_histogram_equilisation), + : run_histogram_equilisation_(run_histogram_equilisation), max_feature_count_per_image_(max_feature_count_per_image), min_distance_between_features_(min_distance_between_features), fundemental_matrix_ransac_threshold_(fundemental_matrix_threshold), @@ -39,11 +47,18 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, max_time_difference_(max_time_difference), previous_frame_time_(0.0), prev_prune_time_(0.0) { - readIntrinsicParameter(camera_config_file); + m_camera = + CameraFactory::instance()->generateCameraFromYamlFile(camera_config_file); feature_pruning_period_ = 1.0 / feature_pruning_frequency; if (run_histogram_equilisation) { clahe_ = cv::createCLAHE(3.0, cv::Size(8, 8)); } + + if (fisheye) std::cout << "!!!FISH EYE NOT WORKING"; + // base_mask_ = fisheye_mask.clone(); + else + base_mask_ = cv::Mat(m_camera->imageHeight(), m_camera->imageWidth(), + CV_8UC1, cv::Scalar(255)); } void FeatureTracker::RegisterEventObserver( @@ -107,9 +122,29 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, previous_undistorted_pts_, feature_ids_, feature_track_lengh_); - DetectNewFeaturePoints( - current_points, cur_un_pts, pre_processed_img, - max_feature_count_per_image_ - current_points.size()); + vector status; + cv::Mat mask = CreateMask(current_points, feature_track_lengh_, status); + + reduceVector(previous_points_, status); + reduceVector(previous_undistorted_pts_, status); + reduceVector(current_points, status); + reduceVector(cur_un_pts, status); + reduceVector(feature_ids_, status); + reduceVector(feature_track_lengh_, status); + int n_max_point_to_detect = + max_feature_count_per_image_ - current_points.size(); + if (n_max_point_to_detect > 0) { + vector newly_generated_points; + cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, + n_max_point_to_detect, 0.01, + min_distance_between_features_, mask); + for (const cv::Point2f &p : newly_generated_points) { + current_points.push_back(p); + cur_un_pts.push_back(UndistortPoint(p, m_camera)); + feature_ids_.push_back(feature_counter_++); + feature_track_lengh_.push_back(1); + } + } vector pts_velocity; GetPointVelocty(current_image_time_s - previous_frame_time_, cur_un_pts, @@ -138,8 +173,17 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, previous_undistorted_pts_.clear(); feature_ids_.clear(); feature_track_lengh_.clear(); - DetectNewFeaturePoints(previous_points_, previous_undistorted_pts_, - pre_processed_img, max_feature_count_per_image_); + + vector newly_generated_points; + cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, + max_feature_count_per_image_, 0.01, + min_distance_between_features_, base_mask_); + for (const cv::Point2f &p : newly_generated_points) { + previous_points_.push_back(p); + previous_undistorted_pts_.push_back(UndistortPoint(p, m_camera)); + feature_ids_.push_back(feature_counter_++); + feature_track_lengh_.push_back(1); + } previous_frame_time_ = current_time; prev_prune_time_ = current_time; @@ -147,68 +191,35 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, return; } -cv::Mat FeatureTracker::setMask(vector &curr_pts) { - cv::Mat mask; - if (fisheye_) - mask = fisheye_mask.clone(); - else - mask = cv::Mat(m_camera->imageHeight(), m_camera->imageWidth(), CV_8UC1, - cv::Scalar(255)); - - // prefer to keep features that are tracked for long time - vector>> cnt_pts_id; +cv::Mat FeatureTracker::CreateMask(vector &curr_pts, + vector &track_length, + vector &status_out) { + cv::Mat mask = base_mask_.clone(); - for (size_t i = 0; i < curr_pts.size(); i++) - cnt_pts_id.push_back(make_pair(feature_track_lengh_[i], - make_pair(curr_pts[i], feature_ids_[i]))); + std::vector indices(track_length.size()); + std::iota(indices.begin(), indices.end(), 0); + std::sort(indices.begin(), indices.end(), + [track_length](int A, int B) -> bool { + return track_length[A] < track_length[B]; + }); - sort(cnt_pts_id.begin(), cnt_pts_id.end(), - [](const pair> &a, - const pair> &b) { - return a.first > b.first; - }); + status_out.assign(curr_pts.size(), false); - curr_pts.clear(); - feature_ids_.clear(); - feature_track_lengh_.clear(); - - for (auto &it : cnt_pts_id) { - if (mask.at(it.second.first) == 255) { - curr_pts.push_back(it.second.first); - feature_ids_.push_back(it.second.second); - feature_track_lengh_.push_back(it.first); - cv::circle(mask, it.second.first, min_distance_between_features_, 0, -1); + for (size_t i : indices) { + if (mask.at(curr_pts[i]) == 255) { + cv::circle(mask, curr_pts[i], min_distance_between_features_, 0, -1); + status_out[i] = true; } } - return mask; -} -void FeatureTracker::AddPoints( - vector &curr_pts, vector &cur_un_pts, - const camodocal::CameraPtr m_camera, - const vector &newly_generated_points) { - for (const cv::Point2f &p : newly_generated_points) { - curr_pts.push_back(p); - cur_un_pts.push_back(UndistortPoint(p, m_camera)); - feature_ids_.push_back(feature_counter_++); - feature_track_lengh_.push_back(1); - } + return mask; } void FeatureTracker::DetectNewFeaturePoints( vector ¤t_points, vector ¤t_undistorted_points, - const cv::Mat &pre_processed_img, int n_max_point_to_detect) { - cv::Mat mask = setMask(current_points); - if (n_max_point_to_detect > 0) { - vector newly_generated_points; - cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, - n_max_point_to_detect, 0.01, - min_distance_between_features_, mask); - AddPoints(current_points, current_undistorted_points, m_camera, - newly_generated_points); - } -} + const vector &feature_track_length, const cv::Mat &pre_processed_img, + int n_max_point_to_detect) {} vector FeatureTracker::rejectWithF( const vector &cur_un_pts, @@ -253,10 +264,6 @@ void FeatureTracker::PrunePointsUsingRansac( reduceVector(ids, status); reduceVector(track_counts, status); } -void FeatureTracker::readIntrinsicParameter(const string &calib_file) { - spdlog::info("reading paramerter of camera {}", calib_file.c_str()); - m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); -} void FeatureTracker::GetPointVelocty( double dt, const vector &cur_un_pts, From 3bf3fb9f1b75aeb44d8029e5b8d3d6bb81167cef Mon Sep 17 00:00:00 2001 From: ptan044 Date: Tue, 4 Mar 2025 14:33:57 +0000 Subject: [PATCH 23/37] [34] Add guard for ransac. Segfaults when n_points<8 --- .../include/feature_tracker/feature_tracker.h | 16 ++++-- feature_tracker/src/feature_tracker.cpp | 55 +++++++++++-------- 2 files changed, 42 insertions(+), 29 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 7b95bf5e3..3bd611048 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -36,13 +36,18 @@ class FeatureTracker { private: void RestartTracker(const cv::Mat &pre_processed_img, double current_time); - cv::Mat CreateMask(vector &curr_pts, - vector &track_length, - vector &status_out); + cv::Mat CreateMask(vector &curr_pts, vector &track_length, + vector &status_out); - void AddPoints(vector &curr_pts, vector &cur_un_pts, + void AddPoints(const cv::Mat image, const cv::Mat mask, + const int max_number_new_of_points, + const int min_distance_between_points, const camodocal::CameraPtr m_camera, - const vector &newly_generated_points); + vector &points, + vector &undistorted_points, + vector &track_length, vector &feature_ids + + ); void PrunePointsUsingRansac(vector &curr_points, vector &curr_un_points, @@ -86,7 +91,6 @@ class FeatureTracker { cv::Ptr clahe_; - bool fisheye_; bool run_histogram_equilisation_; uint max_feature_count_per_image_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 5f5624401..49d1043c5 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -116,34 +116,33 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, for (size_t i = 0; i < current_points.size(); i++) { cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); } + for (int &n : feature_track_lengh_) n++; } // Prune and detect new points if (current_image_time_s > prev_prune_time_ + feature_pruning_period_) { - PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, - previous_undistorted_pts_, feature_ids_, - feature_track_lengh_); + if (current_points.size() > 7) { + PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, + previous_undistorted_pts_, feature_ids_, + feature_track_lengh_); + } else { + std::cout << "!!!Cannot prune because too few points!!! " << std::endl; + } + vector status; cv::Mat mask = CreateMask(current_points, feature_track_lengh_, status); - reduceVector(previous_points_, status); reduceVector(previous_undistorted_pts_, status); reduceVector(current_points, status); reduceVector(cur_un_pts, status); reduceVector(feature_ids_, status); reduceVector(feature_track_lengh_, status); + int n_max_point_to_detect = max_feature_count_per_image_ - current_points.size(); if (n_max_point_to_detect > 0) { - vector newly_generated_points; - cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, - n_max_point_to_detect, 0.01, - min_distance_between_features_, mask); - for (const cv::Point2f &p : newly_generated_points) { - current_points.push_back(p); - cur_un_pts.push_back(UndistortPoint(p, m_camera)); - feature_ids_.push_back(feature_counter_++); - feature_track_lengh_.push_back(1); - } + AddPoints(pre_processed_img, mask, n_max_point_to_detect, + min_distance_between_features_, m_camera, current_points, + cur_un_pts, feature_track_lengh_, feature_ids_); } vector pts_velocity; @@ -175,15 +174,9 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, feature_track_lengh_.clear(); vector newly_generated_points; - cv::goodFeaturesToTrack(pre_processed_img, newly_generated_points, - max_feature_count_per_image_, 0.01, - min_distance_between_features_, base_mask_); - for (const cv::Point2f &p : newly_generated_points) { - previous_points_.push_back(p); - previous_undistorted_pts_.push_back(UndistortPoint(p, m_camera)); - feature_ids_.push_back(feature_counter_++); - feature_track_lengh_.push_back(1); - } + AddPoints(pre_processed_img, base_mask_, max_feature_count_per_image_, + min_distance_between_features_, m_camera, previous_points_, + previous_undistorted_pts_, feature_track_lengh_, feature_ids_); previous_frame_time_ = current_time; prev_prune_time_ = current_time; @@ -282,6 +275,22 @@ void FeatureTracker::GetPointVelocty( } } +void FeatureTracker::AddPoints( + const cv::Mat image, const cv::Mat mask, const int max_number_new_of_points, + const int min_distance_between_points, const camodocal::CameraPtr m_camera, + vector &points, vector &undistorted_points, + vector &track_length, vector &feature_ids) { + vector newly_generated_points; + cv::goodFeaturesToTrack(image, newly_generated_points, + max_number_new_of_points, 0.01, + min_distance_between_points, mask); + for (const cv::Point2f &p : newly_generated_points) { + points.push_back(p); + undistorted_points.push_back(UndistortPoint(p, m_camera)); + feature_ids.push_back(feature_counter_++); + track_length.push_back(1); + } +} cv::Point2f FeatureTracker::UndistortPoint( const cv::Point2f point, const camodocal::CameraPtr camera) const { Eigen::Vector2d a(point.x, point.y); From c366f03e58c3495ea2e45cd1c970f7105f35df69 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Fri, 7 Mar 2025 13:30:26 +0000 Subject: [PATCH 24/37] [34] Minor cleanup --- .../include/feature_tracker/feature_tracker.h | 34 ++-- feature_tracker/src/feature_tracker.cpp | 185 +++++++++--------- 2 files changed, 103 insertions(+), 116 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 3bd611048..2df36fe40 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -31,38 +31,31 @@ class FeatureTracker { double max_time_difference); void RegisterEventObserver( std::shared_ptr event_observer); - void ProcessNewFrame(cv::Mat img, double time_s); + void ProcessNewFrame(const cv::Mat &img, const double time_s); private: void RestartTracker(const cv::Mat &pre_processed_img, double current_time); - cv::Mat CreateMask(vector &curr_pts, vector &track_length, - vector &status_out); - void AddPoints(const cv::Mat image, const cv::Mat mask, const int max_number_new_of_points, const int min_distance_between_points, const camodocal::CameraPtr m_camera, vector &points, vector &undistorted_points, - vector &track_length, vector &feature_ids + vector &track_length, vector &feature_ids); - ); + void PrunePoints(vector &curr_points, + vector &curr_un_points, + vector &prev_points, + vector &prev_un_points, vector &ids, + vector &track_counts, const vector &status); - void PrunePointsUsingRansac(vector &curr_points, - vector &curr_un_points, - vector &prev_points, - vector &prev_un_points, - vector &ids, - vector &track_counts) const; + cv::Mat CreateMask(vector &curr_pts, vector &track_length, + vector &status_out); - vector rejectWithF(const vector &cur_un_pts, - const vector &prev_un_pts) const; - void DetectNewFeaturePoints(vector ¤t_points, - vector ¤t_undistorted_points, - const vector &feature_track_length, - const cv::Mat &pre_processed_img, - int n_max_point_to_detect); + void RejectUsingRansac(const vector &cur_un_pts, + const vector &prev_un_pts, + vector &status_out) const; void GetPointVelocty(double dt, const vector &cur_un_pts, const vector &prev_un_pts, @@ -76,6 +69,7 @@ class FeatureTracker { cv::Mat base_mask_; cv::Mat previous_pre_processed_image_; double previous_frame_time_; + vector previous_undistorted_pts_; vector previous_points_; vector feature_ids_; @@ -84,7 +78,7 @@ class FeatureTracker { double fx_; double fy_; - camodocal::CameraPtr m_camera; + camodocal::CameraPtr camera_model_; static int feature_counter_; // Static to ensure unique id between different // instances diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 49d1043c5..25f18e709 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -47,7 +47,7 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, max_time_difference_(max_time_difference), previous_frame_time_(0.0), prev_prune_time_(0.0) { - m_camera = + camera_model_ = CameraFactory::instance()->generateCameraFromYamlFile(camera_config_file); feature_pruning_period_ = 1.0 / feature_pruning_frequency; if (run_histogram_equilisation) { @@ -57,7 +57,7 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, if (fisheye) std::cout << "!!!FISH EYE NOT WORKING"; // base_mask_ = fisheye_mask.clone(); else - base_mask_ = cv::Mat(m_camera->imageHeight(), m_camera->imageWidth(), + base_mask_ = cv::Mat(camera_model_->imageHeight(), camera_model_->imageWidth(), CV_8UC1, cv::Scalar(255)); } @@ -67,8 +67,8 @@ void FeatureTracker::RegisterEventObserver( event_observer_->OnRegistered(); } -void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, - double current_image_time_s) { +void FeatureTracker::ProcessNewFrame(const cv::Mat &new_frame, + const double current_image_time_s) { cv::Mat pre_processed_img; if (run_histogram_equilisation_) { clahe_->apply(new_frame, pre_processed_img); @@ -103,45 +103,44 @@ void FeatureTracker::ProcessNewFrame(cv::Mat new_frame, previous_points_, current_points, status, err, cv::Size(21, 21), 3); + for (size_t i = 0; i < current_points.size(); i++) { + cur_un_pts.push_back(UndistortPoint(current_points[i], camera_model_)); + } + for (size_t i = 0; i < current_points.size(); i++) - if (status[i] && !inBorder(current_points[i], m_camera->imageWidth(), - m_camera->imageHeight())) + if (status[i] && !inBorder(current_points[i], camera_model_->imageWidth(), + camera_model_->imageHeight())) status[i] = 0; - reduceVector(previous_points_, status); - reduceVector(previous_undistorted_pts_, status); - reduceVector(current_points, status); - reduceVector(feature_ids_, status); - reduceVector(feature_track_lengh_, status); - for (size_t i = 0; i < current_points.size(); i++) { - cur_un_pts.push_back(UndistortPoint(current_points[i], m_camera)); - } + PrunePoints(current_points, cur_un_pts, previous_points_, + previous_undistorted_pts_, feature_track_lengh_, feature_ids_, + status); for (int &n : feature_track_lengh_) n++; - } // Prune and detect new points + } + + // Prune and detect new points if (current_image_time_s > prev_prune_time_ + feature_pruning_period_) { - if (current_points.size() > 7) { - PrunePointsUsingRansac(current_points, cur_un_pts, previous_points_, - previous_undistorted_pts_, feature_ids_, - feature_track_lengh_); - } else { - std::cout << "!!!Cannot prune because too few points!!! " << std::endl; + if (current_points.size() > 8) { + vector ransac_status; + + RejectUsingRansac(cur_un_pts, previous_undistorted_pts_, ransac_status); + PrunePoints(current_points, cur_un_pts, previous_points_, + previous_undistorted_pts_, feature_track_lengh_, feature_ids_, + ransac_status); } - vector status; + vector status; cv::Mat mask = CreateMask(current_points, feature_track_lengh_, status); - reduceVector(previous_points_, status); - reduceVector(previous_undistorted_pts_, status); - reduceVector(current_points, status); - reduceVector(cur_un_pts, status); - reduceVector(feature_ids_, status); - reduceVector(feature_track_lengh_, status); + PrunePoints(current_points, cur_un_pts, previous_points_, + previous_undistorted_pts_, feature_track_lengh_, feature_ids_, + status); int n_max_point_to_detect = max_feature_count_per_image_ - current_points.size(); if (n_max_point_to_detect > 0) { AddPoints(pre_processed_img, mask, n_max_point_to_detect, - min_distance_between_features_, m_camera, current_points, + min_distance_between_features_, camera_model_, current_points, cur_un_pts, feature_track_lengh_, feature_ids_); } @@ -175,7 +174,7 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, vector newly_generated_points; AddPoints(pre_processed_img, base_mask_, max_feature_count_per_image_, - min_distance_between_features_, m_camera, previous_points_, + min_distance_between_features_, camera_model_, previous_points_, previous_undistorted_pts_, feature_track_lengh_, feature_ids_); previous_frame_time_ = current_time; @@ -184,9 +183,40 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, return; } +void FeatureTracker::AddPoints( + const cv::Mat image, const cv::Mat mask, const int max_number_new_of_points, + const int min_distance_between_points, const camodocal::CameraPtr m_camera, + vector &points, vector &undistorted_points, + vector &track_length, vector &feature_ids) { + vector newly_generated_points; + cv::goodFeaturesToTrack(image, newly_generated_points, + max_number_new_of_points, 0.01, + min_distance_between_points, mask); + for (const cv::Point2f &p : newly_generated_points) { + points.push_back(p); + undistorted_points.push_back(UndistortPoint(p, m_camera)); + feature_ids.push_back(feature_counter_++); + track_length.push_back(1); + } +} + +void FeatureTracker::PrunePoints(vector &curr_points, + vector &curr_un_points, + vector &prev_points, + vector &prev_un_points, + vector &ids, vector &track_counts, + const vector &status) { + reduceVector(curr_points, status); + reduceVector(curr_un_points, status); + reduceVector(prev_points, status); + reduceVector(prev_un_points, status); + reduceVector(ids, status); + reduceVector(track_counts, status); +} + cv::Mat FeatureTracker::CreateMask(vector &curr_pts, vector &track_length, - vector &status_out) { + vector &status_out) { cv::Mat mask = base_mask_.clone(); std::vector indices(track_length.size()); @@ -196,66 +226,45 @@ cv::Mat FeatureTracker::CreateMask(vector &curr_pts, return track_length[A] < track_length[B]; }); - status_out.assign(curr_pts.size(), false); + status_out.assign(curr_pts.size(), 0); for (size_t i : indices) { if (mask.at(curr_pts[i]) == 255) { cv::circle(mask, curr_pts[i], min_distance_between_features_, 0, -1); - status_out[i] = true; + status_out[i] = 1; } } return mask; } -void FeatureTracker::DetectNewFeaturePoints( - vector ¤t_points, - vector ¤t_undistorted_points, - const vector &feature_track_length, const cv::Mat &pre_processed_img, - int n_max_point_to_detect) {} - -vector FeatureTracker::rejectWithF( - const vector &cur_un_pts, - const vector &prev_un_pts) const { - vector status; - if (cur_un_pts.size() < 8) { - return status; - } - - vector curr_scaled_undistorted_pts(cur_un_pts.size()), - prev_scaled_undistorted_pts(prev_un_pts.size()); - - for (size_t i = 0; i < prev_un_pts.size(); i++) { - curr_scaled_undistorted_pts[i] = - cv::Point2f(fx_ * cur_un_pts[i].x + m_camera->imageWidth() / 2.0, - fy_ * cur_un_pts[i].y + m_camera->imageHeight() / 2.0); +void FeatureTracker::RejectUsingRansac( + const vector ¤t_undistorted_points, + const vector &previous_undistorted_points, + vector &status_out) const { + status_out.clear(); + size_t n_points = current_undistorted_points.size(); + + if (n_points > 7) { + vector curr_scaled_undistorted_pts(n_points), + prev_scaled_undistorted_pts(n_points); + + for (size_t i = 0; i < n_points; i++) { + curr_scaled_undistorted_pts[i] = cv::Point2f( + fx_ * current_undistorted_points[i].x + camera_model_->imageWidth() / 2.0, + fy_ * current_undistorted_points[i].y + + camera_model_->imageHeight() / 2.0); + + prev_scaled_undistorted_pts[i] = cv::Point2f( + fx_ * previous_undistorted_points[i].x + camera_model_->imageWidth() / 2.0, + fy_ * previous_undistorted_points[i].y + + camera_model_->imageHeight() / 2.0); + } - prev_scaled_undistorted_pts[i] = - cv::Point2f(fx_ * prev_un_pts[i].x + m_camera->imageWidth() / 2.0, - fy_ * prev_un_pts[i].y + m_camera->imageHeight() / 2.0); + cv::findFundamentalMat( + prev_scaled_undistorted_pts, curr_scaled_undistorted_pts, cv::FM_RANSAC, + fundemental_matrix_ransac_threshold_, 0.99, status_out); } - - cv::findFundamentalMat(prev_scaled_undistorted_pts, - curr_scaled_undistorted_pts, cv::FM_RANSAC, - fundemental_matrix_ransac_threshold_, 0.99, status); - - return status; -} - -void FeatureTracker::PrunePointsUsingRansac( - vector ¤t_points, - vector ¤t_undistorted_points, - vector &previous_points, - vector &previous_undistorted_points, vector &ids, - vector &track_counts) const { - vector status = - rejectWithF(current_undistorted_points, previous_undistorted_points); - reduceVector(current_points, status); - reduceVector(current_undistorted_points, status); - reduceVector(previous_points, status); - reduceVector(previous_undistorted_points, status); - reduceVector(ids, status); - reduceVector(track_counts, status); } void FeatureTracker::GetPointVelocty( @@ -275,27 +284,11 @@ void FeatureTracker::GetPointVelocty( } } -void FeatureTracker::AddPoints( - const cv::Mat image, const cv::Mat mask, const int max_number_new_of_points, - const int min_distance_between_points, const camodocal::CameraPtr m_camera, - vector &points, vector &undistorted_points, - vector &track_length, vector &feature_ids) { - vector newly_generated_points; - cv::goodFeaturesToTrack(image, newly_generated_points, - max_number_new_of_points, 0.01, - min_distance_between_points, mask); - for (const cv::Point2f &p : newly_generated_points) { - points.push_back(p); - undistorted_points.push_back(UndistortPoint(p, m_camera)); - feature_ids.push_back(feature_counter_++); - track_length.push_back(1); - } -} cv::Point2f FeatureTracker::UndistortPoint( const cv::Point2f point, const camodocal::CameraPtr camera) const { Eigen::Vector2d a(point.x, point.y); Eigen::Vector3d b; - m_camera->liftProjective(a, b); + camera->liftProjective(a, b); return cv::Point2f(b.x() / b.z(), b.y() / b.z()); } From 379af29314ea49aeca0ed0aa767bea413e04fad9 Mon Sep 17 00:00:00 2001 From: ptan044 Date: Fri, 7 Mar 2025 13:34:19 +0000 Subject: [PATCH 25/37] [34] Remove using namespace std --- .../include/feature_tracker/feature_tracker.h | 43 ++++----- feature_tracker/src/feature_tracker.cpp | 88 +++++++++---------- 2 files changed, 63 insertions(+), 68 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index 2df36fe40..e6292a605 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -16,7 +16,6 @@ #include "feature_tracker/parameters.h" #include "feature_tracker/tic_toc.h" -using namespace std; using namespace camodocal; using namespace Eigen; @@ -40,26 +39,28 @@ class FeatureTracker { const int max_number_new_of_points, const int min_distance_between_points, const camodocal::CameraPtr m_camera, - vector &points, - vector &undistorted_points, - vector &track_length, vector &feature_ids); + std::vector &points, + std::vector &undistorted_points, + std::vector &track_length, std::vector &feature_ids); - void PrunePoints(vector &curr_points, - vector &curr_un_points, - vector &prev_points, - vector &prev_un_points, vector &ids, - vector &track_counts, const vector &status); + void PrunePoints(std::vector &curr_points, + std::vector &curr_un_points, + std::vector &prev_points, + std::vector &prev_un_points, + std::vector &ids, std::vector &track_counts, + const std::vector &status); - cv::Mat CreateMask(vector &curr_pts, vector &track_length, - vector &status_out); + cv::Mat CreateMask(std::vector &curr_pts, + std::vector &track_length, + std::vector &status_out); - void RejectUsingRansac(const vector &cur_un_pts, - const vector &prev_un_pts, - vector &status_out) const; + void RejectUsingRansac(const std::vector &cur_un_pts, + const std::vector &prev_un_pts, + std::vector &status_out) const; - void GetPointVelocty(double dt, const vector &cur_un_pts, - const vector &prev_un_pts, - vector &pts_velocity_out) const; + void GetPointVelocty(double dt, const std::vector &cur_un_pts, + const std::vector &prev_un_pts, + std::vector &pts_velocity_out) const; cv::Point2f UndistortPoint(const cv::Point2f point, const camodocal::CameraPtr camera) const; @@ -70,10 +71,10 @@ class FeatureTracker { cv::Mat previous_pre_processed_image_; double previous_frame_time_; - vector previous_undistorted_pts_; - vector previous_points_; - vector feature_ids_; - vector feature_track_lengh_; + std::vector previous_undistorted_pts_; + std::vector previous_points_; + std::vector feature_ids_; + std::vector feature_track_lengh_; double prev_prune_time_; double fx_; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 25f18e709..061dac2b7 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -15,15 +15,7 @@ bool inBorder(const cv::Point2f &pt, const uint col, const uint row, } template -void reduceVector(vector &v, const vector status) { - int j = 0; - for (size_t i = 0; i < int(v.size()); i++) - if (status[i]) v[j++] = v[i]; - v.resize(j); -} - -template -void reduceVector(vector &v, const vector status) { +void reduceVector(std::vector &v, const std::vector status) { int j = 0; for (size_t i = 0; i < int(v.size()); i++) if (status[i]) v[j++] = v[i]; @@ -57,8 +49,8 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, if (fisheye) std::cout << "!!!FISH EYE NOT WORKING"; // base_mask_ = fisheye_mask.clone(); else - base_mask_ = cv::Mat(camera_model_->imageHeight(), camera_model_->imageWidth(), - CV_8UC1, cv::Scalar(255)); + base_mask_ = cv::Mat(camera_model_->imageHeight(), + camera_model_->imageWidth(), CV_8UC1, cv::Scalar(255)); } void FeatureTracker::RegisterEventObserver( @@ -93,12 +85,12 @@ void FeatureTracker::ProcessNewFrame(const cv::Mat &new_frame, return; } // Find new feature points by optical flow - vector current_points; - vector cur_un_pts; + std::vector current_points; + std::vector cur_un_pts; if (previous_points_.size() > 0) { - vector status; - vector err; + std::vector status; + std::vector err; cv::calcOpticalFlowPyrLK(previous_pre_processed_image_, pre_processed_img, previous_points_, current_points, status, err, cv::Size(21, 21), 3); @@ -122,7 +114,7 @@ void FeatureTracker::ProcessNewFrame(const cv::Mat &new_frame, // Prune and detect new points if (current_image_time_s > prev_prune_time_ + feature_pruning_period_) { if (current_points.size() > 8) { - vector ransac_status; + std::vector ransac_status; RejectUsingRansac(cur_un_pts, previous_undistorted_pts_, ransac_status); PrunePoints(current_points, cur_un_pts, previous_points_, @@ -130,7 +122,7 @@ void FeatureTracker::ProcessNewFrame(const cv::Mat &new_frame, ransac_status); } - vector status; + std::vector status; cv::Mat mask = CreateMask(current_points, feature_track_lengh_, status); PrunePoints(current_points, cur_un_pts, previous_points_, previous_undistorted_pts_, feature_track_lengh_, feature_ids_, @@ -144,7 +136,7 @@ void FeatureTracker::ProcessNewFrame(const cv::Mat &new_frame, cur_un_pts, feature_track_lengh_, feature_ids_); } - vector pts_velocity; + std::vector pts_velocity; GetPointVelocty(current_image_time_s - previous_frame_time_, cur_un_pts, previous_undistorted_pts_, pts_velocity); @@ -172,7 +164,7 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, feature_ids_.clear(); feature_track_lengh_.clear(); - vector newly_generated_points; + std::vector newly_generated_points; AddPoints(pre_processed_img, base_mask_, max_feature_count_per_image_, min_distance_between_features_, camera_model_, previous_points_, previous_undistorted_pts_, feature_track_lengh_, feature_ids_); @@ -186,9 +178,9 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, void FeatureTracker::AddPoints( const cv::Mat image, const cv::Mat mask, const int max_number_new_of_points, const int min_distance_between_points, const camodocal::CameraPtr m_camera, - vector &points, vector &undistorted_points, - vector &track_length, vector &feature_ids) { - vector newly_generated_points; + std::vector &points, std::vector &undistorted_points, + std::vector &track_length, std::vector &feature_ids) { + std::vector newly_generated_points; cv::goodFeaturesToTrack(image, newly_generated_points, max_number_new_of_points, 0.01, min_distance_between_points, mask); @@ -200,12 +192,12 @@ void FeatureTracker::AddPoints( } } -void FeatureTracker::PrunePoints(vector &curr_points, - vector &curr_un_points, - vector &prev_points, - vector &prev_un_points, - vector &ids, vector &track_counts, - const vector &status) { +void FeatureTracker::PrunePoints(std::vector &curr_points, + std::vector &curr_un_points, + std::vector &prev_points, + std::vector &prev_un_points, + std::vector &ids, std::vector &track_counts, + const std::vector &status) { reduceVector(curr_points, status); reduceVector(curr_un_points, status); reduceVector(prev_points, status); @@ -214,9 +206,9 @@ void FeatureTracker::PrunePoints(vector &curr_points, reduceVector(track_counts, status); } -cv::Mat FeatureTracker::CreateMask(vector &curr_pts, - vector &track_length, - vector &status_out) { +cv::Mat FeatureTracker::CreateMask(std::vector &curr_pts, + std::vector &track_length, + std::vector &status_out) { cv::Mat mask = base_mask_.clone(); std::vector indices(track_length.size()); @@ -239,26 +231,28 @@ cv::Mat FeatureTracker::CreateMask(vector &curr_pts, } void FeatureTracker::RejectUsingRansac( - const vector ¤t_undistorted_points, - const vector &previous_undistorted_points, - vector &status_out) const { + const std::vector ¤t_undistorted_points, + const std::vector &previous_undistorted_points, + std::vector &status_out) const { status_out.clear(); size_t n_points = current_undistorted_points.size(); if (n_points > 7) { - vector curr_scaled_undistorted_pts(n_points), + std::vector curr_scaled_undistorted_pts(n_points), prev_scaled_undistorted_pts(n_points); for (size_t i = 0; i < n_points; i++) { - curr_scaled_undistorted_pts[i] = cv::Point2f( - fx_ * current_undistorted_points[i].x + camera_model_->imageWidth() / 2.0, - fy_ * current_undistorted_points[i].y + - camera_model_->imageHeight() / 2.0); - - prev_scaled_undistorted_pts[i] = cv::Point2f( - fx_ * previous_undistorted_points[i].x + camera_model_->imageWidth() / 2.0, - fy_ * previous_undistorted_points[i].y + - camera_model_->imageHeight() / 2.0); + curr_scaled_undistorted_pts[i] = + cv::Point2f(fx_ * current_undistorted_points[i].x + + camera_model_->imageWidth() / 2.0, + fy_ * current_undistorted_points[i].y + + camera_model_->imageHeight() / 2.0); + + prev_scaled_undistorted_pts[i] = + cv::Point2f(fx_ * previous_undistorted_points[i].x + + camera_model_->imageWidth() / 2.0, + fy_ * previous_undistorted_points[i].y + + camera_model_->imageHeight() / 2.0); } cv::findFundamentalMat( @@ -268,9 +262,9 @@ void FeatureTracker::RejectUsingRansac( } void FeatureTracker::GetPointVelocty( - double dt, const vector &cur_un_pts, - const vector &prev_un_pts, - vector &pts_velocity_out) const { + double dt, const std::vector &cur_un_pts, + const std::vector &prev_un_pts, + std::vector &pts_velocity_out) const { pts_velocity_out.clear(); for (size_t i = 0; i < cur_un_pts.size(); i++) { From 829af89fd6db4c753ea8d700bf91d28dacf86b47 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 07:39:02 +0000 Subject: [PATCH 26/37] [34] Remove unused files --- .../include/feature_tracker/parameters.h | 21 ------------------- .../include/feature_tracker/tic_toc.h | 21 ------------------- 2 files changed, 42 deletions(-) delete mode 100644 feature_tracker/include/feature_tracker/parameters.h delete mode 100644 feature_tracker/include/feature_tracker/tic_toc.h diff --git a/feature_tracker/include/feature_tracker/parameters.h b/feature_tracker/include/feature_tracker/parameters.h deleted file mode 100644 index e855aacea..000000000 --- a/feature_tracker/include/feature_tracker/parameters.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -extern int ROW; -extern int COL; -extern int FOCAL_LENGTH; -const int NUM_OF_CAM = 1; - -extern std::string IMAGE_TOPIC; -extern std::string IMU_TOPIC; -extern std::string FISHEYE_MASK; -extern std::vector CAM_NAMES; -extern int MAX_CNT; -extern int MIN_DIST; -extern int WINDOW_SIZE; -extern int FREQ; -extern double F_THRESHOLD; -extern int SHOW_TRACK; -extern int STEREO_TRACK; -extern int EQUALIZE; -extern int FISHEYE; -extern bool PUB_THIS_FRAME; \ No newline at end of file diff --git a/feature_tracker/include/feature_tracker/tic_toc.h b/feature_tracker/include/feature_tracker/tic_toc.h deleted file mode 100644 index 176ea15e0..000000000 --- a/feature_tracker/include/feature_tracker/tic_toc.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include -#include -#include - -class TicToc { - public: - TicToc() { tic(); } - - void tic() { start = std::chrono::system_clock::now(); } - - double toc() { - end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - start; - return elapsed_seconds.count() * 1000; - } - - private: - std::chrono::time_point start, end; -}; From f6771b00fccc3b85075b2ae1035511acb9e5b523 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 07:44:39 +0000 Subject: [PATCH 27/37] [34] Remove unused imports and using namespace --- .../include/feature_tracker/feature_tracker.h | 23 ++++++------------- feature_tracker/src/feature_tracker.cpp | 20 ++++++++++------ 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.h index e6292a605..532592636 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.h @@ -1,23 +1,9 @@ -#pragma once +#ifndef FEATURE_TRACKER_H +#define FEATURE_TRACKER_H -#include - -#include -#include -#include -#include #include -#include - #include "camodocal/camera_models/CameraFactory.h" -#include "camodocal/camera_models/CataCamera.h" -#include "camodocal/camera_models/PinholeCamera.h" #include "feature_tracker/feature_tracker_observer.hpp" -#include "feature_tracker/parameters.h" -#include "feature_tracker/tic_toc.h" - -using namespace camodocal; -using namespace Eigen; class FeatureTracker { public: @@ -28,8 +14,10 @@ class FeatureTracker { double fundemental_matrix_ransac_threshold, double fx, double fy, double feature_pruning_frequency, double max_time_difference); + void RegisterEventObserver( std::shared_ptr event_observer); + void ProcessNewFrame(const cv::Mat &img, const double time_s); private: @@ -97,3 +85,6 @@ class FeatureTracker { double feature_pruning_period_; std::shared_ptr event_observer_; }; + + +#endif /* FEATURE_TRACKER_H */ diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 061dac2b7..05fdd0690 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -40,7 +40,8 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, previous_frame_time_(0.0), prev_prune_time_(0.0) { camera_model_ = - CameraFactory::instance()->generateCameraFromYamlFile(camera_config_file); + camodocal::CameraFactory::instance()->generateCameraFromYamlFile( + camera_config_file); feature_pruning_period_ = 1.0 / feature_pruning_frequency; if (run_histogram_equilisation) { clahe_ = cv::createCLAHE(3.0, cv::Size(8, 8)); @@ -51,6 +52,7 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, else base_mask_ = cv::Mat(camera_model_->imageHeight(), camera_model_->imageWidth(), CV_8UC1, cv::Scalar(255)); + } void FeatureTracker::RegisterEventObserver( @@ -175,11 +177,14 @@ void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, return; } -void FeatureTracker::AddPoints( - const cv::Mat image, const cv::Mat mask, const int max_number_new_of_points, - const int min_distance_between_points, const camodocal::CameraPtr m_camera, - std::vector &points, std::vector &undistorted_points, - std::vector &track_length, std::vector &feature_ids) { +void FeatureTracker::AddPoints(const cv::Mat image, const cv::Mat mask, + const int max_number_new_of_points, + const int min_distance_between_points, + const camodocal::CameraPtr m_camera, + std::vector &points, + std::vector &undistorted_points, + std::vector &track_length, + std::vector &feature_ids) { std::vector newly_generated_points; cv::goodFeaturesToTrack(image, newly_generated_points, max_number_new_of_points, 0.01, @@ -196,7 +201,8 @@ void FeatureTracker::PrunePoints(std::vector &curr_points, std::vector &curr_un_points, std::vector &prev_points, std::vector &prev_un_points, - std::vector &ids, std::vector &track_counts, + std::vector &ids, + std::vector &track_counts, const std::vector &status) { reduceVector(curr_points, status); reduceVector(curr_un_points, status); From 743099f1159a020b13c6b95e4f5b42cf88e8c77e Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 07:45:28 +0000 Subject: [PATCH 28/37] [34] Create seperated config files --- config/euroc/camera_config.yaml | 47 +++++++++++++++++++++++ config/euroc/feature_tracker_configs.yaml | 7 ++++ feature_tracker/src/main.cpp | 5 ++- 3 files changed, 57 insertions(+), 2 deletions(-) create mode 100644 config/euroc/camera_config.yaml create mode 100644 config/euroc/feature_tracker_configs.yaml diff --git a/config/euroc/camera_config.yaml b/config/euroc/camera_config.yaml new file mode 100644 index 000000000..09d7b2ac0 --- /dev/null +++ b/config/euroc/camera_config.yaml @@ -0,0 +1,47 @@ +%YAML:1.0 + +#camera calibration +model_type: PINHOLE +camera_name: camera +image_width: 752 +image_height: 480 +distortion_parameters: + k1: -2.917e-01 + k2: 8.228e-02 + p1: 5.333e-05 + p2: -1.578e-04 +projection_parameters: + fx: 4.616e+02 + fy: 4.603e+02 + cx: 3.630e+02 + cy: 2.481e+02 + +# Extrinsic parameter between IMU and Camera. +estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. + # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. + # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning. +#If you choose 0 or 1, you should write down the following matrix. +#Rotation from camera frame to imu frame, imu^R_cam +extrinsicRotation: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.0148655429818, -0.999880929698, 0.00414029679422, + 0.999557249008, 0.0149672133247, 0.025715529948, + -0.0257744366974, 0.00375618835797, 0.999660727178] +#Translation from camera frame to imu frame, imu^T_cam +extrinsicTranslation: !!opencv-matrix + rows: 3 + cols: 1 + dt: d + data: [-0.0216401454975,-0.064676986768, 0.00981073058949] + +#feature traker paprameters +max_cnt: 150 # max feature number in feature tracking +min_dist: 30 # min distance between two features +freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image +F_threshold: 1.0 # ransac threshold (pixel) +show_track: 1 # publish tracking image as topic +equalize: 1 # if image is too dark or light, trun on equalize to find enough features +fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points + diff --git a/config/euroc/feature_tracker_configs.yaml b/config/euroc/feature_tracker_configs.yaml new file mode 100644 index 000000000..7b4c1c1a8 --- /dev/null +++ b/config/euroc/feature_tracker_configs.yaml @@ -0,0 +1,7 @@ +#feature traker paprameters +max_cnt: 150 # max feature number in feature tracking +min_dist: 30 # min distance between two features +freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image +F_threshold: 1.0 # ransac threshold (pixel) +equalize: true # if image is too dark or light, trun on equalize to find enough features +fisheye: false # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points diff --git a/feature_tracker/src/main.cpp b/feature_tracker/src/main.cpp index ec74b4cee..0ca77f8f9 100644 --- a/feature_tracker/src/main.cpp +++ b/feature_tracker/src/main.cpp @@ -12,8 +12,8 @@ int main(int argc, char** argv) { FeatureTracker feat( "/home/rosdev/workspace/ros_ws/src/VINS-Mono/config/euroc/" - "euroc_config.yaml", - false, true, 150, 30, 1.0, 460, 460, 10,1.0); + "camera_config.yaml", + false, true, 150, 30, 1.0, 460, 460, 10, 1.0); std::shared_ptr observer = std::make_shared(); @@ -36,6 +36,7 @@ int main(int argc, char** argv) { std::string line; double time_covnersion = pow(10.0, 9); // Read the CSV file line by line + while (std::getline(tsFile, line)) { std::stringstream ss(line); double timestamp; From 83319003f7e692f69f148782703f7608d34c4c62 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 07:45:44 +0000 Subject: [PATCH 29/37] [34] Update rerun observer to actually use rerun --- .../feature_tracker_observer_spdlog_rerun.hpp | 3 ++ .../feature_tracker_observer_spdlog_rerun.cpp | 37 +++++++++++++++++-- 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp b/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp index 1a6af0de4..31531213a 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp @@ -1,12 +1,15 @@ #ifndef FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP #define FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP +#include #include "feature_tracker/feature_tracker_observer.hpp" class FeatureTrackerObserverSPDRerun : public FeatureTrackerObserver { public: // FeatureTrackerObserverSPDRerun() = default; ~FeatureTrackerObserverSPDRerun(); +// const auto rec = rerun::RecordingStream("rerun_example_image_formats"); private: + std::unique_ptr recorder_; void OnRegistered() final; void OnRestart() final; diff --git a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp index 8273aca0e..bc8dc3a5a 100644 --- a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp +++ b/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp @@ -1,6 +1,9 @@ #include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" +// #include #include "spdlog/spdlog.h" +#include + // FeatureTrackerObserverSPDRerun::FeatureTrackerObserverSPDRerun() { // spdlog::info("FeatureTracker observer created"); // } @@ -11,6 +14,9 @@ FeatureTrackerObserverSPDRerun::~FeatureTrackerObserverSPDRerun() { void FeatureTrackerObserverSPDRerun::OnRegistered() { spdlog::set_level(spdlog::level::debug); spdlog::info("FeatureTracker observer Registered"); + recorder_ = std::make_unique("TEST"); + recorder_->spawn().exit_on_failure(); + // rec = rerun::RecordingStream("rerun_example_image"); } void FeatureTrackerObserverSPDRerun::OnRestart() { @@ -38,8 +44,33 @@ void FeatureTrackerObserverSPDRerun::OnProcessedImage( spdlog::info("Features have been pruned and new features added"); cv::Mat img = CreateOpticalFlowImage(new_frame, features, track_count, 20, points_velocity); - cv::imshow("Image", img); + recorder_->log("Optical Flow Image", + rerun::Image::from_rgb24(img, {img.cols, img.rows})); // Wait for 100 milliseconds before moving to the next image - cv::waitKey(100); -} \ No newline at end of file +} + + +// Adapters so we can borrow an OpenCV image easily into Rerun images without copying: +template <> +struct rerun::CollectionAdapter { + /// Borrow for non-temporary. + Collection operator()(const cv::Mat& img) { + assert( + "OpenCV matrix was expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U + ); + + return Collection::borrow(img.data, img.total() * img.channels()); + } + + // Do a full copy for temporaries (otherwise the data might be deleted when the temporary is destroyed). + Collection operator()(cv::Mat&& img) { + assert( + "OpenCV matrix was expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U + ); + + std::vector img_vec(img.total() * img.channels()); + img_vec.assign(img.data, img.data + img.total() * img.channels()); + return Collection::take_ownership(std::move(img_vec)); + } +}; \ No newline at end of file From 3c2e38d3fe11256039455bba37cb76ab714356b6 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 07:57:17 +0000 Subject: [PATCH 30/37] [34] Seperate rerun observer and main to seperate package --- feature_tracker/CMakeLists.txt | 16 ++--- feature_tracker_rerun/CMakeLists.txt | 72 +++++++++++++++++++ .../feature_tracker_rerunConfig.cmake.in | 17 +++++ .../feature_tracker_observer_spdlog_rerun.hpp | 0 .../feature_tracker_observer_spdlog_rerun.cpp | 0 .../src/main.cpp | 0 6 files changed, 94 insertions(+), 11 deletions(-) create mode 100644 feature_tracker_rerun/CMakeLists.txt create mode 100644 feature_tracker_rerun/cmake/feature_tracker_rerunConfig.cmake.in rename {feature_tracker => feature_tracker_rerun}/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp (100%) rename {feature_tracker => feature_tracker_rerun}/src/feature_tracker_observer_spdlog_rerun.cpp (100%) rename {feature_tracker => feature_tracker_rerun}/src/main.cpp (100%) diff --git a/feature_tracker/CMakeLists.txt b/feature_tracker/CMakeLists.txt index b3152bd60..3990fb023 100644 --- a/feature_tracker/CMakeLists.txt +++ b/feature_tracker/CMakeLists.txt @@ -5,24 +5,18 @@ set(CMAKE_CXX_FLAGS "-std=c++17") find_package(OpenCV REQUIRED) find_package(camodocal REQUIRED) find_package(Eigen3 REQUIRED) -find_package(spdlog REQUIRED) # Define your library target -add_library(feature_tracker STATIC src/feature_tracker.cpp src/feature_tracker_observer.cpp src/feature_tracker_observer_spdlog_rerun.cpp) -target_link_libraries(feature_tracker PRIVATE ${OpenCV_LIBS} ${CERES_LIBRARIES} spdlog::spdlog_header_only camodocal::camodocal ) +add_library(feature_tracker STATIC src/feature_tracker.cpp src/feature_tracker_observer.cpp) +target_link_libraries(feature_tracker PRIVATE ${OpenCV_LIBS} ${CERES_LIBRARIES} camodocal::camodocal) # Set include directories target_include_directories(feature_tracker PUBLIC - $ - $ + $ + $ ) -add_executable(feature_tracker_run - src/main.cpp - - ) -target_link_libraries(feature_tracker_run feature_tracker ${OpenCV_LIBS} spdlog::spdlog_header_only) # Install the library include(GNUInstallDirs) install(TARGETS feature_tracker @@ -64,4 +58,4 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/feature_trackerConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/feature_trackerConfigVersion.cmake DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/feature_tracker -) \ No newline at end of file +) diff --git a/feature_tracker_rerun/CMakeLists.txt b/feature_tracker_rerun/CMakeLists.txt new file mode 100644 index 000000000..b1fc6e7c2 --- /dev/null +++ b/feature_tracker_rerun/CMakeLists.txt @@ -0,0 +1,72 @@ +cmake_minimum_required(VERSION 3.0) +project(feature_tracker_rerun VERSION 1.0) +set(CMAKE_CXX_FLAGS "-std=c++17") +include(FetchContent) +FetchContent_Declare(rerun_sdk URL + https://github.com/rerun-io/rerun/releases/latest/download/rerun_cpp_sdk.zip) +FetchContent_MakeAvailable(rerun_sdk) + +find_package(OpenCV REQUIRED) +find_package(feature_tracker REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) + + +# Define your library target +add_library(feature_tracker_rerun STATIC src/feature_tracker_observer_spdlog_rerun.cpp) +target_link_libraries(feature_tracker_rerun PRIVATE ${OpenCV_LIBS} spdlog::spdlog_header_only rerun_sdk feature_tracker::feature_tracker camodocal::camodocal) + +# Set include directories +target_include_directories(feature_tracker_rerun + PUBLIC + $ + $ +) + +add_executable(feature_tracker_run + src/main.cpp + +) +target_link_libraries(feature_tracker_run feature_tracker_rerun feature_tracker ${OpenCV_LIBS}) +# Install the library +include(GNUInstallDirs) +install(TARGETS feature_tracker_rerun + EXPORT feature_tracker_rerunTargets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +# Install headers +install(DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +# Generate and install CMake package configuration files +install(EXPORT feature_tracker_rerunTargets + FILE feature_tracker_rerunTargets.cmake + NAMESPACE feature_tracker_rerun:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/feature_tracker_rerun +) + + +# Generate and install CMake package configuration files +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/feature_tracker_rerunConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion +) + +configure_package_config_file( + ${CMAKE_CURRENT_LIST_DIR}/cmake/feature_tracker_rerunConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/feature_tracker_rerunConfig.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/feature_tracker_rerun +) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/feature_tracker_rerunConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/feature_tracker_rerunConfigVersion.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/feature_tracker_rerun +) diff --git a/feature_tracker_rerun/cmake/feature_tracker_rerunConfig.cmake.in b/feature_tracker_rerun/cmake/feature_tracker_rerunConfig.cmake.in new file mode 100644 index 000000000..c15235c22 --- /dev/null +++ b/feature_tracker_rerun/cmake/feature_tracker_rerunConfig.cmake.in @@ -0,0 +1,17 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/feature_tracker_rerunTargets.cmake") + +check_required_components(feature_tracker_rerun) + +# Provide the target `feature_tracker_rerun::feature_tracker_rerun` +# Alias for convenience +if(NOT TARGET feature_tracker_rerun::feature_tracker_rerun) + add_library(feature_tracker_rerun::feature_tracker_rerun INTERFACE IMPORTED) + set_target_properties(feature_tracker_rerun::feature_tracker_rerun PROPERTIES + INTERFACE_LINK_LIBRARIES feature_tracker_rerun::feature_tracker_rerun + ) +endif() +include(CMakeFindDependencyMacro) +find_dependency(camodocal REQUIRED) +find_dependency(spdlog REQUIRED) \ No newline at end of file diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp b/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp similarity index 100% rename from feature_tracker/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp rename to feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp diff --git a/feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp b/feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp similarity index 100% rename from feature_tracker/src/feature_tracker_observer_spdlog_rerun.cpp rename to feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp diff --git a/feature_tracker/src/main.cpp b/feature_tracker_rerun/src/main.cpp similarity index 100% rename from feature_tracker/src/main.cpp rename to feature_tracker_rerun/src/main.cpp From dbc6d28f9bff40560e4da9144f443ac43d494d46 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 08:05:10 +0000 Subject: [PATCH 31/37] [34] Rename feature tracker header and remove spdlog --- feature_tracker/cmake/feature_trackerConfig.cmake.in | 1 - .../{feature_tracker.h => feature_tracker.hpp} | 6 +++--- feature_tracker/src/feature_tracker.cpp | 3 +-- 3 files changed, 4 insertions(+), 6 deletions(-) rename feature_tracker/include/feature_tracker/{feature_tracker.h => feature_tracker.hpp} (97%) diff --git a/feature_tracker/cmake/feature_trackerConfig.cmake.in b/feature_tracker/cmake/feature_trackerConfig.cmake.in index 4426b986a..24e601f6e 100644 --- a/feature_tracker/cmake/feature_trackerConfig.cmake.in +++ b/feature_tracker/cmake/feature_trackerConfig.cmake.in @@ -14,4 +14,3 @@ if(NOT TARGET feature_tracker::feature_tracker) endif() include(CMakeFindDependencyMacro) find_dependency(camodocal REQUIRED) -find_dependency(spdlog REQUIRED) \ No newline at end of file diff --git a/feature_tracker/include/feature_tracker/feature_tracker.h b/feature_tracker/include/feature_tracker/feature_tracker.hpp similarity index 97% rename from feature_tracker/include/feature_tracker/feature_tracker.h rename to feature_tracker/include/feature_tracker/feature_tracker.hpp index 532592636..cb890b0d6 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ b/feature_tracker/include/feature_tracker/feature_tracker.hpp @@ -1,5 +1,5 @@ -#ifndef FEATURE_TRACKER_H -#define FEATURE_TRACKER_H +#ifndef FEATURE_TRACKER_HPP +#define FEATURE_TRACKER_HPP #include #include "camodocal/camera_models/CameraFactory.h" @@ -87,4 +87,4 @@ class FeatureTracker { }; -#endif /* FEATURE_TRACKER_H */ +#endif /* FEATURE_TRACKER_HPP */ diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 05fdd0690..98c89e46e 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -1,10 +1,9 @@ -#include "feature_tracker/feature_tracker.h" +#include "feature_tracker/feature_tracker.hpp" #include #include #include -#include "spdlog/spdlog.h" int FeatureTracker::feature_counter_ = 0; bool inBorder(const cv::Point2f &pt, const uint col, const uint row, From 2c5eb289c6803c052d24b04b31463a4ee114efac Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 08:30:50 +0000 Subject: [PATCH 32/37] [34] Add additional logging to rerun --- .../feature_tracker_observer.hpp | 9 ++- feature_tracker/src/feature_tracker.cpp | 8 ++- .../feature_tracker_observer_spdlog_rerun.hpp | 9 ++- .../feature_tracker_observer_spdlog_rerun.cpp | 70 ++++++++++++------- 4 files changed, 67 insertions(+), 29 deletions(-) diff --git a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp index 48e0b42be..e0e70957f 100644 --- a/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp @@ -16,7 +16,14 @@ class FeatureTrackerObserver double previous_image_time_s) = 0; virtual void OnImageTimeMovingBackwards(double current_image_time_s, double previous_image_time_s) = 0; - virtual void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, + + virtual void OnImageRecieved(const cv::Mat &new_frame, + double current_image_time_s) = 0; + virtual void OnHistogramEqualisation(const cv::Mat &new_frame, + double current_image_time_s) = 0; + + virtual void OnProcessedImage(const cv::Mat &new_frame, + double current_image_time_s, std::vector features, std::vector undistorted_features, diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 98c89e46e..96f8c858d 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -51,7 +51,6 @@ FeatureTracker::FeatureTracker(std::string camera_config_file, bool fisheye, else base_mask_ = cv::Mat(camera_model_->imageHeight(), camera_model_->imageWidth(), CV_8UC1, cv::Scalar(255)); - } void FeatureTracker::RegisterEventObserver( @@ -63,8 +62,15 @@ void FeatureTracker::RegisterEventObserver( void FeatureTracker::ProcessNewFrame(const cv::Mat &new_frame, const double current_image_time_s) { cv::Mat pre_processed_img; + if (event_observer_) { + event_observer_->OnImageRecieved(new_frame, current_image_time_s); + } if (run_histogram_equilisation_) { clahe_->apply(new_frame, pre_processed_img); + if (event_observer_) { + event_observer_->OnHistogramEqualisation(pre_processed_img, + current_image_time_s); + } } else pre_processed_img = new_frame; diff --git a/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp b/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp index 31531213a..c5f1caa9b 100644 --- a/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp +++ b/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp @@ -1,12 +1,13 @@ #ifndef FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP #define FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP #include + #include "feature_tracker/feature_tracker_observer.hpp" class FeatureTrackerObserverSPDRerun : public FeatureTrackerObserver { public: // FeatureTrackerObserverSPDRerun() = default; ~FeatureTrackerObserverSPDRerun(); -// const auto rec = rerun::RecordingStream("rerun_example_image_formats"); + // const auto rec = rerun::RecordingStream("rerun_example_image_formats"); private: std::unique_ptr recorder_; @@ -17,12 +18,16 @@ class FeatureTrackerObserverSPDRerun : public FeatureTrackerObserver { double previous_image_time_s) final; void OnImageTimeMovingBackwards(double current_image_time_s, double previous_image_time_s) final; - void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, + void OnProcessedImage(const cv::Mat &new_frame, double current_image_time_s, std::vector features, std::vector undistorted_features, std::vector ids, std::vector track_count, std::vector points_velocity) final; + void OnImageRecieved(const cv::Mat &new_frame, + double current_image_time_s) final; + void OnHistogramEqualisation(const cv::Mat &new_frame, + double current_image_time_s) final; }; #endif /* FEATURE_TRACKER_OBSERVER_SPDLOG_RERUN_HPP */ diff --git a/feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp b/feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp index bc8dc3a5a..08bdac03f 100644 --- a/feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp +++ b/feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp @@ -1,9 +1,10 @@ #include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" // #include -#include "spdlog/spdlog.h" #include +#include "spdlog/spdlog.h" + // FeatureTrackerObserverSPDRerun::FeatureTrackerObserverSPDRerun() { // spdlog::info("FeatureTracker observer created"); // } @@ -12,9 +13,9 @@ FeatureTrackerObserverSPDRerun::~FeatureTrackerObserverSPDRerun() { spdlog::info("Destroying Observer"); } void FeatureTrackerObserverSPDRerun::OnRegistered() { - spdlog::set_level(spdlog::level::debug); + spdlog::set_level(spdlog::level::info); spdlog::info("FeatureTracker observer Registered"); - recorder_ = std::make_unique("TEST"); + recorder_ = std::make_unique("Feature Tracker"); recorder_->spawn().exit_on_failure(); // rec = rerun::RecordingStream("rerun_example_image"); } @@ -33,44 +34,63 @@ void FeatureTrackerObserverSPDRerun::OnImageTimeMovingBackwards( double current_image_time_s, double previous_image_time_s) { spdlog::info("Timestamp for current image is before previous image"); } +void FeatureTrackerObserverSPDRerun::OnImageRecieved( + const cv::Mat& new_frame, double current_image_time_s) { + cv::Mat show_img; + cv::cvtColor(new_frame, show_img, cv::COLOR_GRAY2RGB); + recorder_->log("1. Raw Image", + rerun::Image::from_rgb24( + show_img, {static_cast(show_img.cols), + static_cast(show_img.rows)})); +} +void FeatureTrackerObserverSPDRerun::OnHistogramEqualisation( + const cv::Mat& new_frame, double current_image_time_s) { + cv::Mat show_img; + cv::cvtColor(new_frame, show_img, cv::COLOR_GRAY2RGB); + recorder_->log("2. Histogram Equalised", + rerun::Image::from_rgb24( + show_img, {static_cast(show_img.cols), + static_cast(show_img.rows)})); +} void FeatureTrackerObserverSPDRerun::OnProcessedImage( - cv::Mat new_frame, double current_image_time_s, + const cv::Mat& new_frame, double current_image_time_s, std::vector features, std::vector undistorted_features, std::vector ids, std::vector track_count, std::vector points_velocity) { - spdlog::info("Features have been pruned and new features added"); + spdlog::debug("Features have been pruned and new features added"); cv::Mat img = CreateOpticalFlowImage(new_frame, features, track_count, 20, points_velocity); - recorder_->log("Optical Flow Image", - rerun::Image::from_rgb24(img, {img.cols, img.rows})); + recorder_->log( + "3. Optical Flow Image", + rerun::Image::from_rgb24(img, {static_cast(img.cols), + static_cast(img.rows)})); // Wait for 100 milliseconds before moving to the next image } - -// Adapters so we can borrow an OpenCV image easily into Rerun images without copying: +// Adapters so we can borrow an OpenCV image easily into Rerun images without +// copying: template <> struct rerun::CollectionAdapter { - /// Borrow for non-temporary. - Collection operator()(const cv::Mat& img) { - assert( - "OpenCV matrix was expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U - ); + /// Borrow for non-temporary. + Collection operator()(const cv::Mat& img) { + assert("OpenCV matrix was expected have bit depth CV_U8" && + CV_MAT_DEPTH(img.type()) == CV_8U); - return Collection::borrow(img.data, img.total() * img.channels()); - } + return Collection::borrow(img.data, img.total() * img.channels()); + } - // Do a full copy for temporaries (otherwise the data might be deleted when the temporary is destroyed). - Collection operator()(cv::Mat&& img) { - assert( - "OpenCV matrix was expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U - ); + // Do a full copy for temporaries (otherwise the data might be deleted when + // the temporary is destroyed). + Collection operator()(cv::Mat&& img) { + assert("OpenCV matrix was expected have bit depth CV_U8" && + CV_MAT_DEPTH(img.type()) == CV_8U); - std::vector img_vec(img.total() * img.channels()); - img_vec.assign(img.data, img.data + img.total() * img.channels()); - return Collection::take_ownership(std::move(img_vec)); - } + std::vector img_vec(img.total() * img.channels()); + img_vec.assign(img.data, img.data + img.total() * img.channels()); + return Collection::take_ownership(std::move(img_vec)); + } }; \ No newline at end of file From 6035c2b1a1d81409bc1547a7607fd439c8ae74f5 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 08:33:14 +0000 Subject: [PATCH 33/37] [34] Cleanup main --- feature_tracker_rerun/src/main.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/feature_tracker_rerun/src/main.cpp b/feature_tracker_rerun/src/main.cpp index 0ca77f8f9..35808ef54 100644 --- a/feature_tracker_rerun/src/main.cpp +++ b/feature_tracker_rerun/src/main.cpp @@ -6,7 +6,7 @@ #include #include -#include "feature_tracker/feature_tracker.h" +#include "feature_tracker/feature_tracker.hpp" #include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" #include "spdlog/spdlog.h" int main(int argc, char** argv) { @@ -19,8 +19,7 @@ int main(int argc, char** argv) { // FeatureTrackerObserverSPDRerun observer = FeatureTrackerObserverSPDRerun(); feat.RegisterEventObserver(observer); - std::cout << "HI" << std::endl; - + // Path to your dataset std::string folderPath = "/home/rosdev/workspace/data/MH_01_easy"; std::string imageFolder = folderPath + "/mav0/cam0/data"; From 8dc5693a6dd35cc4608d8f4c6311416b76845b95 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 10:27:41 +0000 Subject: [PATCH 34/37] [34] Update feature tracker demo to use config file --- feature_tracker_rerun/CMakeLists.txt | 4 +- feature_tracker_rerun/src/main.cpp | 113 ++++++++++++++------------- 2 files changed, 59 insertions(+), 58 deletions(-) diff --git a/feature_tracker_rerun/CMakeLists.txt b/feature_tracker_rerun/CMakeLists.txt index b1fc6e7c2..1042931e2 100644 --- a/feature_tracker_rerun/CMakeLists.txt +++ b/feature_tracker_rerun/CMakeLists.txt @@ -10,11 +10,11 @@ find_package(OpenCV REQUIRED) find_package(feature_tracker REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) - +find_package(yaml-cpp REQUIRED) # Define your library target add_library(feature_tracker_rerun STATIC src/feature_tracker_observer_spdlog_rerun.cpp) -target_link_libraries(feature_tracker_rerun PRIVATE ${OpenCV_LIBS} spdlog::spdlog_header_only rerun_sdk feature_tracker::feature_tracker camodocal::camodocal) +target_link_libraries(feature_tracker_rerun PRIVATE ${OpenCV_LIBS} spdlog::spdlog_header_only rerun_sdk feature_tracker::feature_tracker camodocal::camodocal yaml-cpp) # Set include directories target_include_directories(feature_tracker_rerun diff --git a/feature_tracker_rerun/src/main.cpp b/feature_tracker_rerun/src/main.cpp index 35808ef54..d025baf09 100644 --- a/feature_tracker_rerun/src/main.cpp +++ b/feature_tracker_rerun/src/main.cpp @@ -1,5 +1,8 @@ +#include // Ensure you have yaml-cpp installed and linked +#include + #include -#include // For file existence check +#include #include #include #include @@ -9,97 +12,95 @@ #include "feature_tracker/feature_tracker.hpp" #include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" #include "spdlog/spdlog.h" + int main(int argc, char** argv) { - FeatureTracker feat( + // Default values for file paths + std::string default_feature_tracker_config = + "/home/rosdev/workspace/ros_ws/src/VINS-Mono/config/euroc/" + "feature_tracker_configs.yaml"; + std::string default_camera_config = "/home/rosdev/workspace/ros_ws/src/VINS-Mono/config/euroc/" - "camera_config.yaml", - false, true, 150, 30, 1.0, 460, 460, 10, 1.0); + "camera_config.yaml"; + std::string default_test_data_set_images = + "/home/rosdev/workspace/data/MH_01_easy/mav0/cam0/data"; + std::string default_test_data_set_csv = + "/home/rosdev/workspace/data/MH_01_easy/mav0/cam0/data.csv"; + // Parse command-line arguments + + std::string feature_tracker_config_file = + (argc > 1) ? argv[1] : default_feature_tracker_config; + std::string camera_config_file = (argc > 2) ? argv[2] : default_camera_config; + std::string image_folder = + (argc > 3) ? argv[4] : default_test_data_set_images; + std::string timestamp_file = (argc > 4) ? argv[4] : default_test_data_set_csv; + + std::cout << "Using the following paths:" + << "\n\tFeature Tracker Config: " << feature_tracker_config_file + << "\n\tCamera Config: " << camera_config_file + << "\n\tImage Folder: " << image_folder + << "\n\tTimestamp File: " << timestamp_file << std::endl; + + // Load parameters from YAML file + YAML::Node config = YAML::LoadFile(default_feature_tracker_config); + if (!config) { + std::cerr << "Failed to load config file: " << camera_config_file + << std::endl; + return 1; + } + + // Extract parameters + bool use_equalize = config["equalize"].as(); + bool use_fisheye = config["fisheye"].as(); + int max_features = config["max_cnt"].as(); + int min_distance = config["min_dist"].as(); + double ransac_threshold = config["F_threshold"].as(); + double freq = config["freq"].as(); + + FeatureTracker feat(camera_config_file, use_fisheye, use_equalize, + max_features, min_distance, ransac_threshold, 460, 460, + freq, ransac_threshold); + std::shared_ptr observer = std::make_shared(); - - // FeatureTrackerObserverSPDRerun observer = FeatureTrackerObserverSPDRerun(); feat.RegisterEventObserver(observer); - - // Path to your dataset - std::string folderPath = "/home/rosdev/workspace/data/MH_01_easy"; - std::string imageFolder = folderPath + "/mav0/cam0/data"; - std::string timestampFile = folderPath + "/mav0/cam0/data.csv"; // Open the timestamp file - std::ifstream tsFile(timestampFile); + std::ifstream tsFile(timestamp_file); if (!tsFile.is_open()) { - std::cerr << "Error opening timestamp file: " << timestampFile << std::endl; + std::cerr << "Error opening timestamp file: " << timestamp_file + << std::endl; return 1; } std::string line; - double time_covnersion = pow(10.0, 9); - // Read the CSV file line by line + double time_conversion = pow(10.0, 9); while (std::getline(tsFile, line)) { std::stringstream ss(line); double timestamp; std::string imageFilename; - // Read the timestamp ss >> timestamp; - - // Skip the comma after the timestamp ss.ignore(1, ','); - - // Read the filename (after the comma) std::getline(ss, imageFilename); - - // Trim leading/trailing spaces from filename imageFilename.erase(0, imageFilename.find_first_not_of(" \t\r\n")); imageFilename.erase(imageFilename.find_last_not_of(" \t\r\n") + 1); - // Strip out any potential newline or carriage return characters - imageFilename.erase( - std::remove(imageFilename.begin(), imageFilename.end(), '\n'), - imageFilename.end()); - imageFilename.erase( - std::remove(imageFilename.begin(), imageFilename.end(), '\r'), - imageFilename.end()); - - // Construct the full image path - std::string imagePath = imageFolder + "/" + imageFilename; - - // Debug: Print the image path - - // Check if the file exists + std::string imagePath = image_folder + "/" + imageFilename; if (!std::filesystem::exists(imagePath)) { std::cerr << "File does not exist: " << imagePath << std::endl; continue; } - // Read the image - cv::Mat image = - cv::imread(imagePath, cv::IMREAD_GRAYSCALE); // Read as color image - - // Check if image was loaded successfully + cv::Mat image = cv::imread(imagePath, cv::IMREAD_GRAYSCALE); if (image.empty()) { std::cerr << "Error loading image: " << imagePath << std::endl; continue; } - // Display the image - - feat.ProcessNewFrame(image, timestamp / time_covnersion); - // cv::imshow("Image", image); - - // Wait for 100 milliseconds before moving to the next image - // if (cv::waitKey(1) == 'q') { // Press 'q' to quit - // break; - // } - - // Optionally, print the timestamp to the console (or use it for - // synchronization) + feat.ProcessNewFrame(image, timestamp / time_conversion); } - // Close the file and cleanup tsFile.close(); - cv::destroyAllWindows(); - return 0; } From 48b1e3b5f608e0ab4859477d50eb0113a0e87aa4 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 11:15:41 +0000 Subject: [PATCH 35/37] [34] Fix feature_tracker_ros to run with entire vins estimator --- .../feature_tracker_node.hpp | 8 +++-- .../include/ros_parameter_reader.h | 6 ---- .../launch/feature_tracker.launch | 15 ++++++--- .../src/feature_tracker_node.cpp | 31 +++++++++++++++++-- vins_estimator/launch/euroc.launch | 14 ++++----- 5 files changed, 52 insertions(+), 22 deletions(-) delete mode 100644 feature_tracker_ros/include/ros_parameter_reader.h diff --git a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp index 64cd28523..34114b819 100644 --- a/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp +++ b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp @@ -7,7 +7,7 @@ #include #include -#include "feature_tracker/feature_tracker.h" +#include "feature_tracker/feature_tracker.hpp" #include "feature_tracker/feature_tracker_observer.hpp" class FeatureTrackerNode : public FeatureTrackerObserver { @@ -23,13 +23,17 @@ class FeatureTrackerNode : public FeatureTrackerObserver { double previous_image_time_s) final; void OnImageTimeMovingBackwards(double current_image_time_s, double previous_image_time_s) final; - void OnProcessedImage(cv::Mat new_frame, double current_image_time_s, + void OnProcessedImage(const cv::Mat& new_frame, double current_image_time_s, std::vector features, std::vector undistorted_features, std::vector ids, std::vector track_count, std::vector points_velocity) final; + void OnImageRecieved(const cv::Mat& new_frame, + double current_image_time_s) final; + void OnHistogramEqualisation(const cv::Mat& new_frame, + double current_image_time_s) final; bool ReadParameters(std::string& configFilePath, int& maxFeatureCount, int& minFeatureDistance, int& pruningFrequency, double& ransacThreshold, diff --git a/feature_tracker_ros/include/ros_parameter_reader.h b/feature_tracker_ros/include/ros_parameter_reader.h deleted file mode 100644 index 4ac1a282c..000000000 --- a/feature_tracker_ros/include/ros_parameter_reader.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once -#include -#include "feature_tracker/parameters.h" -#include - -void readParameters(ros::NodeHandle &n); \ No newline at end of file diff --git a/feature_tracker_ros/launch/feature_tracker.launch b/feature_tracker_ros/launch/feature_tracker.launch index a9f49e52c..8fc3029d9 100644 --- a/feature_tracker_ros/launch/feature_tracker.launch +++ b/feature_tracker_ros/launch/feature_tracker.launch @@ -1,7 +1,14 @@ - - - + + + + + + - + \ No newline at end of file diff --git a/feature_tracker_ros/src/feature_tracker_node.cpp b/feature_tracker_ros/src/feature_tracker_node.cpp index 8194380f7..222d8f875 100644 --- a/feature_tracker_ros/src/feature_tracker_node.cpp +++ b/feature_tracker_ros/src/feature_tracker_node.cpp @@ -25,6 +25,8 @@ void FeatureTrackerNode::ImageCallback( bool FeatureTrackerNode::Start() { restart_flag_.data = true; + // TODO Make this from a param + min_track_count_to_publish_ = 1; feature_id_channel_.name = "Feature Ids"; feature_u_pixel_channel_.name = "Feature U Pixel"; @@ -32,6 +34,7 @@ bool FeatureTrackerNode::Start() { feature_x_velocity_channel_.name = "Feature X Velocity"; feature_y_velocity_channel_.name = "Feature Y Velocity"; + // TODO 1: Try to make this pass in pointer instead. feature_points_msg_.channels.push_back(feature_id_channel_); feature_points_msg_.channels.push_back(feature_u_pixel_channel_); feature_points_msg_.channels.push_back(feature_v_pixel_channel_); @@ -40,7 +43,8 @@ bool FeatureTrackerNode::Start() { first_image_ = false; - // Set this upon first camera message received + // TODO 2: Add frame for this during first image. + // TODO 2: because no one consumes this. optical_flow_img_.header.frame_id = "TODO"; optical_flow_img_.encoding = sensor_msgs::image_encodings::BGR8; @@ -58,6 +62,10 @@ bool FeatureTrackerNode::Start() { } // Initialize FeatureTracker with parameters + + // TODO 3: Set the fx,fy from ros param. + // Consider obtaining from camera params, however, depending on model there + // might not be a fx,fy feature_tracker_ = std::make_unique( config_file_path, use_fisheye, enable_histogram_equalization, max_feature_count, min_feature_distance, ransac_threshold, @@ -141,7 +149,7 @@ void FeatureTrackerNode::OnImageTimeMovingBackwards( << std::setprecision(2) << previous_image_time_s); } void FeatureTrackerNode::OnProcessedImage( - cv::Mat new_frame, double current_image_time_s, + const cv::Mat& new_frame, double current_image_time_s, std::vector features, std::vector undistorted_features, std::vector ids, std::vector track_count, std::vector points_velocity) { @@ -169,6 +177,14 @@ void FeatureTrackerNode::OnProcessedImage( feature_y_velocity_channel_.values.push_back(points_velocity[i].y); } } + // TODO 1: Try to make this pass in pointer instead. So that we dont need to + // clear and pushback again + feature_points_msg_.channels.clear(); + feature_points_msg_.channels.push_back(feature_id_channel_); + feature_points_msg_.channels.push_back(feature_u_pixel_channel_); + feature_points_msg_.channels.push_back(feature_v_pixel_channel_); + feature_points_msg_.channels.push_back(feature_x_velocity_channel_); + feature_points_msg_.channels.push_back(feature_y_velocity_channel_); optical_flow_img_.header.stamp = current_image_time_; optical_flow_img_.image = CreateOpticalFlowImage( @@ -178,6 +194,17 @@ void FeatureTrackerNode::OnProcessedImage( feature_point_cloud_publisher_.publish(feature_points_msg_); } +void FeatureTrackerNode::OnImageRecieved(const cv::Mat& new_frame, + double current_image_time_s) { + ROS_DEBUG_STREAM("Received Image with timestamp: " << current_image_time_s + << "s"); +}; +void FeatureTrackerNode::OnHistogramEqualisation(const cv::Mat& new_frame, + double current_image_time_s) { + ROS_DEBUG_STREAM("Performed Histogram equalisation on image with timestamp: " + << current_image_time_s << "s"); +}; + int main(int argc, char** argv) { ros::init(argc, argv, "feature_tracker_node"); std::shared_ptr feature_tracker_node = diff --git a/vins_estimator/launch/euroc.launch b/vins_estimator/launch/euroc.launch index 54f2eff78..56e0c9320 100644 --- a/vins_estimator/launch/euroc.launch +++ b/vins_estimator/launch/euroc.launch @@ -1,11 +1,9 @@ - - - - - - - + + + + + @@ -20,4 +18,4 @@ - + \ No newline at end of file From 4ca00f73da28c965c7a7cfb7f227c0ece84ea4b8 Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 11:16:04 +0000 Subject: [PATCH 36/37] [34] Delete unused ros_parameter_reader --- feature_tracker_ros/CMakeLists.txt | 2 +- .../src/ros_parameter_reader.cpp | 64 ------------------- 2 files changed, 1 insertion(+), 65 deletions(-) delete mode 100644 feature_tracker_ros/src/ros_parameter_reader.cpp diff --git a/feature_tracker_ros/CMakeLists.txt b/feature_tracker_ros/CMakeLists.txt index bf5b90be9..1e714126c 100644 --- a/feature_tracker_ros/CMakeLists.txt +++ b/feature_tracker_ros/CMakeLists.txt @@ -25,7 +25,7 @@ include_directories( catkin_package() add_executable(feature_tracker_node - src/feature_tracker_node.cpp src/ros_parameter_reader.cpp + src/feature_tracker_node.cpp ) target_link_libraries(feature_tracker_node ${catkin_LIBRARIES} ${OpenCV_LIBS} feature_tracker::feature_tracker) diff --git a/feature_tracker_ros/src/ros_parameter_reader.cpp b/feature_tracker_ros/src/ros_parameter_reader.cpp deleted file mode 100644 index 5a2161aaa..000000000 --- a/feature_tracker_ros/src/ros_parameter_reader.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "ros_parameter_reader.h" - -std::string IMAGE_TOPIC; -std::string IMU_TOPIC; -std::vector CAM_NAMES; -std::string FISHEYE_MASK; -int MAX_CNT; -int MIN_DIST; -int WINDOW_SIZE; -int FREQ; -double F_THRESHOLD; -int SHOW_TRACK; -int STEREO_TRACK; -int EQUALIZE; -int ROW; -int COL; -int FOCAL_LENGTH; -int FISHEYE; -bool PUB_THIS_FRAME; - -template -T readParam(ros::NodeHandle &n, std::string name) { - T ans; - if (n.getParam(name, ans)) { - ROS_INFO_STREAM("Loaded " << name << ": " << ans); - } else { - ROS_ERROR_STREAM("Failed to load " << name); - n.shutdown(); - } - return ans; -} - -void readParameters(ros::NodeHandle &n) { - std::string config_file; - config_file = readParam(n, "config_file"); - cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); - if (!fsSettings.isOpened()) { - std::cerr << "ERROR: Wrong path to settings" << std::endl; - } - std::string VINS_FOLDER_PATH = readParam(n, "vins_folder"); - - fsSettings["image_topic"] >> IMAGE_TOPIC; - fsSettings["imu_topic"] >> IMU_TOPIC; - MAX_CNT = fsSettings["max_cnt"]; - MIN_DIST = fsSettings["min_dist"]; - ROW = fsSettings["image_height"]; - COL = fsSettings["image_width"]; - FREQ = fsSettings["freq"]; - F_THRESHOLD = fsSettings["F_threshold"]; - SHOW_TRACK = fsSettings["show_track"]; - EQUALIZE = fsSettings["equalize"]; - FISHEYE = fsSettings["fisheye"]; - if (FISHEYE == 1) FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"; - CAM_NAMES.push_back(config_file); - - WINDOW_SIZE = 20; - STEREO_TRACK = false; - FOCAL_LENGTH = 460; - PUB_THIS_FRAME = false; - - if (FREQ == 0) FREQ = 100; - - fsSettings.release(); -} From 97a92fa01f267bd80265e5f53a0a79fe16a83b9f Mon Sep 17 00:00:00 2001 From: TanPinDa Date: Sat, 8 Mar 2025 11:30:09 +0000 Subject: [PATCH 37/37] [34] Add dependencies for rerun --- install-deps.sh | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/install-deps.sh b/install-deps.sh index 1d82dbfd6..7aefaaf83 100644 --- a/install-deps.sh +++ b/install-deps.sh @@ -9,6 +9,29 @@ sudo apt install -y --no-install-recommends git libgoogle-glog-dev \ libeigen3-dev \ libsuitesparse-dev + +# FOR RERUN +sudo apt-get -y install \ + libclang-dev \ + libatk-bridge2.0 \ + libfontconfig1-dev \ + libfreetype6-dev \ + libglib2.0-dev \ + libgtk-3-dev \ + libssl-dev \ + libxcb-render0-dev \ + libxcb-shape0-dev \ + libxcb-xfixes0-dev \ + libxkbcommon-dev \ + patchelf + +sudo add-apt-repository ppa:kisak/kisak-mesa +sudo apt-get update +sudo apt-get install -y mesa-vulkan-drivers +sudo pip3 install rerun-sdk +ENV XDG_RUNTIME_DIR=$XDG_RUNTIME_DIR +# END FOR RERUN + mkdir ./deps cd ./deps