more estimation logic

raphaelchang · Sep 12, 2019 · 327f2bf · 327f2bf
1 parent 23ee5ad
commit 327f2bf
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 10 deletions.
diff --git a/src/data/feature.cc b/src/data/feature.cc
@@ -62,10 +62,33 @@ Vector3d Feature::GetWorldPoint()
     return worldPoint_;
 }
 
+Vector3d Feature::GetEstimatedWorldPoint()
+{
+    if (worldPointEstimateCached_)
+    {
+        return worldPointEstimate_;
+    }
+    Vector3d worldFramePt = GetBearing();
+    bool wasCompressed = frame_.IsCompressed();
+    worldFramePt *= frame_.GetDepthImage().at<double>((int)(kpt_.pt.y + 0.5), (int)(kpt_.pt.x + 0.5));
+    if (wasCompressed)
+    {
+        frame_.CompressImages();
+    }
+    worldPointEstimate_ = util::TFUtil::TransformPoint(frame_.GetEstimatedPose(), worldFramePt);
+    worldPointEstimateCached_ = true;
+    return worldPointEstimate_;
+}
+
 bool Feature::HasWorldPoint() const
 {
     return frame_.HasPose() && frame_.HasDepthImage();
 }
 
+bool Feature::HasEstimatedWorldPoint() const
+{
+    return frame_.HasEstimatedPose() && frame_.HasDepthImage();
+}
+
 }
 }
diff --git a/src/data/feature.h b/src/data/feature.h
@@ -24,14 +24,18 @@ class Feature
 
     Vector3d GetBearing() const;
     Vector3d GetWorldPoint();
+    Vector3d GetEstimatedWorldPoint();
     bool HasWorldPoint() const;
+    bool HasEstimatedWorldPoint() const;
 
     bool worldPointCached_{false};
+    bool worldPointEstimateCached_{false};
 private:
     Frame &frame_;
     cv::KeyPoint kpt_;
     cv::Mat descriptor_;
     Vector3d worldPoint_;
+    Vector3d worldPointEstimate_;
 };
 
 }

diff --git a/src/data/landmark.cc b/src/data/landmark.cc
@@ -18,6 +18,12 @@ void Landmark::AddObservation(Feature obs, bool compute_gnd)
             groundTruth_ = obs.GetWorldPoint();
             hasGroundTruth_ = true;
         }
+        if (obs.HasEstimatedWorldPoint())
+        {
+            posEstimate_ = obs.GetEstimatedWorldPoint();
+            obsForEst_.push_back(obs);
+            hasPosEstimate_ = true;
+        }
     }
     if (idToIndex_.find(obs.GetFrame().GetID()) != idToIndex_.end())
     {

diff --git a/src/reconstruction/triangulator.cc b/src/reconstruction/triangulator.cc
@@ -41,16 +41,16 @@ bool Triangulator::TriangulateNViews(const std::vector<data::Feature> &views, Ve
     {
         Matrix<double, 3, 4> pose;
         Matrix<double, 3, 4> invpose;
-        if (view.GetFrame().HasPose())
-        {
-            pose = view.GetFrame().GetInversePose();
-            invpose = view.GetFrame().GetPose();
-        }
-        else if (view.GetFrame().HasEstimatedPose())
+        if (view.GetFrame().HasEstimatedPose())
         {
             pose = view.GetFrame().GetEstimatedInversePose();
             invpose = view.GetFrame().GetEstimatedPose();
         }
+        else if (view.GetFrame().HasPose())
+        {
+            pose = view.GetFrame().GetInversePose();
+            invpose = view.GetFrame().GetPose();
+        }
         else
         {
             continue;
@@ -74,13 +74,13 @@ bool Triangulator::CheckReprojectionErrors(const std::vector<data::Feature> &vie
     for (const data::Feature &view : views)
     {
         Matrix<double, 3, 4> pose;
-        if (view.GetFrame().HasPose())
+        if (view.GetFrame().HasEstimatedPose())
         {
-            pose = view.GetFrame().GetInversePose();
+            pose = view.GetFrame().GetEstimatedInversePose();
         }
-        else if (view.GetFrame().HasEstimatedPose())
+        else if (view.GetFrame().HasPose())
         {
-            pose = view.GetFrame().GetEstimatedInversePose();
+            pose = view.GetFrame().GetInversePose();
         }
         else
         {