working bundle adjustment

CMakeLists.txt

@@ -4,7 +4,7 @@ project(omni_slam_eval)
 
 set(CMAKE_VERBOSE_MAKEFILE FALSE)
 
-add_definitions("-std=c++11")
+add_definitions("-std=c++17")
 if (NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE)
 endif (NOT CMAKE_BUILD_TYPE)

src/camera/camera_model.h

@@ -15,13 +15,20 @@ template <typename T = double>
 class CameraModel
 {
 public:
+    enum Type
+    {
+        kPerspective,
+        kDoubleSphere
+    };
+
     CameraModel(std::string name)
     {
         name_ = name;
     }
     virtual bool ProjectToImage(const Matrix<T, 3, 1> &bearing, Matrix<T, 2, 1> &pixel) const = 0;
     virtual bool UnprojectToBearing(const Matrix<T, 2, 1> &pixel, Matrix<T, 3, 1> &bearing) const = 0;
-    virtual T GetFOV() = 0;
+    virtual T GetFOV() const = 0;
+    virtual Type GetType() const = 0;
 
 private:
     std::string name_;

src/camera/double_sphere.h

@@ -11,6 +11,8 @@ namespace camera
 template <typename T = double>
 class DoubleSphere : public CameraModel<T>
 {
+    template <typename> friend class DoubleSphere;
+
 public:
     DoubleSphere(const T fx, const T fy, const T cx, const T cy, const T chi, const T alpha)
         : CameraModel<T>("DoubleSphere"),
@@ -27,6 +29,12 @@ class DoubleSphere : public CameraModel<T>
         sinTheta_ = sin(theta);
     }
 
+    template <typename U>
+    DoubleSphere(const CameraModel<U> &other)
+        : DoubleSphere(T(static_cast<const DoubleSphere<U>&>(other).fx_), T(static_cast<const DoubleSphere<U>&>(other).fy_), T(static_cast<const DoubleSphere<U>&>(other).cx_), T(static_cast<const DoubleSphere<U>&>(other).cy_), T(static_cast<const DoubleSphere<U>&>(other).chi_), T(static_cast<const DoubleSphere<U>&>(other).alpha_))
+    {
+    }
+
     bool ProjectToImage(const Matrix<T, 3, 1> &bearing, Matrix<T, 2, 1> &pixel) const
     {
         const T &x = bearing(0);
@@ -49,6 +57,14 @@ class DoubleSphere : public CameraModel<T>
         //{
             //return false;
         //}
+
+        //Matrix<T, 3, 1> bearing_h;
+        //bearing_h << x, y, (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z));
+        //Matrix<T, 3, 1> pixel_h = cameraMat_ * bearing_h;
+        //pixel = pixel_h.hnormalized();
+        T denom = (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z));
+        pixel(0) = x * fx_ / denom + cx_;
+        pixel(1) = y * fy_ / denom + cy_;
         if (alpha_ > 0.5)
         {
             if (z <= sinTheta_ * d1)
@@ -65,14 +81,6 @@ class DoubleSphere : public CameraModel<T>
                 return false;
             }
         }
-
-        //Matrix<T, 3, 1> bearing_h;
-        //bearing_h << x, y, (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z));
-        //Matrix<T, 3, 1> pixel_h = cameraMat_ * bearing_h;
-        //pixel = pixel_h.hnormalized();
-        T denom = (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z));
-        pixel(0) = x * fx_ / denom + cx_;
-        pixel(1) = y * fy_ / denom + cy_;
         if (pixel(0) > 2. * cx_ || pixel(0) < 0. || pixel(1) > 2. * cy_ || pixel(1) < 0.)
         {
             return false;
@@ -107,7 +115,8 @@ class DoubleSphere : public CameraModel<T>
 
         return true;
     }
-    T GetFOV()
+
+    T GetFOV() const
     {
         T mx = cx_ / fx_;
         T r2;
@@ -124,6 +133,11 @@ class DoubleSphere : public CameraModel<T>
         return 2. * (M_PI / 2 - atan2(beta * mz - chi_, beta * mx));
     }
 
+    typename CameraModel<T>::Type GetType() const
+    {
+        return CameraModel<T>::kDoubleSphere;
+    }
+
 private:
     T fx_;
     T fy_;

src/camera/perspective.h

@@ -11,6 +11,8 @@ namespace camera
 template <typename T = double>
 class Perspective : public CameraModel<T>
 {
+    template <typename> friend class Perspective;
+
 public:
     Perspective(const T fx, const T fy, const T cx, const T cy)
         : CameraModel<T>("Perspective"),
@@ -22,6 +24,12 @@ class Perspective : public CameraModel<T>
         cameraMat_ << fx_, 0., cx_, 0., fy_, cy_, 0., 0., 1.;
     }
 
+    template <typename U>
+    Perspective(const CameraModel<U> &other)
+        : Perspective(T(static_cast<const Perspective<U>&>(other).fx_), T(static_cast<const Perspective<U>&>(other).fy_), T(static_cast<const Perspective<U>&>(other).cx_), T(static_cast<const Perspective<U>&>(other).cy_))
+    {
+    }
+
     bool ProjectToImage(const Matrix<T, 3, 1> &bearing, Matrix<T, 2, 1> &pixel) const
     {
         const T &x = bearing(0);
@@ -48,11 +56,16 @@ class Perspective : public CameraModel<T>
         return true;
     }
 
-    T GetFOV()
+    T GetFOV() const
     {
         return 2. * atan(cx_ / fx_);
     }
 
+    typename CameraModel<T>::Type GetType() const
+    {
+        return CameraModel<T>::kPerspective;
+    }
+
 private:
     T fx_;
     T fy_;

src/data/landmark.cc

@@ -32,6 +32,11 @@ const std::vector<Feature>& Landmark::GetObservations() const
     return obs_;
 }
 
+const std::vector<Feature>& Landmark::GetObservationsForEstimate() const
+{
+    return obsForEst_;
+}
+
 std::vector<Feature>& Landmark::GetObservations()
 {
     return obs_;
@@ -75,6 +80,12 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const
 void Landmark::SetEstimatedPosition(const Vector3d &pos)
 {
     posEstimate_ = pos;
+    obsForEst_.clear();
+    obsForEst_.reserve(obs_.size());
+    for (data::Feature &feat : obs_)
+    {
+        obsForEst_.push_back(feat);
+    }
     hasPosEstimate_ = true;
 }
 

src/data/landmark.h

@@ -19,6 +19,7 @@ class Landmark
     void AddObservation(Feature obs, bool compute_gnd = true);
     const std::vector<Feature>& GetObservations() const;
     std::vector<Feature>& GetObservations();
+    const std::vector<Feature>& GetObservationsForEstimate() const;
     bool IsObservedInFrame(const int frame_id) const;
     const int GetFirstFrameID() const;
     const int GetNumObservations() const;
@@ -32,6 +33,7 @@ class Landmark
 
 private:
     std::vector<Feature> obs_;
+    std::vector<Feature> obsForEst_;
     std::map<int, int> idToIndex_;
     Vector3d groundTruth_;
     Vector3d posEstimate_;

src/optimization/bundle_adjuster.cc

@@ -2,6 +2,9 @@
 
 #include "reprojection_error.h"
 
+#include "camera/double_sphere.h"
+#include "camera/perspective.h"
+
 namespace omni_slam
 {
 namespace optimization
@@ -11,23 +14,16 @@ BundleAdjuster::BundleAdjuster(int max_iterations, bool log)
 {
     problem_.reset(new ceres::Problem());
     solverOptions_.max_num_iterations = max_iterations;
-    solverOptions_.linear_solver_type = ceres::DENSE_SCHUR;
+    solverOptions_.linear_solver_type = ceres::ITERATIVE_SCHUR;
+    solverOptions_.preconditioner_type = ceres::SCHUR_JACOBI;
     solverOptions_.minimizer_progress_to_stdout = log;
     solverOptions_.logging_type = log ? ceres::PER_MINIMIZER_ITERATION : ceres::SILENT;
-    //solverOptions_.check_gradients = true;
 }
 
 bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
 {
-    int numLandmarks = 0;
-    for (const data::Landmark &landmark : landmarks)
-    {
-        if (landmark.HasEstimatedPosition() || landmark.HasGroundTruth())
-        {
-            numLandmarks++;
-        }
-    }
-    std::vector<double> landmarkEstimates(3 * numLandmarks, 0);
+    std::vector<double> landmarkEstimates;
+    landmarkEstimates.reserve(3 * landmarks.size());
     std::map<int, std::vector<double>> framePoses;
     std::map<int, data::Frame*> estFrames;
     ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
@@ -62,7 +58,8 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
         {
             continue;
         }
-        for (const data::Feature &feature : landmark.GetObservations())
+        const std::vector<data::Feature> obs = landmark.HasEstimatedPosition() ? landmark.GetObservationsForEstimate() : landmark.GetObservations();
+        for (const data::Feature &feature : obs)
         {
             if (!feature.GetFrame().HasEstimatedPose() && feature.GetFrame().HasPose())
             {
@@ -72,7 +69,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
                 }
                 if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
                 {
-                    framePoses[feature.GetFrame().GetID()].insert(framePoses[feature.GetFrame().GetID()].end(), feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12);
+                    framePoses[feature.GetFrame().GetID()] = std::vector<double>(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12);
                     problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12);
                     problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()][0]);
                 }
@@ -81,7 +78,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
             {
                 if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
                 {
-                    framePoses[feature.GetFrame().GetID()].insert(framePoses[feature.GetFrame().GetID()].end(), feature.GetFrame().GetEstimatedInversePose().data(), feature.GetFrame().GetEstimatedInversePose().data() + 12);
+                    framePoses[feature.GetFrame().GetID()] = std::vector<double>(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12);
                     problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12);
                     estFrames[feature.GetFrame().GetID()] = const_cast<data::Frame*>(&feature.GetFrame());
                 }
@@ -90,8 +87,19 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
             {
                 continue;
             }
-            ceres::CostFunction *cost_function = ReprojectionError::Create(feature);
-            problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()][0], &landmarkEstimates[landmarkEstimates.size() - 3]);
+            ceres::CostFunction *cost_function = nullptr;
+            if (feature.GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective)
+            {
+                cost_function = ReprojectionError<camera::Perspective>::Create(feature);
+            }
+            else if (feature.GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere)
+            {
+                cost_function = ReprojectionError<camera::DoubleSphere>::Create(feature);
+            }
+            if (cost_function != nullptr)
+            {
+                problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()][0], &landmarkEstimates[landmarkEstimates.size() - 3]);
+            }
         }
     }
 

src/optimization/reprojection_error.h

@@ -1,8 +1,6 @@
 #ifndef _REPROJECTION_ERROR_H_
 #define _REPROJECTION_ERROR_H_
 
-#include "camera/camera_model.h"
-#include "camera/double_sphere.h"
 #include "data/feature.h"
 #include <ceres/ceres.h>
 #include "util/tf_util.h"
@@ -12,6 +10,7 @@ namespace omni_slam
 namespace optimization
 {
 
+template <template<typename> class C>
 class ReprojectionError
 {
 public:
@@ -24,25 +23,19 @@ class ReprojectionError
     bool operator()(const T* const camera_pose, const T* const point, T *reproj_error) const
     {
         Matrix<T, 2, 1> reprojPoint;
-        //const camera::CameraModel<T> &camera((camera::CameraModel<T>&)feature_.GetFrame().GetCameraModel());
-        const camera::DoubleSphere<T> camera(T(295.936), T(295.936), T(512), T(512), T(0.3), T(0.6666667));
+        C<T> camera(feature_.GetFrame().GetCameraModel());
         const Matrix<T, 3, 4> pose = Map<const Matrix<T, 3, 4>>(camera_pose);
         const Matrix<T, 3, 1> worldPt = Map<const Matrix<T, 3, 1>>(point);
         const Matrix<T, 3, 1> camPt = util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, worldPt));
-        if (camera.ProjectToImage(camPt, reprojPoint))
-        {
-            reproj_error[0] = reprojPoint(0) - T(feature_.GetKeypoint().pt.x);
-            reproj_error[1] = reprojPoint(1) - T(feature_.GetKeypoint().pt.y);
-            return true;
-        }
-        reproj_error[0] = T(0.);
-        reproj_error[1] = T(0.);
+        camera.ProjectToImage(camPt, reprojPoint);
+        reproj_error[0] = reprojPoint(0) - T(feature_.GetKeypoint().pt.x);
+        reproj_error[1] = reprojPoint(1) - T(feature_.GetKeypoint().pt.y);
         return true;
     }
 
     static ceres::CostFunction* Create(const data::Feature &feature)
     {
-        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 12, 3>(new ReprojectionError(feature));
+        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 12, 3>(new ReprojectionError<C>(feature));
     }
 
 private: