From 444a1c6226b381546e350079351a6d014ab55daf Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Wed, 9 Oct 2019 23:32:30 -0400 Subject: [PATCH] delete tracks outside of image --- src/feature/lk_tracker.cc | 7 +++++++ src/module/stereo_module.cc | 5 ++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/feature/lk_tracker.cc b/src/feature/lk_tracker.cc index 9ca6f17..4fc873c 100644 --- a/src/feature/lk_tracker.cc +++ b/src/feature/lk_tracker.cc @@ -127,6 +127,13 @@ int LKTracker::DoTrack(std::vector &landmarks, data::Frame &cur_ } } } + Vector2d pixelCur; + pixelCur << results[i].x, results[i].y; + Vector3d tmp; + if (!cur_frame.GetCameraModel().UnprojectToBearing(pixelCur, tmp)) + { + continue; + } if (status[i] == 1 && err[i] <= errThresh_) { cv::KeyPoint kpt(results[i], origKpt[i].size); diff --git a/src/module/stereo_module.cc b/src/module/stereo_module.cc index d950d26..0c6492e 100644 --- a/src/module/stereo_module.cc +++ b/src/module/stereo_module.cc @@ -72,7 +72,10 @@ void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, dou cv::circle(curMask_, pt1, 3, cv::Scalar(255, 0, 0), -1); cv::circle(curMask_, pt2 + cv::Point2f(curMask_.cols / 2., 0), 3, cv::Scalar(255, 0, 0), -1); cv::line(curMask_, pt1, pt2 + cv::Point2f(curMask_.cols / 2., 0), cv::Scalar(255, 0, 0), 1); - cv::circle(curDepth_, pt1, 2, cv::Scalar((depth / maxDepth_ - err) * 255, (depth / maxDepth_ - err) * 255, depth / maxDepth_ * 255)); + double depthColor = 1 - depth / maxDepth_; + depthColor *= 0.9 * depthColor; + depthColor += 0.1; + cv::circle(curDepth_, pt1, 2, cv::Scalar((depthColor - err) * 255, (depthColor - err) * 255, depthColor * 255)); } void StereoModule::Visualization::Draw(cv::Mat &img, const cv::Mat &stereo_img)