From 327f2bf64b5fe865585b9d3739ddc276e316a0a4 Mon Sep 17 00:00:00 2001
From: raphaelchang <raphaelchang1227@gmail.com>
Date: Thu, 12 Sep 2019 02:55:05 -0400
Subject: [PATCH] more estimation logic

---
 src/data/feature.cc                | 23 +++++++++++++++++++++++
 src/data/feature.h                 |  4 ++++
 src/data/landmark.cc               |  6 ++++++
 src/reconstruction/triangulator.cc | 20 ++++++++++----------
 4 files changed, 43 insertions(+), 10 deletions(-)

diff --git a/src/data/feature.cc b/src/data/feature.cc
index f098278..483014f 100644
--- 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
index a944761..2a921a4 100644
--- 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
index 50d1563..7eb0a5c 100644
--- 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
index 31a1e5e..4172a7c 100644
--- 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
         {