diff --git a/common/print_tools.hpp b/common/print_tools.hpp index 0427ebc..e2fc58a 100644 --- a/common/print_tools.hpp +++ b/common/print_tools.hpp @@ -63,7 +63,7 @@ struct dependent_false : std::false_type }; inline void printHelloWorldVIO() { - std::cout << BLUE + std::cout << BLUE << std::endl << " _ _ _ _ __ __ _ _ __ _____ ___ \n" " | | | | ___| | | ___ \\ \\ / ___ _ __| | __| | \\ \\ / |_ _/ _ \\ \n" " | |_| |/ _ | | |/ _ \\ \\ \\ /\\ / / _ \\| '__| |/ _` | \\ \\ / / | | | | |\n" diff --git a/vio_hw/CMakeLists.txt b/vio_hw/CMakeLists.txt index eddcb23..2c76407 100644 --- a/vio_hw/CMakeLists.txt +++ b/vio_hw/CMakeLists.txt @@ -53,4 +53,9 @@ add_executable(hello_world_vio node/hello_world_vio.cpp) add_backward(hello_world_vio) target_link_libraries(hello_world_vio vio_hw +) +add_executable(run_kitti_node node/run_kitti_node.cpp) +add_backward(run_kitti_node) +target_link_libraries(run_kitti_node + vio_hw ) \ No newline at end of file diff --git a/vio_hw/internal/camera_calibration.hpp b/vio_hw/internal/camera_calibration.hpp index 3034abb..8dbc67f 100755 --- a/vio_hw/internal/camera_calibration.hpp +++ b/vio_hw/internal/camera_calibration.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include namespace viohw { diff --git a/vio_hw/internal/frame.hpp b/vio_hw/internal/frame.hpp index c271425..d8b54cd 100644 --- a/vio_hw/internal/frame.hpp +++ b/vio_hw/internal/frame.hpp @@ -32,6 +32,7 @@ class Frame void ComputeKeypoint(const cv::Point2f &pt, Keypoint &kp); void UpdateKeypoint(const int lmid, const cv::Point2f &pt); void RemoveKeypointById(const int lmid); + Keypoint GetKeypointById(const int lmid) const; // For using frame in ordered containers bool operator<(const Frame &f) const { return id_ < f.id_; } diff --git a/vio_hw/internal/map_manager.hpp b/vio_hw/internal/map_manager.hpp index 1c4afda..5f3448e 100644 --- a/vio_hw/internal/map_manager.hpp +++ b/vio_hw/internal/map_manager.hpp @@ -29,7 +29,8 @@ class MapManager void AddKeypointsToFrame(const std::vector &vpts, const std::vector &vdescs, Frame &frame); void RemoveObsFromCurFrameById(const int lmid); - + std::shared_ptr GetKeyframe(const int kfid) const; + std::shared_ptr GetMapPoint(const int lmid) const; private: int lm_id_, kf_id_; int num_lms_, num_kfs_; diff --git a/vio_hw/internal/visual_frontend.hpp b/vio_hw/internal/visual_frontend.hpp index 4786a57..9b8513c 100644 --- a/vio_hw/internal/visual_frontend.hpp +++ b/vio_hw/internal/visual_frontend.hpp @@ -6,6 +6,7 @@ #include "vio_hw/internal/map_manager.hpp" #include "vio_hw/internal/setting.hpp" #include "vio_hw/internal/tracker/tracker_base.hpp" +#include "vio_hw/internal/viz/visualization_base.hpp" namespace viohw { class VisualFrontEnd @@ -21,7 +22,7 @@ class VisualFrontEnd // construction function VisualFrontEnd(SettingPtr setting, FramePtr frame, MapManagerPtr manager, - TrackerBasePtr tracker); + TrackerBasePtr tracker, VisualizationBasePtr viz); // tracking left image bool TrackerMono(cv::Mat &image, double time); @@ -35,11 +36,27 @@ class VisualFrontEnd // optical flow tracking for frame before and after void KLTTracking(); + // filter outlier via epipolar constraint + void Epipolar2d2dFiltering(); + + // draw tracker result and show in ui + void ShowTrackingResult(); + + // check current frame is keyframe + bool CheckIsNewKeyframe(); + + // compute visual frontend pose + void ComputePose(); + + // update motion model, IMU or Constant model + void UpdateMotionModel(); + private: MapManagerPtr map_manager_; FramePtr current_frame_; SettingPtr param_; TrackerBasePtr tracker_; + VisualizationBasePtr viz_; ConstantMotionModel motion_model_; diff --git a/vio_hw/internal/viz/rviz_visualization.hpp b/vio_hw/internal/viz/rviz_visualization.hpp index 68b2f21..649d61a 100644 --- a/vio_hw/internal/viz/rviz_visualization.hpp +++ b/vio_hw/internal/viz/rviz_visualization.hpp @@ -2,13 +2,22 @@ #define VIO_HELLO_WORLD_RVIZ_VISUALIZATION_HPP #include "vio_hw/internal/viz/visualization_base.hpp" +#include "vio_hw/internal/viz/camera_visualizer.hpp" +#include +#include +#include +#include +#include +#include +#include +#include namespace viohw { class RvizVisualization : public VisualizationBase { public: - RvizVisualization() = default; + explicit RvizVisualization(); bool showTrackerResultImage(const cv::Mat &img) override; @@ -20,6 +29,11 @@ class RvizVisualization : public VisualizationBase bool addPoint(const Eigen::Vector3d &t, const Eigen::Vector3d &color) override; + + private: + CameraPoseVisualization camera_pose_visual_; + ros::Publisher pub_kfs_traj_, pub_vo_traj_; + ros::Publisher pub_img_tracker_result_; }; } // namespace viohw diff --git a/vio_hw/internal/viz/visualization_base.hpp b/vio_hw/internal/viz/visualization_base.hpp index 8318d61..0c478fd 100644 --- a/vio_hw/internal/viz/visualization_base.hpp +++ b/vio_hw/internal/viz/visualization_base.hpp @@ -39,6 +39,9 @@ class VisualizationBase }; +typedef std::shared_ptr VisualizationBasePtr; +typedef std::shared_ptr VisualizationBaseConstPtr; + } // namespace viohw #endif // VIO_HELLO_WORLD_VISUALIZATION_BASE_HPP diff --git a/vio_hw/node/hello_world_vio.cpp b/vio_hw/node/hello_world_vio.cpp index a5d8ebc..88a090d 100644 --- a/vio_hw/node/hello_world_vio.cpp +++ b/vio_hw/node/hello_world_vio.cpp @@ -22,7 +22,7 @@ #include "tceres/solver.h" #include "vio_hw/internal/world_manager.hpp" -DEFINE_string(config_file_path, "../vio_hw/params/euroc_stereo_imu.yaml", +DEFINE_string(config_file_path, "../vio_hw/params/euroc/euroc_stereo_imu.yaml", "config file path"); class SensorManager diff --git a/vio_hw/node/run_kitti_node.cpp b/vio_hw/node/run_kitti_node.cpp new file mode 100644 index 0000000..08e6394 --- /dev/null +++ b/vio_hw/node/run_kitti_node.cpp @@ -0,0 +1,127 @@ +// Copyright (C) 2007 Free Software Foundation, Inc. +// Everyone is permitted to copy and distribute verbatim copies +// of this license document, but changing it is not allowed. +// This file is part of vio-hello-world Copyright (C) 2023 ZJU +// You should have received a copy of the GNU General Public License +// along with vio-hello-world. If not, see . +// Author: weihao(isweihao@zju.edu.cn), M.S at Zhejiang University + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "feat/harris/harris.h" +#include "tceres/problem.h" +#include "tceres/solver.h" +#include "vio_hw/internal/world_manager.hpp" + +DEFINE_string(config_file_path, "../vio_hw/params/kitti/kitti_00-02.yaml", "config file path"); +DEFINE_string(kitti_dataset_path, + "/home/weihao/dataset/kitti/data_odometry_gray/dataset/sequences/00", + "kitti dataset path"); +void LoadKittiImagesTimestamps(const std::string &str_path_to_sequence, + std::vector &str_image_left_vec_path, + std::vector &str_image_right_vec_path, + std::vector ×tamps_vec); +class SensorManager +{ + public: + explicit SensorManager(viohw::WorldManager *slam_manager) : slam_world_(slam_manager) { + LOG(INFO) << "Sensor Manager is create."; + LoadKittiImagesTimestamps(fLS::FLAGS_kitti_dataset_path, image_left_vec_path, + image_right_vec_path, vec_timestamp); + }; + + ~SensorManager() = default; + + void syncProcess() { + LOG(INFO) << "Start the measurement reader thread"; + const size_t num_images = image_left_vec_path.size(); + for (int ni = 0; ni < num_images; ni++) { + if (slam_world_->getParams()->slam_setting_.stereo_mode_) { + std::lock_guard lock(img_mutex_); + double timestamp = vec_timestamp[ni]; + cv::Mat image0 = cv::imread(image_left_vec_path[ni], cv::IMREAD_GRAYSCALE); + cv::Mat image1 = cv::imread(image_right_vec_path[ni], cv::IMREAD_GRAYSCALE); + assert(!image0.empty() && !image1.empty()); + slam_world_->addNewStereoImages(timestamp, image0, image1); + } + std::chrono::milliseconds dura(40); + std::this_thread::sleep_for(dura); + } + } + + private: + std::queue img0_buf_; + std::queue img1_buf_; + std::mutex img_mutex_; + std::vector image_left_vec_path, image_right_vec_path; + std::vector vec_timestamp; + viohw::WorldManager *slam_world_; +}; + +int main(int argc, char **argv) { + google::InitGoogleLogging("hello_world_vio"); + FLAGS_stderrthreshold = google::INFO; + FLAGS_colorlogtostderr = true; + google::ParseCommandLineFlags(&argc, &argv, true); + ros::init(argc, argv, "hello_world_vio"); + + auto params = std::make_shared(FLAGS_config_file_path); + std::cout << *params << std::endl; + viohw::WorldManager world_manager(params); + // Start the SLAM thread + std::thread estimate_thread(&viohw::WorldManager::run, &world_manager); + + SensorManager sensor_manager(&world_manager); + // Start the sensor measurement grab thread + std::thread sensor_grab_thread(&SensorManager::syncProcess, &sensor_manager); + + ros::spin(); + return 0; +} + +inline void LoadKittiImagesTimestamps(const std::string &str_path_to_sequence, + std::vector &str_image_left_vec_path, + std::vector &str_image_right_vec_path, + std::vector ×tamps_vec) { + using namespace std; + string strPathTimeFile = str_path_to_sequence + "/times.txt"; + + std::ifstream fTimes(strPathTimeFile, ios::in | ios::app); + + if (!fTimes.is_open()) LOG(FATAL) << "Open Failed"; + while (!fTimes.eof()) { + string s; + getline(fTimes, s); + if (!s.empty()) { + stringstream ss; + ss << s; + double t; + ss >> t; + timestamps_vec.push_back(t); + } + } + + string strPrefixLeft = str_path_to_sequence + "/image_0/"; + string strPrefixRight = str_path_to_sequence + "/image_1/"; + + const size_t nTimes = timestamps_vec.size(); + str_image_left_vec_path.resize(nTimes); + str_image_right_vec_path.resize(nTimes); + + for (int i = 0; i < nTimes; i++) { + stringstream ss; + ss << setfill('0') << setw(6) << i; + str_image_left_vec_path[i] = strPrefixLeft + ss.str() + ".png"; + str_image_right_vec_path[i] = strPrefixRight + ss.str() + ".png"; + } + fTimes.close(); +} diff --git a/vio_hw/params/euroc_stereo_imu.yaml b/vio_hw/params/euroc/euroc_stereo_imu.yaml similarity index 97% rename from vio_hw/params/euroc_stereo_imu.yaml rename to vio_hw/params/euroc/euroc_stereo_imu.yaml index 5baafb0..4a2dad3 100644 --- a/vio_hw/params/euroc_stereo_imu.yaml +++ b/vio_hw/params/euroc/euroc_stereo_imu.yaml @@ -19,8 +19,8 @@ FeatureAndTracker: feature.Type: 0 tracker.Type: 0 # common params - max.kps.num: 200 - max.kps.distance: 30 + max.kps.num: 300 + max.kps.distance: 20 use.clahe: 1 clahe.val: 3 track.keyframetoframe: 0 diff --git a/vio_hw/params/kitti/kitti_00-02.yaml b/vio_hw/params/kitti/kitti_00-02.yaml new file mode 100644 index 0000000..83813af --- /dev/null +++ b/vio_hw/params/kitti/kitti_00-02.yaml @@ -0,0 +1,75 @@ +%YAML 1.0 +--- + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. +#-------------------------------------------------------------------------------------------- + +Camera: + topic.left.right: [ "/cam0/image_raw", "/cam1/image_raw" ] + model.left.right: [ "pinhole","pinhole" ] + left.resolution: [ 1241, 376 ] + right.resolution: [ 1241, 376 ] + left.K: [718.856, 718.856, 607.1928, 185.2157] + right.K: [718.856, 718.856, 607.1928, 185.2157] + left.distortion.coefficient: [0.0, 0.0, 0.0, 0.0] + right.distortion.coefficient: [0.0, 0.0, 0.0, 0.0] + +FeatureAndTracker: + feature.Type: 0 + tracker.Type: 0 + # common params + max.kps.num: 500 + max.kps.distance: 20 + use.clahe: 1 + clahe.val: 3 + track.keyframetoframe: 0 + use.brief: 1 + # param for gftt + kp.quality.level: 0.001 + # param for KLT + klt.use.prior: 1 + klt.win.size: 9 + klt.pyr.level: 3 + + klt.max.iter: 30 + klt.max.px.precision: 0.01 + klt.max.fb.dist: 0.5 + klt.err: 30. + +IMU: + topic: "/imu0" + acc_n: 0.1 + acc_w: 0.2 + gyr_n: 0.3 + gyr_w: 0.4 + +SLAM: + stereo.mode: 1 + use.imu: 1 + force.realtime: 1 + use.Rviz: 1 + use.Pangolin: 0 + +# Camera Extrinsic parameters T_b_ci ( v_b = T_b_ci * v_ci ) +Extrinsic: + camera.num: 2 + body_T_cam0: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [1., 0., 0., 0., + 0., 1., 0., 0., + 0., 0., 1., 0., + 0., 0., 0., 1.] + + body_T_cam1: !!opencv-matrix + rows: 4 + cols: 4 + dt: d + data: [1., 0., 0., 0.537165718864418, + 0., 1., 0., 0., + 0., 0., 1., 0., + 0., 0., 0., 1.] + + diff --git a/vio_hw/params/rviz/hw-vio.rviz b/vio_hw/params/rviz/hw-vio.rviz new file mode 100644 index 0000000..2ef85e3 --- /dev/null +++ b/vio_hw/params/rviz/hw-vio.rviz @@ -0,0 +1,131 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 871 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /hello_world_vio/image_track + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: tracker_result + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9953978061676025 + Target Frame: + Yaw: 1.5403972864151 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2096 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000003a500000771fc0200000009fb0000001200530065006c0065006300740069006f006e0000000027000000b0000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df0000034f000003c3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000003d50000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c0074007200610063006b00650072005f0072006500730075006c00740100000408000003900000002600ffffff000000010000015f00000771fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000027000007710000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f000000005efc0100000002fb0000000800540069006d0065010000000000000f00000006dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000b4f0000077100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3840 + X: 0 + Y: 0 + tracker_result: + collapsed: false diff --git a/vio_hw/src/camera_calibration.cpp b/vio_hw/src/camera_calibration.cpp index 149ed71..c2398bb 100755 --- a/vio_hw/src/camera_calibration.cpp +++ b/vio_hw/src/camera_calibration.cpp @@ -43,18 +43,16 @@ CameraCalibration::CameraCalibration(const std::string &model, double fx, img_w_(img_w), img_h_(img_h), img_size_(img_w, img_h) { - std::cout << "\n Setting up camera, model selected : " << model << "\n"; + // std::cout << "\n Setting up camera, model selected : " << model << "\n"; if (model == "pinhole") { model_ = Pinhole; - std::cout << "\nPinhole Camera Model created\n"; + // std::cout << "\nPinhole Camera Model created\n"; } else if (model == "fisheye") { model_ = Fisheye; - std::cout << "\nFisheye Camera Model selected"; + // std::cout << "\nFisheye Camera Model selected"; } else { - std::cout << "\nNo supported camera model provided!"; - std::cout << "\nChoosee between: pinhole / fisheye"; - exit(-1); + LOG(FATAL) << "No supported camera model provided!, Choose between: pinhole / fisheye"; } K_ << fx_, 0., cx_, 0., fy_, cy_, 0., 0., 1.; @@ -72,11 +70,11 @@ CameraCalibration::CameraCalibration(const std::string &model, double fx, cv::eigen2cv(Tc0ci_.rotationMatrix(), Rcv_c0ci_); cv::eigen2cv(Tc0ci_.translation(), tcv_c0ci_); - std::cout << "\n Camera Calibration set as : \n\n"; - std::cout << "\n K = \n" << K_; - std::cout << "\n\n D = " << D_.transpose(); - - std::cout << "\n opt K = \n" << Kcv_; + // std::cout << "\n Camera Calibration set as : \n\n"; + // std::cout << "\n K = \n" << K_; + // std::cout << "\n\n D = " << D_.transpose(); + // + // std::cout << "\n opt K = \n" << Kcv_; const int nborder = 5; roi_rect_ = cv::Rect(cv::Point2i(nborder, nborder), @@ -136,10 +134,10 @@ void CameraCalibration::setUndistMap(const double alpha) { // Setup roi mask / rect setROIMask(roi_rect_); - std::cout << "\n Undist Camera Calibration set as : \n\n"; - std::cout << "\n K = \n" << K_; - std::cout << "\n\n D = " << D_.transpose(); - std::cout << "\n\n ROI = " << roi_rect_; + // std::cout << "\n Undist Camera Calibration set as : \n\n"; + // std::cout << "\n K = \n" << K_; + // std::cout << "\n\n D = " << D_.transpose(); + // std::cout << "\n\n ROI = " << roi_rect_; } void CameraCalibration::setUndistStereoMap(const cv::Mat &R, const cv::Mat &P, @@ -213,9 +211,9 @@ void CameraCalibration::setupExtrinsic(const Sophus::SE3d &Tc0ci) { cv::eigen2cv(Tcic0_.rotationMatrix(), Rcv_cic0_); cv::eigen2cv(Tcic0_.translation(), tcv_cic0_); - std::cout << "\n Camera Extrinsic : \n\n"; - std::cout << "\n Rc0ci = \n" << Tc0ci_.rotationMatrix(); - std::cout << "\n\n tc0ci = " << Tc0ci_.translation().transpose(); + // std::cout << "\n Camera Extrinsic : \n\n"; + // std::cout << "\n Rc0ci = \n" << Tc0ci_.rotationMatrix(); + // std::cout << "\n\n tc0ci = " << Tc0ci_.translation().transpose(); Eigen::Vector3d tc0ci = Tc0ci_.translation(); if ((fabs(tc0ci.x()) > fabs(tc0ci.y()) && tc0ci.x() < 0) || diff --git a/vio_hw/src/frame.cpp b/vio_hw/src/frame.cpp index 9d2c64b..6dfc7ba 100644 --- a/vio_hw/src/frame.cpp +++ b/vio_hw/src/frame.cpp @@ -185,4 +185,16 @@ void Frame::RemoveKeypointById(const int lmid) { } mapkps_.erase(lmid); } + +Keypoint Frame::GetKeypointById(const int lmid) const +{ + std::lock_guard lock(kps_mutex_); + + auto it = mapkps_.find(lmid); + if( it == mapkps_.end() ) { + return Keypoint(); + } + + return it->second; +} } // namespace viohw diff --git a/vio_hw/src/map_manager.cpp b/vio_hw/src/map_manager.cpp index cb9e249..2780ebc 100644 --- a/vio_hw/src/map_manager.cpp +++ b/vio_hw/src/map_manager.cpp @@ -45,18 +45,23 @@ void MapManager::AddKeyframe() { // Create a copy of Cur. Frame shared_ptr for } void MapManager::ExtractKeypoints(const cv::Mat& im, const cv::Mat& im_raw) { - std::vector kps = current_frame_->getKeypoints(); + std::vector kps = current_frame_->getKeypoints(); // TODO brief calculate if (param_->feat_tracker_setting_.use_brief_) { //................. } + cv::Mat mask = cv::Mat(im.rows, im.cols, CV_8UC1, cv::Scalar(255)); + for (auto& pt : kps) { + cv::circle(mask, pt.px_, param_->feat_tracker_setting_.max_feature_dis_, 0, -1); + } + int num_need_detect = param_->feat_tracker_setting_.max_feature_num_ - kps.size(); if (num_need_detect > 0) { std::vector new_kps; feature_extractor_->setMaxKpsNumber(num_need_detect); - feature_extractor_->detect(im, new_kps); + feature_extractor_->detect(im, new_kps, mask); if (!new_kps.empty()) { std::vector desc_pts; cv::KeyPoint::convert(new_kps, desc_pts); @@ -131,8 +136,7 @@ void MapManager::AddMapPoint(const cv::Mat& desc) { } // Remove a MP obs from cur Frame -void MapManager::RemoveObsFromCurFrameById(const int lmid) -{ +void MapManager::RemoveObsFromCurFrameById(const int lmid) { // Remove cur obs current_frame_->RemoveKeypointById(lmid); @@ -140,4 +144,24 @@ void MapManager::RemoveObsFromCurFrameById(const int lmid) // TODO } +std::shared_ptr MapManager::GetKeyframe(const int kfid) const { + std::lock_guard lock(kf_mutex_); + + auto it = map_kfs_.find(kfid); + if (it == map_kfs_.end()) { + return nullptr; + } + return it->second; +} + +std::shared_ptr MapManager::GetMapPoint(const int lmid) const { + std::lock_guard lock(lm_mutex_); + + auto it = map_lms_.find(lmid); + if (it == map_lms_.end()) { + return nullptr; + } + return it->second; +} + } // namespace viohw diff --git a/vio_hw/src/tracker/optical_flow_impl.cpp b/vio_hw/src/tracker/optical_flow_impl.cpp index fc41cfd..b04d2a8 100644 --- a/vio_hw/src/tracker/optical_flow_impl.cpp +++ b/vio_hw/src/tracker/optical_flow_impl.cpp @@ -18,7 +18,7 @@ void OpticalFlowImpl::trackerAndMatcher(const std::vector &vprevpyr, assert(vprevpyr.size() == vcurpyr.size()); if (vkps.empty()) { - // std::cout << "\n \t >>> No kps were provided to kltTracking()!\n"; + LOG(WARNING) << ">>>No kps were provided to kltTracking()!<<<"; return; } diff --git a/vio_hw/src/visual_frontend.cpp b/vio_hw/src/visual_frontend.cpp index b58b2e0..e8ba75e 100644 --- a/vio_hw/src/visual_frontend.cpp +++ b/vio_hw/src/visual_frontend.cpp @@ -3,8 +3,9 @@ namespace viohw { VisualFrontEnd::VisualFrontEnd(viohw::SettingPtr state, viohw::FramePtr frame, - viohw::MapManagerPtr map, viohw::TrackerBasePtr tracker) - : param_(state), current_frame_(frame), map_manager_(map), tracker_(tracker) { + viohw::MapManagerPtr map, viohw::TrackerBasePtr tracker, + VisualizationBasePtr viz) + : param_(state), current_frame_(frame), map_manager_(map), tracker_(tracker), viz_(viz) { use_clahe_ = param_->feat_tracker_setting_.use_clahe_; if (use_clahe_) { @@ -31,17 +32,31 @@ bool VisualFrontEnd::VisualTracking(cv::Mat& image, double time) { } bool VisualFrontEnd::TrackerMono(cv::Mat& image, double time) { + // preprocess PreProcessImage(image); + // first frame is keyframe if (current_frame_->id_ == 0) { return true; } + // tracking from frame to frame KLTTracking(); - // Only For Test. - return true; - return false; + // outlier filter + Epipolar2d2dFiltering(); + + // show tracking result to ui + ShowTrackingResult(); + + // compute current visual frontend pose + ComputePose(); + + // update motion model + UpdateMotionModel(); + + // check is new keyframe + return CheckIsNewKeyframe(); } void VisualFrontEnd::PreProcessImage(cv::Mat& img_raw) { @@ -120,4 +135,78 @@ void VisualFrontEnd::KLTTracking() { } } +void VisualFrontEnd::Epipolar2d2dFiltering() { + // Get prev KF + auto pkf = map_manager_->GetKeyframe(current_frame_->kfid_); + + if (pkf == nullptr) { + LOG(FATAL) << "ERROR! Previous Kf does not exist yet (epipolar2d2d())."; + } + + // Get cur. Frame nb kps + size_t nbkps = current_frame_->nbkps_; + + if (nbkps < 8) { + LOG(WARNING) << "Not enough kps to compute Essential Matrix"; + return; + } + std::vector vkpsids; + std::vector > vkfbvs, vcurbvs; + std::vector vkf_px, vcur_px; + + for (const auto& it : current_frame_->mapkps_) { + auto& kp = it.second; + + // Get the prev. KF related kp if it exists + auto kf_kp = pkf->GetKeypointById(kp.lmid_); + + if (kf_kp.lmid_ != kp.lmid_) { + continue; + } + vkfbvs.push_back(kf_kp.bv_); + vcurbvs.push_back(kp.bv_); + vkf_px.push_back(kf_kp.px_); + vcur_px.push_back(kp.px_); + vkpsids.push_back(kp.lmid_); + // TODO + } + std::vector inliers; + cv::findFundamentalMat(vkf_px, vcur_px, cv::FM_RANSAC, 3, 0.99, inliers); + assert(vkf_px.size() == vcur_px.size() && vcur_px.size() == inliers.size()); + for (size_t i = 0; i < inliers.size(); i++) { + if (!inliers[i]) { + map_manager_->RemoveObsFromCurFrameById(vkpsids[i]); + } + } +} + +void VisualFrontEnd::ShowTrackingResult() { + cv::Mat draw_tracker_image; + cv::cvtColor(cur_img_, draw_tracker_image, cv::COLOR_GRAY2BGR); + // Get prev KF + auto pkf = map_manager_->GetKeyframe(current_frame_->kfid_); + std::vector vkf_px, vcur_px; + for (const auto& it : current_frame_->mapkps_) { + auto& kp = it.second; + // Get the prev. KF related kp if it exists + auto kf_kp = pkf->GetKeypointById(kp.lmid_); + if (kf_kp.lmid_ != kp.lmid_) { + continue; + } + vkf_px.push_back(kf_kp.px_); + vcur_px.push_back(kp.px_); + cv::arrowedLine(draw_tracker_image, kp.px_, kf_kp.px_, cv::Scalar(0, 255, 0), 2, 8, 0, 0.3); + cv::circle(draw_tracker_image, kf_kp.px_, 2, cv::Scalar(0, 255, 0), -1); + } + // cv::imshow("tracker", draw_tracker_image); + // cv::waitKey(1); + viz_->showTrackerResultImage(draw_tracker_image); +} + +bool VisualFrontEnd::CheckIsNewKeyframe() { return true; } + +void VisualFrontEnd::ComputePose() {} + +void VisualFrontEnd::UpdateMotionModel() {} + } // namespace viohw diff --git a/vio_hw/src/viz/rviz_visualization.cpp b/vio_hw/src/viz/rviz_visualization.cpp index f4b6660..68cce17 100644 --- a/vio_hw/src/viz/rviz_visualization.cpp +++ b/vio_hw/src/viz/rviz_visualization.cpp @@ -2,27 +2,38 @@ namespace viohw { -bool RvizVisualization::addTrajectory(const Eigen::Matrix3d& Q, - const Eigen::Vector3d& t) { +bool RvizVisualization::addTrajectory(const Eigen::Matrix3d& Q, const Eigen::Vector3d& t) { std::cout << "rviz addTrajectory\n"; return true; } bool RvizVisualization::showTrackerResultImage(const cv::Mat& img) { - std::cout << "rviz showTrackerResultImage\n"; - return false; + if (pub_img_tracker_result_.getNumSubscribers() == 0) { + return false; + } + + std_msgs::Header header; + header.frame_id = "world"; + header.stamp = ros::Time::now(); + sensor_msgs::ImagePtr imgTrackMsg = cv_bridge::CvImage(header, "rgb8", img).toImageMsg(); + pub_img_tracker_result_.publish(imgTrackMsg); + return true; } -bool RvizVisualization::addKFTrajectory(const Eigen::Matrix3d& Q, - const Eigen::Vector3d& t) { +bool RvizVisualization::addKFTrajectory(const Eigen::Matrix3d& Q, const Eigen::Vector3d& t) { std::cout << "rviz addKFTrajectory\n"; return false; } -bool RvizVisualization::addPoint(const Eigen::Vector3d& t, - const Eigen::Vector3d& color) { +bool RvizVisualization::addPoint(const Eigen::Vector3d& t, const Eigen::Vector3d& color) { std::cout << "rviz addPoint\n"; return false; } +RvizVisualization::RvizVisualization() : camera_pose_visual_(1, 0, 0, 1) { + LOG(INFO) << "Create RvizVisualization"; + ros::NodeHandle n("~"); + pub_img_tracker_result_ = n.advertise("image_track", 1000); +} + } // namespace viohw diff --git a/vio_hw/src/world_manager.cpp b/vio_hw/src/world_manager.cpp index 841ec33..e3bcb10 100644 --- a/vio_hw/src/world_manager.cpp +++ b/vio_hw/src/world_manager.cpp @@ -15,7 +15,7 @@ WorldManager::WorldManager(std::shared_ptr& setting) : params_(setting) feature_extractor_ = FeatureBase::Create(feature_options); // create visualization - VisualizationBase::VisualizationOption viz_option{VisualizationBase::PANGOLIN}; + VisualizationBase::VisualizationOption viz_option{VisualizationBase::RVIZ}; viz_ = VisualizationBase::Create(viz_option); // create feature tracker @@ -34,13 +34,13 @@ WorldManager::WorldManager(std::shared_ptr& setting) : params_(setting) map_manager_.reset(new MapManager(params_, current_frame_, feature_extractor_, tracker_)); // create visual frontend - visual_frontend_.reset(new VisualFrontEnd(params_, current_frame_, map_manager_, tracker_)); + visual_frontend_.reset(new VisualFrontEnd(params_, current_frame_, map_manager_, tracker_, viz_)); // create mapping thread, and mapping will create sub thread for Estimator and LoopClosing mapping_.reset(new Mapping(params_, map_manager_, current_frame_)); com::printHelloWorldVIO(); - com::printFZ(); + com::printKeyboard(); } void WorldManager::run() { @@ -142,7 +142,7 @@ void WorldManager::setupCalibration() { // TODO: Change this and directly add the extrinsic parameters within the // constructor (maybe set default parameters on extrinsic with identity / // zero) - // calib_model_right_->setupExtrinsic(params_->T_left_right_); + calib_model_right_->setupExtrinsic(params_->extrinsic_setting_.T_left_right_); } } } // namespace viohw \ No newline at end of file