From 6ee9c3b3e259cffb2ecbc400930264a3424f9c5a Mon Sep 17 00:00:00 2001
From: raphaelchang <raphaelchang1227@gmail.com>
Date: Thu, 19 Sep 2019 17:07:15 -0400
Subject: [PATCH] add stereo constraints

---
 src/optimization/bundle_adjuster.cc   | 21 ++++++++++++++++++---
 src/optimization/reprojection_error.h | 26 +++++++++++++++++++++++++-
 2 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc
index d88e7e6..3107860 100644
--- a/src/optimization/bundle_adjuster.cc
+++ b/src/optimization/bundle_adjuster.cc
@@ -110,13 +110,28 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
                 continue;
             }
             ceres::CostFunction *cost_function = nullptr;
+            const data::Feature *stereoFeat = feature.GetFrame().HasStereoImage() ? landmark.GetStereoObservationByFrameID(feature.GetFrame().GetID()) : nullptr;
             if (feature.GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective)
             {
-                cost_function = ReprojectionError<camera::Perspective>::Create(feature);
+                if (stereoFeat != nullptr)
+                {
+                    cost_function = ReprojectionError<camera::Perspective>::Create(feature, *stereoFeat);
+                }
+                else
+                {
+                    cost_function = ReprojectionError<camera::Perspective>::Create(feature);
+                }
             }
             else if (feature.GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere)
             {
-                cost_function = ReprojectionError<camera::DoubleSphere>::Create(feature);
+                if (stereoFeat != nullptr)
+                {
+                    cost_function = ReprojectionError<camera::DoubleSphere>::Create(feature, *stereoFeat);
+                }
+                else
+                {
+                    cost_function = ReprojectionError<camera::DoubleSphere>::Create(feature);
+                }
             }
             if (cost_function != nullptr)
             {
@@ -142,7 +157,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
         bool hasEstCameraPoses = false;
         for (const data::Feature &feature : landmark.GetObservations())
         {
-            if (feature.GetFrame().HasEstimatedPose())
+            if (feature.GetFrame().HasEstimatedPose() && feature.GetFrame().IsEstimatedByLandmark(landmark.GetID()))
             {
                 hasEstCameraPoses = true;
             }
diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h
index 4cfc68c..2c5e2fc 100644
--- a/src/optimization/reprojection_error.h
+++ b/src/optimization/reprojection_error.h
@@ -15,7 +15,16 @@ class ReprojectionError
 {
 public:
     ReprojectionError(const data::Feature &feature)
-        : feature_(feature)
+        : feature_(feature),
+        stereoFeature_(feature),
+        hasStereo_(false)
+    {
+    }
+
+    ReprojectionError(const data::Feature &feature, const data::Feature &stereo_feature)
+        : feature_(feature),
+        stereoFeature_(stereo_feature),
+        hasStereo_(true)
     {
     }
 
@@ -32,6 +41,14 @@ class ReprojectionError
         camera.ProjectToImage(camPt, reprojPoint);
         reproj_error[0] = reprojPoint(0) - T(feature_.GetKeypoint().pt.x);
         reproj_error[1] = reprojPoint(1) - T(feature_.GetKeypoint().pt.y);
+        if (hasStereo_ && stereoFeature_.GetFrame().HasStereoImage())
+        {
+            Matrix<T, 2, 1> reprojPoint2;
+            Matrix<T, 3, 4> stereoPose = stereoFeature_.GetFrame().GetStereoPose().cast<T>();
+            camera.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);
+        }
         return true;
     }
 
@@ -40,8 +57,15 @@ class ReprojectionError
         return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(new ReprojectionError<C>(feature));
     }
 
+    static ceres::CostFunction* Create(const data::Feature &feature, const data::Feature &stereo_feature)
+    {
+        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(new ReprojectionError<C>(feature, stereo_feature));
+    }
+
 private:
     const data::Feature feature_;
+    const data::Feature stereoFeature_;
+    bool hasStereo_;
 };
 
 }