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/CMakeLists.txt b/feature_tracker/CMakeLists.txt index e085e4e37..3990fb023 100644 --- a/feature_tracker/CMakeLists.txt +++ b/feature_tracker/CMakeLists.txt @@ -5,17 +5,16 @@ 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) -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 - $ - $ + $ + $ ) # Install the library @@ -59,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/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.h deleted file mode 100644 index ca5687d4a..000000000 --- a/feature_tracker/include/feature_tracker/feature_tracker.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once - -#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/parameters.h" -#include "feature_tracker/tic_toc.h" - -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(); - - void readImage(const cv::Mat &_img, double _cur_time); - - void setMask(); - - void addPoints(); - - bool updateID(unsigned int i); - - void readIntrinsicParameter(const string &calib_file); - - void showUndistortion(const string &name); - - void rejectWithF(); - - void undistortedPoints(); - - cv::Mat mask; - cv::Mat fisheye_mask; - cv::Mat prev_img, cur_img, forw_img; - vector n_pts; - vector prev_pts, cur_pts, forw_pts; - vector prev_un_pts, cur_un_pts; - vector pts_velocity; - vector ids; - vector track_cnt; - map cur_un_pts_map; - map prev_un_pts_map; - camodocal::CameraPtr m_camera; - double cur_time; - double prev_time; - - static int n_id; -}; diff --git a/feature_tracker/include/feature_tracker/feature_tracker.hpp b/feature_tracker/include/feature_tracker/feature_tracker.hpp new file mode 100644 index 000000000..cb890b0d6 --- /dev/null +++ b/feature_tracker/include/feature_tracker/feature_tracker.hpp @@ -0,0 +1,90 @@ +#ifndef FEATURE_TRACKER_HPP +#define FEATURE_TRACKER_HPP + +#include +#include "camodocal/camera_models/CameraFactory.h" +#include "feature_tracker/feature_tracker_observer.hpp" + +class FeatureTracker { + public: + 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(const cv::Mat &img, const double time_s); + + private: + void RestartTracker(const cv::Mat &pre_processed_img, double current_time); + + 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, + std::vector &points, + std::vector &undistorted_points, + std::vector &track_length, std::vector &feature_ids); + + 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(std::vector &curr_pts, + std::vector &track_length, + std::vector &status_out); + + void RejectUsingRansac(const std::vector &cur_un_pts, + const std::vector &prev_un_pts, + std::vector &status_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; + + std::string GenerateStateString() const; + + cv::Mat base_mask_; + cv::Mat previous_pre_processed_image_; + double previous_frame_time_; + + std::vector previous_undistorted_pts_; + std::vector previous_points_; + std::vector feature_ids_; + std::vector feature_track_lengh_; + double prev_prune_time_; + + double fx_; + double fy_; + camodocal::CameraPtr camera_model_; + + static int feature_counter_; // Static to ensure unique id between different + // instances + + cv::Ptr clahe_; + + bool fisheye_; + bool run_histogram_equilisation_; + uint max_feature_count_per_image_; + uint min_distance_between_features_; + double fundemental_matrix_ransac_threshold_; + + double max_time_difference_; + double feature_pruning_frequency_; + double feature_pruning_period_; + std::shared_ptr event_observer_; +}; + + +#endif /* FEATURE_TRACKER_HPP */ 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..e0e70957f --- /dev/null +++ b/feature_tracker/include/feature_tracker/feature_tracker_observer.hpp @@ -0,0 +1,45 @@ +#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 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, + + std::vector ids, + std::vector track_count, + std::vector points_velocity) = 0; + + cv::Mat CreateTrackedFeatureImage(cv::Mat image, + 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/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; -}; diff --git a/feature_tracker/src/feature_tracker.cpp b/feature_tracker/src/feature_tracker.cpp index 47b6aafec..96f8c858d 100644 --- a/feature_tracker/src/feature_tracker.cpp +++ b/feature_tracker/src/feature_tracker.cpp @@ -1,267 +1,309 @@ -#include "feature_tracker/feature_tracker.h" -#include "spdlog/spdlog.h" +#include "feature_tracker/feature_tracker.hpp" -int FeatureTracker::n_id = 0; +#include +#include +#include -bool inBorder(const cv::Point2f &pt) { - 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(std::vector &v, const std::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); } -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) + : 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), + previous_frame_time_(0.0), + prev_prune_time_(0.0) { + camera_model_ = + 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)); + } -void FeatureTracker::setMask() { - if (FISHEYE) - mask = fisheye_mask.clone(); + if (fisheye) std::cout << "!!!FISH EYE NOT WORKING"; + // base_mask_ = fisheye_mask.clone(); else - mask = cv::Mat(ROW, COL, 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++) - cnt_pts_id.push_back( - make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i]))); - - sort(cnt_pts_id.begin(), cnt_pts_id.end(), - [](const pair> &a, - const pair> &b) { - return a.first > b.first; - }); - - forw_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); - ids.push_back(it.second.second); - track_cnt.push_back(it.first); - cv::circle(mask, it.second.first, MIN_DIST, 0, -1); - } - } + base_mask_ = cv::Mat(camera_model_->imageHeight(), + camera_model_->imageWidth(), CV_8UC1, cv::Scalar(255)); } -void FeatureTracker::addPoints() { - for (auto &p : n_pts) { - forw_pts.push_back(p); - ids.push_back(-1); - track_cnt.push_back(1); - } +void FeatureTracker::RegisterEventObserver( + std::shared_ptr event_observer) { + event_observer_ = event_observer; + event_observer_->OnRegistered(); } -void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { - cv::Mat img; - TicToc t_r; - cur_time = _cur_time; - - if (EQUALIZE) { - cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); - TicToc t_c; - clahe->apply(_img, img); - spdlog::debug("CLAHE costs: {}ms", t_c.toc()); +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 - img = _img; + pre_processed_img = new_frame; - if (forw_img.empty()) { - prev_img = cur_img = forw_img = img; - } else { - forw_img = img; + if (current_image_time_s > previous_frame_time_ + max_time_difference_) { + if (event_observer_) { + event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, + previous_frame_time_); + } + RestartTracker(pre_processed_img, current_image_time_s); + return; } - forw_pts.clear(); + if (current_image_time_s < previous_frame_time_) { + if (event_observer_) { + event_observer_->OnDurationBetweenFrameTooLarge(current_image_time_s, + previous_frame_time_); + } + RestartTracker(pre_processed_img, current_image_time_s); + return; + } + // Find new feature points by optical flow + std::vector current_points; + std::vector cur_un_pts; - if (cur_pts.size() > 0) { - TicToc t_o; - vector status; - vector err; - cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, + if (previous_points_.size() > 0) { + 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); - for (int i = 0; i < int(forw_pts.size()); i++) - if (status[i] && !inBorder(forw_pts[i])) status[i] = 0; - reduceVector(prev_pts, status); - reduceVector(cur_pts, status); - reduceVector(forw_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 (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], camera_model_->imageWidth(), + camera_model_->imageHeight())) + status[i] = 0; - for (auto &n : track_cnt) n++; - - if (PUB_THIS_FRAME) { - rejectWithF(); - spdlog::debug("set mask begins"); - TicToc t_m; - setMask(); - spdlog::debug("set mask costs {}ms", t_m.toc()); - - spdlog::debug("detect feature begins"); - TicToc t_t; - int n_max_cnt = MAX_CNT - static_cast(forw_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); - } else - n_pts.clear(); - spdlog::debug("detect feature costs: {}ms", t_t.toc()); - - spdlog::debug("add feature begins"); - TicToc t_a; - addPoints(); - spdlog::debug("selectFeature costs: {}ms", t_a.toc()); + PrunePoints(current_points, cur_un_pts, previous_points_, + previous_undistorted_pts_, feature_track_lengh_, feature_ids_, + status); + + for (int &n : feature_track_lengh_) n++; } - prev_img = cur_img; - prev_pts = cur_pts; - prev_un_pts = cur_un_pts; - cur_img = forw_img; - cur_pts = forw_pts; - undistortedPoints(); - prev_time = cur_time; -} -void FeatureTracker::rejectWithF() { - if (forw_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++) { - Eigen::Vector3d tmp_p; - m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_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; - 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), - 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; - un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); + // Prune and detect new points + if (current_image_time_s > prev_prune_time_ + feature_pruning_period_) { + if (current_points.size() > 8) { + std::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); + } + + 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_, + 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_, camera_model_, current_points, + cur_un_pts, feature_track_lengh_, feature_ids_); + } + + std::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_, feature_track_lengh_, 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; +} - vector status; - cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, - 0.99, status); - int size_a = cur_pts.size(); - reduceVector(prev_pts, status); - reduceVector(cur_pts, status); - reduceVector(forw_pts, status); - reduceVector(cur_un_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 costs: {}ms", t_f.toc()); +void FeatureTracker::RestartTracker(const cv::Mat &pre_processed_img, + double current_time) { + if (event_observer_) { + event_observer_->OnRestart(); } + previous_points_.clear(); + previous_undistorted_pts_.clear(); + feature_ids_.clear(); + feature_track_lengh_.clear(); + + 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_); + + previous_frame_time_ = current_time; + prev_prune_time_ = current_time; + previous_pre_processed_image_ = pre_processed_img; + return; } -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::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, + 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::readIntrinsicParameter(const string &calib_file) { - spdlog::info("reading paramerter of camera {}", calib_file.c_str()); - m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file); +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); + reduceVector(prev_un_points, status); + reduceVector(ids, status); + reduceVector(track_counts, status); } -void FeatureTracker::showUndistortion(const string &name) { - cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0)); - vector distortedp, undistortedp; - for (int i = 0; i < COL; i++) - for (int j = 0; j < ROW; 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 (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(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) { - undistortedImg.at(pp.at(1, 0) + 300, - pp.at(0, 0) + 300) = - cur_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)); +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()); + 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]; + }); + + 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] = 1; } } - cv::imshow(name, undistortedImg); - cv::waitKey(0); + + return mask; } -void FeatureTracker::undistortedPoints() { - 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); - 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); - } - // 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) { - 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; - pts_velocity.push_back(cv::Point2f(v_x, v_y)); - } else - pts_velocity.push_back(cv::Point2f(0, 0)); - } else { - pts_velocity.push_back(cv::Point2f(0, 0)); - } +void FeatureTracker::RejectUsingRansac( + 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) { + 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); } - } else { - for (unsigned int i = 0; i < cur_pts.size(); i++) { - pts_velocity.push_back(cv::Point2f(0, 0)); + + cv::findFundamentalMat( + prev_scaled_undistorted_pts, curr_scaled_undistorted_pts, cv::FM_RANSAC, + fundemental_matrix_ransac_threshold_, 0.99, status_out); + } +} + +void FeatureTracker::GetPointVelocty( + 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++) { + 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)); } } - prev_un_pts_map = cur_un_pts_map; } + +cv::Point2f FeatureTracker::UndistortPoint( + const cv::Point2f point, const camodocal::CameraPtr camera) const { + Eigen::Vector2d a(point.x, point.y); + Eigen::Vector3d b; + camera->liftProjective(a, b); + return cv::Point2f(b.x() / b.z(), b.y() / b.z()); +} + +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(); + return ss.str(); +} \ No newline at end of file diff --git a/feature_tracker/src/feature_tracker_observer.cpp b/feature_tracker/src/feature_tracker_observer.cpp new file mode 100644 index 000000000..e193dd6ea --- /dev/null +++ b/feature_tracker/src/feature_tracker_observer.cpp @@ -0,0 +1,36 @@ +#include "feature_tracker/feature_tracker_observer.hpp" + +#include +cv::Mat 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); + } + 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])*50; // 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_rerun/CMakeLists.txt b/feature_tracker_rerun/CMakeLists.txt new file mode 100644 index 000000000..1042931e2 --- /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) +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 yaml-cpp) + +# 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_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp b/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp new file mode 100644 index 000000000..c5f1caa9b --- /dev/null +++ b/feature_tracker_rerun/include/feature_tracker/feature_tracker_observer_spdlog_rerun.hpp @@ -0,0 +1,33 @@ +#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; + 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(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 new file mode 100644 index 000000000..08bdac03f --- /dev/null +++ b/feature_tracker_rerun/src/feature_tracker_observer_spdlog_rerun.cpp @@ -0,0 +1,96 @@ +#include "feature_tracker/feature_tracker_observer_spdlog_rerun.hpp" + +// #include +#include + +#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::info); + spdlog::info("FeatureTracker observer Registered"); + recorder_ = std::make_unique("Feature Tracker"); + recorder_->spawn().exit_on_failure(); + // rec = rerun::RecordingStream("rerun_example_image"); +} + +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::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( + 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::debug("Features have been pruned and new features added"); + cv::Mat img = CreateOpticalFlowImage(new_frame, features, track_count, 20, + points_velocity); + 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: +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 diff --git a/feature_tracker_rerun/src/main.cpp b/feature_tracker_rerun/src/main.cpp new file mode 100644 index 000000000..d025baf09 --- /dev/null +++ b/feature_tracker_rerun/src/main.cpp @@ -0,0 +1,106 @@ +#include // Ensure you have yaml-cpp installed and linked +#include + +#include +#include +#include +#include +#include +#include +#include + +#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) { + // 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"; + 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(); + feat.RegisterEventObserver(observer); + + // Open the timestamp file + std::ifstream tsFile(timestamp_file); + if (!tsFile.is_open()) { + std::cerr << "Error opening timestamp file: " << timestamp_file + << std::endl; + return 1; + } + + std::string line; + double time_conversion = pow(10.0, 9); + + while (std::getline(tsFile, line)) { + std::stringstream ss(line); + double timestamp; + std::string imageFilename; + + ss >> timestamp; + ss.ignore(1, ','); + std::getline(ss, imageFilename); + imageFilename.erase(0, imageFilename.find_first_not_of(" \t\r\n")); + imageFilename.erase(imageFilename.find_last_not_of(" \t\r\n") + 1); + + std::string imagePath = image_folder + "/" + imageFilename; + if (!std::filesystem::exists(imagePath)) { + std::cerr << "File does not exist: " << imagePath << std::endl; + continue; + } + + cv::Mat image = cv::imread(imagePath, cv::IMREAD_GRAYSCALE); + if (image.empty()) { + std::cerr << "Error loading image: " << imagePath << std::endl; + continue; + } + + feat.ProcessNewFrame(image, timestamp / time_conversion); + } + + tsFile.close(); + return 0; +} 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/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..34114b819 --- /dev/null +++ b/feature_tracker_ros/include/feature_tracker_ros/feature_tracker_node.hpp @@ -0,0 +1,68 @@ +#ifndef FEATURE_TRACKER_NODE_HPP +#define FEATURE_TRACKER_NODE_HPP +#include +#include +#include +#include +#include +#include + +#include "feature_tracker/feature_tracker.hpp" +#include "feature_tracker/feature_tracker_observer.hpp" + +class FeatureTrackerNode : public FeatureTrackerObserver { + public: + ~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(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, + bool& enableHistogramEqualization, bool& useFisheye); + + void ImageCallback(const sensor_msgs::ImageConstPtr& img_msg); + + void StartPublishersAndSubscribers(); + + std::unique_ptr feature_tracker_; + cv_bridge::CvImage optical_flow_img_; + + int min_track_count_to_publish_; + bool first_image_; + 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_; + sensor_msgs::Image optical_flow_img_msg_; +}; + +#endif /* FEATURE_TRACKER_NODE_HPP */ 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 new file mode 100644 index 000000000..8fc3029d9 --- /dev/null +++ b/feature_tracker_ros/launch/feature_tracker.launch @@ -0,0 +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 9863183a0..222d8f875 100644 --- a/feature_tracker_ros/src/feature_tracker_node.cpp +++ b/feature_tracker_ros/src/feature_tracker_node.cpp @@ -1,230 +1,219 @@ +#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[NUM_OF_CAM]; -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) { - 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 +#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); +} - 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; +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"; + feature_v_pixel_channel_.name = "Feature V Pixel"; + 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_); + feature_points_msg_.channels.push_back(feature_x_velocity_channel_); + feature_points_msg_.channels.push_back(feature_y_velocity_channel_); + + first_image_ = false; + + // 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; + + // 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; } - 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 &cur_pts = trackerData[i].cur_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(cur_pts[j].x); - v_of_point.values.push_back(cur_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); - - for (unsigned int j = 0; j < trackerData[i].cur_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::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].cur_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::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); - } - } - // cv::imshow("vis", stereo_img); - // cv::waitKey(5); - pub_match.publish(ptr->toImageMsg()); - } + // 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, + /*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; } - ROS_INFO("whole feature tracker processing costs: %f", t_r.toc()); + + 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; } -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); - - for (int i = 0; i < NUM_OF_CAM; i++) - trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); - - 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::StartPublishersAndSubscribers() { + ROS_INFO("Creating Subscribers and Publishers"); + feature_point_cloud_publisher_ = + nh_.advertise("feature", 1000); + optical_flow_image_publisher_ = + 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() { + ROS_INFO("FeatureTrackerNode has been registered by feature tracker"); +} + +void FeatureTrackerNode::OnRestart() { + ROS_INFO("FeatureTracker has been restarted"); + pub_restart_.publish(restart_flag_); +} + +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); +} + +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( + 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) { + 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 < points_velocity.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); } } + // 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( + 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); - */ +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 = + std::make_shared(); + if (!feature_tracker_node->Start()) { + ROS_ERROR("Failed to start FeatureTrackerNode"); + ros::shutdown(); + } + ROS_INFO("FeatureTrackerNode started."); ros::spin(); return 0; -} - -// new points velocity is 0, pub or not? -// track cnt > 1 pub? \ No newline at end of file +} \ No newline at end of file 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(); -} 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 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 { 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