diff --git a/CMakeLists.txt b/CMakeLists.txt index 4faee83..2a59405 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/camera/camera_model.h b/src/camera/camera_model.h index 8fc1650..1fce111 100644 --- a/src/camera/camera_model.h +++ b/src/camera/camera_model.h @@ -15,13 +15,20 @@ template class CameraModel { public: + enum Type + { + kPerspective, + kDoubleSphere + }; + CameraModel(std::string name) { name_ = name; } virtual bool ProjectToImage(const Matrix &bearing, Matrix &pixel) const = 0; virtual bool UnprojectToBearing(const Matrix &pixel, Matrix &bearing) const = 0; - virtual T GetFOV() = 0; + virtual T GetFOV() const = 0; + virtual Type GetType() const = 0; private: std::string name_; diff --git a/src/camera/double_sphere.h b/src/camera/double_sphere.h index c57f6ee..1e2347e 100644 --- a/src/camera/double_sphere.h +++ b/src/camera/double_sphere.h @@ -11,6 +11,8 @@ namespace camera template class DoubleSphere : public CameraModel { + template friend class DoubleSphere; + public: DoubleSphere(const T fx, const T fy, const T cx, const T cy, const T chi, const T alpha) : CameraModel("DoubleSphere"), @@ -27,6 +29,12 @@ class DoubleSphere : public CameraModel sinTheta_ = sin(theta); } + template + DoubleSphere(const CameraModel &other) + : DoubleSphere(T(static_cast&>(other).fx_), T(static_cast&>(other).fy_), T(static_cast&>(other).cx_), T(static_cast&>(other).cy_), T(static_cast&>(other).chi_), T(static_cast&>(other).alpha_)) + { + } + bool ProjectToImage(const Matrix &bearing, Matrix &pixel) const { const T &x = bearing(0); @@ -49,6 +57,14 @@ class DoubleSphere : public CameraModel //{ //return false; //} + + //Matrix bearing_h; + //bearing_h << x, y, (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z)); + //Matrix 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 return false; } } - - //Matrix bearing_h; - //bearing_h << x, y, (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z)); - //Matrix 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 return true; } - T GetFOV() + + T GetFOV() const { T mx = cx_ / fx_; T r2; @@ -124,6 +133,11 @@ class DoubleSphere : public CameraModel return 2. * (M_PI / 2 - atan2(beta * mz - chi_, beta * mx)); } + typename CameraModel::Type GetType() const + { + return CameraModel::kDoubleSphere; + } + private: T fx_; T fy_; diff --git a/src/camera/perspective.h b/src/camera/perspective.h index 0b3febe..4d741ae 100644 --- a/src/camera/perspective.h +++ b/src/camera/perspective.h @@ -11,6 +11,8 @@ namespace camera template class Perspective : public CameraModel { + template friend class Perspective; + public: Perspective(const T fx, const T fy, const T cx, const T cy) : CameraModel("Perspective"), @@ -22,6 +24,12 @@ class Perspective : public CameraModel cameraMat_ << fx_, 0., cx_, 0., fy_, cy_, 0., 0., 1.; } + template + Perspective(const CameraModel &other) + : Perspective(T(static_cast&>(other).fx_), T(static_cast&>(other).fy_), T(static_cast&>(other).cx_), T(static_cast&>(other).cy_)) + { + } + bool ProjectToImage(const Matrix &bearing, Matrix &pixel) const { const T &x = bearing(0); @@ -48,11 +56,16 @@ class Perspective : public CameraModel return true; } - T GetFOV() + T GetFOV() const { return 2. * atan(cx_ / fx_); } + typename CameraModel::Type GetType() const + { + return CameraModel::kPerspective; + } + private: T fx_; T fy_; diff --git a/src/data/landmark.cc b/src/data/landmark.cc index 2281273..50d1563 100644 --- a/src/data/landmark.cc +++ b/src/data/landmark.cc @@ -32,6 +32,11 @@ const std::vector& Landmark::GetObservations() const return obs_; } +const std::vector& Landmark::GetObservationsForEstimate() const +{ + return obsForEst_; +} + std::vector& 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; } diff --git a/src/data/landmark.h b/src/data/landmark.h index ee970d7..f68c71e 100644 --- a/src/data/landmark.h +++ b/src/data/landmark.h @@ -19,6 +19,7 @@ class Landmark void AddObservation(Feature obs, bool compute_gnd = true); const std::vector& GetObservations() const; std::vector& GetObservations(); + const std::vector& 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 obs_; + std::vector obsForEst_; std::map idToIndex_; Vector3d groundTruth_; Vector3d posEstimate_; diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc index d58e408..e210049 100644 --- a/src/optimization/bundle_adjuster.cc +++ b/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 &landmarks) { - int numLandmarks = 0; - for (const data::Landmark &landmark : landmarks) - { - if (landmark.HasEstimatedPosition() || landmark.HasGroundTruth()) - { - numLandmarks++; - } - } - std::vector landmarkEstimates(3 * numLandmarks, 0); + std::vector landmarkEstimates; + landmarkEstimates.reserve(3 * landmarks.size()); std::map> framePoses; std::map estFrames; ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); @@ -62,7 +58,8 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) { continue; } - for (const data::Feature &feature : landmark.GetObservations()) + const std::vector 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 &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(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 &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(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12); problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12); estFrames[feature.GetFrame().GetID()] = const_cast(&feature.GetFrame()); } @@ -90,8 +87,19 @@ bool BundleAdjuster::Optimize(std::vector &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::Create(feature); + } + else if (feature.GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere) + { + cost_function = ReprojectionError::Create(feature); + } + if (cost_function != nullptr) + { + problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()][0], &landmarkEstimates[landmarkEstimates.size() - 3]); + } } } diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h index 33b80fa..5ec3604 100644 --- a/src/optimization/reprojection_error.h +++ b/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 #include "util/tf_util.h" @@ -12,6 +10,7 @@ namespace omni_slam namespace optimization { +template 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 reprojPoint; - //const camera::CameraModel &camera((camera::CameraModel&)feature_.GetFrame().GetCameraModel()); - const camera::DoubleSphere camera(T(295.936), T(295.936), T(512), T(512), T(0.3), T(0.6666667)); + C camera(feature_.GetFrame().GetCameraModel()); const Matrix pose = Map>(camera_pose); const Matrix worldPt = Map>(point); const Matrix 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(new ReprojectionError(feature)); + return new ceres::AutoDiffCostFunction(new ReprojectionError(feature)); } private: