From c848cbd4908b640306860362200ba371469d56c8 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Wed, 2 Oct 2019 19:54:03 -0400 Subject: [PATCH] 1 pt ransac --- src/data/landmark.cc | 12 +++ src/data/landmark.h | 1 + src/feature/lk_tracker.cc | 36 ++++++- src/feature/lk_tracker.h | 2 +- src/feature/tracker.h | 2 +- src/module/tracking_module.cc | 14 +++ src/odometry/five_point.cc | 150 ++++++++++++++++++++------ src/odometry/five_point.h | 15 ++- src/optimization/reprojection_error.h | 6 +- src/ros/odometry_eval.cc | 2 +- src/ros/tracking_eval.cc | 2 +- 11 files changed, 194 insertions(+), 48 deletions(-) diff --git a/src/data/landmark.cc b/src/data/landmark.cc index 0662927..9475aa6 100644 --- a/src/data/landmark.cc +++ b/src/data/landmark.cc @@ -49,10 +49,22 @@ void Landmark::AddStereoObservation(Feature obs) void Landmark::RemoveLastObservation() { int id = obs_.back().GetFrame().GetID(); + if (idToStereoIndex_.find(id) != idToStereoIndex_.end() && stereoObs_.back().GetFrame().GetID() == id) + { + stereoObs_.pop_back(); + idToStereoIndex_.erase(id); + } obs_.pop_back(); idToIndex_.erase(id); } +void Landmark::RemoveLastStereoObservation() +{ + int id = stereoObs_.back().GetFrame().GetID(); + stereoObs_.pop_back(); + idToStereoIndex_.erase(id); +} + const std::vector& Landmark::GetObservations() const { return obs_; diff --git a/src/data/landmark.h b/src/data/landmark.h index d023834..08bebd8 100644 --- a/src/data/landmark.h +++ b/src/data/landmark.h @@ -20,6 +20,7 @@ class Landmark void AddObservation(Feature obs, bool compute_gnd = true); void AddStereoObservation(Feature obs); void RemoveLastObservation(); + void RemoveLastStereoObservation(); const std::vector& GetObservations() const; std::vector& GetObservations(); const std::vector& GetStereoObservations() const; diff --git a/src/feature/lk_tracker.cc b/src/feature/lk_tracker.cc index 2000403..901219e 100644 --- a/src/feature/lk_tracker.cc +++ b/src/feature/lk_tracker.cc @@ -22,7 +22,7 @@ void LKTracker::Init(data::Frame &init_frame) prevFrame_ = &init_frame; } -int LKTracker::Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors) +int LKTracker::Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo) { if (prevFrame_ == nullptr) { @@ -34,6 +34,9 @@ int LKTracker::Track(std::vector &landmarks, data::Frame &cur_fr std::vector pointsToTrack; std::vector origKpt; std::vector origInx; + std::vector stereoPointsToTrack; + std::vector stereoOrigKpt; + std::vector stereoOrigInx; for (int i = 0; i < landmarks.size(); i++) { data::Landmark &landmark = landmarks[i]; @@ -44,6 +47,16 @@ int LKTracker::Track(std::vector &landmarks, data::Frame &cur_fr origKpt.push_back(feat->GetKeypoint()); origInx.push_back(i); } + if (cur_frame.HasStereoImage() && prevFrame_->HasStereoImage()) + { + const data::Feature *stereoFeat = landmark.GetStereoObservationByFrameID(prevId); + if (stereoFeat != nullptr) + { + stereoPointsToTrack.push_back(stereoFeat->GetKeypoint().pt); + stereoOrigKpt.push_back(stereoFeat->GetKeypoint()); + stereoOrigInx.push_back(i); + } + } } if (pointsToTrack.size() == 0) { @@ -53,7 +66,14 @@ int LKTracker::Track(std::vector &landmarks, data::Frame &cur_fr std::vector results; std::vector status; std::vector err; + std::vector stereoResults; + std::vector stereoStatus; + std::vector stereoErr; cv::calcOpticalFlowPyrLK(prevFrame_->GetImage(), cur_frame.GetImage(), pointsToTrack, results, status, err, windowSize_, numScales_, termCrit_, 0); + if (stereoPointsToTrack.size() > 0) + { + cv::calcOpticalFlowPyrLK(prevFrame_->GetStereoImage(), cur_frame.GetStereoImage(), stereoPointsToTrack, stereoResults, stereoStatus, stereoErr, windowSize_, numScales_, termCrit_, 0); + } errors.clear(); int numGood = 0; for (int i = 0; i < results.size(); i++) @@ -93,6 +113,20 @@ int LKTracker::Track(std::vector &landmarks, data::Frame &cur_fr numGood++; } } + for (int i = 0; i < stereoResults.size(); i++) + { + data::Landmark &landmark = landmarks[stereoOrigInx[i]]; + if (!landmark.IsObservedInFrame(cur_frame.GetID())) + { + continue; + } + if (stereoStatus[i] == 1 && stereoErr[i] <= errThresh_) + { + cv::KeyPoint kpt(stereoResults[i], stereoOrigKpt[i].size); + data::Feature feat(cur_frame, kpt); + landmark.AddStereoObservation(feat); + } + } if (prevCompressed) { prevFrame_->CompressImages(); diff --git a/src/feature/lk_tracker.h b/src/feature/lk_tracker.h index b60a7b5..571be96 100644 --- a/src/feature/lk_tracker.h +++ b/src/feature/lk_tracker.h @@ -18,7 +18,7 @@ class LKTracker : public Tracker public: LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int term_count = 50, const double term_eps = 0.01); void Init(data::Frame &init_frame); - int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors); + int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo = true); private: cv::TermCriteria termCrit_; diff --git a/src/feature/tracker.h b/src/feature/tracker.h index c2d109f..2a5c6b1 100644 --- a/src/feature/tracker.h +++ b/src/feature/tracker.h @@ -13,7 +13,7 @@ class Tracker { public: virtual void Init(data::Frame &init_frame) = 0; - virtual int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors) = 0; + virtual int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors, bool stereo = true) = 0; }; } diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index 838a8b6..59d8ca2 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -55,6 +55,20 @@ void TrackingModule::Update(std::unique_ptr &frame) landmarks_[i].RemoveLastObservation(); } } + if (frames_.back()->HasStereoImage()) + { + Matrix3d E; + std::vector inlierIndices; + fivePointChecker_->ComputeE(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices, true); + std::unordered_set inlierSet(inlierIndices.begin(), inlierIndices.end()); + for (int i = 0; i < landmarks_.size(); i++) + { + if (landmarks_[i].GetStereoObservationByFrameID(frames_.back()->GetID()) != nullptr && inlierSet.find(i) == inlierSet.end()) + { + landmarks_[i].RemoveLastStereoObservation(); + } + } + } } int i = 0; diff --git a/src/odometry/five_point.cc b/src/odometry/five_point.cc index 595ed3d..b44bec2 100644 --- a/src/odometry/five_point.cc +++ b/src/odometry/five_point.cc @@ -14,9 +14,11 @@ namespace omni_slam namespace odometry { -FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int num_ceres_threads) +FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int trans_ransac_iterations, double reprojection_threshold, int num_ceres_threads) : ransacIterations_(ransac_iterations), epipolarThreshold_(epipolar_threshold), + transIterations_(trans_ransac_iterations), + reprojectionThreshold_(reprojection_threshold), numCeresThreads_(num_ceres_threads) { } @@ -32,7 +34,7 @@ int FivePoint::Compute(const std::vector &landmarks, data::Frame int bestPoints = 0; Matrix bestPose; std::vector bestXs; - std::vector bestFeats; + std::vector> bestFeats; std::vector bestIds; for (int i = 0; i < rs.size(); i++) { @@ -41,7 +43,7 @@ int FivePoint::Compute(const std::vector &landmarks, data::Frame P.block<3, 1>(0, 3) = ts[i]; int goodPoints = 0; std::vector xs; - std::vector feats; + std::vector> feats; std::vector ids; for (int inx : inlier_indices) { @@ -54,22 +56,11 @@ int FivePoint::Compute(const std::vector &landmarks, data::Frame if (cur_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(X), pix) && prev_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(P, X)), pix)) { goodPoints++; - if (prev_frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr) + if (prev_frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(cur_frame.GetID()) != nullptr) { + const data::Feature *stereoFeat = landmark.GetStereoObservationByFrameID(cur_frame.GetID()); xs.push_back(landmark.GetEstimatedPosition()); - feats.push_back(feat2); - ids.push_back(landmark.GetID()); - } - else if (!prev_frame.HasStereoImage() && landmark.HasEstimatedPosition()) - { - xs.push_back(landmark.GetEstimatedPosition()); - feats.push_back(feat2); - ids.push_back(landmark.GetID()); - } - else if (!prev_frame.HasStereoImage() && landmark.HasGroundTruth()) - { - xs.push_back(landmark.GetGroundTruth()); - feats.push_back(feat2); + feats.push_back({feat2, stereoFeat}); ids.push_back(landmark.GetID()); } } @@ -100,14 +91,21 @@ int FivePoint::Compute(const std::vector &landmarks, data::Frame estPose = bestPose; t << 0, 0, 0; } - ComputeTranslation(bestXs, bestFeats, util::TFUtil::GetRotationFromPoseMatrix(estPose), t); + std::vector transInliers; + inliers = ComputeTranslation(bestXs, bestFeats, util::TFUtil::GetRotationFromPoseMatrix(estPose), t, transInliers); + std::vector transInlierIds; + transInlierIds.reserve(inliers); + for (int i : transInliers) + { + transInlierIds.push_back(bestIds[i]); + } estPose.block<3, 1>(0, 3) = t; - cur_frame.SetEstimatedInversePose(estPose, bestIds); + cur_frame.SetEstimatedInversePose(estPose, transInlierIds); return inliers; } -int FivePoint::ComputeE(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const +int FivePoint::ComputeE(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices, bool stereo) const { std::vector x1; std::vector x2; @@ -115,10 +113,10 @@ int FivePoint::ComputeE(const std::vector &landmarks, const data int i = 0; for (const data::Landmark &landmark : landmarks) { - if (landmark.IsObservedInFrame(frame2.GetID()) && landmark.IsObservedInFrame(frame1.GetID())) + const data::Feature *feat1 = stereo ? landmark.GetStereoObservationByFrameID(frame1.GetID()) : landmark.GetObservationByFrameID(frame1.GetID()); + const data::Feature *feat2 = stereo ? landmark.GetStereoObservationByFrameID(frame2.GetID()) : landmark.GetObservationByFrameID(frame2.GetID()); + if (feat1 != nullptr && feat2 != nullptr) { - const data::Feature *feat1 = landmark.GetObservationByFrameID(frame1.GetID()); - const data::Feature *feat2 = landmark.GetObservationByFrameID(frame2.GetID()); x1.push_back(feat1->GetBearing().normalized()); x2.push_back(feat2->GetBearing().normalized()); indexToLandmarkIndex[x1.size() - 1] = i; @@ -129,8 +127,8 @@ int FivePoint::ComputeE(const std::vector &landmarks, const data { return 0; } - int inliers = RANSAC(x1, x2, E); - std::vector indices = GetInlierIndices(x1, x2, E); + int inliers = ERANSAC(x1, x2, E); + std::vector indices = GetEInlierIndices(x1, x2, E); inlier_indices.clear(); inlier_indices.reserve(indices.size()); for (int inx : indices) @@ -140,7 +138,7 @@ int FivePoint::ComputeE(const std::vector &landmarks, const data return inliers; } -int FivePoint::RANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const +int FivePoint::ERANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const { int maxInliers = 0; #pragma omp parallel for @@ -169,7 +167,7 @@ int FivePoint::RANSAC(const std::vector &x1, const std::vector maxInliers) @@ -183,7 +181,7 @@ int FivePoint::RANSAC(const std::vector &x1, const std::vector FivePoint::GetInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const +std::vector FivePoint::GetEInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const { std::vector indices; for (int i = 0; i < x1.size(); i++) @@ -305,10 +303,63 @@ void FivePoint::EssentialToPoses(const Matrix3d &E, std::vector &rs, s ts.emplace_back(-UWtVt.transpose() * U.col(2)); } -void FivePoint::ComputeTranslation(const std::vector &xs, const std::vector &ys, const Matrix3d &R, Vector3d &t) const +int FivePoint::ComputeTranslation(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, Vector3d &t, std::vector &inlier_indices) const +{ + int inliers = TranslationRANSAC(xs, ys, R, t); + inlier_indices = GetTranslationInlierIndices(xs, ys, R, t); + std::vector xs_inliers; + std::vector> ys_inliers; + xs_inliers.reserve(inlier_indices.size()); + ys_inliers.reserve(inlier_indices.size()); + for (int inx : inlier_indices) + { + xs_inliers.push_back(xs[inx]); + ys_inliers.push_back(ys[inx]); + } + OptimizeTranslation(xs_inliers, ys_inliers, R, t); + return inliers; +} + +int FivePoint::TranslationRANSAC(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, Vector3d &t) const +{ + int bestInliers = 0; + std::random_device rd; + std::mt19937 eng(rd()); + std::uniform_int_distribution<> distr(0, xs.size() - 1); + std::set randset; + while (randset.size() < std::min(transIterations_, (int)xs.size())) + { + randset.insert(distr(eng)); + } + std::vector indices; + std::copy(randset.begin(), randset.end(), std::back_inserter(indices)); + + #pragma omp parallel for + for (int i = 0; i < indices.size(); i++) + { + std::vector x{xs[indices[i]]}; + std::vector> y{ys[indices[i]]}; + Vector3d tmpT = t; + if (OptimizeTranslation(x, y, R, tmpT)) + { + int inliers = GetTranslationInlierIndices(xs, ys, R, tmpT).size(); + #pragma omp critical + { + if (inliers > bestInliers) + { + t = tmpT; + bestInliers = inliers; + } + } + } + } + return bestInliers; +} + +bool FivePoint::OptimizeTranslation(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, Vector3d &t) const { ceres::Problem problem; - ceres::LossFunction *loss_function = new ceres::HuberLoss(0.001); + ceres::LossFunction *loss_function = nullptr; std::vector landmarks; landmarks.reserve(3 * xs.size()); Quaterniond quat(R); @@ -322,13 +373,13 @@ void FivePoint::ComputeTranslation(const std::vector &xs, const std::v landmarks.push_back(x(1)); landmarks.push_back(x(2)); ceres::CostFunction *cost_function = nullptr; - if (ys[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective) + if (ys[i].first->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective) { - cost_function = optimization::ReprojectionError::Create(*ys[i]); + cost_function = optimization::ReprojectionError::Create(*ys[i].first, *ys[i].second); } - else if (ys[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere) + else if (ys[i].first->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere) { - cost_function = optimization::ReprojectionError::Create(*ys[i]); + cost_function = optimization::ReprojectionError::Create(*ys[i].first, *ys[i].second); } if (cost_function != nullptr) { @@ -347,10 +398,39 @@ void FivePoint::ComputeTranslation(const std::vector &xs, const std::v ceres::Solve(options, &problem, &summary); if (!summary.IsSolutionUsable()) { - return; + return false; } const Vector3d tRes = Map(&tData[0]); t = tRes; + return true; +} + +std::vector FivePoint::GetTranslationInlierIndices(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, const Vector3d &t) const +{ + std::vector indices; + double thresh = reprojectionThreshold_ * reprojectionThreshold_; + for (int i = 0; i < xs.size(); i++) + { + Matrix pose; + pose.block<3, 3>(0, 0) = R; + pose.block<3, 1>(0, 3) = t; + const Vector3d camPt = util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, xs[i])); + Vector2d reprojPoint; + ys[i].first->GetFrame().GetCameraModel().ProjectToImage(camPt, reprojPoint); + Vector2d stereoReprojPoint; + ys[i].second->GetFrame().GetStereoCameraModel().ProjectToImage(util::TFUtil::TransformPoint(ys[i].second->GetFrame().GetStereoPose(), camPt), stereoReprojPoint); + Vector2d pt; + pt << ys[i].first->GetKeypoint().pt.x, ys[i].first->GetKeypoint().pt.y; + Vector2d stereoPt; + stereoPt << ys[i].second->GetKeypoint().pt.x, ys[i].second->GetKeypoint().pt.y; + double reprojError = (reprojPoint - pt).squaredNorm(); + double stereoReprojError = (stereoReprojPoint - stereoPt).squaredNorm(); + if (reprojError < thresh && stereoReprojError < thresh) + { + indices.push_back(i); + } + } + return indices; } Vector3d FivePoint::TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix &pose1, const Matrix &pose2) const diff --git a/src/odometry/five_point.h b/src/odometry/five_point.h index 7137b7a..2010c7d 100644 --- a/src/odometry/five_point.h +++ b/src/odometry/five_point.h @@ -15,21 +15,26 @@ namespace odometry class FivePoint : public PoseEstimator { public: - FivePoint(int ransac_iterations, double epipolar_threshold, int num_ceres_threads = 1); + 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 ComputeE(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, 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: - int RANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const; + int ERANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const; void FivePointRelativePose(const std::vector &x1, const std::vector &x2, std::vector &Es) const; - std::vector GetInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const; + std::vector GetEInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const; void EssentialToPoses(const Matrix3d &E, std::vector &rs, std::vector &ts) const; - void ComputeTranslation(const std::vector &xs, const std::vector &ys, const Matrix3d &R, Vector3d &t) const; + int ComputeTranslation(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, Vector3d &t, std::vector &inlier_indices) const; + int TranslationRANSAC(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, Vector3d &t) const; + bool OptimizeTranslation(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, Vector3d &t) const; + std::vector GetTranslationInlierIndices(const std::vector &xs, const std::vector> &ys, const Matrix3d &R, const Vector3d &t) const; Vector3d TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix &pose1, const Matrix &pose2) const; int ransacIterations_; + int transIterations_; double epipolarThreshold_; + double reprojectionThreshold_; int numCeresThreads_; }; diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h index 0337c87..e967bce 100644 --- a/src/optimization/reprojection_error.h +++ b/src/optimization/reprojection_error.h @@ -47,8 +47,8 @@ class ReprojectionError Matrix stereoPose = stereoFeature_.GetFrame().GetStereoPose().cast(); C stereoCamera(feature_.GetFrame().GetStereoCameraModel()); stereoCamera.ProjectToImage(util::TFUtil::TransformPoint(stereoPose, camPt), reprojPoint2); - reproj_error[0] += reprojPoint(0) - T(stereoFeature_.GetKeypoint().pt.x); - reproj_error[1] += reprojPoint(1) - T(stereoFeature_.GetKeypoint().pt.y); + reproj_error[2] = reprojPoint2(0) - T(stereoFeature_.GetKeypoint().pt.x); + reproj_error[3] = reprojPoint2(1) - T(stereoFeature_.GetKeypoint().pt.y); } return true; } @@ -60,7 +60,7 @@ class ReprojectionError static ceres::CostFunction* Create(const data::Feature &feature, const data::Feature &stereo_feature) { - return new ceres::AutoDiffCostFunction(new ReprojectionError(feature, stereo_feature)); + return new ceres::AutoDiffCostFunction(new ReprojectionError(feature, stereo_feature)); } private: diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index 7813aab..1bd7ed4 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -41,7 +41,7 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod 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, numCeresThreads)); + //unique_ptr poseEstimator(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, iterations, reprojThresh, numCeresThreads)); unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres)); odometryModule_.reset(new module::OdometryModule(poseEstimator, bundleAdjuster)); diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index bcd1198..eb7ffeb 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -49,7 +49,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod unique_ptr tracker(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh)); - unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold)); + unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, 0, 0)); trackingModule_.reset(new module::TrackingModule(detector, tracker, checker, minFeaturesRegion, maxFeaturesRegion)); }