From 858f56972efb54765cbc28022a30d39f6eb21ef9 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Tue, 8 Oct 2019 00:25:02 -0400 Subject: [PATCH] Descriptor based tracking --- CMakeLists.txt | 3 +- launch/odometry_eval.launch | 3 ++ launch/reconstruction_eval.launch | 3 +- launch/slam_eval.launch | 6 ++- launch/slam_eval_kitti.launch | 4 +- launch/slam_eval_t265.launch | 4 +- launch/slam_eval_tum.launch | 9 ++-- launch/tracking_eval.launch | 3 +- src/data/feature.cc | 5 ++ src/data/feature.h | 2 + src/data/landmark.cc | 18 +++++++ src/data/landmark.h | 2 + src/feature/descriptor_tracker.cc | 88 +++++++++++++++++++++++++++++++ src/feature/descriptor_tracker.h | 30 +++++++++++ src/feature/detector.cc | 55 ++++++++++++++----- src/feature/detector.h | 8 +-- src/feature/lk_tracker.cc | 6 ++- src/feature/matcher.cc | 14 +++-- src/feature/matcher.h | 2 +- src/feature/region.h | 18 +++++++ src/feature/tracker.cc | 4 +- src/feature/tracker.h | 1 + src/module/matching_module.cc | 9 ++-- src/module/matching_module.h | 4 +- src/module/odometry_module.cc | 4 +- src/module/odometry_module.h | 2 +- src/module/tracking_module.cc | 65 +++++++++++++---------- src/module/tracking_module.h | 5 +- src/odometry/five_point.cc | 2 +- src/odometry/five_point.h | 2 +- src/odometry/pnp.cc | 2 +- src/odometry/pnp.h | 2 +- src/odometry/pose_estimator.cc | 2 +- src/odometry/pose_estimator.h | 4 +- src/ros/odometry_eval.cc | 22 ++++++-- src/ros/slam_eval.cc | 2 +- src/ros/tracking_eval.cc | 34 ++++++++++-- src/stereo/stereo_matcher.cc | 4 +- 38 files changed, 362 insertions(+), 91 deletions(-) create mode 100644 src/feature/descriptor_tracker.cc create mode 100644 src/feature/descriptor_tracker.h create mode 100644 src/feature/region.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f302062..bbed6b8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(omni_slam_eval) set(CMAKE_VERBOSE_MAKEFILE FALSE) if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) endif (NOT CMAKE_BUILD_TYPE) # find catkin dependencies @@ -65,6 +65,7 @@ add_library(omni_slam_eval_lib src/data/landmark.cc src/feature/tracker.cc src/feature/lk_tracker.cc + src/feature/descriptor_tracker.cc src/feature/detector.cc src/feature/matcher.cc src/reconstruction/triangulator.cc diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index 8ba926b..c6ee1c0 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -24,6 +24,8 @@ camera_parameters: $(arg camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 2000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + keyframe_interval: 1 + tracker_type: 'lk' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 @@ -33,6 +35,7 @@ tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 + odometry_type: 'pnp' pnp_inlier_threshold: 3.0 pnp_iterations: 3000 bundle_adjustment_max_iterations: 1000 diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index fe348b1..c3922fb 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -19,13 +19,14 @@ camera_parameters: $(arg camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + keyframe_interval: 1 + tracker_type: 'lk' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 - tracker_template_update_rate: 1 min_features_per_region: 10 max_features_per_region: 5000 max_reprojection_error: 5.0 diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch index cbb2440..c9842e7 100644 --- a/launch/slam_eval.launch +++ b/launch/slam_eval.launch @@ -29,15 +29,17 @@ stereo_camera_parameters: $(arg stereo_camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + keyframe_interval: 1 + tracker_type: 'lk' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 - tracker_error_threshold: 20.0 - tracker_template_update_rate: 1 + tracker_error_threshold: 128 min_features_per_region: 100 max_features_per_region: 1000 + odometry_type: 'pnp' pnp_inlier_threshold: 3.0 pnp_iterations: 3000 max_reprojection_error: 5.0 diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch index 19baca9..fbafbea 100644 --- a/launch/slam_eval_kitti.launch +++ b/launch/slam_eval_kitti.launch @@ -29,15 +29,17 @@ stereo_camera_parameters: $(arg stereo_camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + keyframe_interval: 1 + tracker_type: 'lk' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 - tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 + odometry_type: 'pnp' pnp_inlier_threshold: 3.0 pnp_iterations: 3000 max_reprojection_error: 2.0 diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch index f98ae02..cf1c761 100644 --- a/launch/slam_eval_t265.launch +++ b/launch/slam_eval_t265.launch @@ -29,15 +29,17 @@ stereo_camera_parameters: $(arg stereo_camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + keyframe_interval: 1 + tracker_type: 'lk' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 - tracker_template_update_rate: 1 min_features_per_region: 100 max_features_per_region: 1000 + odometry_type: 'pnp' pnp_inlier_threshold: 3.0 pnp_iterations: 3000 max_reprojection_error: 5.0 diff --git a/launch/slam_eval_tum.launch b/launch/slam_eval_tum.launch index a17c496..83cdb9c 100644 --- a/launch/slam_eval_tum.launch +++ b/launch/slam_eval_tum.launch @@ -27,17 +27,20 @@ camera_model: '$(arg camera_model)' camera_parameters: $(arg camera_params) stereo_camera_parameters: $(arg stereo_camera_params) - detector_type: 'GFTT' - detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + detector_type: 'ORB' + detector_parameters: {nfeatures: 1000} + descriptor_type: 'ORB' + keyframe_interval: 1 + tracker_type: 'descriptor' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 1000000.0 - tracker_template_update_rate: 1 min_features_per_region: 10 max_features_per_region: 1000 + odometry_type: 'pnp' pnp_inlier_threshold: 3.0 pnp_iterations: 3000 max_reprojection_error: 5.0 diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch index d78fc97..1dd1fdb 100644 --- a/launch/tracking_eval.launch +++ b/launch/tracking_eval.launch @@ -17,15 +17,16 @@ camera_parameters: $(arg camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 100, qualityLevel: 0.01, minDistance: 5, blockSize: 5} + tracker_type: 'lk' tracker_window_size: 128 tracker_num_scales: 4 tracker_checker_epipolar_threshold: 0.008 tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 3.0 tracker_error_threshold: 20.0 - tracker_template_update_rate: 1 min_features_per_region: 10 max_features_per_region: 999999 + keyframe_interval: 1 diff --git a/src/data/feature.cc b/src/data/feature.cc index 01d2a31..40ebbb2 100644 --- a/src/data/feature.cc +++ b/src/data/feature.cc @@ -37,6 +37,11 @@ const cv::Mat& Feature::GetDescriptor() const return descriptor_; } +void Feature::SetDescriptor(const cv::Mat& descriptor) +{ + descriptor_ = descriptor.clone(); +} + Vector3d Feature::GetBearing() const { Vector3d cameraFramePt; diff --git a/src/data/feature.h b/src/data/feature.h index f049f97..52a7b3f 100644 --- a/src/data/feature.h +++ b/src/data/feature.h @@ -22,6 +22,8 @@ class Feature const cv::KeyPoint& GetKeypoint() const; const cv::Mat& GetDescriptor() const; + void SetDescriptor(const cv::Mat& descriptor); + Vector3d GetBearing() const; Vector3d GetWorldPoint(); Vector3d GetEstimatedWorldPoint(); diff --git a/src/data/landmark.cc b/src/data/landmark.cc index 9475aa6..f70662e 100644 --- a/src/data/landmark.cc +++ b/src/data/landmark.cc @@ -115,6 +115,15 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const return &obs_[idToIndex_.at(frame_id)]; } +Feature* Landmark::GetObservationByFrameID(const int frame_id) +{ + if (idToIndex_.find(frame_id) == idToIndex_.end()) + { + return nullptr; + } + return &obs_[idToIndex_.at(frame_id)]; +} + const Feature* Landmark::GetStereoObservationByFrameID(const int frame_id) const { if (idToStereoIndex_.find(frame_id) == idToStereoIndex_.end()) @@ -124,6 +133,15 @@ const Feature* Landmark::GetStereoObservationByFrameID(const int frame_id) const return &stereoObs_[idToStereoIndex_.at(frame_id)]; } +Feature* Landmark::GetStereoObservationByFrameID(const int frame_id) +{ + if (idToStereoIndex_.find(frame_id) == idToStereoIndex_.end()) + { + return nullptr; + } + return &stereoObs_[idToStereoIndex_.at(frame_id)]; +} + void Landmark::SetEstimatedPosition(const Vector3d &pos, const std::vector &frame_ids) { posEstimate_ = pos; diff --git a/src/data/landmark.h b/src/data/landmark.h index 08bebd8..95f07b1 100644 --- a/src/data/landmark.h +++ b/src/data/landmark.h @@ -28,7 +28,9 @@ class Landmark const int GetFirstFrameID() const; const int GetNumObservations() const; const Feature* GetObservationByFrameID(const int frame_id) const; + Feature* GetObservationByFrameID(const int frame_id); const Feature* GetStereoObservationByFrameID(const int frame_id) const; + Feature* GetStereoObservationByFrameID(const int frame_id); void SetEstimatedPosition(const Vector3d &pos, const std::vector &frame_ids); void SetEstimatedPosition(const Vector3d &pos); diff --git a/src/feature/descriptor_tracker.cc b/src/feature/descriptor_tracker.cc new file mode 100644 index 0000000..1c3d821 --- /dev/null +++ b/src/feature/descriptor_tracker.cc @@ -0,0 +1,88 @@ +#include "descriptor_tracker.h" +#include "util/tf_util.h" + +namespace omni_slam +{ +namespace feature +{ + +DescriptorTracker::DescriptorTracker(std::string detector_type, std::string descriptor_type, std::map det_args, std::map desc_args, const float match_thresh, const int keyframe_interval) + : Tracker(keyframe_interval), + Matcher(descriptor_type, match_thresh), + Detector(detector_type, descriptor_type, det_args, desc_args) +{ +} + +int DescriptorTracker::DoTrack(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo) +{ + std::vector curLandmarks; + std::vector prevLandmarks; + int imsize = std::max(cur_frame.GetImage().rows, cur_frame.GetImage().cols); + #pragma omp parallel for collapse(2) + for (int i = 0; i < Region::rs.size() - 1; i++) + { + for (int j = 0; j < Region::ts.size() - 1; j++) + { + feature::Detector detector(*static_cast(this)); + detector.DetectInRadialRegion(cur_frame, curLandmarks, feature::Region::rs[i] * imsize, feature::Region::rs[i+1] * imsize, feature::Region::ts[j], feature::Region::ts[j+1]); + detector.DetectInRadialRegion(cur_frame, curLandmarks, feature::Region::rs[i] * imsize, feature::Region::rs[i+1] * imsize, feature::Region::ts[j], feature::Region::ts[j+1], true); + } + } + std::vector origInx; + int i = 0; + for (const data::Landmark &landmark : landmarks) + { + if (landmark.IsObservedInFrame(keyframeId_)) + { + data::Landmark l; + l.AddObservation(*landmark.GetObservationByFrameID(keyframeId_), false); + if (stereo) + { + const data::Feature* stereoFeat = landmark.GetStereoObservationByFrameID(keyframeId_); + if (stereoFeat != nullptr) + { + l.AddStereoObservation(*stereoFeat); + } + } + prevLandmarks.push_back(l); + origInx.push_back(i); + } + i++; + } + std::vector matches; + std::vector stereoMatches; + std::vector indices; + std::vector> distances; + std::vector> stereoDistances; + std::vector stereoIndices; + Match(curLandmarks, prevLandmarks, matches, distances, indices, false); + if (stereo) + { + Match(curLandmarks, prevLandmarks, stereoMatches, stereoDistances, stereoIndices, true); + } + errors.clear(); + int numGood = 0; + for (int i = 0; i < matches.size(); i++) + { + data::Landmark &landmark = landmarks[origInx[indices[i]]]; + data::Feature feat(cur_frame, matches[i].GetObservationByFrameID(cur_frame.GetID())->GetKeypoint(), matches[i].GetObservationByFrameID(cur_frame.GetID())->GetDescriptor()); + landmark.AddObservation(feat); + errors.push_back(distances[i][0]); + numGood++; + } + for (int i = 0; i < stereoMatches.size(); i++) + { + data::Landmark &landmark = landmarks[origInx[stereoIndices[i]]]; + if (!landmark.IsObservedInFrame(cur_frame.GetID())) + { + continue; + } + data::Feature feat(cur_frame, stereoMatches[i].GetObservationByFrameID(cur_frame.GetID())->GetKeypoint(), stereoMatches[i].GetObservationByFrameID(cur_frame.GetID())->GetDescriptor(), true); + landmark.AddStereoObservation(feat); + } + + return numGood; +} + +} +} diff --git a/src/feature/descriptor_tracker.h b/src/feature/descriptor_tracker.h new file mode 100644 index 0000000..b27ba00 --- /dev/null +++ b/src/feature/descriptor_tracker.h @@ -0,0 +1,30 @@ +#ifndef _DESCRIPTOR_TRACKER_H_ +#define _DESCRIPTOR_TRACKER_H_ + +#include "tracker.h" +#include "matcher.h" +#include "detector.h" +#include "region.h" +#include +#include "data/frame.h" +#include "data/landmark.h" +#include "odometry/five_point.h" +#include + +namespace omni_slam +{ +namespace feature +{ + +class DescriptorTracker : public Tracker, public Matcher, public Detector +{ +public: + DescriptorTracker(std::string detector_type, std::string descriptor_type, std::map det_args, std::map desc_args, const float match_thresh = 0., const int keyframe_interval = 1); + +private: + int DoTrack(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo); +}; + +} +} +#endif /* _DESCRIPTOR_TRACKER_H_ */ diff --git a/src/feature/detector.cc b/src/feature/detector.cc index 5cc1b94..9751cb5 100644 --- a/src/feature/detector.cc +++ b/src/feature/detector.cc @@ -425,21 +425,28 @@ Detector::Detector(const Detector &other) { } -int Detector::Detect(data::Frame &frame, std::vector &landmarks) const +int Detector::Detect(data::Frame &frame, std::vector &landmarks, bool stereo) const { cv::Mat noarr; - return DetectInRegion(frame, landmarks, noarr); + return DetectInRegion(frame, landmarks, noarr, stereo); } -int Detector::DetectInRectangularRegion(data::Frame &frame, std::vector &landmarks, cv::Point2f start, cv::Point2f end) const +int Detector::DetectInRectangularRegion(data::Frame &frame, std::vector &landmarks, cv::Point2f start, cv::Point2f end, bool stereo) const { + bool compressed = frame.IsCompressed(); cv::Mat mask = cv::Mat::zeros(frame.GetImage().size(), CV_8U); mask(cv::Rect(start, end)) = 255; - return DetectInRegion(frame, landmarks, mask); + int count = DetectInRegion(frame, landmarks, mask, stereo); + if (compressed) + { + frame.CompressImages(); + } + return count; } -int Detector::DetectInRadialRegion(data::Frame &frame, std::vector &landmarks, double start_r, double end_r, double start_t, double end_t) const +int Detector::DetectInRadialRegion(data::Frame &frame, std::vector &landmarks, double start_r, double end_r, double start_t, double end_t, bool stereo) const { + bool compressed = frame.IsCompressed(); cv::Mat mask = cv::Mat::zeros(frame.GetImage().size(), CV_8U); for (int i = 0; i < mask.rows; i++) { @@ -455,26 +462,32 @@ int Detector::DetectInRadialRegion(data::Frame &frame, std::vector &landmarks, cv::Mat &mask) const +int Detector::DetectInRegion(data::Frame &frame, std::vector &landmarks, cv::Mat &mask, bool stereo) const { bool compressed = frame.IsCompressed(); std::vector kpts; - detector_->detect(frame.GetImage(), kpts, mask); + const cv::Mat &img = stereo ? frame.GetStereoImage() : frame.GetImage(); + detector_->detect(img, kpts, mask); cv::Mat descs; if (descriptor_.get() != nullptr) { if (descriptorType_ == "LUCID") { cv::Mat rgb; - cv::cvtColor(frame.GetImage(), rgb, CV_GRAY2BGR); + cv::cvtColor(img, rgb, CV_GRAY2BGR); descriptor_->compute(rgb, kpts, descs); } else { - descriptor_->compute(frame.GetImage(), kpts, descs); + descriptor_->compute(img, kpts, descs); } } for (int i = 0; i < kpts.size(); i++) @@ -484,13 +497,27 @@ int Detector::DetectInRegion(data::Frame &frame, std::vector &la if (descriptor_.get() != nullptr) { cv::Mat desc = descs.row(i); - data::Feature feat(frame, kpt, desc); - landmark.AddObservation(feat); + data::Feature feat(frame, kpt, desc, stereo); + if (stereo) + { + landmark.AddStereoObservation(feat); + } + else + { + landmark.AddObservation(feat); + } } else { - data::Feature feat(frame, kpt); - landmark.AddObservation(feat); + data::Feature feat(frame, kpt, stereo); + if (stereo) + { + landmark.AddStereoObservation(feat); + } + else + { + landmark.AddObservation(feat); + } } #pragma omp critical { diff --git a/src/feature/detector.h b/src/feature/detector.h index 5900a84..a5d4ed4 100644 --- a/src/feature/detector.h +++ b/src/feature/detector.h @@ -56,10 +56,10 @@ class Detector Detector(std::string detector_type, std::string descriptor_type, std::map det_args, std::map desc_args); Detector(const Detector &other); - int Detect(data::Frame &frame, std::vector &landmarks) const; - int DetectInRectangularRegion(data::Frame &frame, std::vector &landmarks, cv::Point2f start, cv::Point2f end) const; - int DetectInRadialRegion(data::Frame &frame, std::vector &landmarks, double start_r, double end_r, double start_t, double end_t) const; - int DetectInRegion(data::Frame &frame, std::vector &landmarks, cv::Mat &mask) const; + int Detect(data::Frame &frame, std::vector &landmarks, bool stereo = false) const; + int DetectInRectangularRegion(data::Frame &frame, std::vector &landmarks, cv::Point2f start, cv::Point2f end, bool stereo = false) const; + int DetectInRadialRegion(data::Frame &frame, std::vector &landmarks, double start_r, double end_r, double start_t, double end_t, bool stereo = false) const; + int DetectInRegion(data::Frame &frame, std::vector &landmarks, cv::Mat &mask, bool stereo = false) const; std::string GetDetectorType(); std::string GetDescriptorType(); diff --git a/src/feature/lk_tracker.cc b/src/feature/lk_tracker.cc index 2b79ea8..9ca6f17 100644 --- a/src/feature/lk_tracker.cc +++ b/src/feature/lk_tracker.cc @@ -46,6 +46,10 @@ int LKTracker::DoTrack(std::vector &landmarks, data::Frame &cur_ origKpt.push_back(feat->GetKeypoint()); origInx.push_back(i); } + if (!stereo) + { + continue; + } if (cur_frame.HasStereoImage() && !keyframeStereoImg_.empty()) { const data::Feature *stereoFeat = landmark.GetStereoObservationByFrameID(keyframeId_); @@ -142,7 +146,7 @@ int LKTracker::DoTrack(std::vector &landmarks, data::Frame &cur_ if (stereoStatus[i] == 1 && stereoErr[i] <= errThresh_) { cv::KeyPoint kpt(stereoResults[i], stereoOrigKpt[i].size); - data::Feature feat(cur_frame, kpt); + data::Feature feat(cur_frame, kpt, true); landmark.AddStereoObservation(feat); } } diff --git a/src/feature/matcher.cc b/src/feature/matcher.cc index 6c154d4..e830b9c 100644 --- a/src/feature/matcher.cc +++ b/src/feature/matcher.cc @@ -18,13 +18,13 @@ Matcher::Matcher(std::string descriptor_type, double max_dist) } } -std::map, int> Matcher::Match(const std::vector &train, const std::vector &query, std::vector &matches, std::vector> &distances) const +std::map, int> Matcher::Match(const std::vector &train, const std::vector &query, std::vector &matches, std::vector> &distances, std::vector &query_match_indices, bool stereo) const { std::map trainDescriptors; std::map> trainDescInxToFeature; for (const data::Landmark &landmark : train) { - for (const data::Feature &feat : landmark.GetObservations()) + for (const data::Feature &feat : stereo ? landmark.GetStereoObservations() : landmark.GetObservations()) { const int id = feat.GetFrame().GetID(); trainDescInxToFeature[id].push_back(&feat); @@ -33,19 +33,24 @@ std::map, int> Matcher::Match(const std::vector queryDescriptors; std::map> queryDescInxToFeature; + std::map featureToQueryInx; + int i = 0; for (const data::Landmark &landmark : query) { - for (const data::Feature &feat : landmark.GetObservations()) + for (const data::Feature &feat : stereo ? landmark.GetStereoObservations() : landmark.GetObservations()) { const int id = feat.GetFrame().GetID(); queryDescInxToFeature[id].push_back(&feat); queryDescriptors[id].push_back(feat.GetDescriptor()); + featureToQueryInx[&feat] = i; } + i++; } std::map, int> numMatches; matches.clear(); distances.clear(); + query_match_indices.clear(); std::map featureToMatchesInx; for (auto &queryPair : queryDescriptors) { @@ -68,9 +73,10 @@ std::map, int> Matcher::Match(const std::vector()); + query_match_indices.push_back(featureToQueryInx[queryFeat]); } matches[featureToMatchesInx[queryFeat]].AddObservation(*trainDescInxToFeature[trainPair.first][match.trainIdx]); distances[featureToMatchesInx[queryFeat]].push_back(fabs(match.distance)); diff --git a/src/feature/matcher.h b/src/feature/matcher.h index 1998227..102d83d 100644 --- a/src/feature/matcher.h +++ b/src/feature/matcher.h @@ -18,7 +18,7 @@ class Matcher public: Matcher(std::string descriptor_type, double max_dist = 0); - std::map, int> Match(const std::vector &train, const std::vector &query, std::vector &matches, std::vector> &distances) const; + std::map, int> Match(const std::vector &train, const std::vector &query, std::vector &matches, std::vector> &distances, std::vector &query_match_indices, bool stereo = false) const; private: cv::Ptr matcher_; diff --git a/src/feature/region.h b/src/feature/region.h new file mode 100644 index 0000000..fffd021 --- /dev/null +++ b/src/feature/region.h @@ -0,0 +1,18 @@ +#ifndef _REGION_H_ +#define _REGION_H_ + +namespace omni_slam +{ +namespace feature +{ +namespace Region +{ + +const std::vector rs{0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.7}; +const std::vector ts{-M_PI, -3 * M_PI / 4, -M_PI / 2, -M_PI / 4, 0, M_PI / 4, M_PI / 2, 3 * M_PI / 4, M_PI}; + +} +} +} + +#endif /* _REGION_H_ */ diff --git a/src/feature/tracker.cc b/src/feature/tracker.cc index d52623f..5122410 100644 --- a/src/feature/tracker.cc +++ b/src/feature/tracker.cc @@ -16,6 +16,7 @@ void Tracker::Init(data::Frame &init_frame) frameNum_ = 0; prevId_ = init_frame.GetID(); prevFrame_ = &init_frame; + keyframe_ = &init_frame; keyframeId_ = init_frame.GetID(); keyframeImg_ = init_frame.GetImage().clone(); if (init_frame.HasStereoImage()) @@ -26,7 +27,7 @@ void Tracker::Init(data::Frame &init_frame) const data::Frame* Tracker::GetLastKeyframe() { - return prevFrame_; + return keyframe_; } int Tracker::Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo) @@ -43,6 +44,7 @@ int Tracker::Track(std::vector &landmarks, data::Frame &cur_fram prevFrame_ = &cur_frame; if (++frameNum_ % keyframeInterval_ == 0) { + keyframe_ = &cur_frame; keyframeId_ = cur_frame.GetID(); keyframeImg_ = cur_frame.GetImage().clone(); if (cur_frame.HasStereoImage()) diff --git a/src/feature/tracker.h b/src/feature/tracker.h index bbbe78a..0365ee8 100644 --- a/src/feature/tracker.h +++ b/src/feature/tracker.h @@ -25,6 +25,7 @@ class Tracker int keyframeId_; int prevId_; const data::Frame *prevFrame_; + const data::Frame *keyframe_; private: virtual int DoTrack(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo) = 0; diff --git a/src/module/matching_module.cc b/src/module/matching_module.cc index 5ee2b44..4980964 100644 --- a/src/module/matching_module.cc +++ b/src/module/matching_module.cc @@ -30,12 +30,12 @@ void MatchingModule::Update(std::unique_ptr &frame) vector curLandmarks; int imsize = max(frames_.back()->GetImage().rows, frames_.back()->GetImage().cols); #pragma omp parallel for collapse(2) - for (int i = 0; i < rs_.size() - 1; i++) + for (int i = 0; i < feature::Region::rs.size() - 1; i++) { - for (int j = 0; j < ts_.size() - 1; j++) + for (int j = 0; j < feature::Region::ts.size() - 1; j++) { feature::Detector detector(*detector_); - detector.DetectInRadialRegion(*frames_.back(), curLandmarks, rs_[i] * imsize, rs_[i+1] * imsize, ts_[j], ts_[j+1]); + detector.DetectInRadialRegion(*frames_.back(), curLandmarks, feature::Region::rs[i] * imsize, feature::Region::rs[i+1] * imsize, feature::Region::ts[j], feature::Region::ts[j+1]); } } @@ -61,7 +61,8 @@ void MatchingModule::Update(std::unique_ptr &frame) vector matches; vector> distances; - map, int> numMatches = matcher_->Match(landmarks_, curLandmarks, matches, distances); + vector indices; + map, int> numMatches = matcher_->Match(landmarks_, curLandmarks, matches, distances, indices); map, int> numGoodMatches; map, priority_queue> goodDists; map, priority_queue> badDists; diff --git a/src/module/matching_module.h b/src/module/matching_module.h index ad6d3c9..5735ca3 100644 --- a/src/module/matching_module.h +++ b/src/module/matching_module.h @@ -7,6 +7,7 @@ #include "feature/matcher.h" #include "feature/detector.h" +#include "feature/region.h" #include "data/frame.h" #include "data/landmark.h" @@ -58,9 +59,6 @@ class MatchingModule std::vector> frames_; std::vector landmarks_; - const std::vector rs_{0, 0.1, 0.2, 0.3, 0.4, 0.49}; - const std::vector ts_{-M_PI, -3 * M_PI / 4, -M_PI / 2, -M_PI / 4, 0, M_PI / 4, M_PI / 2, 3 * M_PI / 4, M_PI}; - double overlapThresh_; double distThresh_; diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc index 2e6bd84..9e35a5e 100644 --- a/src/module/odometry_module.cc +++ b/src/module/odometry_module.cc @@ -18,13 +18,13 @@ OdometryModule::OdometryModule(std::unique_ptr &&pose_e { } -void OdometryModule::Update(std::vector &landmarks, std::vector> &frames) +void OdometryModule::Update(std::vector &landmarks, std::unique_ptr &cur_frame, const data::Frame *keyframe) { if (landmarks.size() == 0) { return; } - poseEstimator_->Compute(landmarks, *frames.back(), **next(frames.rbegin())); + poseEstimator_->Compute(landmarks, *cur_frame, *keyframe); } void OdometryModule::BundleAdjust(std::vector &landmarks) diff --git a/src/module/odometry_module.h b/src/module/odometry_module.h index 1e1248f..d6e10d1 100644 --- a/src/module/odometry_module.h +++ b/src/module/odometry_module.h @@ -29,7 +29,7 @@ class OdometryModule OdometryModule(std::unique_ptr &pose_estimator, std::unique_ptr &bundle_adjuster); OdometryModule(std::unique_ptr &&pose_estimator, std::unique_ptr &&bundle_adjuster); - void Update(std::vector &landmarks, std::vector> &frames); + void Update(std::vector &landmarks, std::unique_ptr &cur_frame, const data::Frame *keyframe); void BundleAdjust(std::vector &landmarks); Stats& GetStats(); diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index 2975d9d..191b8d7 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -33,6 +33,7 @@ void TrackingModule::Update(std::unique_ptr &frame) if (frameNum_ == 0) { tracker_->Init(*frames_.back()); + lastKeyframe_ = tracker_->GetLastKeyframe(); visualization_.Init(frames_.back()->GetImage().size(), landmarks_.size()); @@ -40,14 +41,14 @@ void TrackingModule::Update(std::unique_ptr &frame) return; } - const data::Frame *keyframe = tracker_->GetLastKeyframe(); + lastKeyframe_ = tracker_->GetLastKeyframe(); vector trackErrors; int tracks = tracker_->Track(landmarks_, *frames_.back(), trackErrors); if (fivePointChecker_ && tracks > 0) { Matrix3d E; std::vector inlierIndices; - fivePointChecker_->ComputeE(landmarks_, *keyframe, *frames_.back(), E, inlierIndices); + fivePointChecker_->ComputeE(landmarks_, *lastKeyframe_, *frames_.back(), E, inlierIndices); std::unordered_set inlierSet(inlierIndices.begin(), inlierIndices.end()); for (int i = 0; i < landmarks_.size(); i++) { @@ -60,7 +61,7 @@ void TrackingModule::Update(std::unique_ptr &frame) { Matrix3d E; std::vector inlierIndices; - fivePointChecker_->ComputeE(landmarks_, *keyframe, *frames_.back(), E, inlierIndices, true); + fivePointChecker_->ComputeE(landmarks_, *lastKeyframe_, *frames_.back(), E, inlierIndices, true); std::unordered_set inlierSet(inlierIndices.begin(), inlierIndices.end()); for (int i = 0; i < landmarks_.size(); i++) { @@ -86,35 +87,38 @@ void TrackingModule::Update(std::unique_ptr &frame) double y = obs->GetKeypoint().pt.y - frames_.back()->GetImage().rows / 2. + 0.5; double r = sqrt(x * x + y * y) / imsize; double t = util::MathUtil::FastAtan2(y, x); - vector::const_iterator ri = upper_bound(rs_.begin(), rs_.end(), r); - vector::const_iterator ti = upper_bound(ts_.begin(), ts_.end(), t); - int rinx = min((int)(ri - rs_.begin()), (int)(rs_.size() - 1)) - 1; - int tinx = min((int)(ti - ts_.begin()), (int)(ts_.size() - 1)) - 1; + vector::const_iterator ri = upper_bound(feature::Region::rs.begin(), feature::Region::rs.end(), r); + vector::const_iterator ti = upper_bound(feature::Region::ts.begin(), feature::Region::ts.end(), t); + int rinx = min((int)(ri - feature::Region::rs.begin()), (int)(feature::Region::rs.size() - 1)) - 1; + int tinx = min((int)(ti - feature::Region::ts.begin()), (int)(feature::Region::ts.size() - 1)) - 1; regionCount_[{rinx, tinx}]++; regionLandmarks_[{rinx, tinx}].push_back(&landmark); const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID()); - Vector2d pixelGnd; - if (frames_.back()->HasPose() && landmark.HasGroundTruth()) + if (obsPrev != nullptr) { - if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd)) + Vector2d pixelGnd; + if (frames_.back()->HasPose() && landmark.HasGroundTruth()) { - Vector2d pixel; - pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y; - double error = (pixel - pixelGnd).norm(); + if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd)) + { + Vector2d pixel; + pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y; + double error = (pixel - pixelGnd).norm(); - visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i); + visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i); - double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5; - double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5; - double rg = sqrt(xg * xg + yg * yg) / imsize; - stats_.radialErrors.emplace_back(vector{rg, error}); - stats_.frameErrors.emplace_back(vector{(double)landmark.GetNumObservations() - 1, (double)i, rg, error}); + double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5; + double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5; + double rg = sqrt(xg * xg + yg * yg) / imsize; + stats_.radialErrors.emplace_back(vector{rg, error}); + stats_.frameErrors.emplace_back(vector{(double)landmark.GetNumObservations() - 1, (double)i, rg, error}); + } + } + else + { + visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i); } - } - else - { - visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i); } stats_.trackLengths[i]++; numGood++; @@ -156,13 +160,13 @@ void TrackingModule::Redetect() } int imsize = max(frames_.back()->GetImage().rows, frames_.back()->GetImage().cols); #pragma omp parallel for collapse(2) - for (int i = 0; i < rs_.size() - 1; i++) + for (int i = 0; i < feature::Region::rs.size() - 1; i++) { - for (int j = 0; j < ts_.size() - 1; j++) + for (int j = 0; j < feature::Region::ts.size() - 1; j++) { if (regionCount_.find({i, j}) == regionCount_.end() || regionCount_.at({i, j}) < minFeaturesRegion_) { - detector_->DetectInRadialRegion(*frames_.back(), landmarks_, rs_[i] * imsize, rs_[i+1] * imsize, ts_[j], ts_[j+1]); + detector_->DetectInRadialRegion(*frames_.back(), landmarks_, feature::Region::rs[i] * imsize, feature::Region::rs[i+1] * imsize, feature::Region::ts[j], feature::Region::ts[j+1]); } } } @@ -171,9 +175,9 @@ void TrackingModule::Redetect() void TrackingModule::Prune() { #pragma omp parallel for collapse(2) - for (int i = 0; i < rs_.size() - 1; i++) + for (int i = 0; i < feature::Region::rs.size() - 1; i++) { - for (int j = 0; j < ts_.size() - 1; j++) + for (int j = 0; j < feature::Region::ts.size() - 1; j++) { if (regionCount_.find({i, j}) != regionCount_.end() && regionCount_.at({i, j}) > maxFeaturesRegion_) { @@ -201,6 +205,11 @@ std::vector>& TrackingModule::GetFrames() return frames_; } +const data::Frame* TrackingModule::GetLastKeyframe() +{ + return lastKeyframe_; +} + TrackingModule::Stats& TrackingModule::GetStats() { return stats_; diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h index bc0b0b7..22deaf3 100644 --- a/src/module/tracking_module.h +++ b/src/module/tracking_module.h @@ -7,6 +7,7 @@ #include "feature/tracker.h" #include "feature/detector.h" +#include "feature/region.h" #include "odometry/five_point.h" #include "data/frame.h" #include "data/landmark.h" @@ -36,6 +37,7 @@ class TrackingModule std::vector& GetLandmarks(); std::vector>& GetFrames(); + const data::Frame* GetLastKeyframe(); Stats& GetStats(); void Visualize(cv::Mat &base_img); @@ -65,9 +67,8 @@ class TrackingModule std::vector> frames_; std::vector landmarks_; + const data::Frame *lastKeyframe_; - const std::vector rs_{0, 0.1, 0.2, 0.3, 0.4, 0.49}; - const std::vector ts_{-M_PI, -3 * M_PI / 4, -M_PI / 2, -M_PI / 4, 0, M_PI / 4, M_PI / 2, 3 * M_PI / 4, M_PI}; int minFeaturesRegion_; int maxFeaturesRegion_; std::map, int> regionCount_; diff --git a/src/odometry/five_point.cc b/src/odometry/five_point.cc index b44bec2..ae5d127 100644 --- a/src/odometry/five_point.cc +++ b/src/odometry/five_point.cc @@ -23,7 +23,7 @@ FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int trans { } -int FivePoint::Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const +int FivePoint::Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame, std::vector &inlier_indices) const { Matrix3d E; int inliers = ComputeE(landmarks, prev_frame, cur_frame, E, inlier_indices); diff --git a/src/odometry/five_point.h b/src/odometry/five_point.h index 2010c7d..7f5e98e 100644 --- a/src/odometry/five_point.h +++ b/src/odometry/five_point.h @@ -17,7 +17,7 @@ class FivePoint : public PoseEstimator public: FivePoint(int ransac_iterations, double epipolar_threshold, int trans_ransac_iterations, double reprojection_threshold, int num_ceres_threads = 1); - int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const; + int Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame, std::vector &inlier_indices) const; int ComputeE(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices, bool stereo = false) const; private: diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc index e965ad4..88b3de7 100644 --- a/src/odometry/pnp.cc +++ b/src/odometry/pnp.cc @@ -23,7 +23,7 @@ PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_th { } -int PNP::Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const +int PNP::Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame, std::vector &inlier_indices) const { std::vector xs; std::vector yns; diff --git a/src/odometry/pnp.h b/src/odometry/pnp.h index 939fba4..c00c7f9 100644 --- a/src/odometry/pnp.h +++ b/src/odometry/pnp.h @@ -19,7 +19,7 @@ class PNP : public PoseEstimator { public: PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads = 1); - int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const; + int Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame, std::vector &inlier_indices) const; private: int RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const; diff --git a/src/odometry/pose_estimator.cc b/src/odometry/pose_estimator.cc index a5c87d7..bad55a1 100644 --- a/src/odometry/pose_estimator.cc +++ b/src/odometry/pose_estimator.cc @@ -5,7 +5,7 @@ namespace omni_slam namespace odometry { -int PoseEstimator::Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame) const +int PoseEstimator::Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame) const { std::vector temp; return Compute(landmarks, cur_frame, prev_frame, temp); diff --git a/src/odometry/pose_estimator.h b/src/odometry/pose_estimator.h index 339589d..341ebec 100644 --- a/src/odometry/pose_estimator.h +++ b/src/odometry/pose_estimator.h @@ -12,8 +12,8 @@ namespace odometry class PoseEstimator { public: - virtual int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const = 0; - int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame) const; + virtual int Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame, std::vector &inlier_indices) const = 0; + int Compute(const std::vector &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame) const; }; } diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index 1bd7ed4..112e11f 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -29,6 +29,8 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod double fivePointThreshold; int fivePointRansacIterations; + string odometryType; + this->nhp_.param("output_frame", cameraFrame_, std::string("map")); this->nhp_.param("pnp_inlier_threshold", reprojThresh, 10.); this->nhp_.param("pnp_iterations", iterations, 1000); @@ -40,8 +42,22 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod this->nhp_.param("tracker_checker_epipolar_threshold", fivePointThreshold, 0.01745240643); this->nhp_.param("tracker_checker_iterations", fivePointRansacIterations, 1000); - unique_ptr poseEstimator(new odometry::PNP(iterations, reprojThresh, numCeresThreads)); - //unique_ptr poseEstimator(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, iterations, reprojThresh, numCeresThreads)); + this->nhp_.param("odometry_type", odometryType, string("pnp")); + + unique_ptr poseEstimator; + if (odometryType == "pnp") + { + poseEstimator.reset(new odometry::PNP(iterations, reprojThresh, numCeresThreads)); + } + else if (odometryType == "five_point") + { + poseEstimator.reset(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, iterations, reprojThresh, numCeresThreads)); + } + else + { + ROS_ERROR("Invalid odometry type specified"); + } + unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres)); odometryModule_.reset(new module::OdometryModule(poseEstimator, bundleAdjuster)); @@ -75,7 +91,7 @@ template void OdometryEval::ProcessFrame(unique_ptr &&frame) { this->trackingModule_->Update(frame); - odometryModule_->Update(this->trackingModule_->GetLandmarks(), this->trackingModule_->GetFrames()); + odometryModule_->Update(this->trackingModule_->GetLandmarks(), this->trackingModule_->GetFrames().back(), this->trackingModule_->GetLastKeyframe()); this->trackingModule_->Redetect(); this->visualized_ = false; diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc index 3316780..01177e7 100644 --- a/src/ros/slam_eval.cc +++ b/src/ros/slam_eval.cc @@ -31,7 +31,7 @@ void SLAMEval::InitPublishers() void SLAMEval::ProcessFrame(unique_ptr &&frame) { trackingModule_->Update(frame); - odometryModule_->Update(trackingModule_->GetLandmarks(), trackingModule_->GetFrames()); + odometryModule_->Update(trackingModule_->GetLandmarks(), trackingModule_->GetFrames().back(), trackingModule_->GetLastKeyframe()); reconstructionModule_->Update(trackingModule_->GetLandmarks()); if (baSlidingWindow_ > 0 && baSlidingInterval_ > 0 && (frameNum_ + 1) % baSlidingInterval_ == 0) { diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index b855a4d..b727271 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -1,6 +1,7 @@ #include "tracking_eval.h" #include "feature/lk_tracker.h" +#include "feature/descriptor_tracker.h" #include "feature/detector.h" #include "odometry/five_point.h" @@ -16,40 +17,65 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod : EvalBase(nh, nh_private) { string detectorType; + string descriptorType; int trackerWindowSize; int trackerNumScales; - int trackerTemplateUpdateRate; + int keyframeInterval; double fivePointThreshold; int fivePointRansacIterations; double trackerDeltaPixelErrorThresh; double trackerErrorThresh; map detectorParams; + map descriptorParams; int minFeaturesRegion; int maxFeaturesRegion; + string trackerType; this->nhp_.param("detector_type", detectorType, string("GFTT")); + this->nhp_.param("descriptor_type", descriptorType, string("ORB")); this->nhp_.param("tracker_window_size", trackerWindowSize, 128); this->nhp_.param("tracker_num_scales", trackerNumScales, 4); this->nhp_.param("tracker_checker_epipolar_threshold", fivePointThreshold, 0.01745240643); this->nhp_.param("tracker_checker_iterations", fivePointRansacIterations, 1000); this->nhp_.param("tracker_delta_pixel_error_threshold", trackerDeltaPixelErrorThresh, 5.0); this->nhp_.param("tracker_error_threshold", trackerErrorThresh, 20.); - this->nhp_.param("tracker_template_update_rate", trackerTemplateUpdateRate, 1); this->nhp_.param("min_features_per_region", minFeaturesRegion, 5); this->nhp_.param("max_features_per_region", maxFeaturesRegion, 5000); this->nhp_.getParam("detector_parameters", detectorParams); + this->nhp_.getParam("descriptor_parameters", descriptorParams); + this->nhp_.param("keyframe_interval", keyframeInterval, 1); + this->nhp_.param("tracker_type", trackerType, string("lk")); unique_ptr detector; if (feature::Detector::IsDetectorTypeValid(detectorType)) { - detector.reset(new feature::Detector(detectorType, detectorParams)); + if (trackerType == "lk") + { + detector.reset(new feature::Detector(detectorType, detectorParams)); + } + else if (trackerType == "descriptor") + { + detector.reset(new feature::Detector(detectorType, descriptorType, detectorParams, descriptorParams)); + } } else { ROS_ERROR("Invalid feature detector specified"); } - unique_ptr tracker(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh, trackerTemplateUpdateRate)); + unique_ptr tracker; + if (trackerType == "lk") + { + tracker.reset(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh, keyframeInterval)); + } + else if (trackerType == "descriptor") + { + tracker.reset(new feature::DescriptorTracker(detectorType, descriptorType, detectorParams, descriptorParams, trackerErrorThresh, keyframeInterval)); + } + else + { + ROS_ERROR("Invalid tracker type specified"); + } unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, 0, 0)); diff --git a/src/stereo/stereo_matcher.cc b/src/stereo/stereo_matcher.cc index facabb4..486f4ea 100644 --- a/src/stereo/stereo_matcher.cc +++ b/src/stereo/stereo_matcher.cc @@ -32,6 +32,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &landma return 0; } std::vector pointsToMatch; + std::vector descriptors; std::vector bearings1; std::vector origInx; for (int i = 0; i < landmarks.size(); i++) @@ -45,6 +46,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &landma if (feat != nullptr) { pointsToMatch.push_back(feat->GetKeypoint()); + descriptors.push_back(feat->GetDescriptor()); bearings1.push_back(util::TFUtil::WorldFrameToCameraFrame(feat->GetBearing().normalized())); origInx.push_back(i); } @@ -66,7 +68,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &landma { int inx = *it; Vector3d &bearing1 = bearings1[inx]; - data::Feature feat(frame, matchedPoints[inx], true); + data::Feature feat(frame, matchedPoints[inx], descriptors[inx], true); Vector3d bearing2 = util::TFUtil::WorldFrameToCameraFrame(feat.GetBearing().normalized()); RowVector3d epiplane1 = bearing2.transpose() * E;