From 73dbec9b704492feade9c23221a2806657b88afa Mon Sep 17 00:00:00 2001 From: Fabien SERVANT Date: Fri, 8 Sep 2023 13:27:07 +0200 Subject: [PATCH 1/4] Move costfunction and manifold in dedicated files --- .../sfm/BundleAdjustmentPanoramaCeres.cpp | 225 +----------- .../sfm/BundleAdjustmentPanoramaCeres.hpp | 3 +- .../sfm/BundleAdjustmentSymbolicCeres.cpp | 329 +----------------- .../sfm/BundleAdjustmentSymbolicCeres.hpp | 6 +- .../sfm/costfunctions/panoramaEquidistant.hpp | 91 +++++ .../sfm/costfunctions/panoramaPinhole.hpp | 102 ++++++ .../sfm/costfunctions/projection.hpp | 98 ++++++ .../sfm/costfunctions/projectionSimple.hpp | 81 +++++ .../sfm/costfunctions/rotationPrior.hpp | 61 ++++ src/aliceVision/sfm/liealgebra.hpp | 329 ------------------ src/aliceVision/sfm/manifolds/intrinsics.hpp | 180 ++++++++++ src/aliceVision/sfm/manifolds/se3.hpp | 165 +++++++++ src/aliceVision/sfm/manifolds/so2.hpp | 64 ++++ src/aliceVision/sfm/manifolds/so3.hpp | 137 ++++++++ src/aliceVision/sfm/utils/alignment.cpp | 2 +- 15 files changed, 1010 insertions(+), 863 deletions(-) create mode 100644 src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp create mode 100644 src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp create mode 100644 src/aliceVision/sfm/costfunctions/projection.hpp create mode 100644 src/aliceVision/sfm/costfunctions/projectionSimple.hpp create mode 100644 src/aliceVision/sfm/costfunctions/rotationPrior.hpp delete mode 100644 src/aliceVision/sfm/liealgebra.hpp create mode 100644 src/aliceVision/sfm/manifolds/intrinsics.hpp create mode 100644 src/aliceVision/sfm/manifolds/se3.hpp create mode 100644 src/aliceVision/sfm/manifolds/so2.hpp create mode 100644 src/aliceVision/sfm/manifolds/so3.hpp diff --git a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp index 6f7d461f40..9c41a30e4b 100644 --- a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp +++ b/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp @@ -10,226 +10,25 @@ #include #include #include -#include #include #include +#include + #include +#include +#include +#include + #include namespace fs = boost::filesystem; namespace aliceVision { -class CostRotationPrior : public ceres::SizedCostFunction<3, 9, 9> { -public: - explicit CostRotationPrior(const Eigen::Matrix3d & two_R_one) : _two_R_one(two_R_one) { - - } - - bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { - - const double * parameter_rotation_one = parameters[0]; - const double * parameter_rotation_two = parameters[1]; - - const Eigen::Map> oneRo(parameter_rotation_one); - const Eigen::Map> twoRo(parameter_rotation_two); - - Eigen::Matrix3d two_R_one_est = twoRo * oneRo.transpose(); - Eigen::Matrix3d error_R = two_R_one_est * _two_R_one.transpose(); - Eigen::Vector3d error_r = SO3::logm(error_R); - - residuals[0] = error_r(0); - residuals[1] = error_r(1); - residuals[2] = error_r(2); - - if (jacobians == nullptr) { - return true; - } - - if (jacobians[0]) { - Eigen::Map> J(jacobians[0]); - - J = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_B<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), oneRo); - } - - if (jacobians[1]) { - Eigen::Map> J(jacobians[1]); - - J = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), twoRo); - } - - return true; - } - -private: - Eigen::Matrix3d _two_R_one; -}; - -class CostEquidistant : public ceres::SizedCostFunction<2, 9, 9, 7> { -public: - CostEquidistant(Vec2 fi, Vec2 fj, std::shared_ptr & intrinsic) : _fi(fi), _fj(fj), _intrinsic(intrinsic) { - - } - - bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { - - Vec2 pt_i = _fi; - Vec2 pt_j = _fj; - - const double * parameter_rotation_i = parameters[0]; - const double * parameter_rotation_j = parameters[1]; - const double * parameter_intrinsics = parameters[2]; - - const Eigen::Map> iRo(parameter_rotation_i); - const Eigen::Map> jRo(parameter_rotation_j); - - _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); - _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); - _intrinsic->setDistortionParamsFn(3, [&](auto index) { return parameter_intrinsics[4 + index]; }); - - Eigen::Matrix3d R = jRo * iRo.transpose(); - - - geometry::Pose3 T_pose3(R, Vec3({0,0,0})); - Eigen::Matrix4d T = T_pose3.getHomogeneous(); - - Vec2 pt_i_cam = _intrinsic->ima2cam(pt_i); - Vec2 pt_i_undist = _intrinsic->removeDistortion(pt_i_cam); - Vec4 pt_i_sphere = _intrinsic->toUnitSphere(pt_i_undist).homogeneous(); - - Vec2 pt_j_est = _intrinsic->project(T_pose3, pt_i_sphere, true); - - residuals[0] = pt_j_est(0) - pt_j(0); - residuals[1] = pt_j_est(1) - pt_j(1); - - if (jacobians == nullptr) { - return true; - } - - if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); - - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); - } - - if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); - - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); - } - - if (jacobians[2] != nullptr) { - Eigen::Map> J(jacobians[2]); - - Eigen::Matrix Jhomogenous = Eigen::Matrix::Identity(); - - Eigen::Matrix Jscale = _intrinsic->getDerivativeProjectWrtScale(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtScale(pt_i_undist); - Eigen::Matrix Jpp = _intrinsic->getDerivativeProjectWrtPrincipalPoint(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtPrincipalPoint(); - Eigen::Matrix Jdisto = _intrinsic->getDerivativeProjectWrtDisto(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtDisto(pt_i_cam); - - J.block<2, 2>(0, 0) = Jscale; - J.block<2, 2>(0, 2) = Jpp; - J.block<2, 3>(0, 4) = Jdisto; - } - - return true; - } - -private: - Vec2 _fi; - Vec2 _fj; - std::shared_ptr _intrinsic; -}; - -class CostPinHole : public ceres::CostFunction { -public: - CostPinHole(Vec2 fi, Vec2 fj, std::shared_ptr & intrinsic) : _fi(fi), _fj(fj), _intrinsic(intrinsic) { - - set_num_residuals(2); - - mutable_parameter_block_sizes()->push_back(9); - mutable_parameter_block_sizes()->push_back(9); - mutable_parameter_block_sizes()->push_back(intrinsic->getParams().size()); - } - - bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { - - Vec2 pt_i = _fi; - Vec2 pt_j = _fj; - - const double * parameter_rotation_i = parameters[0]; - const double * parameter_rotation_j = parameters[1]; - const double * parameter_intrinsics = parameters[2]; - - const Eigen::Map> iRo(parameter_rotation_i); - const Eigen::Map> jRo(parameter_rotation_j); - - _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); - _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); - - size_t params_size = _intrinsic->getParamsSize(); - size_t disto_size = _intrinsic->getDistortionParamsSize(); - size_t offset = params_size - disto_size; - - _intrinsic->setDistortionParamsFn(disto_size, [&](auto index) - { - return parameter_intrinsics[offset + index]; - }); - - Eigen::Matrix3d R = jRo * iRo.transpose(); - geometry::Pose3 T_pose3(R, Vec3({0,0,0})); - Eigen::Matrix4d T = T_pose3.getHomogeneous(); - - Vec2 pt_i_cam = _intrinsic->ima2cam(pt_i); - Vec2 pt_i_undist = _intrinsic->removeDistortion(pt_i_cam); - Vec4 pt_i_sphere = _intrinsic->toUnitSphere(pt_i_undist).homogeneous(); - - Vec2 pt_j_est = _intrinsic->project(T_pose3, pt_i_sphere, true); - - residuals[0] = pt_j_est(0) - pt_j(0); - residuals[1] = pt_j_est(1) - pt_j(1); - - if (jacobians == nullptr) { - return true; - } - - if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); - - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); - } - - if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); - - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); - } - - if (jacobians[2] != nullptr) { - Eigen::Map> J(jacobians[2], 2, params_size); - - Eigen::Matrix Jhomogenous = Eigen::Matrix::Identity(); - - Eigen::Matrix Jscale = _intrinsic->getDerivativeProjectWrtScale(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtScale(pt_i); - Eigen::Matrix Jpp = _intrinsic->getDerivativeProjectWrtPrincipalPoint(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtPrincipalPoint(); - Eigen::Matrix Jdisto = _intrinsic->getDerivativeProjectWrtDisto(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtDisto(pt_i_cam); - - J.block<2, 2>(0, 0) = Jscale; - J.block<2, 2>(0, 2) = Jpp; - J.block(0, 4, 2, disto_size) = Jdisto; - } - - return true; - } -private: - Vec2 _fi; - Vec2 _fj; - std::shared_ptr _intrinsic; -}; namespace sfm { @@ -420,9 +219,9 @@ void BundleAdjustmentPanoramaCeres::addExtrinsicsToProblem(const sfmData::SfMDat /*Define rotation parameterization*/ #if ALICEVISION_CERES_HAS_MANIFOLD - problem.AddParameterBlock(poseBlockPtr, 9, new SO3::Manifold); + problem.AddParameterBlock(poseBlockPtr, 9, new SO3Manifold); #else - problem.AddParameterBlock(poseBlockPtr, 9, new utils::ManifoldToParameterizationWrapper(new SO3::Manifold)); + problem.AddParameterBlock(poseBlockPtr, 9, new utils::ManifoldToParameterizationWrapper(new SO3Manifold)); #endif // keep the camera extrinsics constants @@ -612,19 +411,19 @@ void BundleAdjustmentPanoramaCeres::addConstraints2DToProblem(const sfmData::SfM if (equidistant != nullptr) { - ceres::CostFunction* costFunction = new CostEquidistant(constraint.ObservationFirst.x, constraint.ObservationSecond.x, equidistant); + ceres::CostFunction* costFunction = new CostPanoramaEquidistant(constraint.ObservationFirst.x, constraint.ObservationSecond.x, equidistant); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); /* Symmetry */ - costFunction = new CostEquidistant(constraint.ObservationSecond.x, constraint.ObservationFirst.x, equidistant); + costFunction = new CostPanoramaEquidistant(constraint.ObservationSecond.x, constraint.ObservationFirst.x, equidistant); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); } else if (pinhole != nullptr) { - ceres::CostFunction* costFunction = new CostPinHole(constraint.ObservationFirst.x, constraint.ObservationSecond.x, pinhole); + ceres::CostFunction* costFunction = new CostPanoramaPinHole(constraint.ObservationFirst.x, constraint.ObservationSecond.x, pinhole); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); /* Symmetry */ - costFunction = new CostPinHole(constraint.ObservationSecond.x, constraint.ObservationFirst.x, pinhole); + costFunction = new CostPanoramaPinHole(constraint.ObservationSecond.x, constraint.ObservationFirst.x, pinhole); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); } else { diff --git a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp b/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp index 60eb3708a1..032cf0ab8f 100644 --- a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp +++ b/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp @@ -12,9 +12,8 @@ #include #include +#include #include -#include "liealgebra.hpp" - namespace aliceVision { diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp index 4d83f714d1..71e7236f58 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp @@ -6,15 +6,23 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include -#include + #include + +#include #include #include #include - #include -#include + +#include +#include +#include + +#include +#include + #include @@ -28,317 +36,6 @@ using namespace aliceVision::camera; using namespace aliceVision::geometry; -class IntrinsicsManifoldSymbolic : public utils::CeresManifold { - public: - explicit IntrinsicsManifoldSymbolic(size_t parametersSize, double focalRatio, bool lockFocal, - bool lockFocalRatio, bool lockCenter, bool lockDistortion) - : _ambientSize(parametersSize), - _focalRatio(focalRatio), - _lockFocal(lockFocal), - _lockFocalRatio(lockFocalRatio), - _lockCenter(lockCenter), - _lockDistortion(lockDistortion) - { - _distortionSize = _ambientSize - 4; - _tangentSize = 0; - - if (!_lockFocal) - { - if (_lockFocalRatio) - { - _tangentSize += 1; - } - else - { - _tangentSize += 2; - } - } - - if (!_lockCenter) - { - _tangentSize += 2; - } - - if (!_lockDistortion) - { - _tangentSize += _distortionSize; - } - } - - virtual ~IntrinsicsManifoldSymbolic() = default; - - - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override - { - for (int i = 0; i < _ambientSize; i++) - { - x_plus_delta[i] = x[i]; - } - - size_t posDelta = 0; - if (!_lockFocal) - { - if (_lockFocalRatio) - { - x_plus_delta[0] = x[0] + delta[posDelta]; - x_plus_delta[1] = x[1] + _focalRatio * delta[posDelta]; - ++posDelta; - } - else - { - x_plus_delta[0] = x[0] + delta[posDelta]; - ++posDelta; - x_plus_delta[1] = x[1] + delta[posDelta]; - ++posDelta; - } - } - - if (!_lockCenter) - { - x_plus_delta[2] = x[2] + delta[posDelta]; - ++posDelta; - - x_plus_delta[3] = x[3] + delta[posDelta]; - ++posDelta; - } - - if (!_lockDistortion) - { - for (int i = 0; i < _distortionSize; i++) - { - x_plus_delta[4 + i] = x[4 + i] + delta[posDelta]; - ++posDelta; - } - } - - return true; - } - - bool PlusJacobian(const double* x, double* jacobian) const override - { - Eigen::Map> J(jacobian, AmbientSize(), TangentSize()); - - J.fill(0); - - size_t posDelta = 0; - if (!_lockFocal) - { - if (_lockFocalRatio) - { - J(0, posDelta) = 1.0; - J(1, posDelta) = _focalRatio; - ++posDelta; - } - else - { - J(0, posDelta) = 1.0; - ++posDelta; - J(1, posDelta) = 1.0; - ++posDelta; - } - } - - if (!_lockCenter) - { - J(2, posDelta) = 1.0; - ++posDelta; - - J(3, posDelta) = 1.0; - ++posDelta; - } - - if (!_lockDistortion) - { - for (int i = 0; i < _distortionSize; i++) - { - J(4 + i, posDelta) = 1.0; - ++posDelta; - } - } - - return true; - } - - bool Minus(const double* y, const double* x, double* delta) const override { - throw std::invalid_argument("IntrinsicsManifold::Minus() should never be called"); - } - - bool MinusJacobian(const double* x, double* jacobian) const override { - throw std::invalid_argument("IntrinsicsManifold::MinusJacobian() should never be called"); - } - - int AmbientSize() const override - { - return _ambientSize; - } - - int TangentSize() const override - { - return _tangentSize; - } - - private: - size_t _distortionSize; - size_t _ambientSize; - size_t _tangentSize; - double _focalRatio; - bool _lockFocal; - bool _lockFocalRatio; - bool _lockCenter; - bool _lockDistortion; -}; - - -class CostProjection : public ceres::CostFunction { -public: - CostProjection(const sfmData::Observation& measured, const std::shared_ptr & intrinsics, bool withRig) : _measured(measured), _intrinsics(intrinsics), _withRig(withRig) - { - set_num_residuals(2); - - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); - mutable_parameter_block_sizes()->push_back(3); - } - - bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override - { - const double * parameter_pose = parameters[0]; - const double * parameter_rig = parameters[1]; - const double * parameter_intrinsics = parameters[2]; - const double * parameter_landmark = parameters[3]; - - const Eigen::Map rTo(parameter_pose); - const Eigen::Map cTr(parameter_rig); - const Eigen::Map pt(parameter_landmark); - - /*Update intrinsics object with estimated parameters*/ - size_t params_size = _intrinsics->getParams().size(); - std::vector params; - for (size_t param_id = 0; param_id < params_size; param_id++) { - params.push_back(parameter_intrinsics[param_id]); - } - _intrinsics->updateFromParams(params); - - const SE3::Matrix T = cTr * rTo; - const geometry::Pose3 T_pose3(T); - - const Vec4 pth = pt.homogeneous(); - - const Vec2 pt_est = _intrinsics->project(T_pose3, pth, true); - const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0; - - residuals[0] = (pt_est(0) - _measured.x(0)) / scale; - residuals[1] = (pt_est(1) - _measured.x(1)) / scale; - - if (jacobians == nullptr) { - return true; - } - - - Eigen::Matrix2d d_res_d_pt_est = Eigen::Matrix2d::Identity() / scale; - - if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_B<4, 4, 4>(cTr, rTo) * getJacobian_AB_wrt_A<4, 4, 4>(Eigen::Matrix4d::Identity(), rTo); - } - - if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_A<4, 4, 4>(cTr, rTo) * getJacobian_AB_wrt_A<4, 4, 4>(Eigen::Matrix4d::Identity(), cTr); - } - - if (jacobians[2] != nullptr) { - Eigen::Map> J(jacobians[2], 2, params_size); - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtParams(T, pth); - } - - if (jacobians[3] != nullptr) { - Eigen::Map> J(jacobians[3]); - - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoint(T, pth) * Eigen::Matrix::Identity(); - } - - return true; - } - -private: - const sfmData::Observation & _measured; - const std::shared_ptr _intrinsics; - bool _withRig; -}; - -class CostProjectionSimple : public ceres::CostFunction { -public: - CostProjectionSimple(const sfmData::Observation& measured, const std::shared_ptr & intrinsics) : _measured(measured), _intrinsics(intrinsics) - { - set_num_residuals(2); - - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); - mutable_parameter_block_sizes()->push_back(3); - } - - bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override - { - const double * parameter_pose = parameters[0]; - const double * parameter_intrinsics = parameters[1]; - const double * parameter_landmark = parameters[2]; - - const Eigen::Map T(parameter_pose); - const Eigen::Map pt(parameter_landmark); - - const Vec4 pth = pt.homogeneous(); - - const Vec4 cpt = T * pth; - - Vec2 pt_est = _intrinsics->project(T, pth, false); - const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0; - - residuals[0] = (pt_est(0) - _measured.x(0)) / scale; - residuals[1] = (pt_est(1) - _measured.x(1)) / scale; - - if (jacobians == nullptr) { - return true; - } - - const geometry::Pose3 T_pose3(T); - size_t params_size = _intrinsics->getParams().size(); - - double d_res_d_pt_est = 1.0 / scale; - - if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoseLeft(T, pth); - } - - if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1], 2, params_size); - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtParams(T, pth); - } - - if (jacobians[2] != nullptr) { - Eigen::Map> J(jacobians[2]); - - J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoint3(T, pth); - } - - return true; - } - -private: - const sfmData::Observation & _measured; - const std::shared_ptr _intrinsics; -}; - - - void BundleAdjustmentSymbolicCeres::addPose(const sfmData::CameraPose& cameraPose, bool isConstant, SE3::Matrix & poseBlock, ceres::Problem& problem, bool refineTranslation, bool refineRotation) { const Mat3& R = cameraPose.getTransform().rotation(); @@ -368,10 +65,10 @@ void BundleAdjustmentSymbolicCeres::addPose(const sfmData::CameraPose& cameraPos if (refineRotation || refineTranslation) { #if ALICEVISION_CERES_HAS_MANIFOLD - problem.SetManifold(poseBlockPtr, new SE3::ManifoldLeft(refineRotation, refineTranslation)); + problem.SetManifold(poseBlockPtr, new SE3ManifoldLeft(refineRotation, refineTranslation)); #else problem.SetParameterization(poseBlockPtr, new utils::ManifoldToParameterizationWrapper( - new SE3::ManifoldLeft(refineRotation, refineTranslation))); + new SE3ManifoldLeft(refineRotation, refineTranslation))); #endif } else diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp index b763d389b2..6cf7ecc41e 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp +++ b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp @@ -9,13 +9,15 @@ #include #include +#include + #include #include -#include + #include #include + #include -#include "liealgebra.hpp" #include diff --git a/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp b/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp new file mode 100644 index 0000000000..e46b50e684 --- /dev/null +++ b/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 9, 9, 7> { +public: + CostPanoramaEquidistant(Vec2 fi, Vec2 fj, std::shared_ptr & intrinsic) : _fi(fi), _fj(fj), _intrinsic(intrinsic) { + + } + + bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { + + Vec2 pt_i = _fi; + Vec2 pt_j = _fj; + + const double * parameter_rotation_i = parameters[0]; + const double * parameter_rotation_j = parameters[1]; + const double * parameter_intrinsics = parameters[2]; + + const Eigen::Map> iRo(parameter_rotation_i); + const Eigen::Map> jRo(parameter_rotation_j); + + _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); + _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); + _intrinsic->setDistortionParamsFn(3, [&](auto index) { return parameter_intrinsics[4 + index]; }); + + Eigen::Matrix3d R = jRo * iRo.transpose(); + + + geometry::Pose3 T_pose3(R, Vec3({0,0,0})); + Eigen::Matrix4d T = T_pose3.getHomogeneous(); + + Vec2 pt_i_cam = _intrinsic->ima2cam(pt_i); + Vec2 pt_i_undist = _intrinsic->removeDistortion(pt_i_cam); + Vec4 pt_i_sphere = _intrinsic->toUnitSphere(pt_i_undist).homogeneous(); + + Vec2 pt_j_est = _intrinsic->project(T_pose3, pt_i_sphere, true); + + residuals[0] = pt_j_est(0) - pt_j(0); + residuals[1] = pt_j_est(1) - pt_j(1); + + if (jacobians == nullptr) { + return true; + } + + if (jacobians[0] != nullptr) { + Eigen::Map> J(jacobians[0]); + + J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); + } + + if (jacobians[1] != nullptr) { + Eigen::Map> J(jacobians[1]); + + J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); + } + + if (jacobians[2] != nullptr) { + Eigen::Map> J(jacobians[2]); + + Eigen::Matrix Jhomogenous = Eigen::Matrix::Identity(); + + Eigen::Matrix Jscale = _intrinsic->getDerivativeProjectWrtScale(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtScale(pt_i_undist); + Eigen::Matrix Jpp = _intrinsic->getDerivativeProjectWrtPrincipalPoint(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtPrincipalPoint(); + Eigen::Matrix Jdisto = _intrinsic->getDerivativeProjectWrtDisto(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtDisto(pt_i_cam); + + J.block<2, 2>(0, 0) = Jscale; + J.block<2, 2>(0, 2) = Jpp; + J.block<2, 3>(0, 4) = Jdisto; + } + + return true; + } + +private: + Vec2 _fi; + Vec2 _fj; + std::shared_ptr _intrinsic; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp b/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp new file mode 100644 index 0000000000..a0adfbbb3e --- /dev/null +++ b/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp @@ -0,0 +1,102 @@ +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +class CostPanoramaPinHole : public ceres::CostFunction { +public: + CostPanoramaPinHole(Vec2 fi, Vec2 fj, std::shared_ptr & intrinsic) : _fi(fi), _fj(fj), _intrinsic(intrinsic) { + + set_num_residuals(2); + + mutable_parameter_block_sizes()->push_back(9); + mutable_parameter_block_sizes()->push_back(9); + mutable_parameter_block_sizes()->push_back(intrinsic->getParams().size()); + } + + bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { + + Vec2 pt_i = _fi; + Vec2 pt_j = _fj; + + const double * parameter_rotation_i = parameters[0]; + const double * parameter_rotation_j = parameters[1]; + const double * parameter_intrinsics = parameters[2]; + + const Eigen::Map> iRo(parameter_rotation_i); + const Eigen::Map> jRo(parameter_rotation_j); + + _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); + _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); + + size_t params_size = _intrinsic->getParamsSize(); + size_t disto_size = _intrinsic->getDistortionParamsSize(); + size_t offset = params_size - disto_size; + + _intrinsic->setDistortionParamsFn(disto_size, [&](auto index) + { + return parameter_intrinsics[offset + index]; + }); + + Eigen::Matrix3d R = jRo * iRo.transpose(); + geometry::Pose3 T_pose3(R, Vec3({0,0,0})); + Eigen::Matrix4d T = T_pose3.getHomogeneous(); + + Vec2 pt_i_cam = _intrinsic->ima2cam(pt_i); + Vec2 pt_i_undist = _intrinsic->removeDistortion(pt_i_cam); + Vec4 pt_i_sphere = _intrinsic->toUnitSphere(pt_i_undist).homogeneous(); + + Vec2 pt_j_est = _intrinsic->project(T_pose3, pt_i_sphere, true); + + residuals[0] = pt_j_est(0) - pt_j(0); + residuals[1] = pt_j_est(1) - pt_j(1); + + if (jacobians == nullptr) { + return true; + } + + if (jacobians[0] != nullptr) { + Eigen::Map> J(jacobians[0]); + + J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); + } + + if (jacobians[1] != nullptr) { + Eigen::Map> J(jacobians[1]); + + J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); + } + + if (jacobians[2] != nullptr) { + Eigen::Map> J(jacobians[2], 2, params_size); + + Eigen::Matrix Jhomogenous = Eigen::Matrix::Identity(); + + Eigen::Matrix Jscale = _intrinsic->getDerivativeProjectWrtScale(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtScale(pt_i); + Eigen::Matrix Jpp = _intrinsic->getDerivativeProjectWrtPrincipalPoint(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtPrincipalPoint(); + Eigen::Matrix Jdisto = _intrinsic->getDerivativeProjectWrtDisto(T, pt_i_sphere) + _intrinsic->getDerivativeProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * _intrinsic->getDerivativeRemoveDistoWrtDisto(pt_i_cam); + + J.block<2, 2>(0, 0) = Jscale; + J.block<2, 2>(0, 2) = Jpp; + J.block(0, 4, 2, disto_size) = Jdisto; + } + + return true; + } + +private: + Vec2 _fi; + Vec2 _fj; + std::shared_ptr _intrinsic; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/costfunctions/projection.hpp b/src/aliceVision/sfm/costfunctions/projection.hpp new file mode 100644 index 0000000000..adf96cd4fc --- /dev/null +++ b/src/aliceVision/sfm/costfunctions/projection.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +class CostProjection : public ceres::CostFunction { +public: + CostProjection(const sfmData::Observation& measured, const std::shared_ptr & intrinsics, bool withRig) : _measured(measured), _intrinsics(intrinsics), _withRig(withRig) + { + set_num_residuals(2); + + mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); + mutable_parameter_block_sizes()->push_back(3); + } + + bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override + { + const double * parameter_pose = parameters[0]; + const double * parameter_rig = parameters[1]; + const double * parameter_intrinsics = parameters[2]; + const double * parameter_landmark = parameters[3]; + + const Eigen::Map rTo(parameter_pose); + const Eigen::Map cTr(parameter_rig); + const Eigen::Map pt(parameter_landmark); + + /*Update intrinsics object with estimated parameters*/ + size_t params_size = _intrinsics->getParams().size(); + std::vector params; + for (size_t param_id = 0; param_id < params_size; param_id++) { + params.push_back(parameter_intrinsics[param_id]); + } + _intrinsics->updateFromParams(params); + + const SE3::Matrix T = cTr * rTo; + const geometry::Pose3 T_pose3(T); + + const Vec4 pth = pt.homogeneous(); + + const Vec2 pt_est = _intrinsics->project(T_pose3, pth, true); + const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0; + + residuals[0] = (pt_est(0) - _measured.x(0)) / scale; + residuals[1] = (pt_est(1) - _measured.x(1)) / scale; + + if (jacobians == nullptr) { + return true; + } + + + Eigen::Matrix2d d_res_d_pt_est = Eigen::Matrix2d::Identity() / scale; + + if (jacobians[0] != nullptr) { + Eigen::Map> J(jacobians[0]); + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_B<4, 4, 4>(cTr, rTo) * getJacobian_AB_wrt_A<4, 4, 4>(Eigen::Matrix4d::Identity(), rTo); + } + + if (jacobians[1] != nullptr) { + Eigen::Map> J(jacobians[1]); + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPose(T, pth) * getJacobian_AB_wrt_A<4, 4, 4>(cTr, rTo) * getJacobian_AB_wrt_A<4, 4, 4>(Eigen::Matrix4d::Identity(), cTr); + } + + if (jacobians[2] != nullptr) { + Eigen::Map> J(jacobians[2], 2, params_size); + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtParams(T, pth); + } + + if (jacobians[3] != nullptr) { + Eigen::Map> J(jacobians[3]); + + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoint(T, pth) * Eigen::Matrix::Identity(); + } + + return true; + } + +private: + const sfmData::Observation & _measured; + const std::shared_ptr _intrinsics; + bool _withRig; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/costfunctions/projectionSimple.hpp b/src/aliceVision/sfm/costfunctions/projectionSimple.hpp new file mode 100644 index 0000000000..890679641d --- /dev/null +++ b/src/aliceVision/sfm/costfunctions/projectionSimple.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +class CostProjectionSimple : public ceres::CostFunction { +public: + CostProjectionSimple(const sfmData::Observation& measured, const std::shared_ptr & intrinsics) : _measured(measured), _intrinsics(intrinsics) + { + set_num_residuals(2); + + mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); + mutable_parameter_block_sizes()->push_back(3); + } + + bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override + { + const double * parameter_pose = parameters[0]; + const double * parameter_intrinsics = parameters[1]; + const double * parameter_landmark = parameters[2]; + + const Eigen::Map T(parameter_pose); + const Eigen::Map pt(parameter_landmark); + + const Vec4 pth = pt.homogeneous(); + + const Vec4 cpt = T * pth; + + Vec2 pt_est = _intrinsics->project(T, pth, false); + const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0; + + residuals[0] = (pt_est(0) - _measured.x(0)) / scale; + residuals[1] = (pt_est(1) - _measured.x(1)) / scale; + + if (jacobians == nullptr) { + return true; + } + + const geometry::Pose3 T_pose3(T); + size_t params_size = _intrinsics->getParams().size(); + + double d_res_d_pt_est = 1.0 / scale; + + if (jacobians[0] != nullptr) { + Eigen::Map> J(jacobians[0]); + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoseLeft(T, pth); + } + + if (jacobians[1] != nullptr) { + Eigen::Map> J(jacobians[1], 2, params_size); + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtParams(T, pth); + } + + if (jacobians[2] != nullptr) { + Eigen::Map> J(jacobians[2]); + + J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoint3(T, pth); + } + + return true; + } + +private: + const sfmData::Observation & _measured; + const std::shared_ptr _intrinsics; +}; + + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/costfunctions/rotationPrior.hpp b/src/aliceVision/sfm/costfunctions/rotationPrior.hpp new file mode 100644 index 0000000000..47bcf78702 --- /dev/null +++ b/src/aliceVision/sfm/costfunctions/rotationPrior.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + + +class CostRotationPrior : public ceres::SizedCostFunction<3, 9, 9> { +public: + explicit CostRotationPrior(const Eigen::Matrix3d & two_R_one) : _two_R_one(two_R_one) { + + } + + bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { + + const double * parameter_rotation_one = parameters[0]; + const double * parameter_rotation_two = parameters[1]; + + const Eigen::Map> oneRo(parameter_rotation_one); + const Eigen::Map> twoRo(parameter_rotation_two); + + Eigen::Matrix3d two_R_one_est = twoRo * oneRo.transpose(); + Eigen::Matrix3d error_R = two_R_one_est * _two_R_one.transpose(); + Eigen::Vector3d error_r = SO3::logm(error_R); + + residuals[0] = error_r(0); + residuals[1] = error_r(1); + residuals[2] = error_r(2); + + if (jacobians == nullptr) { + return true; + } + + if (jacobians[0]) { + Eigen::Map> J(jacobians[0]); + + J = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_B<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), oneRo); + } + + if (jacobians[1]) { + Eigen::Map> J(jacobians[1]); + + J = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), twoRo); + } + + return true; + } + +private: + Eigen::Matrix3d _two_R_one; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/liealgebra.hpp b/src/aliceVision/sfm/liealgebra.hpp deleted file mode 100644 index b741e17e0f..0000000000 --- a/src/aliceVision/sfm/liealgebra.hpp +++ /dev/null @@ -1,329 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. -// This Source Code Form is subject to the terms of the Mozilla Public License, -// v. 2.0. If a copy of the MPL was not distributed with this file, -// You can obtain one at https://mozilla.org/MPL/2.0/. - -#pragma once - -#include -#include -#include -#include -#include - -namespace aliceVision { -namespace SO3 { - -/** -Compute the jacobian of the logarithm wrt changes in the rotation matrix values -@param R the input rotation matrix -@return the jacobian matrix (3*9 matrix) -*/ -inline Eigen::Matrix dlogmdr(const Eigen::Matrix3d & R) { - double p1 = R(2, 1) - R(1, 2); - double p2 = R(0, 2) - R(2, 0); - double p3 = R(1, 0) - R(0, 1); - - double costheta = (R.trace() - 1.0) / 2.0; - if (costheta > 1.0) costheta = 1.0; - else if (costheta < -1.0) costheta = -1.0; - - double theta = acos(costheta); - - if (fabs(theta) < std::numeric_limits::epsilon()) { - Eigen::Matrix J; - J.fill(0); - J(0, 5) = 1; - J(0, 7) = -1; - J(1, 2) = -1; - J(1, 6) = 1; - J(2, 1) = 1; - J(2, 3) = -1; - return J; - } - - double scale = theta / (2.0 * sin(theta)); - - Eigen::Vector3d resnoscale; - resnoscale(0) = p1; - resnoscale(1) = p2; - resnoscale(2) = p3; - - Eigen::Matrix dresdp = Eigen::Matrix3d::Identity() * scale; - Eigen::Matrix dpdmat; - dpdmat.fill(0); - dpdmat(0, 5) = 1; - dpdmat(0, 7) = -1; - dpdmat(1, 2) = -1; - dpdmat(1, 6) = 1; - dpdmat(2, 1) = 1; - dpdmat(2, 3) = -1; - - - double dscaledtheta = -0.5 * theta * cos(theta) / (sin(theta)*sin(theta)) + 0.5 / sin(theta); - double dthetadcostheta = -1.0 / sqrt(-costheta*costheta + 1.0); - - Eigen::Matrix dcosthetadmat; - dcosthetadmat << 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5; - Eigen::Matrix dscaledmat = dscaledtheta * dthetadcostheta * dcosthetadmat; - - return dpdmat * scale + resnoscale * dscaledmat; -} - -class Manifold : public utils::CeresManifold { - public: - ~Manifold() override = default; - - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { - - double* ptrBase = (double*)x; - double* ptrResult = (double*)x_plus_delta; - Eigen::Map > rotation(ptrBase); - Eigen::Map > rotationResult(ptrResult); - - Eigen::Vector3d axis; - axis(0) = delta[0]; - axis(1) = delta[1]; - axis(2) = delta[2]; - double angle = axis.norm(); - - axis.normalize(); - - Eigen::AngleAxisd aa(angle, axis); - Eigen::Matrix3d Rupdate; - Rupdate = aa.toRotationMatrix(); - - rotationResult = Rupdate * rotation; - - return true; - } - - bool PlusJacobian(const double* /*x*/, double* jacobian) const override { - - Eigen::Map> J(jacobian); - //Eigen::Map> R(x); - - J.fill(0); - - J(1, 2) = 1; - J(2, 1) = -1; - J(3, 2) = -1; - J(5, 0) = 1; - J(6, 1) = 1; - J(7, 0) = -1; - - return true; - } - - bool Minus(const double* y, const double* x, double* delta) const override { - throw std::invalid_argument("SO3::Manifold::Minus() should never be called"); - } - - bool MinusJacobian(const double* x, double* jacobian) const override { - throw std::invalid_argument("SO3::Manifold::MinusJacobian() should never be called"); - } - - int AmbientSize() const override { return 9; } - - int TangentSize() const override { return 3; } -}; - -}//namespace SO3 - -namespace SE3 { - - -class ManifoldLeft : public utils::CeresManifold { -public: - ManifoldLeft(bool refineRotation, bool refineTranslation) : - _refineRotation(refineRotation), - _refineTranslation(refineTranslation) - { - } - - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { - - Eigen::Map> T(x); - Eigen::Map> T_result(x_plus_delta); - Eigen::Map> vec_update(delta); - Eigen::Matrix4d T_update = Eigen::Matrix4d::Identity(); - - T_update = expm(vec_update); - T_result = T_update * T; - - return true; - } - - bool PlusJacobian(const double * x, double* jacobian) const override { - - Eigen::Map> J(jacobian); - - J.fill(0); - - if (_refineRotation) - { - J(1, 2) = 1; - J(2, 1) = -1; - - J(4, 2) = -1; - J(6, 0) = 1; - - J(8, 1) = 1; - J(9, 0) = -1; - } - - if (_refineTranslation) - { - J(12, 3) = 1; - J(13, 4) = 1; - J(14, 5) = 1; - } - - return true; - } - - bool Minus(const double* y, const double* x, double* delta) const override { - throw std::invalid_argument("SE3::Manifold::Minus() should never be called"); - } - - bool MinusJacobian(const double* x, double* jacobian) const override { - throw std::invalid_argument("SE3::Manifold::MinusJacobian() should never be called"); - } - - int AmbientSize() const override { - return 16; - } - - int TangentSize() const override { - return 6; - } - -private: - bool _refineRotation; - bool _refineTranslation; -}; - - -class ManifoldRight : public utils::CeresManifold { -public: - ManifoldRight(bool refineRotation, bool refineTranslation) : - _refineRotation(refineRotation), - _refineTranslation(refineTranslation) - { - } - - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { - - Eigen::Map> T(x); - Eigen::Map> T_result(x_plus_delta); - Eigen::Map> vec_update(delta); - Eigen::Matrix4d T_update = Eigen::Matrix4d::Identity(); - - T_update = expm(vec_update); - T_result = T * T_update; - - return true; - } - - bool PlusJacobian(const double * x, double* jacobian) const override { - - Eigen::Map> J(jacobian); - Eigen::Map> T(x); - - J.fill(0); - - if (_refineRotation) - { - J(1, 2) = 1; - J(2, 1) = -1; - - J(4, 2) = -1; - J(6, 0) = 1; - - J(8, 1) = 1; - J(9, 0) = -1; - } - - if (_refineTranslation) - { - J(12, 3) = 1; - J(13, 4) = 1; - J(14, 5) = 1; - } - - return true; - } - - bool Minus(const double* y, const double* x, double* delta) const override { - throw std::invalid_argument("SE3::Manifold::Minus() should never be called"); - } - - bool MinusJacobian(const double* x, double* jacobian) const override { - throw std::invalid_argument("SE3::Manifold::MinusJacobian() should never be called"); - } - - int AmbientSize() const override { - return 16; - } - - int TangentSize() const override { - return 6; - } - -private: - bool _refineRotation; - bool _refineTranslation; -}; - -} //namespace SE3 - - -namespace SO2 { - -class Manifold : public utils::CeresManifold { -public: - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { - - Eigen::Map> T(x); - Eigen::Map> T_result(x_plus_delta); - double update = delta[0]; - - Eigen::Matrix2d T_update = expm(update); - T_result = T_update * T; - - return true; - } - - bool PlusJacobian(const double * x, double* jacobian) const override { - - Eigen::Map> J(jacobian); - - J.fill(0); - - J(1, 0) = 1; - J(2, 0) = -1; - - return true; - } - - bool Minus(const double* y, const double* x, double* delta) const override { - throw std::invalid_argument("SO3::Manifold::Minus() should never be called"); - } - - bool MinusJacobian(const double* x, double* jacobian) const override { - throw std::invalid_argument("SO3::Manifold::MinusJacobian() should never be called"); - } - - int AmbientSize() const override { - return 4; - } - - int TangentSize() const override { - return 1; - } -}; - -} //namespace SO2 - -} //namespace aliceVision diff --git a/src/aliceVision/sfm/manifolds/intrinsics.hpp b/src/aliceVision/sfm/manifolds/intrinsics.hpp new file mode 100644 index 0000000000..4c3ba929fb --- /dev/null +++ b/src/aliceVision/sfm/manifolds/intrinsics.hpp @@ -0,0 +1,180 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +class IntrinsicsManifoldSymbolic : public utils::CeresManifold { + public: + explicit IntrinsicsManifoldSymbolic(size_t parametersSize, + double focalRatio, bool lockFocal, bool lockFocalRatio, + bool lockCenter, bool lockDistortion) + : _ambientSize(parametersSize), + _focalRatio(focalRatio), + _lockFocal(lockFocal), + _lockFocalRatio(lockFocalRatio), + _lockCenter(lockCenter), + _lockDistortion(lockDistortion) + { + _distortionSize = _ambientSize - 4; + _tangentSize = 0; + + if (!_lockFocal) + { + if (_lockFocalRatio) + { + _tangentSize += 1; + } + else + { + _tangentSize += 2; + } + } + + if (!_lockCenter) + { + _tangentSize += 2; + } + + if (!_lockDistortion) + { + _tangentSize += _distortionSize; + } + } + + virtual ~IntrinsicsManifoldSymbolic() = default; + + + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + { + for (int i = 0; i < _ambientSize; i++) + { + x_plus_delta[i] = x[i]; + } + + size_t posDelta = 0; + if (!_lockFocal) + { + if (_lockFocalRatio) + { + x_plus_delta[0] = x[0] + delta[posDelta]; + x_plus_delta[1] = x[1] + _focalRatio * delta[posDelta]; + ++posDelta; + } + else + { + x_plus_delta[0] = x[0] + delta[posDelta]; + ++posDelta; + x_plus_delta[1] = x[1] + delta[posDelta]; + ++posDelta; + } + } + + if (!_lockCenter) + { + x_plus_delta[2] = x[2] + delta[posDelta]; + ++posDelta; + + x_plus_delta[3] = x[3] + delta[posDelta]; + ++posDelta; + } + + if (!_lockDistortion) + { + for (int i = 0; i < _distortionSize; i++) + { + x_plus_delta[4 + i] = x[4 + i] + delta[posDelta]; + ++posDelta; + } + } + + return true; + } + + bool PlusJacobian(const double* x, double* jacobian) const override + { + Eigen::Map> J(jacobian, AmbientSize(), TangentSize()); + + J.fill(0); + + size_t posDelta = 0; + if (!_lockFocal) + { + if (_lockFocalRatio) + { + J(0, posDelta) = 1.0; + J(1, posDelta) = _focalRatio; + ++posDelta; + } + else + { + J(0, posDelta) = 1.0; + ++posDelta; + J(1, posDelta) = 1.0; + ++posDelta; + } + } + + if (!_lockCenter) + { + J(2, posDelta) = 1.0; + ++posDelta; + + J(3, posDelta) = 1.0; + ++posDelta; + } + + if (!_lockDistortion) + { + for (int i = 0; i < _distortionSize; i++) + { + J(4 + i, posDelta) = 1.0; + ++posDelta; + } + } + + return true; + } + + bool Minus(const double* y, const double* x, double* delta) const override { + throw std::invalid_argument("IntrinsicsManifold::Minus() should never be called"); + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + throw std::invalid_argument("IntrinsicsManifold::MinusJacobian() should never be called"); + } + + int AmbientSize() const override + { + return _ambientSize; + } + + int TangentSize() const override + { + return _tangentSize; + } + + private: + size_t _distortionSize; + size_t _ambientSize; + size_t _tangentSize; + double _focalRatio; + bool _lockFocal; + bool _lockFocalRatio; + bool _lockCenter; + bool _lockDistortion; +}; + +} //namespace sfm +} //namespace aliceVision diff --git a/src/aliceVision/sfm/manifolds/se3.hpp b/src/aliceVision/sfm/manifolds/se3.hpp new file mode 100644 index 0000000000..a5f4cfa0f8 --- /dev/null +++ b/src/aliceVision/sfm/manifolds/se3.hpp @@ -0,0 +1,165 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace aliceVision { +namespace sfm { + + +class SE3ManifoldLeft : public utils::CeresManifold { +public: + SE3ManifoldLeft(bool refineRotation, bool refineTranslation) : + _refineRotation(refineRotation), + _refineTranslation(refineTranslation) + { + } + + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { + + Eigen::Map> T(x); + Eigen::Map> T_result(x_plus_delta); + Eigen::Map> vec_update(delta); + Eigen::Matrix4d T_update = Eigen::Matrix4d::Identity(); + + T_update = SE3::expm(vec_update); + T_result = T_update * T; + + return true; + } + + bool PlusJacobian(const double * x, double* jacobian) const override { + + Eigen::Map> J(jacobian); + + J.fill(0); + + if (_refineRotation) + { + J(1, 2) = 1; + J(2, 1) = -1; + + J(4, 2) = -1; + J(6, 0) = 1; + + J(8, 1) = 1; + J(9, 0) = -1; + } + + if (_refineTranslation) + { + J(12, 3) = 1; + J(13, 4) = 1; + J(14, 5) = 1; + } + + return true; + } + + bool Minus(const double* y, const double* x, double* delta) const override { + throw std::invalid_argument("SE3::Manifold::Minus() should never be called"); + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + throw std::invalid_argument("SE3::Manifold::MinusJacobian() should never be called"); + } + + int AmbientSize() const override { + return 16; + } + + int TangentSize() const override { + return 6; + } + +private: + bool _refineRotation; + bool _refineTranslation; +}; + + +class SE3ManifoldRight : public utils::CeresManifold { +public: + SE3ManifoldRight(bool refineRotation, bool refineTranslation) : + _refineRotation(refineRotation), + _refineTranslation(refineTranslation) + { + } + + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { + + Eigen::Map> T(x); + Eigen::Map> T_result(x_plus_delta); + Eigen::Map> vec_update(delta); + Eigen::Matrix4d T_update = Eigen::Matrix4d::Identity(); + + T_update = SE3::expm(vec_update); + T_result = T * T_update; + + return true; + } + + bool PlusJacobian(const double * x, double* jacobian) const override { + + Eigen::Map> J(jacobian); + Eigen::Map> T(x); + + J.fill(0); + + if (_refineRotation) + { + J(1, 2) = 1; + J(2, 1) = -1; + + J(4, 2) = -1; + J(6, 0) = 1; + + J(8, 1) = 1; + J(9, 0) = -1; + } + + if (_refineTranslation) + { + J(12, 3) = 1; + J(13, 4) = 1; + J(14, 5) = 1; + } + + return true; + } + + bool Minus(const double* y, const double* x, double* delta) const override { + throw std::invalid_argument("SE3::Manifold::Minus() should never be called"); + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + throw std::invalid_argument("SE3::Manifold::MinusJacobian() should never be called"); + } + + int AmbientSize() const override { + return 16; + } + + int TangentSize() const override { + return 6; + } + +private: + bool _refineRotation; + bool _refineTranslation; +}; + +} //namespace SE3 + +} //namespace aliceVision diff --git a/src/aliceVision/sfm/manifolds/so2.hpp b/src/aliceVision/sfm/manifolds/so2.hpp new file mode 100644 index 0000000000..fa784addb3 --- /dev/null +++ b/src/aliceVision/sfm/manifolds/so2.hpp @@ -0,0 +1,64 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include +#include + +namespace aliceVision { + +namespace sfm { + +class SO2Manifold : public utils::CeresManifold { +public: + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { + + Eigen::Map> T(x); + Eigen::Map> T_result(x_plus_delta); + double update = delta[0]; + + Eigen::Matrix2d T_update = expm(update); + T_result = T_update * T; + + return true; + } + + bool PlusJacobian(const double * x, double* jacobian) const override { + + Eigen::Map> J(jacobian); + + J.fill(0); + + J(1, 0) = 1; + J(2, 0) = -1; + + return true; + } + + bool Minus(const double* y, const double* x, double* delta) const override { + throw std::invalid_argument("SO3::Manifold::Minus() should never be called"); + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + throw std::invalid_argument("SO3::Manifold::MinusJacobian() should never be called"); + } + + int AmbientSize() const override { + return 4; + } + + int TangentSize() const override { + return 1; + } +}; + +} //namespace sfm + +} //namespace aliceVision diff --git a/src/aliceVision/sfm/manifolds/so3.hpp b/src/aliceVision/sfm/manifolds/so3.hpp new file mode 100644 index 0000000000..98582c7c22 --- /dev/null +++ b/src/aliceVision/sfm/manifolds/so3.hpp @@ -0,0 +1,137 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include +#include + +namespace aliceVision { +namespace SO3 { + +/** +Compute the jacobian of the logarithm wrt changes in the rotation matrix values +@param R the input rotation matrix +@return the jacobian matrix (3*9 matrix) +*/ +inline Eigen::Matrix dlogmdr(const Eigen::Matrix3d & R) { + double p1 = R(2, 1) - R(1, 2); + double p2 = R(0, 2) - R(2, 0); + double p3 = R(1, 0) - R(0, 1); + + double costheta = (R.trace() - 1.0) / 2.0; + if (costheta > 1.0) costheta = 1.0; + else if (costheta < -1.0) costheta = -1.0; + + double theta = acos(costheta); + + if (fabs(theta) < std::numeric_limits::epsilon()) { + Eigen::Matrix J; + J.fill(0); + J(0, 5) = 1; + J(0, 7) = -1; + J(1, 2) = -1; + J(1, 6) = 1; + J(2, 1) = 1; + J(2, 3) = -1; + return J; + } + + double scale = theta / (2.0 * sin(theta)); + + Eigen::Vector3d resnoscale; + resnoscale(0) = p1; + resnoscale(1) = p2; + resnoscale(2) = p3; + + Eigen::Matrix dresdp = Eigen::Matrix3d::Identity() * scale; + Eigen::Matrix dpdmat; + dpdmat.fill(0); + dpdmat(0, 5) = 1; + dpdmat(0, 7) = -1; + dpdmat(1, 2) = -1; + dpdmat(1, 6) = 1; + dpdmat(2, 1) = 1; + dpdmat(2, 3) = -1; + + + double dscaledtheta = -0.5 * theta * cos(theta) / (sin(theta)*sin(theta)) + 0.5 / sin(theta); + double dthetadcostheta = -1.0 / sqrt(-costheta*costheta + 1.0); + + Eigen::Matrix dcosthetadmat; + dcosthetadmat << 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5; + Eigen::Matrix dscaledmat = dscaledtheta * dthetadcostheta * dcosthetadmat; + + return dpdmat * scale + resnoscale * dscaledmat; +} +} + +namespace sfm { + +class SO3Manifold : public utils::CeresManifold { + public: + ~SO3Manifold() override = default; + + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { + + double* ptrBase = (double*)x; + double* ptrResult = (double*)x_plus_delta; + Eigen::Map > rotation(ptrBase); + Eigen::Map > rotationResult(ptrResult); + + Eigen::Vector3d axis; + axis(0) = delta[0]; + axis(1) = delta[1]; + axis(2) = delta[2]; + double angle = axis.norm(); + + axis.normalize(); + + Eigen::AngleAxisd aa(angle, axis); + Eigen::Matrix3d Rupdate; + Rupdate = aa.toRotationMatrix(); + + rotationResult = Rupdate * rotation; + + return true; + } + + bool PlusJacobian(const double* /*x*/, double* jacobian) const override { + + Eigen::Map> J(jacobian); + //Eigen::Map> R(x); + + J.fill(0); + + J(1, 2) = 1; + J(2, 1) = -1; + J(3, 2) = -1; + J(5, 0) = 1; + J(6, 1) = 1; + J(7, 0) = -1; + + return true; + } + + bool Minus(const double* y, const double* x, double* delta) const override { + throw std::invalid_argument("SO3::Manifold::Minus() should never be called"); + } + + bool MinusJacobian(const double* x, double* jacobian) const override { + throw std::invalid_argument("SO3::Manifold::MinusJacobian() should never be called"); + } + + int AmbientSize() const override { return 9; } + + int TangentSize() const override { return 3; } +}; + +}//namespace sfm + +} //namespace aliceVision diff --git a/src/aliceVision/sfm/utils/alignment.cpp b/src/aliceVision/sfm/utils/alignment.cpp index 7f63c89d37..50894b40f1 100644 --- a/src/aliceVision/sfm/utils/alignment.cpp +++ b/src/aliceVision/sfm/utils/alignment.cpp @@ -6,7 +6,7 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include -#include +#include #include #include From 6508ba2cb4242b396754e42c9bfd65eba26b0bfa Mon Sep 17 00:00:00 2001 From: Fabien SERVANT Date: Fri, 8 Sep 2023 15:21:25 +0200 Subject: [PATCH 2/4] merge panorama and symbolic --- .../sfm/BundleAdjustmentPanoramaCeres.cpp | 565 ------------------ .../sfm/BundleAdjustmentPanoramaCeres.hpp | 257 -------- .../sfm/BundleAdjustmentSymbolicCeres.cpp | 86 ++- .../sfm/BundleAdjustmentSymbolicCeres.hpp | 17 + src/aliceVision/sfm/CMakeLists.txt | 2 - .../sfm/costfunctions/panoramaEquidistant.hpp | 31 +- .../sfm/costfunctions/panoramaPinhole.hpp | 33 +- .../sfm/costfunctions/rotationPrior.hpp | 31 +- .../ReconstructionEngine_panorama.cpp | 31 +- src/aliceVision/sfmDataIO/alembicIO_test.cpp | 2 +- 10 files changed, 186 insertions(+), 869 deletions(-) delete mode 100644 src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp delete mode 100644 src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp diff --git a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp deleted file mode 100644 index 9c41a30e4b..0000000000 --- a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.cpp +++ /dev/null @@ -1,565 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// This Source Code Form is subject to the terms of the Mozilla Public License, -// v. 2.0. If a copy of the MPL was not distributed with this file, -// You can obtain one at https://mozilla.org/MPL/2.0/. - -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include -#include -#include - -#include - -namespace fs = boost::filesystem; - -namespace aliceVision { - - - -namespace sfm { - -using namespace aliceVision::camera; -using namespace aliceVision::geometry; - - -void BundleAdjustmentPanoramaCeres::CeresOptions::setDenseBA() -{ - // default configuration use a DENSE representation - preconditionerType = ceres::JACOBI; - linearSolverType = ceres::DENSE_SCHUR; - sparseLinearAlgebraLibraryType = ceres::SUITE_SPARSE; // not used but just to avoid a warning in ceres - ALICEVISION_LOG_DEBUG("BundleAdjustmentParnorama[Ceres]: DENSE_SCHUR"); -} - -void BundleAdjustmentPanoramaCeres::CeresOptions::setSparseBA() -{ - preconditionerType = ceres::JACOBI; - // if Sparse linear solver are available - // descending priority order by efficiency (SUITE_SPARSE > CX_SPARSE > EIGEN_SPARSE) - if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE)) - { - sparseLinearAlgebraLibraryType = ceres::SUITE_SPARSE; - linearSolverType = ceres::SPARSE_SCHUR; - ALICEVISION_LOG_DEBUG("BundleAdjustmentParnorama[Ceres]: SPARSE_SCHUR, SUITE_SPARSE"); - } -#if ALICEVISION_CERES_HAS_CXSPARSE - else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE)) - { - sparseLinearAlgebraLibraryType = ceres::CX_SPARSE; - linearSolverType = ceres::SPARSE_SCHUR; - ALICEVISION_LOG_DEBUG("BundleAdjustmentParnorama[Ceres]: SPARSE_SCHUR, CX_SPARSE"); - } -#endif - else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::EIGEN_SPARSE)) - { - sparseLinearAlgebraLibraryType = ceres::EIGEN_SPARSE; - linearSolverType = ceres::SPARSE_SCHUR; - ALICEVISION_LOG_DEBUG("BundleAdjustmentParnorama[Ceres]: SPARSE_SCHUR, EIGEN_SPARSE"); - } - else - { - linearSolverType = ceres::DENSE_SCHUR; - ALICEVISION_LOG_WARNING("BundleAdjustmentParnorama[Ceres]: no sparse BA available, fallback to dense BA."); - } -} - -bool BundleAdjustmentPanoramaCeres::Statistics::exportToFile(const std::string& folder, const std::string& filename) const -{ - std::ofstream os; - os.open((fs::path(folder) / filename).string(), std::ios::app); - - if(!os.is_open()) - { - ALICEVISION_LOG_DEBUG("Unable to open the Bundle adjustment statistics file: '" << filename << "'."); - return false; - } - - os.seekp(0, std::ios::end); // put the cursor at the end - - if(os.tellp() == std::streampos(0)) // 'tellp' return the cursor's position - { - // if the file does't exist: add a header. - os << "Time/BA(s);RefinedPose;ConstPose;IgnoredPose;" - "RefinedPts;ConstPts;IgnoredPts;" - "RefinedK;ConstK;IgnoredK;" - "ResidualBlocks;SuccessIteration;BadIteration;" - "InitRMSE;FinalRMSE;" - "d=-1;d=0;d=1;d=2;d=3;d=4;" - "d=5;d=6;d=7;d=8;d=9;d=10+;\n"; - } - - std::map> states = parametersStates; - std::size_t posesWithDistUpperThanTen = 0; - - for(const auto& it : nbCamerasPerDistance) - if (it.first >= 10) - posesWithDistUpperThanTen += it.second; - - os << time << ";" - << states[EParameter::POSE][EParameterState::REFINED] << ";" - << states[EParameter::POSE][EParameterState::CONSTANT] << ";" - << states[EParameter::POSE][EParameterState::IGNORED] << ";" - << states[EParameter::INTRINSIC][EParameterState::REFINED] << ";" - << states[EParameter::INTRINSIC][EParameterState::CONSTANT] << ";" - << states[EParameter::INTRINSIC][EParameterState::IGNORED] << ";" - << nbResidualBlocks << ";" - << nbSuccessfullIterations << ";" - << nbUnsuccessfullIterations << ";" - << RMSEinitial << ";" - << RMSEfinal << ";"; - - for(int i = -1; i < 10; ++i) - { - auto cdIt = nbCamerasPerDistance.find(i); - if(cdIt != nbCamerasPerDistance.end()) - os << cdIt->second << ";"; - else - os << "0;"; - } - - os << posesWithDistUpperThanTen << ";\n"; - - os.close(); - return true; -} - -void BundleAdjustmentPanoramaCeres::Statistics::show() const -{ - std::map> states = parametersStates; - std::stringstream ss; - - if(!nbCamerasPerDistance.empty()) - { - std::size_t nbCamNotConnected = 0; - std::size_t nbCamDistEqZero = 0; - std::size_t nbCamDistEqOne = 0; - std::size_t nbCamDistUpperOne = 0; - - for(const auto & camdistIt : nbCamerasPerDistance) - { - if(camdistIt.first < 0) - nbCamNotConnected += camdistIt.second; - else if(camdistIt.first == 0) - nbCamDistEqZero += camdistIt.second; - else if(camdistIt.first == 1) - nbCamDistEqOne += camdistIt.second; - else if(camdistIt.first > 1) - nbCamDistUpperOne += camdistIt.second; - } - - ss << "\t- local strategy enabled: yes\n" - << "\t- graph-distances distribution:\n" - << "\t - not connected: " << nbCamNotConnected << " cameras\n" - << "\t - D = 0: " << nbCamDistEqZero << " cameras\n" - << "\t - D = 1: " << nbCamDistEqOne << " cameras\n" - << "\t - D > 1: " << nbCamDistUpperOne << " cameras\n"; - } - else - { - ss << "\t- local strategy enabled: no\n"; - } - - ALICEVISION_LOG_INFO("Bundle Adjustment Statistics:\n" - << ss.str() - << "\t- adjustment duration: " << time << " s\n" - << "\t- poses:\n" - << "\t - # refined: " << states[EParameter::POSE][EParameterState::REFINED] << "\n" - << "\t - # constant: " << states[EParameter::POSE][EParameterState::CONSTANT] << "\n" - << "\t - # ignored: " << states[EParameter::POSE][EParameterState::IGNORED] << "\n" - << "\t- intrinsics:\n" - << "\t - # refined: " << states[EParameter::INTRINSIC][EParameterState::REFINED] << "\n" - << "\t - # constant: " << states[EParameter::INTRINSIC][EParameterState::CONSTANT] << "\n" - << "\t - # ignored: " << states[EParameter::INTRINSIC][EParameterState::IGNORED] << "\n" - << "\t- # residual blocks: " << nbResidualBlocks << "\n" - << "\t- # successful iterations: " << nbSuccessfullIterations << "\n" - << "\t- # unsuccessful iterations: " << nbUnsuccessfullIterations << "\n" - << "\t- initial RMSE: " << RMSEinitial << "\n" - << "\t- final RMSE: " << RMSEfinal); -} - -void BundleAdjustmentPanoramaCeres::setSolverOptions(ceres::Solver::Options& solverOptions) const -{ - solverOptions.preconditioner_type = _ceresOptions.preconditionerType; - solverOptions.linear_solver_type = _ceresOptions.linearSolverType; - solverOptions.sparse_linear_algebra_library_type = _ceresOptions.sparseLinearAlgebraLibraryType; - solverOptions.minimizer_progress_to_stdout = _ceresOptions.verbose; - solverOptions.logging_type = ceres::SILENT; - solverOptions.num_threads = _ceresOptions.nbThreads; - solverOptions.max_num_iterations = 300; - - -#if CERES_VERSION_MAJOR < 2 - solverOptions.num_linear_solver_threads = _ceresOptions.nbThreads; -#endif -} - -void BundleAdjustmentPanoramaCeres::addExtrinsicsToProblem(const sfmData::SfMData& sfmData, BundleAdjustment::ERefineOptions refineOptions, ceres::Problem& problem) -{ - const bool refineRotation = refineOptions & BundleAdjustment::REFINE_ROTATION; - - const auto addPose = [&](const sfmData::CameraPose& cameraPose, bool isConstant, SO3::Matrix& poseBlock) - { - const Mat3& R = cameraPose.getTransform().rotation(); - poseBlock = R; - double* poseBlockPtr = poseBlock.data(); - - /*Define rotation parameterization*/ -#if ALICEVISION_CERES_HAS_MANIFOLD - problem.AddParameterBlock(poseBlockPtr, 9, new SO3Manifold); -#else - problem.AddParameterBlock(poseBlockPtr, 9, new utils::ManifoldToParameterizationWrapper(new SO3Manifold)); -#endif - - // keep the camera extrinsics constants - if(cameraPose.isLocked() || isConstant || !refineRotation) - { - // set the whole parameter block as constant. - _statistics.addState(EParameter::POSE, EParameterState::CONSTANT); - problem.SetParameterBlockConstant(poseBlockPtr); - return; - } - - _statistics.addState(EParameter::POSE, EParameterState::REFINED); - }; - - // setup poses data - for(const auto& posePair : sfmData.getPoses()) - { - const IndexT poseId = posePair.first; - const sfmData::CameraPose& pose = posePair.second; - - // skip camera pose set as Ignored in the Local strategy - if(getPoseState(poseId) == EParameterState::IGNORED) - { - _statistics.addState(EParameter::POSE, EParameterState::IGNORED); - continue; - } - - const bool isConstant = (getPoseState(poseId) == EParameterState::CONSTANT); - - addPose(pose, isConstant, _posesBlocks[poseId]); - } -} - -void BundleAdjustmentPanoramaCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmData, BundleAdjustment::ERefineOptions refineOptions, ceres::Problem& problem) -{ - const std::size_t minImagesForOpticalCenter = 3; - - const bool refineIntrinsicsOpticalCenter = (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA); - const bool refineIntrinsicsFocalLength = refineOptions & REFINE_INTRINSICS_FOCAL; - const bool refineIntrinsicsDistortion = refineOptions & REFINE_INTRINSICS_DISTORTION; - const bool refineIntrinsics = refineIntrinsicsDistortion || refineIntrinsicsFocalLength || refineIntrinsicsOpticalCenter; - - std::map intrinsicsUsage; - - // count the number of reconstructed views per intrinsic - for(const auto& viewPair: sfmData.getViews()) - { - const sfmData::View& view = *(viewPair.second); - - if(intrinsicsUsage.find(view.getIntrinsicId()) == intrinsicsUsage.end()) { - intrinsicsUsage[view.getIntrinsicId()] = 0; - } - - if(sfmData.isPoseAndIntrinsicDefined(&view)) { - ++intrinsicsUsage.at(view.getIntrinsicId()); - } - } - - for(const auto& intrinsicPair: sfmData.getIntrinsics()) - { - const IndexT intrinsicId = intrinsicPair.first; - const auto& intrinsicPtr = intrinsicPair.second; - const auto usageIt = intrinsicsUsage.find(intrinsicId); - if(usageIt == intrinsicsUsage.end()) { - // if the intrinsic is never referenced by any view, skip it - continue; - } - const std::size_t usageCount = usageIt->second; - - // do not refine an intrinsic does not used by any reconstructed view - if(usageCount == UndefinedIndexT || getIntrinsicState(intrinsicId) == EParameterState::IGNORED) - { - _statistics.addState(EParameter::INTRINSIC, EParameterState::IGNORED); - continue; - } - - assert(isValid(intrinsicPtr->getType())); - - std::vector& intrinsicBlock = _intrinsicsBlocks[intrinsicId]; - intrinsicBlock = intrinsicPtr->getParams(); - - double* intrinsicBlockPtr = intrinsicBlock.data(); - problem.AddParameterBlock(intrinsicBlockPtr, intrinsicBlock.size()); - - // keep the camera intrinsic constant - if(intrinsicPtr->isLocked() || !refineIntrinsics || getIntrinsicState(intrinsicId) == EParameterState::CONSTANT) - { - // set the whole parameter block as constant. - _statistics.addState(EParameter::INTRINSIC, EParameterState::CONSTANT); - problem.SetParameterBlockConstant(intrinsicBlockPtr); - continue; - } - - // constant parameters - std::vector constantIntrinisc; - - // refine the focal length - if(refineIntrinsicsFocalLength) - { - // we don't have an initial guess, but we assume that we use - // a converging lens, so the focal length should be positive. - problem.SetParameterLowerBound(intrinsicBlockPtr, 0, 0.0); - problem.SetParameterLowerBound(intrinsicBlockPtr, 1, 0.0); - } - else - { - // set focal length as constant - constantIntrinisc.push_back(0); - constantIntrinisc.push_back(1); - } - - // optical center - bool optional_center = ((refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA) && (usageCount > minImagesForOpticalCenter)); - if((refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || optional_center) - { - // refine optical center within 10% of the image size. - assert(intrinsicBlock.size() >= 4); - - const double opticalCenterMinPercent = -0.05; - const double opticalCenterMaxPercent = 0.05; - - // add bounds to the principal point - problem.SetParameterLowerBound(intrinsicBlockPtr, 2, opticalCenterMinPercent * intrinsicPtr->w()); - problem.SetParameterUpperBound(intrinsicBlockPtr, 2, opticalCenterMaxPercent * intrinsicPtr->w()); - problem.SetParameterLowerBound(intrinsicBlockPtr, 3, opticalCenterMinPercent * intrinsicPtr->h()); - problem.SetParameterUpperBound(intrinsicBlockPtr, 3, opticalCenterMaxPercent * intrinsicPtr->h()); - } - else - { - // don't refine the optical center - constantIntrinisc.push_back(2); - constantIntrinisc.push_back(3); - } - - // lens distortion - if(!refineIntrinsicsDistortion) { - for(std::size_t i = 4; i < intrinsicBlock.size(); ++i) { - constantIntrinisc.push_back(i); - } - } - - if(!constantIntrinisc.empty()) - { -#if ALICEVISION_CERES_HAS_MANIFOLD - auto* subsetManifold = new ceres::SubsetManifold(intrinsicBlock.size(), constantIntrinisc); - problem.SetManifold(intrinsicBlockPtr, subsetManifold); -#else - ceres::SubsetParameterization* subsetParameterization = new ceres::SubsetParameterization(intrinsicBlock.size(), constantIntrinisc); - problem.SetParameterization(intrinsicBlockPtr, subsetParameterization); -#endif - } - - _statistics.addState(EParameter::INTRINSIC, EParameterState::REFINED); - } -} - - -void BundleAdjustmentPanoramaCeres::addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) -{ - // set a LossFunction to be less penalized by false measurements. - // note: set it to NULL if you don't want use a lossFunction. - ceres::LossFunction* lossFunction = new ceres::HuberLoss(Square(8.0)); // TODO: make the LOSS function and the parameter an option - - for (const auto & constraint : sfmData.getConstraints2D()) { - const sfmData::View& view_1 = sfmData.getView(constraint.ViewFirst); - const sfmData::View& view_2 = sfmData.getView(constraint.ViewSecond); - - assert(getPoseState(view_1.getPoseId()) != EParameterState::IGNORED); - assert(getIntrinsicState(view_1.getIntrinsicId()) != EParameterState::IGNORED); - assert(getPoseState(view_2.getPoseId()) != EParameterState::IGNORED); - assert(getIntrinsicState(view_2.getIntrinsicId()) != EParameterState::IGNORED); - - /* Get pose */ - double * poseBlockPtr_1 = _posesBlocks.at(view_1.getPoseId()).data(); - double * poseBlockPtr_2 = _posesBlocks.at(view_2.getPoseId()).data(); - - /*Get intrinsics */ - double * intrinsicBlockPtr_1 = _intrinsicsBlocks.at(view_1.getIntrinsicId()).data(); - double * intrinsicBlockPtr_2 = _intrinsicsBlocks.at(view_2.getIntrinsicId()).data(); - - /* For the moment assume a unique camera */ - assert(intrinsicBlockPtr_1 == intrinsicBlockPtr_2); - - std::shared_ptr intrinsic = sfmData.getIntrinsicsharedPtr(view_1.getIntrinsicId()); - std::shared_ptr equidistant = std::dynamic_pointer_cast(intrinsic); - std::shared_ptr pinhole = std::dynamic_pointer_cast(intrinsic); - - if (equidistant != nullptr) - { - ceres::CostFunction* costFunction = new CostPanoramaEquidistant(constraint.ObservationFirst.x, constraint.ObservationSecond.x, equidistant); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); - - /* Symmetry */ - costFunction = new CostPanoramaEquidistant(constraint.ObservationSecond.x, constraint.ObservationFirst.x, equidistant); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); - } - else if (pinhole != nullptr) - { - ceres::CostFunction* costFunction = new CostPanoramaPinHole(constraint.ObservationFirst.x, constraint.ObservationSecond.x, pinhole); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); - /* Symmetry */ - costFunction = new CostPanoramaPinHole(constraint.ObservationSecond.x, constraint.ObservationFirst.x, pinhole); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); - } - else { - ALICEVISION_LOG_ERROR("Incompatible camera for a 2D constraint"); - return; - } - } -} - -void BundleAdjustmentPanoramaCeres::addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) -{ - // set a LossFunction to be less penalized by false measurements. - // note: set it to NULL if you don't want use a lossFunction. - ceres::LossFunction* lossFunction = nullptr; - - for (const auto & prior : sfmData.getRotationPriors()) { - - const sfmData::View& view_1 = sfmData.getView(prior.ViewFirst); - const sfmData::View& view_2 = sfmData.getView(prior.ViewSecond); - - assert(getPoseState(view_1.getPoseId()) != EParameterState::IGNORED); - assert(getPoseState(view_2.getPoseId()) != EParameterState::IGNORED); - - double * poseBlockPtr_1 = _posesBlocks.at(view_1.getPoseId()).data(); - double * poseBlockPtr_2 = _posesBlocks.at(view_2.getPoseId()).data(); - - ceres::CostFunction* costFunction = new CostRotationPrior(prior._second_R_first); - problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2); - } -} - -void BundleAdjustmentPanoramaCeres::createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) -{ - // clear previously computed data - resetProblem(); - - // add SfM extrincics to the Ceres problem - addExtrinsicsToProblem(sfmData, refineOptions, problem); - - // add SfM intrinsics to the Ceres problem - addIntrinsicsToProblem(sfmData, refineOptions, problem); - - // add 2D constraints to the Ceres problem - addConstraints2DToProblem(sfmData, refineOptions, problem); - - // add rotation priors to the Ceres problem - addRotationPriorsToProblem(sfmData, refineOptions, problem); -} - -void BundleAdjustmentPanoramaCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const -{ - const bool refinePoses = (refineOptions & REFINE_ROTATION) || (refineOptions & REFINE_TRANSLATION); - const bool refineIntrinsicsOpticalCenter = (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA); - const bool refineIntrinsics = (refineOptions & REFINE_INTRINSICS_FOCAL) || (refineOptions & REFINE_INTRINSICS_DISTORTION) || refineIntrinsicsOpticalCenter; - const bool refineStructure = refineOptions & REFINE_STRUCTURE; - - // update camera poses with refined data - if(refinePoses) - { - // absolute poses - for(auto& posePair : sfmData.getPoses()) - { - const IndexT poseId = posePair.first; - - // do not update a camera pose set as Ignored or Constant in the Local strategy - if(getPoseState(poseId) != EParameterState::REFINED) { - continue; - } - - const SO3::Matrix & poseBlock = _posesBlocks.at(poseId); - - // update the pose - posePair.second.setTransform(poseFromRT(poseBlock, Vec3(0,0,0))); - } - } - - // update camera intrinsics with refined data - if(refineIntrinsics) - { - for(const auto& intrinsicBlockPair: _intrinsicsBlocks) - { - const IndexT intrinsicId = intrinsicBlockPair.first; - - // do not update a camera pose set as Ignored or Constant in the Local strategy - if(getIntrinsicState(intrinsicId) != EParameterState::REFINED) { - continue; - } - - sfmData.getIntrinsics().at(intrinsicId)->updateFromParams(intrinsicBlockPair.second); - } - } -} - -bool BundleAdjustmentPanoramaCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions refineOptions) -{ - // create problem - ceres::Problem problem; - createProblem(sfmData, refineOptions, problem); - - // configure a Bundle Adjustment engine and run it - // make Ceres automatically detect the bundle structure. - ceres::Solver::Options options; - setSolverOptions(options); - - // solve BA - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); - - // print summary - if(_ceresOptions.summary) { - ALICEVISION_LOG_INFO(summary.FullReport()); - } - - // solution is not usable - if(!summary.IsSolutionUsable()) - { - ALICEVISION_LOG_WARNING("Bundle Adjustment failed, the solution is not usable."); - return false; - } - - // update input sfmData with the solution - updateFromSolution(sfmData, refineOptions); - - // store some statitics from the summary - _statistics.time = summary.total_time_in_seconds; - _statistics.nbSuccessfullIterations = summary.num_successful_steps; - _statistics.nbUnsuccessfullIterations = summary.num_unsuccessful_steps; - _statistics.nbResidualBlocks = summary.num_residuals; - _statistics.RMSEinitial = std::sqrt(summary.initial_cost / summary.num_residuals); - _statistics.RMSEfinal = std::sqrt(summary.final_cost / summary.num_residuals); - - return true; -} - - - -} // namespace sfm -} // namespace aliceVision - diff --git a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp b/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp deleted file mode 100644 index 032cf0ab8f..0000000000 --- a/src/aliceVision/sfm/BundleAdjustmentPanoramaCeres.hpp +++ /dev/null @@ -1,257 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// This Source Code Form is subject to the terms of the Mozilla Public License, -// v. 2.0. If a copy of the MPL was not distributed with this file, -// You can obtain one at https://mozilla.org/MPL/2.0/. - -#pragma once - -#include -#include -#include -#include - -#include -#include - -namespace aliceVision { - - -namespace sfmData { -class SfMData; -} // namespace sfmData - -namespace sfm { - -class BundleAdjustmentPanoramaCeres : public BundleAdjustment -{ -public: - - /** - * @brief Contains all ceres parameters. - */ - struct CeresOptions - { - CeresOptions(bool verbose = true, bool multithreaded = true) - : verbose(verbose) - , nbThreads(multithreaded ? omp_get_max_threads() : 1) // set number of threads, 1 if OpenMP is not enabled - { - setDenseBA(); // use dense BA by default - } - - void setDenseBA(); - void setSparseBA(); - - ceres::LinearSolverType linearSolverType; - ceres::PreconditionerType preconditionerType; - ceres::SparseLinearAlgebraLibraryType sparseLinearAlgebraLibraryType; - unsigned int nbThreads; - bool summary = false; - bool verbose = true; - }; - - /** - * @brief Contains all informations related to the performed bundle adjustment. - */ - struct Statistics - { - Statistics() - {} - - /** - * @brief Add a parameter state - * @param[in] parameter A bundle adjustment parameter - * @param[in] state A bundle adjustment state - */ - inline void addState(EParameter parameter, EParameterState state) - { - ++parametersStates[parameter][state]; - } - - /** - * @brief Export statistics about bundle adjustment in a CSV file - * The contents of the file have been writen such that it is easy to handle it with - * a Python script or any spreadsheets (e.g. by copy/past the full content to LibreOffice) - * @param[in] folder The folder where you want to save the statistics file - * @param[in] filename The filename of the statistics file - * @return false it cannot open the file, true if it succeed - */ - bool exportToFile(const std::string& folder, const std::string& filename = "statistics.csv") const; - - /** - * @brief Display statistics about bundle adjustment in the terminal - * Logger need to accept log level - */ - void show() const; - - // public members - - /// number of successful iterations - std::size_t nbSuccessfullIterations = 0; - /// number of unsuccessful iterations - std::size_t nbUnsuccessfullIterations = 0; - /// number of resiudal blocks in the Ceres problem - std::size_t nbResidualBlocks = 0; - /// RMSEinitial: sqrt(initial_cost / num_residuals) - double RMSEinitial = 0.0; - /// RMSEfinal: sqrt(final_cost / num_residuals) - double RMSEfinal = 0.0; - /// time spent to solve the BA (s) - double time = 0.0; - /// number of states per parameter - std::map> parametersStates; - /// The distribution of the cameras for each graph distance - std::map nbCamerasPerDistance; - }; - - /** - * @brief Bundle adjustment constructor - * @param[in] options The user Ceres options - * @see BundleAdjustmentPanoramaCeres::CeresOptions - */ - BundleAdjustmentPanoramaCeres(const BundleAdjustmentPanoramaCeres::CeresOptions& options = CeresOptions()) - : _ceresOptions(options) - { - - } - - /** - * @brief Perform a Bundle Adjustment on the SfM scene with refinement of the requested parameters - * @param[in,out] sfmData The input SfMData contains all the information about the reconstruction - * @param[in] refineOptions The chosen refine flag - * @return false if the bundle adjustment failed else true - * @see BundleAdjustment::Adjust - */ - bool adjust(sfmData::SfMData& sfmData, ERefineOptions refineOptions = REFINE_ALL); - - - /** - * @brief Get bundle adjustment statistics structure - * @return statistics structure const ptr - */ - inline const Statistics& getStatistics() const - { - return _statistics; - } - - /** - * @brief Return true if the bundle adjustment use an external local graph - * @return true if use an external local graph - */ - inline bool useLocalStrategy() const - { - return (_localGraph != nullptr); - } - -private: - - /** - * @brief Clear structures for a new problem - */ - inline void resetProblem() - { - _statistics = Statistics(); - _posesBlocks.clear(); - _intrinsicsBlocks.clear(); - } - - /** - * @brief Set user Ceres options to the solver - * @param[in,out] solverOptions The solver options structure - */ - void setSolverOptions(ceres::Solver::Options& solverOptions) const; - - /** - * @brief Create a parameter block for each extrinsics according to the Ceres format: [Rx, Ry, Rz, tx, ty, tz] - * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the poses and sub-poses - * @param[in] refineOptions The chosen refine flag - * @param[out] problem The Ceres bundle adjustement problem - */ - void addExtrinsicsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); - - /** - * @brief Create a parameter block for each intrinsic according to the Ceres format - * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics - * @param[in] refineOptions The chosen refine flag - * @param[out] problem The Ceres bundle adjustement problem - */ - void addIntrinsicsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); - - /** - * @brief Create a residual block for each 2D constraints - * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics - * @param[in] refineOptions The chosen refine flag - * @param[out] problem The Ceres bundle adjustement problem - */ - void addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); - - /** - * @brief Create a residual block for each rotation priors - * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics - * @param[in] refineOptions The chosen refine flag - * @param[out] problem The Ceres bundle adjustement problem - */ - void addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); - - /** - * @brief Create the Ceres bundle adjustement problem with: - * - extrincics and intrinsics parameters blocks. - * - residuals blocks for each observation. - * @param[in,out] sfmData The input SfMData contains all the information about the reconstruction - * @param[in] refineOptions The chosen refine flag - * @param[out] problem The Ceres bundle adjustement problem - */ - void createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); - - /** - * @brief Update The given SfMData with the solver solution - * @param[in,out] sfmData The input SfMData contains all the information about the reconstruction, notably the poses and sub-poses - * @param[in] refineOptions The chosen refine flag - */ - void updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const; - - /** - * @brief Return the BundleAdjustment::EParameterState for a specific pose. - * @param[in] poseId The pose id - * @return BundleAdjustment::EParameterState (always REFINED if no local strategy) - */ - inline BundleAdjustment::EParameterState getPoseState(IndexT poseId) const - { - return (_localGraph != nullptr ? _localGraph->getPoseState(poseId) : BundleAdjustment::EParameterState::REFINED); - } - - /** - * @brief Return the BundleAdjustment::EParameterState for a specific intrinsic. - * @param[in] intrinsicId The intrinsic id - * @return BundleAdjustment::EParameterState (always REFINED if no local strategy) - */ - inline BundleAdjustment::EParameterState getIntrinsicState(IndexT intrinsicId) const - { - return (_localGraph != nullptr ? _localGraph->getIntrinsicState(intrinsicId) : BundleAdjustment::EParameterState::REFINED); - } - - // private members - - /// use or not the local budle adjustment strategy - std::shared_ptr _localGraph = nullptr; - - /// user Ceres options to use in the solver - CeresOptions _ceresOptions; - - /// last adjustment iteration statisics - Statistics _statistics; - - // data wrappers for refinement - /// poses blocks wrapper - /// block: ceres angleAxis(3) + translation(3) - HashMap _posesBlocks; //TODO : maybe we can use boost::flat_map instead of HashMap ? - - /// intrinsics blocks wrapper - /// block: intrinsics params - HashMap> _intrinsicsBlocks; - -}; - -} // namespace sfm -} // namespace aliceVision diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp index 71e7236f58..b1d3b83aa0 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp @@ -22,7 +22,9 @@ #include #include - +#include +#include +#include #include @@ -389,7 +391,7 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat if(refineIntrinsicsFocalLength) { std::shared_ptr intrinsicScaleOffset = std::dynamic_pointer_cast(intrinsicPtr); - if(intrinsicScaleOffset->getInitialScale().x() > 0 && intrinsicScaleOffset->getInitialScale().y() > 0) + if(intrinsicScaleOffset->getInitialScale().x() > 0 && intrinsicScaleOffset->getInitialScale().y() > 0 && _ceresOptions.useFocalPrior) { // if we have an initial guess, we only authorize a margin around this value. assert(intrinsicBlock.size() >= 1); @@ -553,6 +555,81 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData } } +void BundleAdjustmentSymbolicCeres::addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) +{ + // set a LossFunction to be less penalized by false measurements. + // note: set it to NULL if you don't want use a lossFunction. + ceres::LossFunction* lossFunction = new ceres::HuberLoss(Square(8.0)); // TODO: make the LOSS function and the parameter an option + + for (const auto & constraint : sfmData.getConstraints2D()) { + const sfmData::View& view_1 = sfmData.getView(constraint.ViewFirst); + const sfmData::View& view_2 = sfmData.getView(constraint.ViewSecond); + + assert(getPoseState(view_1.getPoseId()) != EParameterState::IGNORED); + assert(getIntrinsicState(view_1.getIntrinsicId()) != EParameterState::IGNORED); + assert(getPoseState(view_2.getPoseId()) != EParameterState::IGNORED); + assert(getIntrinsicState(view_2.getIntrinsicId()) != EParameterState::IGNORED); + + /* Get pose */ + double * poseBlockPtr_1 = _posesBlocks.at(view_1.getPoseId()).data(); + double * poseBlockPtr_2 = _posesBlocks.at(view_2.getPoseId()).data(); + + /*Get intrinsics */ + double * intrinsicBlockPtr_1 = _intrinsicsBlocks.at(view_1.getIntrinsicId()).data(); + double * intrinsicBlockPtr_2 = _intrinsicsBlocks.at(view_2.getIntrinsicId()).data(); + + /* For the moment assume a unique camera */ + assert(intrinsicBlockPtr_1 == intrinsicBlockPtr_2); + + std::shared_ptr intrinsic = sfmData.getIntrinsicsharedPtr(view_1.getIntrinsicId()); + std::shared_ptr equidistant = std::dynamic_pointer_cast(intrinsic); + std::shared_ptr pinhole = std::dynamic_pointer_cast(intrinsic); + + if (equidistant != nullptr) + { + ceres::CostFunction* costFunction = new CostPanoramaEquidistant(constraint.ObservationFirst.x, constraint.ObservationSecond.x, equidistant); + problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); + + /* Symmetry */ + costFunction = new CostPanoramaEquidistant(constraint.ObservationSecond.x, constraint.ObservationFirst.x, equidistant); + problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); + } + else if (pinhole != nullptr) + { + ceres::CostFunction* costFunction = new CostPanoramaPinHole(constraint.ObservationFirst.x, constraint.ObservationSecond.x, pinhole); + problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); + /* Symmetry */ + costFunction = new CostPanoramaPinHole(constraint.ObservationSecond.x, constraint.ObservationFirst.x, pinhole); + problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); + } + else { + ALICEVISION_LOG_ERROR("Incompatible camera for a 2D constraint"); + return; + } + } +} + +void BundleAdjustmentSymbolicCeres::addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) +{ + // set a LossFunction to be less penalized by false measurements. + // note: set it to NULL if you don't want use a lossFunction. + ceres::LossFunction* lossFunction = nullptr; + + for (const auto & prior : sfmData.getRotationPriors()) { + + const sfmData::View& view_1 = sfmData.getView(prior.ViewFirst); + const sfmData::View& view_2 = sfmData.getView(prior.ViewSecond); + + assert(getPoseState(view_1.getPoseId()) != EParameterState::IGNORED); + assert(getPoseState(view_2.getPoseId()) != EParameterState::IGNORED); + + double * poseBlockPtr_1 = _posesBlocks.at(view_1.getPoseId()).data(); + double * poseBlockPtr_2 = _posesBlocks.at(view_2.getPoseId()).data(); + + ceres::CostFunction* costFunction = new CostRotationPrior(prior._second_R_first); + problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2); + } +} void BundleAdjustmentSymbolicCeres::createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, @@ -574,6 +651,11 @@ void BundleAdjustmentSymbolicCeres::createProblem(const sfmData::SfMData& sfmDat // add SfM landmarks to the Ceres problem addLandmarksToProblem(sfmData, refineOptions, problem); + // add 2D constraints to the Ceres problem + addConstraints2DToProblem(sfmData, refineOptions, problem); + + // add rotation priors to the Ceres problem + addRotationPriorsToProblem(sfmData, refineOptions, problem); } void BundleAdjustmentSymbolicCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp index 6cf7ecc41e..c7645830e8 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp +++ b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp @@ -62,6 +62,7 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment, ceres::Evaluation bool useParametersOrdering = true; bool summary = false; bool verbose = true; + bool useFocalPrior = true; }; /** @@ -225,6 +226,22 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment, ceres::Evaluation */ void addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + /** + * @brief Create a residual block for each 2D constraints + * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics + * @param[in] refineOptions The chosen refine flag + * @param[out] problem The Ceres bundle adjustement problem + */ + void addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + + /** + * @brief Create a residual block for each rotation priors + * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics + * @param[in] refineOptions The chosen refine flag + * @param[out] problem The Ceres bundle adjustement problem + */ + void addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + /** * @brief Create the Ceres bundle adjustement problem with: * - extrincics and intrinsics parameters blocks. diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index e1506d3966..c347c9d0a7 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -21,7 +21,6 @@ set(sfm_files_headers utils/syntheticScene.hpp BundleAdjustment.hpp BundleAdjustmentCeres.hpp - BundleAdjustmentPanoramaCeres.hpp BundleAdjustmentSymbolicCeres.hpp LocalBundleAdjustmentGraph.hpp FrustumFilter.hpp @@ -52,7 +51,6 @@ set(sfm_files_sources utils/statistics.cpp utils/syntheticScene.cpp BundleAdjustmentCeres.cpp - BundleAdjustmentPanoramaCeres.cpp BundleAdjustmentSymbolicCeres.cpp LocalBundleAdjustmentGraph.cpp FrustumFilter.cpp diff --git a/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp b/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp index e46b50e684..99d7740c99 100644 --- a/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp +++ b/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp @@ -11,7 +11,7 @@ namespace aliceVision { namespace sfm { -class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 9, 9, 7> { +class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 16, 16, 7> { public: CostPanoramaEquidistant(Vec2 fi, Vec2 fj, std::shared_ptr & intrinsic) : _fi(fi), _fj(fj), _intrinsic(intrinsic) { @@ -22,12 +22,15 @@ class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 9, 9, 7> { Vec2 pt_i = _fi; Vec2 pt_j = _fj; - const double * parameter_rotation_i = parameters[0]; - const double * parameter_rotation_j = parameters[1]; + const double * parameter_pose_i = parameters[0]; + const double * parameter_pose_j = parameters[1]; const double * parameter_intrinsics = parameters[2]; - const Eigen::Map> iRo(parameter_rotation_i); - const Eigen::Map> jRo(parameter_rotation_j); + const Eigen::Map> iTo(parameter_pose_i); + const Eigen::Map> jTo(parameter_pose_j); + + Eigen::Matrix iRo = iTo.block<3, 3>(0, 0); + Eigen::Matrix jRo = jTo.block<3, 3>(0, 0); _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); @@ -53,15 +56,25 @@ class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 9, 9, 7> { } if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); + + Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); + J.fill(0); + J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); + J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); + J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); } if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); + + Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); + J.fill(0); + J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); + J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); + J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); } if (jacobians[2] != nullptr) { diff --git a/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp b/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp index a0adfbbb3e..a8f691caf9 100644 --- a/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp +++ b/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp @@ -17,8 +17,8 @@ class CostPanoramaPinHole : public ceres::CostFunction { set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(9); - mutable_parameter_block_sizes()->push_back(9); + mutable_parameter_block_sizes()->push_back(16); + mutable_parameter_block_sizes()->push_back(16); mutable_parameter_block_sizes()->push_back(intrinsic->getParams().size()); } @@ -27,12 +27,15 @@ class CostPanoramaPinHole : public ceres::CostFunction { Vec2 pt_i = _fi; Vec2 pt_j = _fj; - const double * parameter_rotation_i = parameters[0]; - const double * parameter_rotation_j = parameters[1]; + const double * parameter_pose_i = parameters[0]; + const double * parameter_pose_j = parameters[1]; const double * parameter_intrinsics = parameters[2]; - const Eigen::Map> iRo(parameter_rotation_i); - const Eigen::Map> jRo(parameter_rotation_j); + const Eigen::Map> iTo(parameter_pose_i); + const Eigen::Map> jTo(parameter_pose_j); + + Eigen::Matrix iRo = iTo.block<3, 3>(0, 0); + Eigen::Matrix jRo = jTo.block<3, 3>(0, 0); _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); @@ -64,15 +67,25 @@ class CostPanoramaPinHole : public ceres::CostFunction { } if (jacobians[0] != nullptr) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); + + Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); + J.fill(0); + J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); + J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); + J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); } if (jacobians[1] != nullptr) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); + + Eigen::Matrix J9 = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); - J = _intrinsic->getDerivativeProjectWrtRotation(T, pt_i_sphere) * getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); + J.fill(0); + J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); + J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); + J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); } if (jacobians[2] != nullptr) { diff --git a/src/aliceVision/sfm/costfunctions/rotationPrior.hpp b/src/aliceVision/sfm/costfunctions/rotationPrior.hpp index 47bcf78702..4c3bbde014 100644 --- a/src/aliceVision/sfm/costfunctions/rotationPrior.hpp +++ b/src/aliceVision/sfm/costfunctions/rotationPrior.hpp @@ -12,7 +12,7 @@ namespace aliceVision { namespace sfm { -class CostRotationPrior : public ceres::SizedCostFunction<3, 9, 9> { +class CostRotationPrior : public ceres::SizedCostFunction<3, 16, 16> { public: explicit CostRotationPrior(const Eigen::Matrix3d & two_R_one) : _two_R_one(two_R_one) { @@ -20,11 +20,14 @@ class CostRotationPrior : public ceres::SizedCostFunction<3, 9, 9> { bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override { - const double * parameter_rotation_one = parameters[0]; - const double * parameter_rotation_two = parameters[1]; + const double * parameter_pose_one = parameters[0]; + const double * parameter_pose_two = parameters[1]; - const Eigen::Map> oneRo(parameter_rotation_one); - const Eigen::Map> twoRo(parameter_rotation_two); + const Eigen::Map> oneTo(parameter_pose_one); + const Eigen::Map> twoTo(parameter_pose_two); + + Eigen::Matrix oneRo = oneTo.block<3, 3>(0, 0); + Eigen::Matrix twoRo = twoTo.block<3, 3>(0, 0); Eigen::Matrix3d two_R_one_est = twoRo * oneRo.transpose(); Eigen::Matrix3d error_R = two_R_one_est * _two_R_one.transpose(); @@ -39,15 +42,25 @@ class CostRotationPrior : public ceres::SizedCostFunction<3, 9, 9> { } if (jacobians[0]) { - Eigen::Map> J(jacobians[0]); + Eigen::Map> J(jacobians[0]); + + Eigen::Matrix J9 = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_B<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), oneRo); - J = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_B<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), oneRo); + J.fill(0); + J.block<3, 3>(0, 0) = J9.block<3, 3>(0, 0); + J.block<3, 3>(0, 4) = J9.block<3, 3>(0, 3); + J.block<3, 3>(0, 8) = J9.block<3, 3>(0, 6); } if (jacobians[1]) { - Eigen::Map> J(jacobians[1]); + Eigen::Map> J(jacobians[1]); + + Eigen::Matrix J9 = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), twoRo); - J = SO3::dlogmdr(error_R) * getJacobian_AB_wrt_A<3, 3, 3>(two_R_one_est, _two_R_one.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(twoRo, oneRo.transpose()) * getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), twoRo); + J.fill(0); + J.block<3, 3>(0, 0) = J9.block<3, 3>(0, 0); + J.block<3, 3>(0, 4) = J9.block<3, 3>(0, 3); + J.block<3, 3>(0, 8) = J9.block<3, 3>(0, 6); } return true; diff --git a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp index f473cc9d33..499f6d5c62 100644 --- a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp +++ b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp @@ -32,7 +32,7 @@ #include -#include +#include #include @@ -381,12 +381,15 @@ bool ReconstructionEngine_panorama::Compute_Global_Rotations(const rotationAvera // Adjust the scene (& remove outliers) bool ReconstructionEngine_panorama::Adjust() { - BundleAdjustmentPanoramaCeres::CeresOptions options; + BundleAdjustmentSymbolicCeres::CeresOptions options; options.summary = true; + options.maxNumIterations = 300; + options.useFocalPrior = false; + options.useParametersOrdering = false; // Start bundle with rotation only - BundleAdjustmentPanoramaCeres BA(options); - bool success = BA.adjust(_sfmData, BundleAdjustmentPanoramaCeres::REFINE_ROTATION); + BundleAdjustmentSymbolicCeres BA(options); + bool success = BA.adjust(_sfmData, BundleAdjustmentSymbolicCeres::REFINE_ROTATION); if(success) { ALICEVISION_LOG_INFO("Rotations successfully refined."); @@ -405,8 +408,8 @@ bool ReconstructionEngine_panorama::Adjust() if(_params.intermediateRefineWithFocal) { - success = BA.adjust(_sfmData, BundleAdjustmentPanoramaCeres::REFINE_ROTATION | - BundleAdjustmentPanoramaCeres::REFINE_INTRINSICS_FOCAL); + success = BA.adjust(_sfmData, BundleAdjustmentSymbolicCeres::REFINE_ROTATION | + BundleAdjustmentSymbolicCeres::REFINE_INTRINSICS_FOCAL); if(success) { ALICEVISION_LOG_INFO("Bundle successfully refined: Rotation + Focal"); @@ -419,9 +422,9 @@ bool ReconstructionEngine_panorama::Adjust() } if(_params.intermediateRefineWithFocalDist) { - success = BA.adjust(_sfmData, BundleAdjustmentPanoramaCeres::REFINE_ROTATION | - BundleAdjustmentPanoramaCeres::REFINE_INTRINSICS_FOCAL | - BundleAdjustmentPanoramaCeres::REFINE_INTRINSICS_DISTORTION); + success = BA.adjust(_sfmData, BundleAdjustmentSymbolicCeres::REFINE_ROTATION | + BundleAdjustmentSymbolicCeres::REFINE_INTRINSICS_FOCAL | + BundleAdjustmentSymbolicCeres::REFINE_INTRINSICS_DISTORTION); if(success) { ALICEVISION_LOG_INFO("Bundle successfully refined: Rotation + Focal + Distortion"); @@ -434,10 +437,10 @@ bool ReconstructionEngine_panorama::Adjust() } // Minimize All - success = BA.adjust(_sfmData, BundleAdjustmentPanoramaCeres::REFINE_ROTATION | - BundleAdjustmentPanoramaCeres::REFINE_INTRINSICS_FOCAL | - BundleAdjustmentPanoramaCeres::REFINE_INTRINSICS_DISTORTION | - BundleAdjustmentPanoramaCeres::REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS); + success = BA.adjust(_sfmData, BundleAdjustmentSymbolicCeres::REFINE_ROTATION | + BundleAdjustmentSymbolicCeres::REFINE_INTRINSICS_FOCAL | + BundleAdjustmentSymbolicCeres::REFINE_INTRINSICS_DISTORTION | + BundleAdjustmentSymbolicCeres::REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS); if(success) { ALICEVISION_LOG_INFO("Bundle successfully refined: Rotation + Focal + Optical Center + Distortion"); @@ -462,7 +465,7 @@ bool ReconstructionEngine_panorama::Adjust() addConstraints2DWithKnownRotation(); // Minimize Rotation - success = BA.adjust(_sfmData, BundleAdjustmentPanoramaCeres::REFINE_ROTATION); + success = BA.adjust(_sfmData, BundleAdjustmentSymbolicCeres::REFINE_ROTATION); if(success) { ALICEVISION_LOG_INFO("Bundle successfully refined: Rotation after cleaning outliers"); diff --git a/src/aliceVision/sfmDataIO/alembicIO_test.cpp b/src/aliceVision/sfmDataIO/alembicIO_test.cpp index 80e612946c..4f9cec1f2c 100644 --- a/src/aliceVision/sfmDataIO/alembicIO_test.cpp +++ b/src/aliceVision/sfmDataIO/alembicIO_test.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include using namespace aliceVision; using namespace aliceVision::sfmData; From 09ff28c674f039ab1f1f0f25d163254c34a50c2d Mon Sep 17 00:00:00 2001 From: Fabien SERVANT Date: Mon, 11 Sep 2023 11:04:39 +0200 Subject: [PATCH 3/4] remove compatibility with old ceres --- .../calibration/distortionEstimation.cpp | 1 - src/aliceVision/sfm/BundleAdjustmentCeres.cpp | 12 +-- .../sfm/BundleAdjustmentSymbolicCeres.cpp | 11 --- src/aliceVision/sfm/manifolds/intrinsics.hpp | 3 +- src/aliceVision/sfm/manifolds/se3.hpp | 5 +- src/aliceVision/sfm/manifolds/so2.hpp | 3 +- src/aliceVision/sfm/manifolds/so3.hpp | 3 +- src/aliceVision/utils/CeresUtils.hpp | 76 ------------------- 8 files changed, 6 insertions(+), 108 deletions(-) delete mode 100644 src/aliceVision/utils/CeresUtils.hpp diff --git a/src/aliceVision/calibration/distortionEstimation.cpp b/src/aliceVision/calibration/distortionEstimation.cpp index efca3c9533..f988a5c740 100644 --- a/src/aliceVision/calibration/distortionEstimation.cpp +++ b/src/aliceVision/calibration/distortionEstimation.cpp @@ -7,7 +7,6 @@ #include "distortionEstimation.hpp" #include -#include #include diff --git a/src/aliceVision/sfm/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentCeres.cpp index a121712d75..7891995307 100644 --- a/src/aliceVision/sfm/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/BundleAdjustmentCeres.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include #include @@ -31,7 +30,7 @@ namespace sfm { using namespace aliceVision::camera; using namespace aliceVision::geometry; -class IntrinsicsManifold : public utils::CeresManifold { +class IntrinsicsManifold : public ceres::Manifold { public: explicit IntrinsicsManifold(size_t parametersSize, double focalRatio, bool lockFocal, bool lockFocalRatio, bool lockCenter, bool lockDistortion) : _ambientSize(parametersSize), @@ -584,13 +583,8 @@ void BundleAdjustmentCeres::addExtrinsicsToProblem(const sfmData::SfMData& sfmDa // subset parametrization if(!constantExtrinsic.empty()) { -#if ALICEVISION_CERES_HAS_MANIFOLD auto* subsetManifold = new ceres::SubsetManifold(6, constantExtrinsic); problem.SetManifold(poseBlockPtr, subsetManifold); -#else - ceres::SubsetParameterization* subsetParameterization = new ceres::SubsetParameterization(6, constantExtrinsic); - problem.SetParameterization(poseBlockPtr, subsetParameterization); -#endif } _statistics.addState(EParameter::POSE, EParameterState::REFINED); @@ -769,11 +763,7 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa IntrinsicsManifold* subsetManifold = new IntrinsicsManifold(intrinsicBlock.size(), focalRatio, lockFocal, lockRatio, lockCenter, lockDistortion); -#if ALICEVISION_CERES_HAS_MANIFOLD problem.SetManifold(intrinsicBlockPtr, subsetManifold); -#else - problem.SetParameterization(intrinsicBlockPtr, new utils::ManifoldToParameterizationWrapper(subsetManifold)); -#endif _statistics.addState(EParameter::INTRINSIC, EParameterState::REFINED); } diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp index b1d3b83aa0..368f3e6878 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp @@ -12,7 +12,6 @@ #include #include #include -#include #include @@ -66,12 +65,7 @@ void BundleAdjustmentSymbolicCeres::addPose(const sfmData::CameraPose& cameraPos if (refineRotation || refineTranslation) { -#if ALICEVISION_CERES_HAS_MANIFOLD problem.SetManifold(poseBlockPtr, new SE3ManifoldLeft(refineRotation, refineTranslation)); -#else - problem.SetParameterization(poseBlockPtr, new utils::ManifoldToParameterizationWrapper( - new SE3ManifoldLeft(refineRotation, refineTranslation))); -#endif } else { @@ -452,12 +446,7 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat IntrinsicsManifoldSymbolic* subsetManifold = new IntrinsicsManifoldSymbolic(intrinsicBlock.size(), focalRatio, lockFocal, lockRatio, lockCenter, lockDistortion); -#if ALICEVISION_CERES_HAS_MANIFOLD problem.SetManifold(intrinsicBlockPtr, subsetManifold); -#else - problem.SetParameterization(intrinsicBlockPtr, new utils::ManifoldToParameterizationWrapper(subsetManifold)); -#endif - _statistics.addState(EParameter::INTRINSIC, EParameterState::REFINED); } } diff --git a/src/aliceVision/sfm/manifolds/intrinsics.hpp b/src/aliceVision/sfm/manifolds/intrinsics.hpp index 4c3ba929fb..e4634d01e7 100644 --- a/src/aliceVision/sfm/manifolds/intrinsics.hpp +++ b/src/aliceVision/sfm/manifolds/intrinsics.hpp @@ -9,13 +9,12 @@ #include #include #include -#include #include namespace aliceVision { namespace sfm { -class IntrinsicsManifoldSymbolic : public utils::CeresManifold { +class IntrinsicsManifoldSymbolic : public ceres::Manifold { public: explicit IntrinsicsManifoldSymbolic(size_t parametersSize, double focalRatio, bool lockFocal, bool lockFocalRatio, diff --git a/src/aliceVision/sfm/manifolds/se3.hpp b/src/aliceVision/sfm/manifolds/se3.hpp index a5f4cfa0f8..6619e909ec 100644 --- a/src/aliceVision/sfm/manifolds/se3.hpp +++ b/src/aliceVision/sfm/manifolds/se3.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include @@ -18,7 +17,7 @@ namespace aliceVision { namespace sfm { -class SE3ManifoldLeft : public utils::CeresManifold { +class SE3ManifoldLeft : public ceres::Manifold { public: SE3ManifoldLeft(bool refineRotation, bool refineTranslation) : _refineRotation(refineRotation), @@ -89,7 +88,7 @@ class SE3ManifoldLeft : public utils::CeresManifold { }; -class SE3ManifoldRight : public utils::CeresManifold { +class SE3ManifoldRight : public ceres::Manifold { public: SE3ManifoldRight(bool refineRotation, bool refineTranslation) : _refineRotation(refineRotation), diff --git a/src/aliceVision/sfm/manifolds/so2.hpp b/src/aliceVision/sfm/manifolds/so2.hpp index fa784addb3..aba1870fda 100644 --- a/src/aliceVision/sfm/manifolds/so2.hpp +++ b/src/aliceVision/sfm/manifolds/so2.hpp @@ -9,14 +9,13 @@ #include #include #include -#include #include namespace aliceVision { namespace sfm { -class SO2Manifold : public utils::CeresManifold { +class SO2Manifold : public ceres::Manifold { public: bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { diff --git a/src/aliceVision/sfm/manifolds/so3.hpp b/src/aliceVision/sfm/manifolds/so3.hpp index 98582c7c22..181209201a 100644 --- a/src/aliceVision/sfm/manifolds/so3.hpp +++ b/src/aliceVision/sfm/manifolds/so3.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include namespace aliceVision { @@ -74,7 +73,7 @@ inline Eigen::Matrix dlogmdr(const Eigen::Matrix3 namespace sfm { -class SO3Manifold : public utils::CeresManifold { +class SO3Manifold : public ceres::Manifold { public: ~SO3Manifold() override = default; diff --git a/src/aliceVision/utils/CeresUtils.hpp b/src/aliceVision/utils/CeresUtils.hpp deleted file mode 100644 index fb530878a1..0000000000 --- a/src/aliceVision/utils/CeresUtils.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2022 AliceVision contributors. -// This Source Code Form is subject to the terms of the Mozilla Public License, -// v. 2.0. If a copy of the MPL was not distributed with this file, -// You can obtain one at https://mozilla.org/MPL/2.0/. - -#pragma once - -#include -#include - -namespace aliceVision { -namespace utils { - -// We can remove this wrapper once we can use ceres-solver 2.1 and newer. We can't do so right now -// because ceres-solver 2.1 breaks with GCC 6.x and older due to -// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56480 -#define ALICEVISION_CERES_HAS_MANIFOLD ((CERES_VERSION_MAJOR * 100 + CERES_VERSION_MINOR) >= 201) - -// Ceres has removed support for CXSPARSE after 2.1 -// See https://github.com/ceres-solver/ceres-solver/commit/2335b5b4b7a4703ca9458aa275ca878945763a34 -#define ALICEVISION_CERES_HAS_CXSPARSE ((CERES_VERSION_MAJOR * 100 + CERES_VERSION_MINOR) <= 201) - -#if ALICEVISION_CERES_HAS_MANIFOLD -using CeresManifold = ceres::Manifold; -#else -class CeresManifold { -public: - virtual ~CeresManifold() = default; - virtual int AmbientSize() const = 0; - virtual int TangentSize() const = 0; - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const = 0; - virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; - virtual bool Minus(const double* y, - const double* x, - double* y_minus_x) const = 0; - virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; -}; - -class ManifoldToParameterizationWrapper : public ceres::LocalParameterization -{ -public: - // takes ownership of manifold - ManifoldToParameterizationWrapper(CeresManifold* manifold) : _manifold{manifold} {} - - ~ManifoldToParameterizationWrapper() override = default; - - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override - { - return _manifold->Plus(x, delta, x_plus_delta); - } - - bool ComputeJacobian(const double* x, double* jacobian) const override - { - return _manifold->PlusJacobian(x, jacobian); - } - - int GlobalSize() const override - { - return _manifold->AmbientSize(); - } - - int LocalSize() const override - { - return _manifold->TangentSize(); - } - -private: - std::unique_ptr _manifold; -}; -#endif - -} // namespace utils -} // namespace aliceVision From c1dea9c2dd769435fb747d1ecb70f0478ea68b35 Mon Sep 17 00:00:00 2001 From: Fabien SERVANT Date: Mon, 11 Sep 2023 11:43:39 +0200 Subject: [PATCH 4/4] move bundle code inside its own directory --- .../localization/VoctreeLocalizer.cpp | 2 +- src/aliceVision/localization/optimization.cpp | 2 +- src/aliceVision/rig/Rig.cpp | 2 +- src/aliceVision/sfm/CMakeLists.txt | 12 ++++++------ .../sfm/LocalBundleAdjustmentGraph.hpp | 2 +- .../sfm/{ => bundle}/BundleAdjustment.hpp | 0 .../sfm/{ => bundle}/BundleAdjustmentCeres.cpp | 2 +- .../sfm/{ => bundle}/BundleAdjustmentCeres.hpp | 2 +- .../BundleAdjustmentSymbolicCeres.cpp | 18 +++++++++--------- .../BundleAdjustmentSymbolicCeres.hpp | 2 +- .../sfm/{ => bundle}/bundleAdjustment_test.cpp | 0 .../costfunctions/panoramaEquidistant.hpp | 0 .../costfunctions/panoramaPinhole.hpp | 0 .../{ => bundle}/costfunctions/projection.hpp | 0 .../costfunctions/projectionSimple.hpp | 0 .../costfunctions/rotationPrior.hpp | 0 .../sfm/{ => bundle}/manifolds/intrinsics.hpp | 0 .../sfm/{ => bundle}/manifolds/se3.hpp | 0 .../sfm/{ => bundle}/manifolds/so2.hpp | 0 .../sfm/{ => bundle}/manifolds/so3.hpp | 0 .../GlobalSfMTranslationAveragingSolver.cpp | 2 +- .../sfm/pipeline/localization/SfMLocalizer.cpp | 4 ++-- .../panorama/ReconstructionEngine_panorama.cpp | 2 +- .../ReconstructionEngine_sequentialSfM.cpp | 4 ++-- src/aliceVision/sfm/sfm.hpp | 4 ++-- src/aliceVision/sfm/sfmFilters.cpp | 2 +- src/aliceVision/sfm/sfmFilters.hpp | 2 +- src/software/pipeline/main_incrementalSfM.cpp | 2 +- .../pipeline/main_sfmTriangulation.cpp | 2 +- src/software/utils/main_computeUncertainty.cpp | 2 +- src/software/utils/main_sfmRegression.cpp | 4 ++-- 31 files changed, 37 insertions(+), 37 deletions(-) rename src/aliceVision/sfm/{ => bundle}/BundleAdjustment.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/BundleAdjustmentCeres.cpp (99%) rename src/aliceVision/sfm/{ => bundle}/BundleAdjustmentCeres.hpp (99%) rename src/aliceVision/sfm/{ => bundle}/BundleAdjustmentSymbolicCeres.cpp (98%) rename src/aliceVision/sfm/{ => bundle}/BundleAdjustmentSymbolicCeres.hpp (99%) rename src/aliceVision/sfm/{ => bundle}/bundleAdjustment_test.cpp (100%) rename src/aliceVision/sfm/{ => bundle}/costfunctions/panoramaEquidistant.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/costfunctions/panoramaPinhole.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/costfunctions/projection.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/costfunctions/projectionSimple.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/costfunctions/rotationPrior.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/manifolds/intrinsics.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/manifolds/se3.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/manifolds/so2.hpp (100%) rename src/aliceVision/sfm/{ => bundle}/manifolds/so3.hpp (100%) diff --git a/src/aliceVision/localization/VoctreeLocalizer.cpp b/src/aliceVision/localization/VoctreeLocalizer.cpp index c11d31617d..8b21e657a9 100644 --- a/src/aliceVision/localization/VoctreeLocalizer.cpp +++ b/src/aliceVision/localization/VoctreeLocalizer.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/aliceVision/localization/optimization.cpp b/src/aliceVision/localization/optimization.cpp index 259df186e8..bd87be7f8a 100644 --- a/src/aliceVision/localization/optimization.cpp +++ b/src/aliceVision/localization/optimization.cpp @@ -6,7 +6,7 @@ #include "optimization.hpp" #include -#include +#include #include #include #include diff --git a/src/aliceVision/rig/Rig.cpp b/src/aliceVision/rig/Rig.cpp index c17cf2a9c9..4452f731ca 100644 --- a/src/aliceVision/rig/Rig.cpp +++ b/src/aliceVision/rig/Rig.cpp @@ -6,7 +6,7 @@ #include "Rig.hpp" #include "ResidualError.hpp" -#include +#include #include #include diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index c347c9d0a7..8a586c5b71 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -19,9 +19,9 @@ set(sfm_files_headers utils/alignment.hpp utils/statistics.hpp utils/syntheticScene.hpp - BundleAdjustment.hpp - BundleAdjustmentCeres.hpp - BundleAdjustmentSymbolicCeres.hpp + bundle/BundleAdjustment.hpp + bundle/BundleAdjustmentCeres.hpp + bundle/BundleAdjustmentSymbolicCeres.hpp LocalBundleAdjustmentGraph.hpp FrustumFilter.hpp ResidualErrorFunctor.hpp @@ -50,8 +50,8 @@ set(sfm_files_sources utils/alignment.cpp utils/statistics.cpp utils/syntheticScene.cpp - BundleAdjustmentCeres.cpp - BundleAdjustmentSymbolicCeres.cpp + bundle/BundleAdjustmentCeres.cpp + bundle/BundleAdjustmentSymbolicCeres.cpp LocalBundleAdjustmentGraph.cpp FrustumFilter.cpp generateReport.cpp @@ -83,7 +83,7 @@ alicevision_add_library(aliceVision_sfm # Unit tests -alicevision_add_test(bundleAdjustment_test.cpp +alicevision_add_test(bundle/bundleAdjustment_test.cpp NAME "sfm_bundleAdjustment" LINKS aliceVision_sfm aliceVision_multiview diff --git a/src/aliceVision/sfm/LocalBundleAdjustmentGraph.hpp b/src/aliceVision/sfm/LocalBundleAdjustmentGraph.hpp index 715ebf1d9c..9720bf2018 100644 --- a/src/aliceVision/sfm/LocalBundleAdjustmentGraph.hpp +++ b/src/aliceVision/sfm/LocalBundleAdjustmentGraph.hpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include diff --git a/src/aliceVision/sfm/BundleAdjustment.hpp b/src/aliceVision/sfm/bundle/BundleAdjustment.hpp similarity index 100% rename from src/aliceVision/sfm/BundleAdjustment.hpp rename to src/aliceVision/sfm/bundle/BundleAdjustment.hpp diff --git a/src/aliceVision/sfm/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp similarity index 99% rename from src/aliceVision/sfm/BundleAdjustmentCeres.cpp rename to src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 7891995307..28da9ba8c2 100644 --- a/src/aliceVision/sfm/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -5,7 +5,7 @@ // v. 2.0. If a copy of the MPL was not distributed with this file, // You can obtain one at https://mozilla.org/MPL/2.0/. -#include +#include #include #include #include diff --git a/src/aliceVision/sfm/BundleAdjustmentCeres.hpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp similarity index 99% rename from src/aliceVision/sfm/BundleAdjustmentCeres.hpp rename to src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp index d49f9a8cfa..dec13fff6f 100644 --- a/src/aliceVision/sfm/BundleAdjustmentCeres.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp similarity index 98% rename from src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp rename to src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp index 368f3e6878..9613bc043c 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp @@ -5,7 +5,7 @@ // v. 2.0. If a copy of the MPL was not distributed with this file, // You can obtain one at https://mozilla.org/MPL/2.0/. -#include +#include #include @@ -15,15 +15,15 @@ #include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp similarity index 99% rename from src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp rename to src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp index c7645830e8..1980f33b20 100644 --- a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include diff --git a/src/aliceVision/sfm/bundleAdjustment_test.cpp b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp similarity index 100% rename from src/aliceVision/sfm/bundleAdjustment_test.cpp rename to src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp diff --git a/src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp b/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp similarity index 100% rename from src/aliceVision/sfm/costfunctions/panoramaEquidistant.hpp rename to src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp diff --git a/src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp b/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp similarity index 100% rename from src/aliceVision/sfm/costfunctions/panoramaPinhole.hpp rename to src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp diff --git a/src/aliceVision/sfm/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp similarity index 100% rename from src/aliceVision/sfm/costfunctions/projection.hpp rename to src/aliceVision/sfm/bundle/costfunctions/projection.hpp diff --git a/src/aliceVision/sfm/costfunctions/projectionSimple.hpp b/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp similarity index 100% rename from src/aliceVision/sfm/costfunctions/projectionSimple.hpp rename to src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp diff --git a/src/aliceVision/sfm/costfunctions/rotationPrior.hpp b/src/aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp similarity index 100% rename from src/aliceVision/sfm/costfunctions/rotationPrior.hpp rename to src/aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp diff --git a/src/aliceVision/sfm/manifolds/intrinsics.hpp b/src/aliceVision/sfm/bundle/manifolds/intrinsics.hpp similarity index 100% rename from src/aliceVision/sfm/manifolds/intrinsics.hpp rename to src/aliceVision/sfm/bundle/manifolds/intrinsics.hpp diff --git a/src/aliceVision/sfm/manifolds/se3.hpp b/src/aliceVision/sfm/bundle/manifolds/se3.hpp similarity index 100% rename from src/aliceVision/sfm/manifolds/se3.hpp rename to src/aliceVision/sfm/bundle/manifolds/se3.hpp diff --git a/src/aliceVision/sfm/manifolds/so2.hpp b/src/aliceVision/sfm/bundle/manifolds/so2.hpp similarity index 100% rename from src/aliceVision/sfm/manifolds/so2.hpp rename to src/aliceVision/sfm/bundle/manifolds/so2.hpp diff --git a/src/aliceVision/sfm/manifolds/so3.hpp b/src/aliceVision/sfm/bundle/manifolds/so3.hpp similarity index 100% rename from src/aliceVision/sfm/manifolds/so3.hpp rename to src/aliceVision/sfm/bundle/manifolds/so3.hpp diff --git a/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp b/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp index 1b19bf59ce..1ca1fbc6b5 100644 --- a/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp +++ b/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp b/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp index d2f4b31ef6..0a813356d0 100644 --- a/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp +++ b/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp @@ -7,8 +7,8 @@ #include "SfMLocalizer.hpp" #include -#include -#include +#include +#include #include #include #include diff --git a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp index 499f6d5c62..721082ec78 100644 --- a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp +++ b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp @@ -32,7 +32,7 @@ #include -#include +#include #include diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp index bcbb831fc2..1f40ba8b14 100644 --- a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp +++ b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp @@ -9,8 +9,8 @@ #include #include #include -#include -#include +#include +#include #include #include diff --git a/src/aliceVision/sfm/sfm.hpp b/src/aliceVision/sfm/sfm.hpp index 438b4a1235..48ac53a228 100644 --- a/src/aliceVision/sfm/sfm.hpp +++ b/src/aliceVision/sfm/sfm.hpp @@ -11,8 +11,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index 229f9bc273..c283e5a3e4 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include diff --git a/src/aliceVision/sfm/sfmFilters.hpp b/src/aliceVision/sfm/sfmFilters.hpp index 3434e19fbd..0bdd6eed53 100644 --- a/src/aliceVision/sfm/sfmFilters.hpp +++ b/src/aliceVision/sfm/sfmFilters.hpp @@ -8,7 +8,7 @@ #pragma once #include -#include +#include namespace aliceVision { diff --git a/src/software/pipeline/main_incrementalSfM.cpp b/src/software/pipeline/main_incrementalSfM.cpp index 747417a301..1278614d25 100644 --- a/src/software/pipeline/main_incrementalSfM.cpp +++ b/src/software/pipeline/main_incrementalSfM.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/software/pipeline/main_sfmTriangulation.cpp b/src/software/pipeline/main_sfmTriangulation.cpp index 36522cf124..ea841bcf7d 100644 --- a/src/software/pipeline/main_sfmTriangulation.cpp +++ b/src/software/pipeline/main_sfmTriangulation.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/software/utils/main_computeUncertainty.cpp b/src/software/utils/main_computeUncertainty.cpp index 6545aa5ba8..88c911b7e7 100644 --- a/src/software/utils/main_computeUncertainty.cpp +++ b/src/software/utils/main_computeUncertainty.cpp @@ -4,7 +4,7 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include -#include +#include #include #include #include diff --git a/src/software/utils/main_sfmRegression.cpp b/src/software/utils/main_sfmRegression.cpp index dc8ad352bb..5dd04703c5 100644 --- a/src/software/utils/main_sfmRegression.cpp +++ b/src/software/utils/main_sfmRegression.cpp @@ -12,8 +12,8 @@ #include #include -#include -#include +#include +#include #include #include