1 pt ransac

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<Feature>& Landmark::GetObservations() const
 {
     return obs_;

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<Feature>& GetObservations() const;
     std::vector<Feature>& GetObservations();
     const std::vector<Feature>& GetStereoObservations() const;

src/feature/lk_tracker.cc

@@ -22,7 +22,7 @@ void LKTracker::Init(data::Frame &init_frame)
     prevFrame_ = &init_frame;
 }
 
-int LKTracker::Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors)
+int LKTracker::Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors, bool stereo)
 {
     if (prevFrame_ == nullptr)
     {
@@ -34,6 +34,9 @@ int LKTracker::Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_fr
     std::vector<cv::Point2f> pointsToTrack;
     std::vector<cv::KeyPoint> origKpt;
     std::vector<int> origInx;
+    std::vector<cv::Point2f> stereoPointsToTrack;
+    std::vector<cv::KeyPoint> stereoOrigKpt;
+    std::vector<int> stereoOrigInx;
     for (int i = 0; i < landmarks.size(); i++)
     {
         data::Landmark &landmark = landmarks[i];
@@ -44,6 +47,16 @@ int LKTracker::Track(std::vector<data::Landmark> &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<data::Landmark> &landmarks, data::Frame &cur_fr
     std::vector<cv::Point2f> results;
     std::vector<unsigned char> status;
     std::vector<float> err;
+    std::vector<cv::Point2f> stereoResults;
+    std::vector<unsigned char> stereoStatus;
+    std::vector<float> 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<data::Landmark> &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();

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<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors);
+    int Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors, bool stereo = true);
 
 private:
     cv::TermCriteria termCrit_;

src/feature/tracker.h

@@ -13,7 +13,7 @@ class Tracker
 {
 public:
     virtual void Init(data::Frame &init_frame) = 0;
-    virtual int Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors) = 0;
+    virtual int Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors, bool stereo = true) = 0;
 };
 
 }

src/module/tracking_module.cc

@@ -55,6 +55,20 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
                 landmarks_[i].RemoveLastObservation();
             }
         }
+        if (frames_.back()->HasStereoImage())
+        {
+            Matrix3d E;
+            std::vector<int> inlierIndices;
+            fivePointChecker_->ComputeE(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices, true);
+            std::unordered_set<int> 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;