diff --git a/.travis.yml b/.travis.yml index 61926ba557..f4ce5a73eb 100644 --- a/.travis.yml +++ b/.travis.yml @@ -107,8 +107,8 @@ script: git clone --branch master https://github.com/alicevision/SfM_quality_evaluation.git $GT_TEST_SOURCE; fi cd $GT_TEST_SOURCE - git checkout 1828a5076ccca86fd523c92a768f1d518dfdbb91 # checkout a specific commit to ensure repeatability - python EvaluationLauncher.py -s "$ALICEVISION_BUILD/$BUILD_SYSTEM-$BUILD_PROCESSOR" -i Benchmarking_Camera_Calibration_2008/ -o ${GT_TEST_OUTPUT} -r ${GT_TEST_RESULTS} --limit 3 + git checkout 203e55c6e70de5f79496407961eb10b15deece4b # checkout a specific commit to ensure repeatability + python EvaluationLauncher.py -s "$ALICEVISION_BUILD/$BUILD_SYSTEM-$BUILD_PROCESSOR" -i Benchmarking_Camera_Calibration_2008/ -o ${GT_TEST_OUTPUT} -r ${GT_TEST_RESULTS} -v --limit 3 cat ${GT_TEST_RESULTS} # Return to root and remove GT huge files to avoid cache problems cd $TRAVIS_BUILD_DIR diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e1b61ed36b..968650aaae 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -278,6 +278,7 @@ if(BUILD_SHARED_LIBS) # Force BOOST to use dynamic libraries (avoid link error with boost program_options) # https://lists.boost.org/boost-users/2009/11/54015.php add_definitions(-DBOOST_ALL_DYN_LINK) + add_definitions(-DBOOST_TEST_DYN_LINK) else() set(Boost_USE_STATIC_LIBS ON) endif() diff --git a/src/aliceVision/camera/CMakeLists.txt b/src/aliceVision/camera/CMakeLists.txt index fea1330213..f82ac9c5df 100644 --- a/src/aliceVision/camera/CMakeLists.txt +++ b/src/aliceVision/camera/CMakeLists.txt @@ -1,24 +1,23 @@ # Headers set(camera_files_headers - camera.hpp - cameraCommon.hpp - cameraUndistortImage.hpp - IntrinsicBase.hpp - IntrinsicInitMode.hpp - Pinhole.hpp - PinholeBrown.hpp - PinholeFisheye.hpp - PinholeFisheye1.hpp - PinholeRadial.hpp + camera.hpp + cameraCommon.hpp + cameraUndistortImage.hpp + IntrinsicBase.hpp + IntrinsicInitMode.hpp + Pinhole.hpp + PinholeBrown.hpp + PinholeFisheye.hpp + PinholeFisheye1.hpp + PinholeRadial.hpp ) alicevision_add_interface(aliceVision_camera SOURCES ${camera_files_headers} LINKS + aliceVision_numeric aliceVision_geometry - aliceVision_multiview aliceVision_image - aliceVision_numeric aliceVision_stl aliceVision_system ) diff --git a/src/aliceVision/camera/Pinhole.hpp b/src/aliceVision/camera/Pinhole.hpp index 20977700d6..f16e5cbf37 100644 --- a/src/aliceVision/camera/Pinhole.hpp +++ b/src/aliceVision/camera/Pinhole.hpp @@ -8,10 +8,10 @@ #pragma once #include +#include +#include #include #include -#include -#include #include #include @@ -106,7 +106,7 @@ class Pinhole : public IntrinsicBase virtual Mat34 get_projective_equivalent(const geometry::Pose3 & pose) const override { Mat34 P; - P_From_KRt(K(), pose.rotation(), pose.translation(), &P); + P_from_KRt(K(), pose.rotation(), pose.translation(), &P); return P; } diff --git a/src/aliceVision/feature/svgVisualization.cpp b/src/aliceVision/feature/svgVisualization.cpp index 520c0c016e..eb40b4817a 100644 --- a/src/aliceVision/feature/svgVisualization.cpp +++ b/src/aliceVision/feature/svgVisualization.cpp @@ -60,6 +60,69 @@ float getStrokeEstimate(const std::pair & imgSize) return std::max(std::max(imgSize.first, imgSize.second) / float(2200), 2.0f); } +inline void drawMatchesSideBySide(svg::svgDrawer& svgStream, + const feature::PointFeature &L, + const feature::PointFeature &R, + std::size_t offset, + float radiusLeft, + float radiusRight, + float strokeLeft, + float strokeRight, + const svg::svgStyle& lineStyle, + const svg::svgStyle& leftStyle, + const svg::svgStyle& rightStyle) +{ + const float xRight = R.x() + offset; + svgStream.drawLine(L.x(), L.y(), xRight, R.y(), lineStyle); + svgStream.drawCircle(L.x(), L.y(), radiusLeft, leftStyle); + svgStream.drawCircle(xRight, R.y(), radiusRight, rightStyle); +} + +inline void drawMatchesSideBySide(svg::svgDrawer& svgStream, + const std::vector& keypointsLeft, + const std::vector& keypointsRight, + std::size_t offset, + float radiusLeft, + float radiusRight, + float strokeLeft, + float strokeRight, + const svg::svgStyle& lineStyle, + const svg::svgStyle& leftStyle, + const svg::svgStyle& rightStyle, + const matching::IndMatches& matches) +{ + for (const matching::IndMatch &m : matches) + { + const feature::PointFeature &L = keypointsLeft[m._i]; + const feature::PointFeature &R = keypointsRight[m._j]; + drawMatchesSideBySide(svgStream, L, R, offset, radiusLeft, radiusRight, strokeLeft, strokeRight, lineStyle, leftStyle, rightStyle); + } +} + +inline void drawInliersSideBySide(svg::svgDrawer& svgStream, + const std::vector& keypointsLeft, + const std::vector& keypointsRight, + std::size_t offset, + float radiusLeft, + float radiusRight, + float strokeLeft, + float strokeRight, + const svg::svgStyle& lineStyle, + const svg::svgStyle& leftStyle, + const svg::svgStyle& rightStyle, + const matching::IndMatches& matches, + std::vector& inliers) +{ + for (const auto &idx : inliers) + { + const auto currMatch = matches[idx]; + const feature::PointFeature &L = keypointsLeft[currMatch._i]; + const feature::PointFeature &R = keypointsRight[currMatch._j]; + + drawMatchesSideBySide(svgStream, L, R, offset, radiusLeft, radiusRight, strokeLeft, strokeRight, lineStyle, leftStyle, rightStyle); + } +} + void drawMatchesSideBySide(const std::string& imagePathLeft, const std::pair& imageSizeLeft, const std::vector& keypointsLeft, @@ -115,10 +178,10 @@ void drawMatchesSideBySide(const std::string& imagePathLeft, void drawHomographyMatches(const std::string& imagePathLeft, const std::pair& imageSizeLeft, - const std::vector& siofeatures_I, + const std::vector& features_I, const std::string& imagePathRight, const std::pair& imageSizeRight, - const std::vector& siofeatures_J, + const std::vector& features_J, const std::vector>& homographiesAndMatches, const matching::IndMatches& putativeMatches, const std::string& outFilename) @@ -132,13 +195,13 @@ void drawHomographyMatches(const std::string& imagePathLeft, svgStream.drawImage(imagePathLeft, imageSizeLeft.first, imageSizeLeft.second); svgStream.drawImage(imagePathRight, imageSizeRight.first, imageSizeRight.second, offset); -// draw little white dots representing putative matches + // draw little white dots representing putative matches for (const auto& match : putativeMatches) { const float radius{1.f}; const float strokeSize{2.f}; - const feature::PointFeature &fI = siofeatures_I.at(match._i); - const feature::PointFeature &fJ = siofeatures_J.at(match._j); + const feature::PointFeature &fI = features_I.at(match._i); + const feature::PointFeature &fJ = features_J.at(match._j); const svg::svgStyle style = svg::svgStyle().stroke("white", strokeSize); svgStream.drawCircle(fI.x(), fI.y(), radius, style); @@ -158,8 +221,8 @@ void drawHomographyMatches(const std::string& imagePathLeft, for (const auto &match : bestMatchesId) { - const feature::PointFeature &fI = siofeatures_I.at(match._i); - const feature::PointFeature &fJ = siofeatures_J.at(match._j); + const feature::PointFeature &fI = features_I.at(match._i); + const feature::PointFeature &fJ = features_J.at(match._j); const svg::svgStyle style = svg::svgStyle().stroke(color, strokeSize); @@ -183,7 +246,6 @@ void drawHomographyMatches(const std::string& imagePathLeft, return; } svgFile.close(); - } void saveMatches2SVG(const std::string &imagePathLeft, diff --git a/src/aliceVision/geometry/frustumIntersection_test.cpp b/src/aliceVision/geometry/frustumIntersection_test.cpp index 7bdfee88ce..2aa941f339 100644 --- a/src/aliceVision/geometry/frustumIntersection_test.cpp +++ b/src/aliceVision/geometry/frustumIntersection_test.cpp @@ -9,7 +9,7 @@ #include "aliceVision/geometry/Frustum.hpp" #include "aliceVision/multiview/NViewDataSet.hpp" -#include "aliceVision/multiview/projection.hpp" +#include "aliceVision/numeric/projection.hpp" #include diff --git a/src/aliceVision/geometry/rigidTransformation3D.cpp b/src/aliceVision/geometry/rigidTransformation3D.cpp index 32ffd1e279..2eb1bf528c 100644 --- a/src/aliceVision/geometry/rigidTransformation3D.cpp +++ b/src/aliceVision/geometry/rigidTransformation3D.cpp @@ -126,13 +126,13 @@ void Refine_RTS(const Mat &x1, } } -bool ACRansac_FindRTS(const Mat &x1, - const Mat &x2, - double &S, - Vec3 &t, - Mat3 &R, - std::vector &vec_inliers, - bool refine) +bool ACRansac_FindRTS(const Mat& x1, + const Mat& x2, + double& S, + Vec3& t, + Mat3& R, + std::vector& vec_inliers, + bool refine) { assert(3 == x1.rows()); assert(3 <= x1.cols()); @@ -142,19 +142,17 @@ bool ACRansac_FindRTS(const Mat &x1, const std::size_t numIterations = 1024; const double dPrecision = std::numeric_limits::infinity(); - Mat4 RTS; + using SolverT = geometry::RTSSolver; + using KernelT = ACKernelAdaptor_PointsRegistrationSRT; - typedef geometry::RTSSolver SolverType; - typedef ACKernelAdaptor_PointsRegistrationSRT< - SolverType, - geometry::RTSSquaredResidualError> KernelType; + const KernelT kernel = KernelT(x1, x2); - KernelType kernel = KernelType(x1, x2); - // Robust estimation of the Projection matrix and its precision - const std::pair ACRansacOut = - robustEstimation::ACRANSAC(kernel, vec_inliers, numIterations, &RTS, dPrecision); + robustEstimation::MatrixModel RTS; - const bool good = decomposeRTS(RTS, S, t, R); + // robust estimation of the Projection matrix and its precision + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, vec_inliers, numIterations, &RTS, dPrecision); + + const bool good = decomposeRTS(RTS.getMatrix(), S, t, R); // return if it is not good or refine is not required if(!good || !refine) diff --git a/src/aliceVision/geometry/rigidTransformation3D.hpp b/src/aliceVision/geometry/rigidTransformation3D.hpp index f6d0613cf0..9661c334bc 100644 --- a/src/aliceVision/geometry/rigidTransformation3D.hpp +++ b/src/aliceVision/geometry/rigidTransformation3D.hpp @@ -9,6 +9,7 @@ #include #include +#include namespace aliceVision { namespace geometry { @@ -149,31 +150,46 @@ void Refine_RTS(const Mat &x1, /** * @brief the Solver to use for ACRansac */ -struct RTSSolver +class RTSSolver : public robustEstimation::ISolver> { +public: - enum + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override { - MINIMUM_SAMPLES = 3 - }; + return 3; + } - enum + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override { - MAX_MODELS = 1 - }; - - // Solve the RTS problem - static void Solve(const Mat &pts1, const Mat &pts2, std::vector *models) + return 1; + } + + // solve the RTS problem + inline void solve(const Mat& pts1, const Mat& pts2, std::vector>& models) const + { + models.push_back( robustEstimation::MatrixModel(Eigen::umeyama(pts1, pts2, true)) ); + } + + void solve(const Mat& x1, const Mat& x2, std::vector>& models, const std::vector& weights) const override { - models->push_back(Eigen::umeyama(pts1, pts2, true)); + throw std::logic_error("RTSSolver does not support problem solving with weights."); } - // Compute the residual of the transformation - static double Error(const Mat4 &RTS, const Vec3 & pt1, const Vec3 & pt2) + // compute the residual of the transformation + inline double error(const robustEstimation::MatrixModel& RTS, const Vec3& pt1, const Vec3& pt2) { - const Mat3 &RS = RTS.topLeftCorner<3, 3>(); - const Vec3 &t = RTS.topRightCorner<3, 1>(); - return (pt2 - (RS*pt1 + t)).norm(); + const Mat4& matrixRTS = RTS.getMatrix(); + const Mat3& RS = matrixRTS.topLeftCorner<3, 3>(); + const Vec3& t = matrixRTS.topRightCorner<3, 1>(); + return(pt2 - (RS*pt1 + t)).norm(); } }; @@ -183,11 +199,12 @@ struct RTSSolver */ struct RTSSquaredResidualError { - // Return the squared error - static double Error(const Mat4 &RTS, const Vec3 & pt1, const Vec3 & pt2) + // return the squared error + inline double error(const robustEstimation::MatrixModel& RTS, const Vec3& pt1, const Vec3& pt2) const { - const Mat3 &RS = RTS.topLeftCorner<3, 3>(); - const Vec3 &t = RTS.topRightCorner<3, 1>(); + const Mat4& matrixRTS = RTS.getMatrix(); + const Mat3& RS = matrixRTS.topLeftCorner<3, 3>(); + const Vec3& t = matrixRTS.topRightCorner<3, 1>(); return (pt2 - (RS*pt1 + t)).squaredNorm(); } }; @@ -195,19 +212,18 @@ struct RTSSquaredResidualError /** * @brief The kernel to use for ACRansac */ -template +template > class ACKernelAdaptor_PointsRegistrationSRT { public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - - ACKernelAdaptor_PointsRegistrationSRT(const Mat & xA, - const Mat & xB) : - x1_(xA), x2_(xB), logalpha0_(log10(M_PI)) //@todo WTF? + using SolverT = SolverT_; + using ModelT = ModelT_; + using ErrorT = ErrorT_; + + ACKernelAdaptor_PointsRegistrationSRT(const Mat& xA, const Mat& xB) + : x1_(xA) + , x2_(xB) + , _logalpha0(log10(M_PI)) //@todo WTF? { assert(3 == x1_.rows()); assert(x1_.rows() == x2_.rows()); @@ -216,40 +232,51 @@ class ACKernelAdaptor_PointsRegistrationSRT // @todo normalize points? } - enum + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; + return _kernelSolver.getMinimumNbRequiredSamples(); + } - enum + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const { - MAX_MODELS = Solver::MAX_MODELS - }; + return _kernelSolver.getMaximumNbModels(); + } - void Fit(const std::vector &samples, std::vector *models) const + void fit(const std::vector& samples, std::vector& models) const { const Mat x1 = ExtractColumns(x1_, samples); const Mat x2 = ExtractColumns(x2_, samples); - Solver::Solve(x1, x2, models); + _kernelSolver.solve(x1, x2, models); } - double Error(size_t sample, const Model &model) const + double error(std::size_t sample, const ModelT& model) const { - return Square(ErrorT::Error(model, x1_.col(sample), x2_.col(sample))); + return Square(_errorEstimator.error(model, x1_.col(sample), x2_.col(sample))); } - void Errors(const Model & model, std::vector & vec_errors) const + void errors(const ModelT& model, std::vector & vec_errors) const { vec_errors.resize(x1_.cols()); - for(size_t sample = 0; sample < x1_.cols(); ++sample) - vec_errors[sample] = Square(ErrorT::Error(model, x1_.col(sample), x2_.col(sample))); + for(std::size_t sample = 0; sample < x1_.cols(); ++sample) + vec_errors[sample] = error(sample,model); } - size_t NumSamples() const { return static_cast (x1_.cols()); } + std::size_t nbSamples() const + { + return static_cast (x1_.cols()); + } - void Unnormalize(Model * model) const { } //-- Do nothing, no normalization + void unnormalize(ModelT& model) const { } //-- Do nothing, no normalization - double logalpha0() const { return logalpha0_; } + double logalpha0() const { return _logalpha0; } double multError() const { return 1.;} @@ -260,8 +287,11 @@ class ACKernelAdaptor_PointsRegistrationSRT double unormalizeError(double val) const { return sqrt(val); } private: - Mat x1_, x2_; // Normalized input data - double logalpha0_; // Alpha0 is used to make the error scale invariant + Mat x1_, x2_; // normalized input data + double _logalpha0; // alpha0 is used to make the error scale invariant + + SolverT _kernelSolver; + ErrorT _errorEstimator; }; /** diff --git a/src/aliceVision/geometry/rigidTransformation3D_test.cpp b/src/aliceVision/geometry/rigidTransformation3D_test.cpp index ae7cff79df..ab933716e0 100644 --- a/src/aliceVision/geometry/rigidTransformation3D_test.cpp +++ b/src/aliceVision/geometry/rigidTransformation3D_test.cpp @@ -5,8 +5,8 @@ // 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 "rigidTransformation3D.hpp" -#include "aliceVision/robustEstimation/ACRansac.hpp" +#include +#include #include @@ -146,11 +146,11 @@ BOOST_AUTO_TEST_CASE(SRT_precision_ACRANSAC_noNoise) Mat3 Rc; Vec3 tc; - std::vector vec_inliers; - const bool result = ACRansac_FindRTS(x1, x2, Sc, tc, Rc, vec_inliers, true); + std::vector inliers; + const bool result = ACRansac_FindRTS(x1, x2, Sc, tc, Rc, inliers, true); BOOST_CHECK(result); - BOOST_CHECK(vec_inliers.size() == nbPoints); + BOOST_CHECK(inliers.size() == nbPoints); ALICEVISION_LOG_DEBUG( "Scale " << Sc << "\n" << @@ -167,9 +167,12 @@ BOOST_AUTO_TEST_CASE(SRT_precision_ACRANSAC_noNoise) Mat4 RTS; composeRTS(Sc, tc, Rc, RTS); + robustEstimation::MatrixModel modelRTS(RTS); + geometry::RTSSquaredResidualError errorEstimator; + for(std::size_t i = 0; i < nbPoints; ++i) { - const double error = geometry::RTSSquaredResidualError::Error(RTS, x1.col(i), x2.col(i)); + const double error = errorEstimator.error(modelRTS, x1.col(i), x2.col(i)); BOOST_CHECK_SMALL(error, 1e-9); } } @@ -202,8 +205,8 @@ BOOST_AUTO_TEST_CASE(SRT_precision_ACRANSAC_noiseByShuffling) Mat3 Rc; Vec3 tc; - std::vector vec_inliers; - const bool result = ACRansac_FindRTS(x1, x2, Sc, tc, Rc, vec_inliers, true); + std::vector inliers; + const bool result = ACRansac_FindRTS(x1, x2, Sc, tc, Rc, inliers, true); ALICEVISION_LOG_DEBUG( "Scale " << Sc << "\n" << @@ -217,7 +220,7 @@ BOOST_AUTO_TEST_CASE(SRT_precision_ACRANSAC_noiseByShuffling) BOOST_CHECK(result); // all the points must be inliers (no noise) - const std::size_t nbInliers = vec_inliers.size(); + const std::size_t nbInliers = inliers.size(); BOOST_CHECK(nbInliers == nbPoints - nbNoisy); Mat inliers1 = Mat3X(3, nbInliers); @@ -225,8 +228,8 @@ BOOST_AUTO_TEST_CASE(SRT_precision_ACRANSAC_noiseByShuffling) for(std::size_t i = 0; i < nbInliers; ++i) { - inliers1.col(i) = x1.col(vec_inliers[i]); - inliers2.col(i) = x2.col(vec_inliers[i]); + inliers1.col(i) = x1.col(inliers[i]); + inliers2.col(i) = x2.col(inliers[i]); } // check scale @@ -235,10 +238,13 @@ BOOST_AUTO_TEST_CASE(SRT_precision_ACRANSAC_noiseByShuffling) Mat4 RTS; composeRTS(Sc, tc, Rc, RTS); + robustEstimation::MatrixModel modelRTS(RTS); + geometry::RTSSquaredResidualError errorEstimator; + // check the residuals for the inliers for(std::size_t i = 0; i < nbInliers; ++i) { - const double error = geometry::RTSSquaredResidualError::Error(RTS, inliers1.col(i), inliers2.col(i)); + const double error = errorEstimator.error(modelRTS, inliers1.col(i), inliers2.col(i)); BOOST_CHECK_SMALL(error, 1e-9); } } diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTij_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTij_test.cpp index 7ec1906cd5..ffdd2db822 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTij_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTij_test.cpp @@ -42,7 +42,7 @@ BOOST_AUTO_TEST_CASE(translation_averaging_globalTi_from_tijs) { bCardiod, bRelative_Translation_PerTriplet ); - d.ExportToPLY("global_translations_from_Tij_GT.ply"); + d.exportToPLY("global_translations_from_Tij_GT.ply"); visibleCamPosToSVGSurface(d._C, "global_translations_from_Tij_GT.svg"); //-- Compute the global translations from the translation heading directions diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTriplets_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTriplets_test.cpp index 6e1936d231..64ad3594e6 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTriplets_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_global_translations_fromTriplets_test.cpp @@ -42,7 +42,7 @@ BOOST_AUTO_TEST_CASE(translation_averaging_globalTi_from_tijs_Triplets) { bCardiod, bRelative_Translation_PerTriplet ); - d.ExportToPLY("global_translations_from_triplets_GT.ply"); + d.exportToPLY("global_translations_from_triplets_GT.ply"); visibleCamPosToSVGSurface(d._C, "global_translations_from_triplets_GT.svg"); //-- Compute the global translations from the triplets of heading directions diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_robust_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_robust_test.cpp index e38d08d38d..b85911f4dd 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_robust_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_robust_test.cpp @@ -10,7 +10,7 @@ #include "aliceVision/linearProgramming/lInfinityCV/resection_kernel.hpp" #include "aliceVision/robustEstimation/maxConsensus.hpp" #include "aliceVision/robustEstimation/ScoreEvaluator.hpp" -#include "aliceVision/multiview/projection.hpp" +#include "aliceVision/numeric/projection.hpp" #include #include @@ -41,21 +41,22 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_Robust_OutlierFree) { // Solve the problem and check that fitted value are good enough { typedef lInfinityCV::kernel::l1PoseResectionKernel KernelType; - const Mat & pt2D = d2._x[nResectionCameraIndex]; - const Mat & pt3D = d2._X; + const Mat& pt2D = d2._x[nResectionCameraIndex]; + const Mat& pt3D = d2._X; + KernelType kernel(pt2D, pt3D); ScoreEvaluator scorer(2*Square(0.6)); - Mat34 P = MaxConsensus(kernel, scorer, nullptr, 128); + robustEstimation::Mat34Model P = maxConsensus(kernel, scorer, nullptr, 128); // Check that Projection matrix is near to the GT : Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() / d.P(nResectionCameraIndex).norm(); - Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); + Mat34 COMPUTED_ProjectionMatrix = P.getMatrix().array() / P.getMatrix().norm(); // Extract K[R|t] Mat3 R,K; Vec3 t; - KRt_From_P(P, &K, &R, &t); + KRt_from_P(P.getMatrix(), &K, &R, &t); d2._R[nResectionCameraIndex] = R; d2._t[nResectionCameraIndex] = t; @@ -66,14 +67,14 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_Robust_OutlierFree) { } } -BOOST_AUTO_TEST_CASE(Resection_L_Infinity_Robust_OneOutlier) { - +BOOST_AUTO_TEST_CASE(Resection_L_Infinity_Robust_OneOutlier) +{ const int nViews = 3; const int nbPoints = 20; const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Modify a dataset (set to 0 and parse new value) (Assert good values) NViewDataSet d2 = d; @@ -98,17 +99,17 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_Robust_OneOutlier) { const Mat & pt3D = d2._X; KernelType kernel(pt2D, pt3D); ScoreEvaluator scorer(Square(0.1)); //Highly intolerant for the test - Mat34 P = MaxConsensus(kernel, scorer, nullptr, 128); + robustEstimation::Mat34Model P = maxConsensus(kernel, scorer, nullptr, 128); // Check that Projection matrix is near to the GT : Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() / d.P(nResectionCameraIndex).norm(); - Mat34 estimatedProjectionMatrix = P.array() / P.norm(); + Mat34 estimatedProjectionMatrix = P.getMatrix().array() / P.getMatrix().norm(); // Extract K[R|t] Mat3 R,K; Vec3 t; - KRt_From_P(P, &K, &R, &t); + KRt_from_P(P.getMatrix(), &K, &R, &t); d2._R[nResectionCameraIndex] = R; d2._t[nResectionCameraIndex] = t; @@ -117,5 +118,5 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_Robust_OneOutlier) { BOOST_CHECK_SMALL(FrobeniusDistance(GT_ProjectionMatrix, estimatedProjectionMatrix), 1e-1 ); BOOST_CHECK_SMALL(reprojectionErrorRMSE(pt2D, pt3D.colwise().homogeneous(), estimatedProjectionMatrix), 0.75); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_test.cpp index 63c0ffcbc4..310e0ba0f4 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_resection_test.cpp @@ -9,7 +9,7 @@ #include #include "aliceVision/multiview/NViewDataSet.hpp" -#include "aliceVision/multiview/projection.hpp" +#include "aliceVision/numeric/projection.hpp" #include #include "aliceVision/linearProgramming/ISolver.hpp" @@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_OSICLP) { const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Modify a dataset (set to 0 and parse new value) (Assert good values) NViewDataSet d2 = d; @@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_OSICLP) { Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-4); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_MOSEK) @@ -105,7 +105,7 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_MOSEK) { const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Modify a dataset (set to 0 and parse new value) (Assert good values) NViewDataSet d2 = d; @@ -152,6 +152,6 @@ BOOST_AUTO_TEST_CASE(Resection_L_Infinity_MOSEK) { Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-4); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } #endif // #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_MOSEK) diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_outlier_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_outlier_test.cpp index a6ac74ea1d..f2409ae49f 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_outlier_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_outlier_test.cpp @@ -10,7 +10,7 @@ #include "aliceVision/numeric/numeric.hpp" #include -#include "aliceVision/multiview/projection.hpp" +#include "aliceVision/numeric/projection.hpp" #include "aliceVision/linearProgramming/ISolver.hpp" #include "aliceVision/linearProgramming/OSIXSolver.hpp" @@ -46,7 +46,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_Noisy_Outlier_OSICLP_SOLVE NRealisticCamerasCardioid(nViews, nbPoints, NViewDatasetConfigurator(focalValue,focalValue,cx,cy,5,0)); - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; @@ -124,7 +124,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_Noisy_Outlier_OSICLP_SOLVE ALICEVISION_LOG_DEBUG_OBJ << "Camera : " << i << " \t:"; for(int k = 0; k < d._x[0].cols(); ++k) { - xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xk = project(d2.P(i), Vec3(d2._X.col(k))); double residual = (xk - d2._x[i].col(k)).norm(); ALICEVISION_LOG_DEBUG_OBJ << Vec2(( xk - d2._x[i].col(k)).array().pow(2)).array().sqrt().mean() <<"\t"; //-- Check that were measurement are not noisy the residual is small @@ -143,7 +143,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_Noisy_Outlier_OSICLP_SOLVE BOOST_CHECK_SMALL(dResidual, 1e-1); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_MOSEK) @@ -159,7 +159,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_Noisy_Outlier_MOSEK) { NRealisticCamerasCardioid(nViews, nbPoints, NViewDatasetConfigurator(focalValue,focalValue,cx,cy,5,0)); - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; @@ -238,7 +238,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_Noisy_Outlier_MOSEK) { ALICEVISION_LOG_DEBUG("\nCamera : " << i << " \t:"; for(int k = 0; k < d._x[0].cols(); ++k) { - xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xk = project(d2.P(i), Vec3(d2._X.col(k))); double residual = (xk - d2._x[i].col(k)).norm(); ALICEVISION_LOG_DEBUG(Vec2(( xk - d2._x[i].col(k)).array().pow(2)).array().sqrt().mean() <<"\t"; //-- Check that were measurement are not noisy the residual is small @@ -257,6 +257,6 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_Noisy_Outlier_MOSEK) { BOOST_CHECK_SMALL(dResidual, 1e-1); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } #endif // ALICEVISION_HAVE_MOSEK diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_test.cpp index f41092c55b..410ebeb0a1 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_tijsAndXis_test.cpp @@ -8,7 +8,7 @@ #include "aliceVision/multiview/NViewDataSet.hpp" #include "aliceVision/numeric/numeric.hpp" #include -#include "aliceVision/multiview/projection.hpp" +#include "aliceVision/numeric/projection.hpp" #include "aliceVision/linearProgramming/ISolver.hpp" #include "aliceVision/linearProgramming/OSIXSolver.hpp" #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_MOSEK) @@ -38,7 +38,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER) { const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; @@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER) { for (size_t i = 0; i < d2._n; ++i) { for(size_t k = 0; k < d._x[0].cols(); ++k) { - xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xk = project(d2.P(i), Vec3(d2._X.col(k))); xsum += Vec2(( xk - d2._x[i].col(k)).array().pow(2)); } } @@ -112,7 +112,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER) { BOOST_CHECK_SMALL(dResidual2D, 1e-4); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER_K) { @@ -122,7 +122,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER_K) { const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, NViewDatasetConfigurator(1000,1000,500,500,5,0)); // Suppose a camera with Unit matrix as K - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; @@ -190,7 +190,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER_K) { for (size_t i = 0; i < d2._n; ++i) { for(size_t k = 0; k < (size_t)d._x[0].cols(); ++k) { - xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xk = project(d2.P(i), Vec3(d2._X.col(k))); xsum += Vec2(( xk - d2._x[i].col(k)).array().pow(2)); } } @@ -200,7 +200,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_OSICLP_SOLVER_K) { BOOST_CHECK_SMALL(dResidual2D, 1e-4); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_MOSEK) @@ -211,7 +211,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_MOSEK) { const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - d.ExportToPLY("test_Before_Infinity.ply"); + d.exportToPLY("test_Before_Infinity.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; @@ -275,7 +275,7 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_MOSEK) { for (size_t i = 0; i < d2._n; ++i) { for(size_t k = 0; k < d._x[0].cols(); ++k) { - xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xk = project(d2.P(i), Vec3(d2._X.col(k))); xsum += Vec2(( xk - d2._x[i].col(k)).array().pow(2)); } } @@ -285,6 +285,6 @@ BOOST_AUTO_TEST_CASE(Translation_Structure_L_Infinity_MOSEK) { BOOST_CHECK_SMALL(dResidual2D, 1e-4); } - d2.ExportToPLY("test_After_Infinity.ply"); + d2.exportToPLY("test_After_Infinity.ply"); } #endif // ALICEVISION_HAVE_MOSEK diff --git a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_triangulation_test.cpp b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_triangulation_test.cpp index 6cd5a693d6..75c5dc52db 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_triangulation_test.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/lInftyCV_triangulation_test.cpp @@ -12,7 +12,7 @@ #include "aliceVision/numeric/numeric.hpp" #include -#include "aliceVision/multiview/projection.hpp" +#include "aliceVision/numeric/projection.hpp" #include "aliceVision/linearProgramming/ISolver.hpp" #include "aliceVision/linearProgramming/OSIXSolver.hpp" @@ -39,7 +39,7 @@ BOOST_AUTO_TEST_CASE(lInfinityCV_Triangulation_OSICLPSOLVER) { std::vector vec_Pi; - d.ExportToPLY("test_Before_Infinity_Triangulation_OSICLP.ply"); + d.exportToPLY("test_Before_Infinity_Triangulation_OSICLP.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation @@ -76,7 +76,7 @@ BOOST_AUTO_TEST_CASE(lInfinityCV_Triangulation_OSICLPSOLVER) { const Vec3 & X = XSolution; Vec2 x1, xsum(0.0,0.0); for (int i = 0; i < d2._n; ++i) { - x1 = Project(d2.P(i), X); + x1 = project(d2.P(i), X); xsum += Vec2((x1-x_ij.col(i)).array().pow(2)); } double dResidual2D = (xsum.array().sqrt().sum()); @@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE(lInfinityCV_Triangulation_OSICLPSOLVER) { BOOST_CHECK_SMALL(dResidual2D, 1e-5); BOOST_CHECK_SMALL(dResidual3D, 1e-5); } - d2.ExportToPLY("test_After_Infinity_Triangulation_OSICLP.ply"); + d2.exportToPLY("test_After_Infinity_Triangulation_OSICLP.ply"); } #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_MOSEK) @@ -99,7 +99,7 @@ BOOST_AUTO_TEST_CASE(computervision_Triangulation_MOSEK) { std::vector vec_Pi; - d.ExportToPLY("test_Before_Infinity_Triangulation_MOSEK.ply"); + d.exportToPLY("test_Before_Infinity_Triangulation_MOSEK.ply"); //-- Test triangulation of all the point NViewDataSet d2 = d; d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation @@ -136,7 +136,7 @@ BOOST_AUTO_TEST_CASE(computervision_Triangulation_MOSEK) { const Vec3 & X = XSolution; Vec2 x1, xsum(0.0,0.0); for (int i = 0; i < d2._n; ++i) { - x1 = Project(d2.P(i), X); + x1 = project(d2.P(i), X); xsum += Vec2((x1-x_ij.col(i)).array().pow(2)); } double dResidual2D = (xsum.array().sqrt().sum()); @@ -148,6 +148,6 @@ BOOST_AUTO_TEST_CASE(computervision_Triangulation_MOSEK) { BOOST_CHECK_SMALL(dResidual2D, 1e-5); BOOST_CHECK_SMALL(dResidual3D, 1e-5); } - d2.ExportToPLY("test_After_Infinity_Triangulation_MOSEK.ply"); + d2.exportToPLY("test_After_Infinity_Triangulation_MOSEK.ply"); } #endif // ALICEVISION_HAVE_MOSEK diff --git a/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.cpp b/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.cpp index 4b2b4f8ccd..9c3ff48321 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.cpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.cpp @@ -5,12 +5,12 @@ // 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 "aliceVision/linearProgramming/ISolver.hpp" -#include "aliceVision/linearProgramming/OSIXSolver.hpp" -#include "aliceVision/linearProgramming/bisectionLP.hpp" +#include +#include +#include -#include "aliceVision/linearProgramming/lInfinityCV/resection.hpp" -#include "aliceVision/linearProgramming/lInfinityCV/resection_kernel.hpp" +#include +#include #include @@ -28,7 +28,8 @@ void translate(const Mat3X & X, const Vec3 & vecTranslation, } } -void l1SixPointResectionSolver::Solve(const Mat &pt2D, const Mat &pt3d, vector *Ps) { +void l1SixPointResectionSolver::solve(const Mat &pt2D, const Mat &pt3d, std::vector& Ps) const +{ assert(2 == pt2D.rows()); assert(3 == pt3d.rows()); assert(6 <= pt2D.cols()); @@ -60,7 +61,7 @@ void l1SixPointResectionSolver::Solve(const Mat &pt2D, const Mat &pt3d, vectorpush_back(P); + Ps.emplace_back(P); } } diff --git a/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.hpp b/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.hpp index 6ed032147c..c3a2e0fa9f 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.hpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/resection_kernel.hpp @@ -5,47 +5,70 @@ // 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/. -#ifndef ALICEVISION_RESECTION_L1_KERNEL_H_ -#define ALICEVISION_RESECTION_L1_KERNEL_H_ +#pragma once + +#include +#include +#include +#include #include -#include "aliceVision/multiview/projection.hpp" -#include "aliceVision/multiview/twoViewKernel.hpp" -#include "aliceVision/numeric/numeric.hpp" namespace aliceVision { namespace lInfinityCV { namespace kernel { -using namespace std; - /** * Six-point resection * P Matrix estimation (Pose estimation) * Rely on L1 Resection algorithm. * Work from 6 to N points. */ -struct l1SixPointResectionSolver { - enum { MINIMUM_SAMPLES = 6 }; - enum { MAX_MODELS = 1 }; - // Solve the problem of camera pose. - // First 3d point will be translated in order to have X0 = (0,0,0,1) - static void Solve(const Mat &pt2D, const Mat &pt3D, vector *P); - - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D) +struct l1SixPointResectionSolver : public robustEstimation::ISolver +{ + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 6; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override { - Vec2 x = Project(P, pt3D); - return (x-pt2D).norm(); + return 1; } + + /** + * @brief Solve the problem of camera pose. + * @note First 3d point will be translated in order to have X0 = (0,0,0,1) + */ + void solve(const Mat& pt2D, const Mat& pt3D, std::vector& P) const; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("l1SixPointResectionSolver does not support problem solving with weights."); + } + }; -//-- Usable solver for the l1 6pt Resection Estimation -typedef twoView::kernel::Kernel l1PoseResectionKernel; +/** + * @brief Usable solver for the l1 6pt Resection Estimation + */ +using l1PoseResectionKernel = robustEstimation::PointFittingKernel; } // namespace kernel } // namespace lInfinityCV } // namespace aliceVision - -#endif // ALICEVISION_RESECTION_L1_KERNEL_H_ diff --git a/src/aliceVision/linearProgramming/lInfinityCV/triplet_tijsAndXis_kernel.hpp b/src/aliceVision/linearProgramming/lInfinityCV/triplet_tijsAndXis_kernel.hpp index e58f9753a3..edc9e5656f 100644 --- a/src/aliceVision/linearProgramming/lInfinityCV/triplet_tijsAndXis_kernel.hpp +++ b/src/aliceVision/linearProgramming/lInfinityCV/triplet_tijsAndXis_kernel.hpp @@ -10,8 +10,8 @@ #include "aliceVision/numeric/numeric.hpp" #include "aliceVision/config.hpp" -#include "aliceVision/multiview/projection.hpp" -#include "aliceVision/multiview/conditioning.hpp" +#include "aliceVision/numeric/projection.hpp" +#include "aliceVision/robustEstimation/conditioning.hpp" #include "aliceVision/multiview/triangulation/Triangulation.hpp" // Linear programming solver(s) @@ -35,16 +35,16 @@ struct TrifocalTensorModel { static double Error(const TrifocalTensorModel & t, const Vec2 & pt1, const Vec2 & pt2, const Vec2 & pt3) { // Triangulate - Triangulation triangulationObj; + multiview::Triangulation triangulationObj; triangulationObj.add(t.P1, pt1); triangulationObj.add(t.P2, pt2); triangulationObj.add(t.P3, pt3); const Vec3 X = triangulationObj.compute(); // Return the maximum observed reprojection error - const double pt1ReProj = (Project(t.P1, X) - pt1).squaredNorm(); - const double pt2ReProj = (Project(t.P2, X) - pt2).squaredNorm(); - const double pt3ReProj = (Project(t.P3, X) - pt3).squaredNorm(); + const double pt1ReProj = (project(t.P1, X) - pt1).squaredNorm(); + const double pt2ReProj = (project(t.P2, X) - pt2).squaredNorm(); + const double pt3ReProj = (project(t.P3, X) - pt3).squaredNorm(); return std::max(pt1ReProj, std::max(pt2ReProj,pt3ReProj)); } @@ -59,15 +59,31 @@ namespace aliceVision{ using namespace aliceVision::trifocal::kernel; /// Solve the translations and the structure of a view-triplet that have known rotations -struct translations_Triplet_Solver { - enum { MINIMUM_SAMPLES = 4 }; - enum { MAX_MODELS = 1 }; +struct translations_Triplet_Solver +{ + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamples() const + { + return 4; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + std::size_t getMaximumNbModels() const + { + return 1; + } /// Solve the computation of the "tensor". - static void Solve( + void solve( const Mat &pt0, const Mat & pt1, const Mat & pt2, - const std::vector & vec_KR, std::vector *P, - const double ThresholdUpperBound) + const std::vector & vec_KR, std::vector &P, + const double ThresholdUpperBound) const { //Build the megaMatMatrix const int n_obs = pt0.cols(); @@ -82,7 +98,7 @@ struct translations_Triplet_Solver { } } //-- Solve the LInfinity translation and structure from Rotation and points data. - std::vector vec_solution((3 + MINIMUM_SAMPLES)*3); + std::vector vec_solution((3 + getMinimumNbRequiredSamples())*3); using namespace aliceVision::lInfinityCV; @@ -111,12 +127,12 @@ struct translations_Triplet_Solver { PTemp.P2 = HStack(vec_KR[1], vec_tis[1]); PTemp.P3 = HStack(vec_KR[2], vec_tis[2]); - P->push_back(PTemp); + P.push_back(PTemp); } } // Compute the residual of reprojections - static double Error( + static double error( const TrifocalTensorModel & Tensor, const Vec2 & pt0, const Vec2 & pt1, const Vec2 & pt2) { diff --git a/src/aliceVision/localization/CCTagLocalizer.cpp b/src/aliceVision/localization/CCTagLocalizer.cpp index 0bc2041bc0..eb370a8e53 100644 --- a/src/aliceVision/localization/CCTagLocalizer.cpp +++ b/src/aliceVision/localization/CCTagLocalizer.cpp @@ -336,7 +336,7 @@ bool CCTagLocalizer::localize(const feature::MapRegionsPerDesc & genQueryRegions Vec3 t_; // Decompose the projection matrix to get K, R and t using // RQ decomposition - KRt_From_P(resectionData.projection_matrix, &K_, &R_, &t_); + KRt_from_P(resectionData.projection_matrix, &K_, &R_, &t_); queryIntrinsics.setK(K_); ALICEVISION_LOG_DEBUG("K estimated\n" << K_); queryIntrinsics.setWidth(imageSize.first); diff --git a/src/aliceVision/localization/VoctreeLocalizer.cpp b/src/aliceVision/localization/VoctreeLocalizer.cpp index daf5d53753..070cf7d6ac 100644 --- a/src/aliceVision/localization/VoctreeLocalizer.cpp +++ b/src/aliceVision/localization/VoctreeLocalizer.cpp @@ -20,7 +20,8 @@ #include #include #include -#include +#include +#include #include #include @@ -563,7 +564,7 @@ bool VoctreeLocalizer::localizeFirstBestResult(const feature::MapRegionsPerDesc Vec3 t_; // Decompose the projection matrix to get K, R and t using // RQ decomposition - KRt_From_P(resectionData.projection_matrix, &K_, &R_, &t_); + KRt_from_P(resectionData.projection_matrix, &K_, &R_, &t_); ALICEVISION_LOG_DEBUG("K estimated\n" << K_); queryIntrinsics.setK(K_); queryIntrinsics.setWidth(queryImageSize.first); @@ -687,7 +688,7 @@ bool VoctreeLocalizer::localizeAllResults(const feature::MapRegionsPerDesc &quer Vec3 t_; // Decompose the projection matrix to get K, R and t using // RQ decomposition - KRt_From_P(resectionData.projection_matrix, &K_, &R_, &t_); + KRt_from_P(resectionData.projection_matrix, &K_, &R_, &t_); queryIntrinsics.setK(K_); ALICEVISION_LOG_DEBUG("K estimated\n" << K_); queryIntrinsics.setWidth(queryImageSize.first); @@ -1133,14 +1134,15 @@ bool VoctreeLocalizer::robustMatching(matching::RegionsDatabaseMatcherPerDesc & // perform guided matching. // So we ignore the previous matches and recompute all matches. out_featureMatches.clear(); - robustEstimation::GuidedMatching< - Mat3, - aliceVision::fundamental::kernel::EpipolarDistanceError>( - geometricFilter.m_F, - queryIntrinsicsBase, // camera::IntrinsicBase of the matched image + + robustEstimation::Mat3Model model(geometricFilter.m_F); + + matching::guidedMatching( + model, + queryIntrinsicsBase, // camera::IntrinsicBase of the matched image matchers.getDatabaseRegionsPerDesc(), // feature::Regions - matchedIntrinsicsBase, // camera::IntrinsicBase of the query image - matchedRegions, // feature::Regions + matchedIntrinsicsBase, // camera::IntrinsicBase of the query image + matchedRegions, // feature::Regions Square(geometricFilter.m_dPrecision_robust), Square(fDistRatio), out_featureMatches); // output diff --git a/src/aliceVision/localization/rigResection.cpp b/src/aliceVision/localization/rigResection.cpp index aaa257fd94..5c2acf4568 100644 --- a/src/aliceVision/localization/rigResection.cpp +++ b/src/aliceVision/localization/rigResection.cpp @@ -5,8 +5,9 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "rigResection.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" #include +#include + #if ALICEVISION_IS_DEFINED(ALICEVISION_HAVE_OPENGV) #include #include @@ -200,7 +201,7 @@ EstimationStatus rigResection(const std::vector &pts2d, if(descTypesPerCamera) { // Check if estimation has strong support - hasStrongSupport = robustEstimation::hasStrongSupport(inliers, *descTypesPerCamera, 3); + hasStrongSupport = matching::hasStrongSupport(inliers, *descTypesPerCamera, 3); } return EstimationStatus(true, hasStrongSupport); } diff --git a/src/aliceVision/matching/CMakeLists.txt b/src/aliceVision/matching/CMakeLists.txt index 530c94b133..43d82cebe2 100644 --- a/src/aliceVision/matching/CMakeLists.txt +++ b/src/aliceVision/matching/CMakeLists.txt @@ -7,6 +7,7 @@ set(matching_files_headers IndMatch.hpp IndMatchDecorator.hpp filters.hpp + guidedMatching.hpp io.hpp matcherType.hpp metric.hpp @@ -14,18 +15,22 @@ set(matching_files_headers CascadeHasher.hpp RegionsMatcher.hpp pairwiseAdjacencyDisplay.hpp + supportEstimation.hpp ) # Sources set(matching_files_sources io.cpp + guidedMatching.cpp matcherType.cpp RegionsMatcher.cpp + supportEstimation.cpp ) alicevision_add_library(aliceVision_matching SOURCES ${matching_files_headers} ${matching_files_sources} PUBLIC_LINKS + aliceVision_camera aliceVision_feature aliceVision_numeric aliceVision_stl diff --git a/src/aliceVision/matching/guidedMatching.cpp b/src/aliceVision/matching/guidedMatching.cpp new file mode 100644 index 0000000000..948c4d81c8 --- /dev/null +++ b/src/aliceVision/matching/guidedMatching.cpp @@ -0,0 +1,73 @@ +// 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 "guidedMatching.hpp" + +namespace aliceVision { +namespace matching { + +unsigned int pix_to_bucket(const Vec2i& x, int W, int H) +{ + if(x(1) == 0) + return x(0); // Top border + if(x(0) == W - 1) + return W - 1 + x(1); // Right border + if(x(1) == H - 1) + return 2 * W + H - 3 - x(0); // Bottom border + return 2 * (W + H - 2) - x(1); // Left border +} + +bool line_to_endPoints(const Vec3& line, int W, int H, Vec2& x0, Vec2& x1) +{ + const double a = line(0); + const double b = line(1); + const double c = line(2); + + float r1, r2; + // Intersection with Y axis (0 or W-1) + if(b != 0) + { + const double x = (b < 0) ? 0 : W - 1; + double y = -(a * x + c) / b; + if(y < 0) + y = 0.; + else if(y >= H) + y = H - 1; + r1 = fabs(a * x + b * y + c); + x0 << x, y; + } + else + { + return false; + } + + // Intersection with X axis (0 or H-1) + if(a != 0) + { + const double y = (a < 0) ? H - 1 : 0; + double x = -(b * y + c) / a; + if(x < 0) + x = 0.; + else if(x >= W) + x = W - 1; + r2 = fabs(a * x + b * y + c); + x1 << x, y; + } + else + { + return false; + } + + // Choose x0 to be as close as the intersection axis + if(r1 > r2) + std::swap(x0, x1); + + return true; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/matching/guidedMatching.hpp b/src/aliceVision/matching/guidedMatching.hpp new file mode 100644 index 0000000000..98e542fad7 --- /dev/null +++ b/src/aliceVision/matching/guidedMatching.hpp @@ -0,0 +1,467 @@ +// 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 matching { + +/** + * @brief Guided Matching (features only): + * Use a model to find valid correspondences: + * Keep the best corresponding points for the given model under the + * user specified distance. + * + * @tparam ModelT The used model type + * @tparam ErrorT The metric to compute distance to the model + * + * @param[in] mod The model + * @param[in] xLeft The left data points + * @param[in] xRight The right data points + * @param[in] errorTh Maximal authorized error threshold + * @param[out] out_validMatches Ouput corresponding index + */ +template +void guidedMatching(const ModelT& mod, const Mat& xLeft, const Mat& xRight, double errorTh, matching::IndMatches& out_validMatches) +{ + assert(xLeft.rows() == xRight.rows()); + + const ErrorT errorEstimator = ErrorT(); + + // looking for the corresponding points that have + // the smallest distance (smaller than the provided Threshold) + for(Mat::Index i = 0; i < xLeft.cols(); ++i) + { + double min = std::numeric_limits::max(); + matching::IndMatch match; + for(Mat::Index j = 0; j < xRight.cols(); ++j) + { + // compute the geometric error: error to the model + const double err = errorEstimator.error(mod, xLeft.col(i), xRight.col(j)); + + // if smaller error update corresponding index + if(err < errorTh && err < min) + { + min = err; + match = matching::IndMatch(i, j); + } + } + if(min < errorTh) + { + // save the best corresponding index + out_validMatches.push_back(match); + } + } + + // remove duplicates (when multiple points at same position exist) + matching::IndMatch::getDeduplicated(out_validMatches); +} + +/** + * @brief Struct to help filtering of correspondence according update of two smallest distance. + * @note useful for descriptor distance ratio filtering + */ +template +struct distanceRatio +{ + /// best and second best distance + DistT bd, sbd; + /// best corresponding index + std::size_t idx; + + distanceRatio() + : bd(std::numeric_limits::max()) + , sbd(std::numeric_limits::max()) + , idx(0) + {} + + /** + * @brief Update match according the provided distance + */ + inline bool update(std::size_t index, DistT dist) + { + if(dist < bd) // best than any previous + { + idx = index; + // update and swap + sbd = dist; + std::swap(bd, sbd); + return true; + } + else if(dist < sbd) + { + sbd = dist; + return true; + } + return false; + } + + /** + * @brief Return if the ratio of distance is ok or not + */ + inline bool isValid(const double distRatio) const + { + // check: + // - that two best distance have been found + // - the distance ratio + return (sbd != std::numeric_limits::max() + && bd < distRatio * sbd); + } +}; + + +/** + * @brief Guided Matching (features + descriptors with distance ratio): + * Use a model to find valid correspondences: + * Keep the best corresponding points for the given model under the + * user specified distance. + * + * @tparam ModelT The used model type + * @tparam ErrorT The metric to compute distance to the model + * @tparam DescriptorT The descriptor type + * @tparam MetricT The metric to compare two descriptors + * + * @param[in] mod The model + * @param[in] xLeft The left data points + * @param[in] lDescriptors The left descriptors + * @param[in] xRight The right data points + * @param[in] rDescriptors The right descriptors + * @param[in] errorTh Maximal authorized error threshold + * @param[in] distRatio Maximal authorized distance ratio + * @param[out] vec_corresponding_index Ouput corresponding index + */ +template +void guidedMatching(const ModelT& mod, + const Mat& xLeft, + const std::vector& lDescriptors, + const Mat& xRight, + const std::vector& rDescriptors, + double errorTh, + double distRatio, + matching::IndMatches& vec_corresponding_index) +{ + assert(xLeft.rows() == xRight.rows()); + assert(xLeft.cols() == lDescriptors.size()); + assert(xRight.cols() == rDescriptors.size()); + + MetricT metric; + + // Looking for the corresponding points that have to satisfy: + // 1. a geometric distance below the provided Threshold + // 2. a distance ratio between descriptors of valid geometric correspondencess + + for(Mat::Index i = 0; i < xLeft.cols(); ++i) + { + + distanceRatio dR; + for(Mat::Index j = 0; j < xRight.cols(); ++j) + { + // Compute the geometric error: error to the model + const double geomErr = ErrorT::Error(mod, // The model + xLeft.col(i), + xRight.col(j)); // The corresponding points + if(geomErr < errorTh) + { + const typename MetricT::ResultType descDist = + metric(lDescriptors[i].getData(), rDescriptors[j].getData(), DescriptorT::static_size); + // Update the corresponding points & distance (if required) + dR.update(j, descDist); + } + } + // Add correspondence only iff the distance ratio is valid + if(dR.isValid(distRatio)) + { + // save the best corresponding index + vec_corresponding_index.emplace_back(i, dR.idx); + } + } + + // Remove duplicates (when multiple points at same position exist) + matching::IndMatch::getDeduplicated(vec_corresponding_index); +} + +/** + * @brief Guided Matching (features + descriptors with distance ratio): + * Use a model to find valid correspondences: + * Keep the best corresponding points for the given model under the + * user specified distance ratio. + * + * @tparam ModelT The used model type + * @tparam ErrorT The metric to compute distance to the model + * + * @param[in] mod The model + * @param[in] camL Optional camera (in order to undistord on the fly feature positions, can be NULL) + * @param[in] lRegions regions (point features & corresponding descriptors) + * @param[in] camR Optional camera (in order to undistord on the fly feature positions, can be NULL) + * @param[in] rRegions regions (point features & corresponding descriptors) + * @param[in] errorTh Maximal authorized error threshold + * @param[in] distRatio Maximal authorized distance ratio + * @param[out] out_matches Ouput corresponding index + */ +template +void guidedMatching(const ModelT& mod, + const camera::IntrinsicBase* camL, + const feature::Regions& lRegions, + const camera::IntrinsicBase* camR, + const feature::Regions& rRegions, + double errorTh, + double distRatio, + matching::IndMatches& out_matches) +{ + // looking for the corresponding points that have to satisfy: + // 1. a geometric distance below the provided Threshold + // 2. a distance ratio between descriptors of valid geometric correspondencess + + const ErrorT errorEstimator = ErrorT(); + + // build region positions arrays (in order to un-distord on-demand point position once) + std::vector lRegionsPos(lRegions.RegionCount()); + std::vector rRegionsPos(rRegions.RegionCount()); + + if(camL && camL->isValid()) + { + for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) + lRegionsPos[i] = camL->get_ud_pixel(lRegions.GetRegionPosition(i)); + } + else + { + for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) + lRegionsPos[i] = lRegions.GetRegionPosition(i); + } + if(camR && camR->isValid()) + { + for(std::size_t i = 0; i < rRegions.RegionCount(); ++i) + rRegionsPos[i] = camR->get_ud_pixel(rRegions.GetRegionPosition(i)); + } + else + { + for(std::size_t i = 0; i < rRegions.RegionCount(); ++i) + rRegionsPos[i] = rRegions.GetRegionPosition(i); + } + + for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) + { + distanceRatio dR; + for(std::size_t j = 0; j < rRegions.RegionCount(); ++j) + { + // compute the geometric error: error to the model + const double geomErr = errorEstimator.error(mod, lRegionsPos[i], rRegionsPos[j]); + + if(geomErr < errorTh) + { + // update the corresponding points & distance (if required) + dR.update(j, lRegions.SquaredDescriptorDistance(i, &rRegions, j)); + } + } + // add correspondence only iff the distance ratio is valid + if(dR.isValid(distRatio)) + { + // save the best corresponding index + out_matches.emplace_back(i, dR.idx); + } + } + + // remove duplicates (when multiple points at same position exist) + matching::IndMatch::getDeduplicated(out_matches); +} + +/** + * @brief Guided Matching (features + descriptors with distance ratio): + * Use a model to find valid correspondences: + * Keep the best corresponding points for the given model under the + * user specified distance ratio. + * + * @tparam ModelT The used model type + * @tparam ErrorT The metric to compute distance to the model + * + * @param[in] mod The model + * @param[in] camL Optional camera (in order to undistord on the fly feature positions, can be NULL) + * @param[in] lRegions regions (point features & corresponding descriptors) + * @param[in] camR Optional camera (in order to undistord on the fly feature positions, can be NULL) + * @param[in] rRegions regions (point features & corresponding descriptors) + * @param[in] errorTh Maximal authorized error threshold + * @param[in] distRatio Maximal authorized distance ratio + * @param[out] out_matchesPerDesc Ouput corresponding index + */ +template +void guidedMatching(const ModelT& mod, + const camera::IntrinsicBase* camL, + const feature::MapRegionsPerDesc& lRegions, + const camera::IntrinsicBase* camR, + const feature::MapRegionsPerDesc& rRegions, + double errorTh, + double distRatio, + matching::MatchesPerDescType& out_matchesPerDesc) +{ + const std::vector descTypes = getCommonDescTypes(lRegions, rRegions); + if(descTypes.empty()) + return; + + for(const feature::EImageDescriberType descType: descTypes) + { + guidedMatching(mod, camL, *lRegions.at(descType), camR, *rRegions.at(descType), errorTh, distRatio, out_matchesPerDesc[descType]); + } +} + +/** + * @brief Compute a bucket index from an epipolar point + * (the one that is closer to image border intersection) + */ +unsigned int pix_to_bucket(const Vec2i &x, int W, int H); + +/** + * @brief Compute intersection of the epipolar line with the image border + */ +bool line_to_endPoints(const Vec3& line, int W, int H, Vec2& x0, Vec2& x1); + +/** + * @brief Guided Matching (features + descriptors with distance ratio): + * Cluster correspondences per epipolar line (faster than exhaustive search). + * Keep the best corresponding points for the given model under the + * user specified distance ratio. + * Can be seen as a variant of robustEstimation method [1]. + * + * @note implementation done here use a pixel grid limited to image border. + * + * @ref [1] Rajvi Shah, Vanshika Shrivastava, and P J Narayanan + * Geometry-aware Feature Matching for Structure from Motion Applications. + * WACV 2015. + * + * @tparam ErrorT The metric to compute distance to the model + * + * @param[in] FMat The fundamental matrix + * @param[in] epipole2 Epipole2 (camera center1 in image plane2; must not be normalized) + * @param[in] camL Optional camera (in order to undistord on the fly feature positions, can be NULL) + * @param[in] lRegions regions (point features & corresponding descriptors) + * @param[in] camR Optional camera (in order to undistord on the fly feature positions, can be NULL) + * @param[in] rRegions regions (point features & corresponding descriptors) + * @param[in] widthR + * @param[in] heightR + * @param[in] errorTh Maximal authorized error threshold (consider it's a square threshold) + * @param[in] distRatio Maximal authorized distance ratio + * @param[out] vec_corresponding_index Ouput corresponding index + */ +template +void guidedMatchingFundamentalFast(const Mat3& FMat, + const Vec3& epipole2, + const camera::IntrinsicBase* camL, + const feature::Regions& lRegions, + const camera::IntrinsicBase* camR, + const feature::Regions& rRegions, + const int widthR, + const int heightR, + double errorTh, + double distRatio, + matching::IndMatches& vec_corresponding_index) +{ + // Looking for the corresponding points that have to satisfy: + // 1. a geometric distance below the provided Threshold + // 2. a distance ratio between descriptors of valid geometric correspondencess + // + // - Cluster left point according their epipolar line border intersection. + // - For each right point, compute threshold limited bandwidth and compare only + // points that belong to this range (limited buckets). + + // Normalize F and epipole for (ep2->p2) line adequation + Mat3 F = FMat; + Vec3 ep2 = epipole2; + if(ep2(2) > 0.0) + { + F = -F; + ep2 = -ep2; + } + ep2 = ep2 / ep2(2); + + //-- + //-- Store point in the corresponding epipolar line bucket + //-- + using Bucket_vec = std::vector; + using Buckets_vec = std::vector; + const int nb_buckets = 2 * (widthR + heightR - 2); + + Buckets_vec buckets(nb_buckets); + for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) + { + // Compute epipolar line + const Vec2 l_pt = (camL && camL->isValid()) ? camL->get_ud_pixel(lRegions.GetRegionPosition(i)) : lRegions.GetRegionPosition(i); + const Vec3 line = F * Vec3(l_pt(0), l_pt(1), 1.); + // If the epipolar line exists in Right image + Vec2 x0, x1; + if(line_to_endPoints(line, widthR, heightR, x0, x1)) + { + // Find in which cluster the point belongs + const auto bucket = pix_to_bucket(x0.cast(), widthR, heightR); + buckets[bucket].push_back(i); + } + } + + // For each point in right image, find if there is good candidates. + std::vector > dR(lRegions.RegionCount()); + for(std::size_t j = 0; j < rRegions.RegionCount(); ++j) + { + // According the point: + // - Compute it's epipolar line from the epipole + // - compute the range of possible bucket by computing + // the epipolar line gauge limitation introduced by the tolerated pixel error + + const Vec2 xR = (camR && camR->isValid()) ? camR->get_ud_pixel(rRegions.GetRegionPosition(j)) : rRegions.GetRegionPosition(j); + const Vec3 l2 = ep2.cross(Vec3(xR(0), xR(1), 1.)); + const Vec2 n = l2.head<2>() * (sqrt(errorTh) / l2.head<2>().norm()); + + const Vec3 l2min = ep2.cross(Vec3(xR(0) - n(0), xR(1) - n(1), 1.)); + const Vec3 l2max = ep2.cross(Vec3(xR(0) + n(0), xR(1) + n(1), 1.)); + + // Compute corresponding buckets + Vec2 x0, x1; + if(!line_to_endPoints(l2min, widthR, heightR, x0, x1)) + continue; + + const auto bucket_start = pix_to_bucket(x0.cast(), widthR, heightR); + + if(!line_to_endPoints(l2max, widthR, heightR, x0, x1)) + continue; + + const auto bucket_stop = pix_to_bucket(x0.cast(), widthR, heightR); + + if(bucket_stop - bucket_start > 0) + { + // test candidate buckets + for(Buckets_vec::const_iterator itBs = buckets.begin() + bucket_start; + itBs != buckets.begin() + bucket_stop; ++itBs) + { + const Bucket_vec & bucket = *itBs; + for(unsigned int i : bucket) + { + // Compute descriptor distance + const double descDist = lRegions.SquaredDescriptorDistance(i, &rRegions, j); + // Update the corresponding points & distance (if required) + dR[i].update(j, descDist); + } + } + } + } + // Check distance ratio validity + for(std::size_t i = 0; i < dR.size(); ++i) + { + if(dR[i].isValid(distRatio)) + { + // save the best corresponding index + vec_corresponding_index.emplace_back(i, dR[i].idx); + } + } +} + +} +} diff --git a/src/aliceVision/matching/supportEstimation.cpp b/src/aliceVision/matching/supportEstimation.cpp new file mode 100644 index 0000000000..7f9b64c463 --- /dev/null +++ b/src/aliceVision/matching/supportEstimation.cpp @@ -0,0 +1,63 @@ +// 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 "supportEstimation.hpp" + +namespace aliceVision{ +namespace matching{ + +bool hasStrongSupport(const std::vector& inliers, + const std::vector& descTypes, std::size_t minimumSamples) +{ + assert(inliers.size() <= descTypes.size()); + + float score = 0; + for(const std::size_t inlier : inliers) + { + score += feature::getStrongSupportCoeff(descTypes[inlier]); + } + return (score > minimumSamples); +} + +bool hasStrongSupport(const std::vector>& inliersPerCamera, + const std::vector>& descTypesPerCamera, + std::size_t minimumSamples) +{ + assert(inliersPerCamera.size() == descTypesPerCamera.size()); //same number of cameras + + float score = 0; + + for(std::size_t camIdx = 0; camIdx < inliersPerCamera.size(); ++camIdx) + { + const auto& inliers = inliersPerCamera.at(camIdx); + const auto& descTypes = descTypesPerCamera.at(camIdx); + + assert(inliers.size() <= descTypes.size()); + + for(const std::size_t inlier : inliers) + { + score += feature::getStrongSupportCoeff(descTypes[inlier]); + } + } + return (score > minimumSamples); +} + +bool hasStrongSupport(const aliceVision::matching::MatchesPerDescType& matchesPerDesc, std::size_t minimumSamples) +{ + float score = 0; + for(const auto& matchesIt : matchesPerDesc) + { + const feature::EImageDescriberType descType = matchesIt.first; + const matching::IndMatches& descMatches = matchesIt.second; + + score += feature::getStrongSupportCoeff(descType) * descMatches.size(); + } + return (score > minimumSamples); +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/matching/supportEstimation.hpp b/src/aliceVision/matching/supportEstimation.hpp new file mode 100644 index 0000000000..efc991a591 --- /dev/null +++ b/src/aliceVision/matching/supportEstimation.hpp @@ -0,0 +1,30 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 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 + +namespace aliceVision { +namespace matching { + +#define ALICEVISION_MINIMUM_SAMPLES_COEF 7 // TODO: TO REMOVE + +bool hasStrongSupport(const std::vector& inliers, + const std::vector& descTypes, + std::size_t minimumSamples); + +bool hasStrongSupport(const std::vector>& inliersPerCamera, + const std::vector>& descTypesPerCamera, + std::size_t minimumSamples); + +bool hasStrongSupport(const matching::MatchesPerDescType& matchesPerDesc, std::size_t minimumSamples); + +} +} diff --git a/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_E_AC.hpp b/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_E_AC.hpp index 953d7645d9..8c6f5d364b 100644 --- a/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_E_AC.hpp +++ b/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_E_AC.hpp @@ -7,109 +7,108 @@ #pragma once -#include "aliceVision/types.hpp" -#include "aliceVision/multiview/essentialKernelSolver.hpp" -#include "aliceVision/multiview/essential.hpp" -#include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" -#include "aliceVision/robustEstimation/guidedMatching.hpp" -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include "aliceVision/matching/IndMatch.hpp" -#include "aliceVision/sfmData/SfMData.hpp" -#include "aliceVision/feature/RegionsPerView.hpp" -#include "aliceVision/matchingImageCollection/GeometricFilterMatrix.hpp" +#include namespace aliceVision { namespace matchingImageCollection { -//-- A contrario essential matrix estimation template functor used for filter pair of putative correspondences +/** + * @brief A contrario essential matrix estimation template functor used for filter pair of putative correspondences + */ struct GeometricFilterMatrix_E_AC : public GeometricFilterMatrix { - GeometricFilterMatrix_E_AC( - double dPrecision = std::numeric_limits::infinity(), - size_t iteration = 1024) + GeometricFilterMatrix_E_AC(double dPrecision = std::numeric_limits::infinity(), + std::size_t iteration = 1024) : GeometricFilterMatrix(dPrecision, std::numeric_limits::infinity(), iteration) , m_E(Mat3::Identity()) {} /** * @brief Given two sets of image points, it estimates the essential matrix - * relating them using a robust method (like A Contrario Ransac). + * relating them using a robust method (like A Contrario Ransac). */ template - EstimationStatus geometricEstimation( - const sfmData::SfMData * sfmData, - const Regions_or_Features_ProviderT& regionsPerView, - const Pair& pairIndex, - const matching::MatchesPerDescType & putativeMatchesPerType, - matching::MatchesPerDescType & out_geometricInliersPerType) + EstimationStatus geometricEstimation(const sfmData::SfMData* sfmData, + const Regions_or_Features_ProviderT& regionsPerView, + const Pair& pairIndex, + const matching::MatchesPerDescType& putativeMatchesPerType, + matching::MatchesPerDescType& out_geometricInliersPerType) { - using namespace aliceVision; - using namespace aliceVision::robustEstimation; out_geometricInliersPerType.clear(); - // Get back corresponding view index - const IndexT iIndex = pairIndex.first; - const IndexT jIndex = pairIndex.second; + // get back corresponding view index + const IndexT I = pairIndex.first; + const IndexT J = pairIndex.second; const std::vector descTypes = regionsPerView.getCommonDescTypes(pairIndex); + if(descTypes.empty()) return EstimationStatus(false, false); - // Reject pair with missing Intrinsic information - const sfmData::View * view_I = sfmData->views.at(iIndex).get(); - const sfmData::View * view_J = sfmData->views.at(jIndex).get(); + // reject pair with missing Intrinsic information + const sfmData::View& viewI = sfmData->getView(I); + const sfmData::View& viewJ = sfmData->getView(J); - // Check that valid cameras can be retrieved for the pair of views - const camera::IntrinsicBase * cam_I = sfmData->getIntrinsicPtr(view_I->getIntrinsicId()); - const camera::IntrinsicBase * cam_J = sfmData->getIntrinsicPtr(view_J->getIntrinsicId()); + // check that valid cameras can be retrieved for the pair of views + const camera::IntrinsicBase* camI = sfmData->getIntrinsicPtr(viewI.getIntrinsicId()); + const camera::IntrinsicBase* camJ = sfmData->getIntrinsicPtr(viewJ.getIntrinsicId()); - if (!cam_I || !cam_J) + if(!camI || !camJ) return EstimationStatus(false, false); - if ( !isPinhole(cam_I->getType()) || !isPinhole(cam_J->getType())) + + if(!isPinhole(camI->getType()) || !isPinhole(camJ->getType())) return EstimationStatus(false, false); - // Get corresponding point regions arrays + // get corresponding point regions arrays Mat xI,xJ; fillMatricesWithUndistortFeaturesMatches(pairIndex, putativeMatchesPerType, sfmData, regionsPerView, descTypes, xI, xJ); - // Define the AContrario adapted Essential matrix solver - typedef ACKernelAdaptorEssential< - aliceVision::essential::kernel::FivePointKernel, - aliceVision::fundamental::kernel::EpipolarDistanceError, - UnnormalizerT, - Mat3> - KernelType; + // define the AContrario adapted Essential matrix solver + using KernelT = multiview::RelativePoseKernel_K< + multiview::relativePose::Essential5PSolver, + multiview::relativePose::FundamentalEpipolarDistanceError, + robustEstimation::Mat3Model>; - const camera::Pinhole * ptrPinhole_I = (const camera::Pinhole*)(cam_I); - const camera::Pinhole * ptrPinhole_J = (const camera::Pinhole*)(cam_J); + const camera::Pinhole* ptrPinholeI = (const camera::Pinhole*)(camI); + const camera::Pinhole* ptrPinholeJ = (const camera::Pinhole*)(camJ); - KernelType kernel( - xI, sfmData->getViews().at(iIndex)->getWidth(), sfmData->getViews().at(iIndex)->getHeight(), - xJ, sfmData->getViews().at(jIndex)->getWidth(), sfmData->getViews().at(jIndex)->getHeight(), - ptrPinhole_I->K(), ptrPinhole_J->K()); + const KernelT kernel(xI, viewI.getWidth(), viewI.getHeight(), + xJ, viewJ.getWidth(), viewJ.getHeight(), ptrPinholeI->K(), ptrPinholeJ->K()); - // Robustly estimate the Essential matrix with A Contrario ransac - const double upper_bound_precision = Square(m_dPrecision); + // robustly estimate the Essential matrix with A Contrario ransac + const double upperBoundPrecision = Square(m_dPrecision); - std::vector inliers; - const std::pair ACRansacOut = ACRANSAC(kernel, inliers, m_stIteration, &m_E, upper_bound_precision); + std::vector inliers; + robustEstimation::Mat3Model model; + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, inliers, m_stIteration, &model, upperBoundPrecision); + m_E = model.getMatrix(); if (inliers.empty()) return EstimationStatus(false, false); m_dPrecision_robust = ACRansacOut.first; - // Fill geometricInliersPerType with inliers from putativeMatchesPerType - copyInlierMatches( - inliers, - putativeMatchesPerType, - descTypes, - out_geometricInliersPerType); + // fill geometricInliersPerType with inliers from putativeMatchesPerType + copyInlierMatches(inliers, + putativeMatchesPerType, + descTypes, + out_geometricInliersPerType); - // Check if resection has strong support - const bool hasStrongSupport = robustEstimation::hasStrongSupport(out_geometricInliersPerType, KernelType::MINIMUM_SAMPLES); + // check if resection has strong support + const bool hasStrongSupport = matching::hasStrongSupport(out_geometricInliersPerType, kernel.getMinimumNbRequiredSamples()); return EstimationStatus(true, hasStrongSupport); } @@ -123,57 +122,52 @@ struct GeometricFilterMatrix_E_AC : public GeometricFilterMatrix * @param matches * @return */ - bool Geometry_guided_matching - ( - const sfmData::SfMData * sfmData, - const feature::RegionsPerView& regionsPerView, - const Pair imageIdsPair, - const double dDistanceRatio, - matching::MatchesPerDescType & matches) override + bool Geometry_guided_matching(const sfmData::SfMData* sfmData, + const feature::RegionsPerView& regionsPerView, + const Pair imageIdsPair, + const double dDistanceRatio, + matching::MatchesPerDescType& matches) override { - if (m_dPrecision_robust != std::numeric_limits::infinity()) + if(!std::isinf(m_dPrecision_robust)) { - // Get back corresponding view index - const IndexT viewId_I = imageIdsPair.first; - const IndexT viewId_J = imageIdsPair.second; - - const sfmData::View * view_I = sfmData->views.at(viewId_I).get(); - const sfmData::View * view_J = sfmData->views.at(viewId_J).get(); - - // Check that valid cameras can be retrieved for the pair of views - const camera::IntrinsicBase * cam_I = - sfmData->getIntrinsics().count(view_I->getIntrinsicId()) ? - sfmData->getIntrinsics().at(view_I->getIntrinsicId()).get() : nullptr; - const camera::IntrinsicBase * cam_J = - sfmData->getIntrinsics().count(view_J->getIntrinsicId()) ? - sfmData->getIntrinsics().at(view_J->getIntrinsicId()).get() : nullptr; - - if (!cam_I || !cam_J) + // get back corresponding view index + const IndexT I = imageIdsPair.first; + const IndexT J = imageIdsPair.second; + + const sfmData::View& viewI = sfmData->getView(I); + const sfmData::View& viewJ = sfmData->getView(J); + + // check that valid cameras can be retrieved for the pair of views + const camera::IntrinsicBase* camI = sfmData->getIntrinsics().count(viewI.getIntrinsicId()) ? + sfmData->getIntrinsics().at(viewI.getIntrinsicId()).get() : nullptr; + const camera::IntrinsicBase* camJ = sfmData->getIntrinsics().count(viewJ.getIntrinsicId()) ? + sfmData->getIntrinsics().at(viewJ.getIntrinsicId()).get() : nullptr; + if(!camI || !camJ) return false; - if ( !isPinhole(cam_I->getType()) || !isPinhole(cam_J->getType())) + if(!isPinhole(camI->getType()) || !isPinhole(camJ->getType())) return false; - const camera::Pinhole * ptrPinhole_I = (const camera::Pinhole*)(cam_I); - const camera::Pinhole * ptrPinhole_J = (const camera::Pinhole*)(cam_J); + const camera::Pinhole* ptrPinholeI = (const camera::Pinhole*)(camI); + const camera::Pinhole* ptrPinholeJ = (const camera::Pinhole*)(camJ); Mat3 F; - FundamentalFromEssential(m_E, ptrPinhole_I->K(), ptrPinhole_J->K(), &F); - - robustEstimation::GuidedMatching( - //aliceVision::fundamental::kernel::SymmetricEpipolarDistanceError>( - F, - cam_I, regionsPerView.getAllRegions(viewId_I), - cam_J, regionsPerView.getAllRegions(viewId_J), - Square(m_dPrecision_robust), Square(dDistanceRatio), - matches); + fundamentalFromEssential(m_E, ptrPinholeI->K(), ptrPinholeJ->K(), &F); + + robustEstimation::Mat3Model model(F); + // multiview::relativePose::FundamentalSymmetricEpipolarDistanceError + matching::guidedMatching( + model, + camI, regionsPerView.getAllRegions(I), + camJ, regionsPerView.getAllRegions(J), + Square(m_dPrecision_robust), Square(dDistanceRatio), + matches); } + return matches.getNbAllMatches() != 0; } - // - //-- Stored data + // stored data Mat3 m_E; }; diff --git a/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_F_AC.hpp b/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_F_AC.hpp index 55aad12a4d..fa12e427bc 100644 --- a/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_F_AC.hpp +++ b/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_F_AC.hpp @@ -7,34 +7,41 @@ #pragma once -#include "aliceVision/matchingImageCollection/GeometricFilterMatrix.hpp" -#include "aliceVision/matchingImageCollection/geometricFilterUtils.hpp" -#include "aliceVision/matching/IndMatch.hpp" -#include "aliceVision/multiview/fundamentalKernelSolver.hpp" -#include "aliceVision/multiview/essential.hpp" -#include "aliceVision/robustEstimation/estimators.hpp" -#include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" -#include "aliceVision/robustEstimation/LORansac.hpp" -#include "aliceVision/robustEstimation/LORansacKernelAdaptor.hpp" -#include "aliceVision/robustEstimation/ScoreEvaluator.hpp" -#include "aliceVision/robustEstimation/guidedMatching.hpp" -#include "aliceVision/sfmData/SfMData.hpp" -#include "aliceVision/feature/RegionsPerView.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace aliceVision { namespace matchingImageCollection { -//-- A contrario fundamental matrix estimation template functor used for filter pair of putative correspondences -struct GeometricFilterMatrix_F_AC: public GeometricFilterMatrix +/** + * @brief A contrario fundamental matrix estimation template functor used for filter pair of putative correspondences + */ +struct GeometricFilterMatrix_F_AC : public GeometricFilterMatrix { - GeometricFilterMatrix_F_AC( - double dPrecision = std::numeric_limits::infinity(), - size_t iteration = 1024, - robustEstimation::ERobustEstimator estimator = robustEstimation::ERobustEstimator::ACRANSAC) + GeometricFilterMatrix_F_AC(double dPrecision = std::numeric_limits::infinity(), + std::size_t iteration = 1024, + robustEstimation::ERobustEstimator estimator = robustEstimation::ERobustEstimator::ACRANSAC, + bool estimateDistortion = false) : GeometricFilterMatrix(dPrecision, std::numeric_limits::infinity(), iteration) , m_F(Mat3::Identity()) , m_estimator(estimator) + , m_estimateDistortion(estimateDistortion) {} /** @@ -42,36 +49,33 @@ struct GeometricFilterMatrix_F_AC: public GeometricFilterMatrix * relating them using a robust method (like A Contrario Ransac). */ template - EstimationStatus geometricEstimation( - const sfmData::SfMData * sfmData, - const Regions_or_Features_ProviderT & regionsPerView, - const Pair& pairIndex, - const matching::MatchesPerDescType & putativeMatchesPerType, - matching::MatchesPerDescType & out_geometricInliersPerType) + EstimationStatus geometricEstimation(const sfmData::SfMData* sfmData, + const Regions_or_Features_ProviderT& regionsPerView, + const Pair& pairIndex, + const matching::MatchesPerDescType& putativeMatchesPerType, + matching::MatchesPerDescType& out_geometricInliersPerType) { - using namespace aliceVision; - using namespace aliceVision::robustEstimation; out_geometricInliersPerType.clear(); - // Get back corresponding view index - const IndexT iIndex = pairIndex.first; - const IndexT jIndex = pairIndex.second; + // get back corresponding view index + const IndexT I = pairIndex.first; + const IndexT J = pairIndex.second; - const sfmData::View * view_I = sfmData->views.at(iIndex).get(); - const sfmData::View * view_J = sfmData->views.at(jIndex).get(); + const sfmData::View& viewI = sfmData->getView(I); + const sfmData::View& viewJ = sfmData->getView(J); - const camera::IntrinsicBase * cam_I = sfmData->getIntrinsicPtr(view_I->getIntrinsicId()); - const camera::IntrinsicBase * cam_J = sfmData->getIntrinsicPtr(view_J->getIntrinsicId()); + const camera::IntrinsicBase* camI = sfmData->getIntrinsicPtr(viewI.getIntrinsicId()); + const camera::IntrinsicBase* camJ = sfmData->getIntrinsicPtr(viewJ.getIntrinsicId()); - const std::pair imageSizeI(sfmData->getViews().at(iIndex)->getWidth(), sfmData->getViews().at(iIndex)->getHeight()); - const std::pair imageSizeJ(sfmData->getViews().at(jIndex)->getWidth(), sfmData->getViews().at(jIndex)->getHeight()); + const std::pair imageSizeI(viewI.getWidth(), viewI.getHeight()); + const std::pair imageSizeJ(viewJ.getWidth(), viewJ.getHeight()); - return geometricEstimation( - regionsPerView.getDataPerDesc(pairIndex.first), regionsPerView.getDataPerDesc(pairIndex.second), - cam_I, cam_J, - imageSizeI, imageSizeJ, - putativeMatchesPerType, - out_geometricInliersPerType); + return geometricEstimation(regionsPerView.getDataPerDesc(pairIndex.first), + regionsPerView.getDataPerDesc(pairIndex.second), + camI, camJ, + imageSizeI, imageSizeJ, + putativeMatchesPerType, + out_geometricInliersPerType); } /** @@ -79,61 +83,74 @@ struct GeometricFilterMatrix_F_AC: public GeometricFilterMatrix * relating them using a robust method (like A Contrario Ransac). */ template - EstimationStatus geometricEstimation( - const MapFeatOrRegionsPerDesc& region_I, - const MapFeatOrRegionsPerDesc& region_J, - const camera::IntrinsicBase * cam_I, - const camera::IntrinsicBase * cam_J, - const std::pair & imageSizeI, // size of the first image - const std::pair & imageSizeJ, // size of the first image - const matching::MatchesPerDescType & putativeMatchesPerType, - matching::MatchesPerDescType & out_geometricInliersPerType) + EstimationStatus geometricEstimation(const MapFeatOrRegionsPerDesc& regionI, + const MapFeatOrRegionsPerDesc& regionJ, + const camera::IntrinsicBase* camI, + const camera::IntrinsicBase* camJ, + const std::pair& imageSizeI, // size of the first image + const std::pair& imageSizeJ, // size of the second image + const matching::MatchesPerDescType& putativeMatchesPerType, + matching::MatchesPerDescType& out_geometricInliersPerType) { - using namespace aliceVision; - using namespace aliceVision::robustEstimation; out_geometricInliersPerType.clear(); - const std::vector descTypes = getCommonDescTypes(region_I, region_J); + const std::vector descTypes = getCommonDescTypes(regionI, regionJ); if(descTypes.empty()) return EstimationStatus(false, false); - // Retrieve all 2D features as undistorted positions into flat arrays + // retrieve all 2D features as undistorted positions into flat arrays Mat xI, xJ; - fillMatricesWithUndistortFeaturesMatches(putativeMatchesPerType, cam_I, cam_J, - region_I, region_J, - descTypes, xI, xJ); - std::vector inliers; - - std::pair estimationPair = geometricEstimation_Mat( - xI, xJ, - imageSizeI, - imageSizeJ, - inliers); - - if (!estimationPair.first) // estimation is not valid + fillMatricesWithUndistortFeaturesMatches(putativeMatchesPerType, camI, camJ, + regionI, regionJ, + descTypes, xI, xJ); + + std::vector inliers; + std::pair estimationPair; + + switch(m_estimator) + { + case robustEstimation::ERobustEstimator::ACRANSAC: + { + if(m_estimateDistortion) + { + estimationPair = geometricEstimation_Mat_ACRANSAC(xI, xJ, imageSizeI, imageSizeJ, inliers); + } + else + estimationPair = geometricEstimation_Mat_ACRANSAC(xI, xJ, imageSizeI, imageSizeJ, inliers); + } + break; + case robustEstimation::ERobustEstimator::LORANSAC: + { + if(m_estimateDistortion) + throw std::invalid_argument("["+std::string(__func__)+"] Using fundamental matrix and f10 solver with LO_RANSAC is not yet implemented"); + + estimationPair = geometricEstimation_Mat_LORANSAC(xI, xJ, imageSizeI, imageSizeJ, inliers); + } + break; + + default: + throw std::runtime_error("["+std::string(__func__)+"] only ACRansac and LORansac are supported!"); + } + + if(!estimationPair.first) // estimation is not valid { assert(inliers.empty()); return EstimationStatus(false, false); } - // Fill geometricInliersPerType with inliers from putativeMatchesPerType - copyInlierMatches( - inliers, - putativeMatchesPerType, - descTypes, - out_geometricInliersPerType); + // fill geometricInliersPerType with inliers from putativeMatchesPerType + copyInlierMatches(inliers, putativeMatchesPerType, descTypes, out_geometricInliersPerType); - // If matches has strong support - const bool hasStrongSupport = robustEstimation::hasStrongSupport(out_geometricInliersPerType, estimationPair.second); + // have matches has strong support + const bool hasStrongSupport = matching::hasStrongSupport(out_geometricInliersPerType, estimationPair.second); return EstimationStatus(true, hasStrongSupport); } /** * @brief Given two sets of image points, it estimates the fundamental matrix - * relating them using a robust method (like A Contrario Ransac). - * + * For ACRANSAC estimator * @param[in] xI The first set of points * @param[in] xJ The second set of points * @param[in] imageSizeI The size of the first image (used for normalizing the points) @@ -141,84 +158,89 @@ struct GeometricFilterMatrix_F_AC: public GeometricFilterMatrix * @param[out] geometric_inliers A vector containing the indices of the inliers * @return true if geometric_inliers is not empty */ - std::pair geometricEstimation_Mat( - const Mat& xI, // points of the first image - const Mat& xJ, // points of the second image - const std::pair & imageSizeI, // size of the first image - const std::pair & imageSizeJ, // size of the first image - std::vector & out_inliers) + template + std::pair geometricEstimation_Mat_ACRANSAC(const Mat& xI, // points of the first image + const Mat& xJ, // points of the second image + const std::pair& imageSizeI, // size of the first image + const std::pair& imageSizeJ, // size of the first image + std::vector& out_inliers) { - using namespace aliceVision; - using namespace aliceVision::robustEstimation; out_inliers.clear(); - switch(m_estimator) - { - case ERobustEstimator::ACRANSAC: - { - // Define the AContrario adapted Fundamental matrix solver - typedef ACKernelAdaptor< - aliceVision::fundamental::kernel::SevenPointSolver, - aliceVision::fundamental::kernel::SimpleError, - //aliceVision::fundamental::kernel::SymmetricEpipolarDistanceError, - UnnormalizerT, - Mat3> - KernelType; - - const KernelType kernel( - xI, imageSizeI.first, imageSizeI.second, - xJ, imageSizeJ.first, imageSizeJ.second, true); - - // Robustly estimate the Fundamental matrix with A Contrario ransac - const double upper_bound_precision = Square(m_dPrecision); - const std::pair ACRansacOut = - ACRANSAC(kernel, out_inliers, m_stIteration, &m_F, upper_bound_precision); - - if(out_inliers.empty()) - return std::make_pair(false, KernelType::MINIMUM_SAMPLES); - - m_dPrecision_robust = ACRansacOut.first; - - return std::make_pair(true, KernelType::MINIMUM_SAMPLES); - } - case ERobustEstimator::LORANSAC: - { - // just a safeguard - if(m_dPrecision == std::numeric_limits::infinity()) - { - throw std::invalid_argument("[GeometricFilterMatrix_F_AC_AC::geometricEstimation] the threshold of the LORANSAC is set to infinity!"); - } + // define the AContrario adapted Fundamental matrix solver + using KernelT = multiview::RelativePoseKernel< + SolverT_, + multiview::relativePose::FundamentalEpipolarDistanceError, + //multiview::relativePose::FundamentalSymmetricEpipolarDistanceError, + multiview::UnnormalizerT, + ModelT_>; - typedef KernelAdaptorLoRansac< - aliceVision::fundamental::kernel::SevenPointSolver, - aliceVision::fundamental::kernel::SymmetricEpipolarDistanceError, - UnnormalizerT, - Mat3, - aliceVision::fundamental::kernel::EightPointSolver> - KernelType; + const KernelT kernel(xI, imageSizeI.first, imageSizeI.second, + xJ, imageSizeJ.first, imageSizeJ.second, true); - const KernelType kernel(xI, imageSizeI.first, imageSizeI.second, - xJ, imageSizeJ.first, imageSizeJ.second, true); + // robustly estimate the Fundamental matrix with A Contrario ransac + const double upperBoundPrecision = Square(m_dPrecision); - //@fixme scorer should be using the pixel error, not the squared version, refactoring needed - const double normalizedThreshold = Square(m_dPrecision * kernel.normalizer2()(0, 0)); - ScoreEvaluator scorer(normalizedThreshold); + ModelT_ model; + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, out_inliers, m_stIteration, &model, upperBoundPrecision); + m_F = model.getMatrix(); - m_F = LO_RANSAC(kernel, scorer, &out_inliers); + if(out_inliers.empty()) + return std::make_pair(false, kernel.getMinimumNbRequiredSamples()); - if(out_inliers.empty()) - return std::make_pair(false, KernelType::MINIMUM_SAMPLES); + m_dPrecision_robust = ACRansacOut.first; - m_dPrecision_robust = m_dPrecision; + return std::make_pair(true, kernel.getMinimumNbRequiredSamples()); + } - return std::make_pair(true, KernelType::MINIMUM_SAMPLES); - } - default: - throw std::runtime_error("[GeometricFilterMatrix_F_AC_AC::geometricEstimation] only ACRansac and LORansac are supported!"); - } - return std::make_pair(false, 0);; + /** + * @brief Given two sets of image points, it estimates the fundamental matrix + * For LORANSAC estimator + * @param[in] xI The first set of points + * @param[in] xJ The second set of points + * @param[in] imageSizeI The size of the first image (used for normalizing the points) + * @param[in] imageSizeJ The size of the second image + * @param[out] geometric_inliers A vector containing the indices of the inliers + * @return true if geometric_inliers is not empty + */ + template + std::pair geometricEstimation_Mat_LORANSAC(const Mat& xI, // points of the first image + const Mat& xJ, // points of the second image + const std::pair& imageSizeI, // size of the first image + const std::pair& imageSizeJ, // size of the first image + std::vector& out_inliers) + { + out_inliers.clear(); + + // just a safeguard + if(m_dPrecision == std::numeric_limits::infinity()) + throw std::invalid_argument("["+std::string(__func__)+"] the threshold of the LORANSAC is set to infinity!"); + + using KernelT = multiview::RelativePoseKernel< + SolverT_, + multiview::relativePose::FundamentalSymmetricEpipolarDistanceError, + multiview::UnnormalizerT, + robustEstimation::Mat3Model, + SolverLsT_>; + + const KernelT kernel(xI, imageSizeI.first, imageSizeI.second, + xJ, imageSizeJ.first, imageSizeJ.second, true); + + //@fixme scorer should be using the pixel error, not the squared version, refactoring needed + const double normalizedThreshold = Square(m_dPrecision * kernel.normalizer2()(0, 0)); + robustEstimation::ScoreEvaluator scorer(normalizedThreshold); + + robustEstimation::Mat3Model model = robustEstimation::LO_RANSAC(kernel, scorer, &out_inliers); + m_F = model.getMatrix(); + + if(out_inliers.empty()) + return std::make_pair(false, kernel.getMinimumNbRequiredSamples()); + + m_dPrecision_robust = m_dPrecision; + + return std::make_pair(true, kernel.getMinimumNbRequiredSamples()); } - + /** * @brief Geometry_guided_matching * @param sfmData @@ -228,48 +250,48 @@ struct GeometricFilterMatrix_F_AC: public GeometricFilterMatrix * @param matches * @return */ - bool Geometry_guided_matching( - const sfmData::SfMData * sfmData, - const feature::RegionsPerView& regionsPerView, - const Pair imageIdsPair, - const double dDistanceRatio, - matching::MatchesPerDescType & matches) override + bool Geometry_guided_matching(const sfmData::SfMData* sfmData, + const feature::RegionsPerView& regionsPerView, + const Pair imageIdsPair, + const double dDistanceRatio, + matching::MatchesPerDescType& matches) override { if (m_dPrecision_robust != std::numeric_limits::infinity()) { - // Get back corresponding view index - const IndexT viewId_I = imageIdsPair.first; - const IndexT viewId_J = imageIdsPair.second; - - const sfmData::View * view_I = sfmData->views.at(viewId_I).get(); - const sfmData::View * view_J = sfmData->views.at(viewId_J).get(); - - // Retrieve corresponding pair camera intrinsic if any - const camera::IntrinsicBase * cam_I = - sfmData->getIntrinsics().count(view_I->getIntrinsicId()) ? - sfmData->getIntrinsics().at(view_I->getIntrinsicId()).get() : nullptr; - const camera::IntrinsicBase * cam_J = - sfmData->getIntrinsics().count(view_J->getIntrinsicId()) ? - sfmData->getIntrinsics().at(view_J->getIntrinsicId()).get() : nullptr; - - // Check the features correspondences that agree in the geometric and photometric domain - robustEstimation::GuidedMatching( - m_F, - cam_I, // camera::IntrinsicBase - regionsPerView.getAllRegions(viewId_I), // feature::Regions - cam_J, // camera::IntrinsicBase - regionsPerView.getAllRegions(viewId_J), // feature::Regions + // get back corresponding view index + const IndexT I = imageIdsPair.first; + const IndexT J = imageIdsPair.second; + + const sfmData::View& viewI = sfmData->getView(I); + const sfmData::View& viewJ = sfmData->getView(J); + + // retrieve corresponding pair camera intrinsic if any + const camera::IntrinsicBase* camI = sfmData->getIntrinsics().count(viewI.getIntrinsicId()) ? + sfmData->getIntrinsics().at(viewI.getIntrinsicId()).get() : nullptr; + const camera::IntrinsicBase* camJ = sfmData->getIntrinsics().count(viewJ.getIntrinsicId()) ? + sfmData->getIntrinsics().at(viewJ.getIntrinsicId()).get() : nullptr; + + robustEstimation::Mat3Model model(m_F); + + // check the features correspondences that agree in the geometric and photometric domain + matching::guidedMatching( + model, + camI, // camera::IntrinsicBase + regionsPerView.getAllRegions(I), // feature::Regions + camJ, // camera::IntrinsicBase + regionsPerView.getAllRegions(J), // feature::Regions Square(m_dPrecision_robust), Square(dDistanceRatio), matches); } return matches.getNbAllMatches() != 0; } - // - //-- Stored data + + // Stored data + Mat3 m_F; robustEstimation::ERobustEstimator m_estimator; + bool m_estimateDistortion; }; } // namespace matchingImageCollection diff --git a/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_H_AC.hpp b/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_H_AC.hpp index b0181383e4..7d5ba91705 100644 --- a/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_H_AC.hpp +++ b/src/aliceVision/matchingImageCollection/GeometricFilterMatrix_H_AC.hpp @@ -7,26 +7,28 @@ #pragma once -#include "aliceVision/multiview/homographyKernelSolver.hpp" -#include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" -#include "aliceVision/robustEstimation/guidedMatching.hpp" - -#include "aliceVision/matching/IndMatch.hpp" -#include "aliceVision/matching/IndMatchDecorator.hpp" -#include "aliceVision/sfmData/SfMData.hpp" -#include "aliceVision/feature/RegionsPerView.hpp" -#include "aliceVision/matchingImageCollection/GeometricFilterMatrix.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace aliceVision { namespace matchingImageCollection { -//-- A contrario homography matrix estimation template functor used for filter pair of putative correspondences +/** + * @brief A contrario homography matrix estimation template functor used for filter pair of putative correspondences + */ struct GeometricFilterMatrix_H_AC : public GeometricFilterMatrix { - GeometricFilterMatrix_H_AC( - double dPrecision = std::numeric_limits::infinity(), - size_t iteration = 1024) + GeometricFilterMatrix_H_AC(double dPrecision = std::numeric_limits::infinity(), + std::size_t iteration = 1024) : GeometricFilterMatrix(dPrecision, std::numeric_limits::infinity(), iteration) , m_H(Mat3::Identity()) {} @@ -36,136 +38,116 @@ struct GeometricFilterMatrix_H_AC : public GeometricFilterMatrix * relating them using a robust method (like A Contrario Ransac). */ template - EstimationStatus geometricEstimation( - const sfmData::SfMData * sfmData, - const Regions_or_Features_ProviderT& regionsPerView, - const Pair& pairIndex, - const matching::MatchesPerDescType & putativeMatchesPerType, - matching::MatchesPerDescType & out_geometricInliersPerType) + EstimationStatus geometricEstimation(const sfmData::SfMData* sfmData, + const Regions_or_Features_ProviderT& regionsPerView, + const Pair& pairIndex, + const matching::MatchesPerDescType& putativeMatchesPerType, + matching::MatchesPerDescType& out_geometricInliersPerType) { - using namespace aliceVision; - using namespace aliceVision::robustEstimation; out_geometricInliersPerType.clear(); - // Get back corresponding view index - const IndexT iIndex = pairIndex.first; - const IndexT jIndex = pairIndex.second; + // get back corresponding view index + const IndexT I = pairIndex.first; + const IndexT J = pairIndex.second; + + const sfmData::View& viewI = sfmData->getView(I); + const sfmData::View& viewJ = sfmData->getView(J); const std::vector descTypes = regionsPerView.getCommonDescTypes(pairIndex); + if(descTypes.empty()) return EstimationStatus(false, false); - // Retrieve all 2D features as undistorted positions into flat arrays + // retrieve all 2D features as undistorted positions into flat arrays Mat xI, xJ; fillMatricesWithUndistortFeaturesMatches(pairIndex, putativeMatchesPerType, sfmData, regionsPerView, descTypes, xI, xJ); - // Define the AContrario adapted Homography matrix solver - typedef ACKernelAdaptor< - aliceVision::homography::kernel::FourPointSolver, - aliceVision::homography::kernel::AsymmetricError, - UnnormalizerI, - Mat3> - KernelType; + // define the AContrario adapted Homography matrix solver + using KernelT = multiview::RelativePoseKernel< + multiview::relativePose::Homography4PSolver, + multiview::relativePose::HomographyAsymmetricError, + multiview::UnnormalizerI, + robustEstimation::Mat3Model>; - KernelType kernel( - xI, sfmData->getViews().at(iIndex)->getWidth(), sfmData->getViews().at(iIndex)->getHeight(), - xJ, sfmData->getViews().at(jIndex)->getWidth(), sfmData->getViews().at(jIndex)->getHeight(), - false); // configure as point to point error model. + const KernelT kernel(xI, viewI.getWidth(), viewI.getHeight(), + xJ, viewJ.getWidth(), viewJ.getHeight(), false); // configure as point to point error model. - // Robustly estimate the Homography matrix with A Contrario ransac - const double upper_bound_precision = Square(m_dPrecision); + // robustly estimate the Homography matrix with A Contrario ransac + const double upperBoundPrecision = Square(m_dPrecision); - std::vector inliers; - const std::pair ACRansacOut = ACRANSAC(kernel, inliers, m_stIteration, &m_H, upper_bound_precision); + std::vector inliers; + robustEstimation::Mat3Model model; + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, inliers, m_stIteration, &model, upperBoundPrecision); + m_H = model.getMatrix(); if (inliers.empty()) return EstimationStatus(false, false); m_dPrecision_robust = ACRansacOut.first; - // Fill geometricInliersPerType with inliers from putativeMatchesPerType - copyInlierMatches( - inliers, - putativeMatchesPerType, - descTypes, - out_geometricInliersPerType); + // fill geometricInliersPerType with inliers from putativeMatchesPerType + copyInlierMatches(inliers, + putativeMatchesPerType, + descTypes, + out_geometricInliersPerType); - // Check if resection has strong support - const bool hasStrongSupport = robustEstimation::hasStrongSupport(out_geometricInliersPerType, KernelType::MINIMUM_SAMPLES); + // check if resection has strong support + const bool hasStrongSupport = matching::hasStrongSupport(out_geometricInliersPerType, kernel.getMinimumNbRequiredSamples()); return EstimationStatus(true, hasStrongSupport); } /** * @brief Export point feature based vector to a matrix [(x,y)'T, (x,y)'T]. - * Use the camera intrinsics in order to get undistorted pixel coordinates + * Use the camera intrinsics in order to get undistorted pixel coordinates */ - template - static void fillMatricesWithUndistortFeatures( - const camera::IntrinsicBase * cam, - const feature::PointFeatures & vec_feats, - MatT & m) + template + static void fillMatricesWithUndistortFeatures(const camera::IntrinsicBase* cam, + const feature::PointFeatures& vec_feats, + MatT& m) { - using Scalar = typename MatT::Scalar; // Output matrix type + using Scalar = typename MatT::Scalar; // output matrix type const bool hasValidIntrinsics = cam && cam->isValid(); - size_t i = 0; + std::size_t i = 0; - if (hasValidIntrinsics) + if(hasValidIntrinsics) { - for( feature::PointFeatures::const_iterator iter = vec_feats.begin(); - iter != vec_feats.end(); ++iter, ++i) - { + for(feature::PointFeatures::const_iterator iter = vec_feats.begin(); iter != vec_feats.end(); ++iter, ++i) m.col(i) = cam->get_ud_pixel(Vec2(iter->x(), iter->y())); - } } else { - for( feature::PointFeatures::const_iterator iter = vec_feats.begin(); - iter != vec_feats.end(); ++iter, ++i) - { + for(feature::PointFeatures::const_iterator iter = vec_feats.begin(); iter != vec_feats.end(); ++iter, ++i) m.col(i) = iter->coords().cast(); - } } } - template - static void createMatricesWithUndistortFeatures( - const camera::IntrinsicBase * cam, - const feature::MapRegionsPerDesc & regionsPerDesc, - MatT & m) + template + static void createMatricesWithUndistortFeatures(const camera::IntrinsicBase* cam, const feature::MapRegionsPerDesc& regionsPerDesc, MatT& m) { - size_t nbRegions = 0; - for(const auto ®ions: regionsPerDesc) - { + std::size_t nbRegions = 0; + for(const auto& regions: regionsPerDesc) nbRegions += regions.second->RegionCount(); - } + m.resize(2, nbRegions); - size_t y = 0; - for(const auto ®ions: regionsPerDesc) + std::size_t y = 0; + for(const auto& regions: regionsPerDesc) { - fillMatricesWithUndistortFeatures( - cam, - regions.second, - m.block(0, y, 2, regions.second->RegionCount())); + fillMatricesWithUndistortFeatures(cam, regions.second, m.block(0, y, 2, regions.second->RegionCount())); y += regions.second->RegionCount(); } } - template - static void createMatricesWithUndistortFeatures( - const camera::IntrinsicBase * cam, - const feature::PointFeatures & vec_feats, - MatT & m) + template + static void createMatricesWithUndistortFeatures(const camera::IntrinsicBase* cam, + const feature::PointFeatures& vec_feats, + MatT& m) { - size_t nbRegions = vec_feats.size(); + std::size_t nbRegions = vec_feats.size(); m.resize(2, nbRegions); - - fillMatricesWithUndistortFeatures( - cam, - vec_feats, - m); + fillMatricesWithUndistortFeatures(cam, vec_feats, m); } /** @@ -177,57 +159,54 @@ struct GeometricFilterMatrix_H_AC : public GeometricFilterMatrix * @param matches * @return */ - bool Geometry_guided_matching - ( - const sfmData::SfMData * sfmData, - const feature::RegionsPerView& regionsPerView, - const Pair imageIdsPair, - const double dDistanceRatio, - matching::MatchesPerDescType & matches) override + bool Geometry_guided_matching(const sfmData::SfMData* sfmData, + const feature::RegionsPerView& regionsPerView, + const Pair imageIdsPair, + const double dDistanceRatio, + matching::MatchesPerDescType& matches) override { - if (m_dPrecision_robust != std::numeric_limits::infinity()) + if(m_dPrecision_robust != std::numeric_limits::infinity()) { const std::vector descTypes = regionsPerView.getCommonDescTypes(imageIdsPair); + if(descTypes.empty()) return false; - // Get back corresponding view index - const IndexT viewId_I = imageIdsPair.first; - const IndexT viewId_J = imageIdsPair.second; + // get back corresponding view index + const IndexT I = imageIdsPair.first; + const IndexT J = imageIdsPair.second; + + const sfmData::View& viewI = sfmData->getView(I); + const sfmData::View& viewJ = sfmData->getView(J); - const sfmData::View * view_I = sfmData->views.at(viewId_I).get(); - const sfmData::View * view_J = sfmData->views.at(viewId_J).get(); + // retrieve corresponding pair camera intrinsic if any + const camera::IntrinsicBase* camI = sfmData->getIntrinsics().count(viewI.getIntrinsicId()) ? + sfmData->getIntrinsics().at(viewI.getIntrinsicId()).get() : nullptr; + const camera::IntrinsicBase* camJ = sfmData->getIntrinsics().count(viewJ.getIntrinsicId()) ? + sfmData->getIntrinsics().at(viewJ.getIntrinsicId()).get() : nullptr; - // Retrieve corresponding pair camera intrinsic if any - const camera::IntrinsicBase * cam_I = - sfmData->getIntrinsics().count(view_I->getIntrinsicId()) ? - sfmData->getIntrinsics().at(view_I->getIntrinsicId()).get() : nullptr; - const camera::IntrinsicBase * cam_J = - sfmData->getIntrinsics().count(view_J->getIntrinsicId()) ? - sfmData->getIntrinsics().at(view_J->getIntrinsicId()).get() : nullptr; + robustEstimation::Mat3Model model(m_H); - if (dDistanceRatio < 0) + if(dDistanceRatio < 0) { - for(const feature::EImageDescriberType descType: descTypes) + for(const feature::EImageDescriberType descType : descTypes) { assert(descType != feature::EImageDescriberType::UNINITIALIZED); matching::IndMatches localMatches; - const feature::Regions& regions_I = regionsPerView.getRegions(viewId_I, descType); - const feature::Regions& regions_J = regionsPerView.getRegions(viewId_J, descType); - const feature::PointFeatures pointsFeaturesI = regions_I.GetRegionsPositions(); - const feature::PointFeatures pointsFeaturesJ = regions_J.GetRegionsPositions(); + const feature::Regions& regionsI = regionsPerView.getRegions(I, descType); + const feature::Regions& regionsJ = regionsPerView.getRegions(J, descType); + const feature::PointFeatures pointsFeaturesI = regionsI.GetRegionsPositions(); + const feature::PointFeatures pointsFeaturesJ = regionsJ.GetRegionsPositions(); - // Filtering based only on region positions + // filtering based only on region positions Mat xI, xJ; - createMatricesWithUndistortFeatures(cam_I, pointsFeaturesI, xI); - createMatricesWithUndistortFeatures(cam_J, pointsFeaturesJ, xJ); + createMatricesWithUndistortFeatures(camI, pointsFeaturesI, xI); + createMatricesWithUndistortFeatures(camJ, pointsFeaturesJ, xJ); - robustEstimation::GuidedMatching - ( - m_H, xI, xJ, Square(m_dPrecision_robust), localMatches); + matching::guidedMatching(model, xI, xJ, Square(m_dPrecision_robust), localMatches); - // Remove matches that have the same (X,Y) coordinates + // remove matches that have the same (X,Y) coordinates matching::IndMatchDecorator matchDeduplicator(localMatches, pointsFeaturesI, pointsFeaturesJ); matchDeduplicator.getDeduplicated(localMatches); matches[descType] = localMatches; @@ -235,21 +214,19 @@ struct GeometricFilterMatrix_H_AC : public GeometricFilterMatrix } else { - // Filtering based on region positions and regions descriptors - robustEstimation::GuidedMatching - ( - m_H, - cam_I, regionsPerView.getAllRegions(viewId_I), - cam_J, regionsPerView.getAllRegions(viewId_J), - Square(m_dPrecision_robust), Square(dDistanceRatio), - matches); + // filtering based on region positions and regions descriptors + matching::guidedMatching(model, + camI, regionsPerView.getAllRegions(I), + camJ, regionsPerView.getAllRegions(J), + Square(m_dPrecision_robust), Square(dDistanceRatio), + matches); } } + return matches.getNbAllMatches() != 0; } - // - //-- Stored data + // stored data Mat3 m_H; }; diff --git a/src/aliceVision/matchingImageCollection/GeometricFilterType.hpp b/src/aliceVision/matchingImageCollection/GeometricFilterType.hpp index 819401d8be..dcb1ddc6a1 100644 --- a/src/aliceVision/matchingImageCollection/GeometricFilterType.hpp +++ b/src/aliceVision/matchingImageCollection/GeometricFilterType.hpp @@ -18,6 +18,7 @@ enum class EGeometricFilterType { NO_FILTERING , FUNDAMENTAL_MATRIX + , FUNDAMENTAL_WITH_DISTORTION , ESSENTIAL_MATRIX , HOMOGRAPHY_MATRIX , HOMOGRAPHY_GROWING @@ -30,11 +31,12 @@ enum class EGeometricFilterType inline std::string EGeometricFilterType_informations() { return "Geometric filter type: Pairwise correspondences filtering thanks to robust model estimation:\n" - "* no_filtering: no geometric filtering\n" - "* fundamental_matrix: fundamental matrix\n" - "* essential_matrix: essential matrix\n" - "* homography_matrix: homography matrix\n" - "* homography_growing: multiple homography matrices [F.Srajer, 2016]\n"; + "* no_filtering: no geometric filtering\n" + "* fundamental_matrix: fundamental matrix\n" + "* fundamental_with_distortion: fundamental matrix with F10 solver [Z.Kukelova, 2015]\n" + "* essential_matrix: essential matrix\n" + "* homography_matrix: homography matrix\n" + "* homography_growing: multiple homography matrices [F.Srajer, 2016]\n"; } /** @@ -46,11 +48,12 @@ inline std::string EGeometricFilterType_enumToString(EGeometricFilterType geomet { switch(geometricFilterType) { - case EGeometricFilterType::NO_FILTERING: return "no_filtering"; - case EGeometricFilterType::FUNDAMENTAL_MATRIX: return "fundamental_matrix"; - case EGeometricFilterType::ESSENTIAL_MATRIX: return "essential_matrix"; - case EGeometricFilterType::HOMOGRAPHY_MATRIX: return "homography_matrix"; - case EGeometricFilterType::HOMOGRAPHY_GROWING: return "homography_growing"; + case EGeometricFilterType::NO_FILTERING: return "no_filtering"; + case EGeometricFilterType::FUNDAMENTAL_MATRIX: return "fundamental_matrix"; + case EGeometricFilterType::FUNDAMENTAL_WITH_DISTORTION: return "fundamental_with_distortion"; + case EGeometricFilterType::ESSENTIAL_MATRIX: return "essential_matrix"; + case EGeometricFilterType::HOMOGRAPHY_MATRIX: return "homography_matrix"; + case EGeometricFilterType::HOMOGRAPHY_GROWING: return "homography_growing"; } throw std::out_of_range("Invalid geometricFilterType enum"); } @@ -65,11 +68,12 @@ inline EGeometricFilterType EGeometricFilterType_stringToEnum(const std::string& std::string model = geometricFilterType; std::transform(model.begin(), model.end(), model.begin(), ::tolower); //tolower - if(model == "no_filtering") return EGeometricFilterType::NO_FILTERING; - if(model == "fundamental_matrix") return EGeometricFilterType::FUNDAMENTAL_MATRIX; - if(model == "essential_matrix") return EGeometricFilterType::ESSENTIAL_MATRIX; - if(model == "homography_matrix") return EGeometricFilterType::HOMOGRAPHY_MATRIX; - if(model == "homography_growing") return EGeometricFilterType::HOMOGRAPHY_GROWING; + if(model == "no_filtering") return EGeometricFilterType::NO_FILTERING; + if(model == "fundamental_matrix") return EGeometricFilterType::FUNDAMENTAL_MATRIX; + if(model == "fundamental_with_distortion") return EGeometricFilterType::FUNDAMENTAL_WITH_DISTORTION; + if(model == "essential_matrix") return EGeometricFilterType::ESSENTIAL_MATRIX; + if(model == "homography_matrix") return EGeometricFilterType::HOMOGRAPHY_MATRIX; + if(model == "homography_growing") return EGeometricFilterType::HOMOGRAPHY_GROWING; throw std::out_of_range("Invalid geometricFilterType: " + geometricFilterType); } diff --git a/src/aliceVision/multiview/AngularRadianErrorKernel.hpp b/src/aliceVision/multiview/AngularRadianErrorKernel.hpp new file mode 100644 index 0000000000..528d2a1e65 --- /dev/null +++ b/src/aliceVision/multiview/AngularRadianErrorKernel.hpp @@ -0,0 +1,71 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 + +/** + * @brief Collection of kernel. + * @ref [1] "Robust and accurate calibration of camera networks". PhD. Pierre MOULON + * + * - AngularRadianErrorKernel: essential matrix estimation between spherical camera + * + * Mainly it add correct data normalization and define the function required + * by the generic ACRANSAC / LORANSAC routine. + */ + +namespace aliceVision { +namespace multiview { + +/** + * @brief Two view Kernel adapter for the A contrario (AC) model estimator. + * Specialization to handle radian angular residual error. + */ +template +class AngularRadianErrorKernel + : public robustEstimation::PointFittingRansacKernel> +{ +public: + + using KernelBase = robustEstimation::PointFittingRansacKernel>; + + AngularRadianErrorKernel(const Mat& x1, const Mat& x2) + : KernelBase(x1, x2) + , _logalpha0(log10(1.0 / 2.0)) + { + assert(3 == x1.rows()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + } + + inline double error(std::size_t sample, const ModelT_& model) const override + { + return Square(KernelBase::error(sample, model)); + } + + void unnormalize(ModelT_& model) const override + { + // do nothing, no normalization in the angular case + } + + double logalpha0() const override {return _logalpha0; } + double multError() const override {return 1./4.;} + Mat3 normalizer1() const override {return Mat3::Identity();} + Mat3 normalizer2() const override {return Mat3::Identity();} + double unormalizeError(double val) const override {return sqrt(val);} + +protected: + /// Alpha0 is used to make the error adaptive to the image size + double _logalpha0; +}; + +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/CMakeLists.txt b/src/aliceVision/multiview/CMakeLists.txt index a1e340401f..f31b468213 100644 --- a/src/aliceVision/multiview/CMakeLists.txt +++ b/src/aliceVision/multiview/CMakeLists.txt @@ -1,19 +1,34 @@ # Headers set(multiview_files_headers affineSolver.hpp - conditioning.hpp essential.hpp - projection.hpp - essentialFivePointSolver.hpp - essentialKernelSolver.hpp - fundamentalKernelSolver.hpp - homographyKernelSolver.hpp + epipolarEquation.hpp knownRotationTranslationKernel.hpp - twoViewKernel.hpp + Unnormalizer.hpp + AngularRadianErrorKernel.hpp + RelativePoseKernel.hpp + ResectionKernel.hpp + relativePose/Essential5PSolver.hpp + relativePose/Essential8PSolver.hpp + relativePose/EssentialKernel.hpp + relativePose/Fundamental7PSolver.hpp + relativePose/Fundamental8PSolver.hpp + relativePose/Fundamental10PSolver.hpp + relativePose/FundamentalError.hpp + relativePose/FundamentalKernel.hpp + relativePose/Homography4PSolver.hpp + relativePose/HomographyError.hpp + relativePose/HomographyKernel.hpp + relativePose/ISolverErrorRelativePose.hpp + resection/EPnPSolver.hpp + resection/EPnPKernel.hpp resection/P3PSolver.hpp resection/P4PfSolver.hpp resection/P5PfrSolver.hpp + resection/ProjectionDistanceError.hpp + resection/Resection6PSolver.hpp resection/ResectionKernel.hpp + resection/ISolverErrorResection.hpp rotationAveraging/common.hpp rotationAveraging/rotationAveraging.hpp rotationAveraging/l1.hpp @@ -28,17 +43,19 @@ set(multiview_files_headers # Sources set(multiview_files_sources affineSolver.cpp - conditioning.cpp essential.cpp - projection.cpp - essentialFivePointSolver.cpp - essentialKernelSolver.cpp - fundamentalKernelSolver.cpp - homographyKernelSolver.cpp - resection/ResectionKernel.cpp + Unnormalizer.cpp + relativePose/Essential5PSolver.cpp + relativePose/Essential8PSolver.cpp + relativePose/Fundamental7PSolver.cpp + relativePose/Fundamental8PSolver.cpp + relativePose/Fundamental10PSolver.cpp + relativePose/Homography4PSolver.cpp + resection/EPnPSolver.cpp resection/P3PSolver.cpp resection/P4PfSolver.cpp resection/P5PfrSolver.cpp + resection/Resection6PSolver.cpp rotationAveraging/l1.cpp rotationAveraging/l2.cpp translationAveraging/solverL2Chordal.cpp @@ -73,15 +90,11 @@ alicevision_add_library(aliceVision_multiview_test_data ) # Unit tests +add_subdirectory(relativePose) add_subdirectory(resection) add_subdirectory(rotationAveraging) add_subdirectory(translationAveraging) add_subdirectory(triangulation) alicevision_add_test(affineSolver_test.cpp NAME "multiview_affineSolver" LINKS aliceVision_multiview) -alicevision_add_test(fundamentalKernelSolver_test.cpp NAME "multiview_fundamentalKernelSolver" LINKS aliceVision_multiview) -alicevision_add_test(essentialFivePointSolver_test.cpp NAME "multiview_essentialFivePointSolver" LINKS aliceVision_multiview aliceVision_multiview_test_data) -alicevision_add_test(essentialKernelSolver_test.cpp NAME "multiview_essentialKernelSolver" LINKS aliceVision_multiview aliceVision_multiview_test_data) -alicevision_add_test(homographyKernelSolver_test.cpp NAME "multiview_homographyKernelSolver" LINKS aliceVision_multiview aliceVision_multiview_test_data) alicevision_add_test(knownRotationTranslationKernel_test.cpp NAME "multiview_knownRotationTranslationKernel" LINKS aliceVision_multiview aliceVision_multiview_test_data) - diff --git a/src/aliceVision/multiview/NViewDataSet.cpp b/src/aliceVision/multiview/NViewDataSet.cpp index e3c0893f81..59c785e241 100644 --- a/src/aliceVision/multiview/NViewDataSet.cpp +++ b/src/aliceVision/multiview/NViewDataSet.cpp @@ -8,7 +8,7 @@ #include "NViewDataSet.hpp" #include -#include +#include #include #include @@ -23,7 +23,7 @@ NViewDatasetConfigurator::NViewDatasetConfigurator(int fx, int fy, {} NViewDataSet NRealisticCamerasRing(size_t nviews, size_t npoints, - const NViewDatasetConfigurator config) + const NViewDatasetConfigurator& config) { //-- Setup a camera circle rig. NViewDataSet d; @@ -61,7 +61,7 @@ NViewDataSet NRealisticCamerasRing(size_t nviews, size_t npoints, 0, 0, 1; d._R[i] = LookAt(lookdir); // Y axis UP d._t[i] = -d._R[i] * camera_center; // [t]=[-RC] Cf HZ. - d._x[i] = Project(d.P(i), d._X); + d._x[i] = project(d.P(i), d._X); d._x_ids[i] = all_point_ids; } return d; @@ -70,11 +70,11 @@ NViewDataSet NRealisticCamerasRing(size_t nviews, size_t npoints, Mat34 NViewDataSet::P(size_t i)const { assert(i < _n); Mat34 P; - P_From_KRt(_K[i], _R[i], _t[i], &P); + P_from_KRt(_K[i], _R[i], _t[i], &P); return P; } -void NViewDataSet::ExportToPLY( +void NViewDataSet::exportToPLY( const std::string & out_file_name)const { std::ofstream outfile; outfile.open(out_file_name.c_str(), std::ios_base::out); @@ -119,7 +119,7 @@ void NViewDataSet::ExportToPLY( } NViewDataSet NRealisticCamerasCardioid(size_t nviews, size_t npoints, - const NViewDatasetConfigurator config) + const NViewDatasetConfigurator& config) { //-- Setup a camera circle rig. NViewDataSet d; @@ -160,7 +160,7 @@ NViewDataSet NRealisticCamerasCardioid(size_t nviews, size_t npoints, 0, 0, 1; d._R[i] = LookAt(lookdir); // Y axis UP d._t[i] = -d._R[i] * camera_center; // [t]=[-RC] Cf HZ. - d._x[i] = Project(d.P(i), d._X); + d._x[i] = project(d.P(i), d._X); d._x_ids[i] = all_point_ids; } return d; diff --git a/src/aliceVision/multiview/NViewDataSet.hpp b/src/aliceVision/multiview/NViewDataSet.hpp index ed6c6d8edc..9137284127 100644 --- a/src/aliceVision/multiview/NViewDataSet.hpp +++ b/src/aliceVision/multiview/NViewDataSet.hpp @@ -14,24 +14,41 @@ namespace aliceVision { -// A N-view metric dataset. -// All points are seen by all cameras. -struct NViewDataSet { - std::vector _K; // Internal parameters (fx, fy, etc). - std::vector _R; // Rotation. - std::vector _t; // Translation. - std::vector _C; // Camera centers. - Mat3X _X; // 3D points. - std::vector _x; // Projected points; may have noise added. - std::vector _x_ids;// Indexes of points corresponding to the projections - - std::size_t _n; // Actual number of cameras. - - //-- Return P=K*[R|t] for the Inth camera +/** + * @brief A N-view metric dataset. + * All points are seen by all cameras. + */ +struct NViewDataSet +{ + /// Internal parameters (fx, fy, etc). + std::vector _K; + /// Rotation. + std::vector _R; + /// Translation. + std::vector _t; + /// Camera centers. + std::vector _C; + /// 3D points. + Mat3X _X; + /// Projected points; may have noise added. + std::vector _x; + /// Indexes of points corresponding to the projections + std::vector _x_ids; + /// Actual number of cameras. + std::size_t _n; + + /** + * @brief Return P=K*[R|t] for the Inth camera + * @param[in] i The Inth camera + * @return P=K*[R|t] for the Inth camera + */ Mat34 P(size_t i) const; - /// Export in PLY the point structure and camera and camera looking dir. - void ExportToPLY(const std::string & out_file_name) const; + /** + * @brief Export in PLY the point structure and camera and camera looking dir. + * @param[in] outFilename + */ + void exportToPLY(const std::string& outFilename) const; }; struct NViewDatasetConfigurator @@ -49,14 +66,14 @@ struct NViewDatasetConfigurator double jitter_amount = 0.01 ); }; -/// Place cameras on a circle with point in the center -NViewDataSet NRealisticCamerasRing(std::size_t nviews, std::size_t npoints, - const NViewDatasetConfigurator - config = NViewDatasetConfigurator()); +/** + * @brief Place cameras on a circle with point in the center + */ +NViewDataSet NRealisticCamerasRing(std::size_t nviews, std::size_t npoints, const NViewDatasetConfigurator& config = NViewDatasetConfigurator()); -/// Place cameras on cardiod shape with point in the center -NViewDataSet NRealisticCamerasCardioid(std::size_t nviews, std::size_t npoints, - const NViewDatasetConfigurator - config = NViewDatasetConfigurator()); +/** + * @brief Place cameras on cardiod shape with point in the center + */ +NViewDataSet NRealisticCamerasCardioid(std::size_t nviews, std::size_t npoints, const NViewDatasetConfigurator& config = NViewDatasetConfigurator()); } // namespace aliceVision diff --git a/src/aliceVision/multiview/RelativePoseKernel.hpp b/src/aliceVision/multiview/RelativePoseKernel.hpp new file mode 100644 index 0000000000..b78d65a095 --- /dev/null +++ b/src/aliceVision/multiview/RelativePoseKernel.hpp @@ -0,0 +1,180 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 multiview { + +/** + * @brief Two view Kernel adapter for the A contrario (AC) model estimator. + * With unknown intrinsic. + * + * @ref [1] "Robust and accurate calibration of camera networks". PhD. Pierre MOULON + * + * @note Handle data normalization and compute the corresponding logalpha 0 + * that depends of the error model (point to line, or point to point) + * + * @note This kernel adapter is working for affine, homography, fundamental matrix + * estimation. + */ +template> +class RelativePoseKernel + : public robustEstimation::PointFittingRansacKernel +{ +public: + + using KernelBase = robustEstimation::PointFittingRansacKernel; + + RelativePoseKernel(const Mat& x1, int w1, int h1, + const Mat& x2, int w2, int h2, + bool pointToLine = true) + : _x1(x1.rows(), x1.cols()) + , _x2(x2.rows(), x2.cols()) + , KernelBase(_x1, _x2) + , _logalpha0(0.0) + , _N1(3, 3) + , _N2(3, 3) + , _pointToLine(pointToLine) + { + assert(2 == x1.rows()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + robustEstimation::normalizePointsFromImageSize(x1, &_x1, &_N1, w1, h1); + robustEstimation::normalizePointsFromImageSize(x2, &_x2, &_N2, w2, h2); + + // logAlpha0 is used to make error data scale invariant + if(pointToLine) + { + // Ratio of containing diagonal image rectangle over image area + const double D = sqrt(w2 * (double) w2 + h2 * (double) h2); // diameter + const double A = w2 * (double) h2; // area + _logalpha0 = log10(2.0 * D / A / _N2(0, 0)); + } + else + { + // ratio of area : unit circle over image area + _logalpha0 = log10(M_PI / (w2 * (double) h2) / (_N2(0, 0) * _N2(0, 0))); + } + } + + void unnormalize(ModelT_& model) const override + { + // Unnormalize model from the computed conditioning. + Mat3 H = model.getMatrix(); + UnnormalizerT_::unnormalize(_N1, _N2, &H); + model.setMatrix(H); + } + + double logalpha0() const override {return _logalpha0; } + double multError() const override {return (_pointToLine)? 0.5 : 1.0;} + Mat3 normalizer1() const override {return _N1;} + Mat3 normalizer2() const override {return _N2;} + double unormalizeError(double val) const override {return sqrt(val) / _N2(0,0);} + +protected: + /// Normalized input data + Mat _x1, _x2; + /// Matrix used to normalize data + Mat3 _N1, _N2; + /// Alpha0 is used to make the error adaptive to the image size + double _logalpha0; + /// Store if error model is pointToLine or point to point + bool _pointToLine; +}; + +/** + * @brief Two view Kernel adapter for the A contrario (AC) model estimator. + * With known intrinsic. + * + * @ref [1] "Robust and accurate calibration of camera networks". PhD. Pierre MOULON + * + * @note Handle data normalization and compute the corresponding logalpha 0 + * that depends of the error model (point to line, or point to point) + * + * @note This kernel adapter is working only for essential matrix + * estimation. + */ +template> +class RelativePoseKernel_K + : public robustEstimation::PointFittingRansacKernel +{ +public: + + using KernelBase = robustEstimation::PointFittingRansacKernel; + + RelativePoseKernel_K(const Mat& x1, int w1, int h1, + const Mat& x2, int w2, int h2, + const Mat3& K1, const Mat3& K2) + : KernelBase(x1, x2) + , _N1(Mat3::Identity()) + , _N2(Mat3::Identity()) + , _logalpha0(0.0) + , _K1(K1) + , _K2(K2) + { + assert(2 == x1.rows()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + robustEstimation::applyTransformationToPoints(x1, _K1.inverse(), &_x1k); + robustEstimation::applyTransformationToPoints(x2, _K2.inverse(), &_x2k); + + // point to line probability (line is the epipolar line) + const double D = sqrt(w2 * static_cast(w2) + h2 * static_cast(h2)); // diameter + const double A = w2 * static_cast(h2); // area + _logalpha0 = log10(2.0 * D / A * .5); + } + + void fit(const std::vector& samples, std::vector& models) const override + { + const Mat x1 = ExtractColumns(_x1k, samples); + const Mat x2 = ExtractColumns(_x2k, samples); + + KernelBase::KernelBase::_kernelSolver.solve(x1, x2, models); + } + + double error(std::size_t sample, const ModelT_& model) const override + { + Mat3 F; + fundamentalFromEssential(model.getMatrix(), _K1, _K2, &F); + const ModelT_ modelF(F); + return _errorEstimator.error(modelF, KernelBase::_x1.col(sample), KernelBase::_x2.col(sample)); + } + + void unnormalize(ModelT_& model) const override + { + // do nothing, no normalization in this case + } + + double logalpha0() const override {return _logalpha0; } + double multError() const override {return 0.5;} // point to line error + Mat3 normalizer1() const override {return _N1;} + Mat3 normalizer2() const override {return _N2;} + double unormalizeError(double val) const override { return val; } + +private: + + Mat _x1k, _x2k; + /// Matrix used to normalize data + Mat3 _N1, _N2; + /// Alpha0 is used to make the error adaptive to the image size + double _logalpha0; + /// Intrinsics camera parameter + Mat3 _K1, _K2; + /// solver error estimation + const ErrorT_ _errorEstimator; +}; + +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/ResectionKernel.hpp b/src/aliceVision/multiview/ResectionKernel.hpp new file mode 100644 index 0000000000..ee785949b7 --- /dev/null +++ b/src/aliceVision/multiview/ResectionKernel.hpp @@ -0,0 +1,142 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 multiview { + +/** + * @brief Pose/Resection Kernel with unknown intrinsic for the A contrario (AC) model estimator. + * to be used with the ACRANSAC / LORANSAC framework. + * + * @ref [1] "Robust and accurate calibration of camera networks". PhD. Pierre MOULON + */ +template> +class ResectionKernel + : public robustEstimation::PointFittingRansacKernel +{ +public: + + using KernelBase = robustEstimation::PointFittingRansacKernel; + + /** + * @brief ResectionKernel constructor with unknown intrinsic + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d 3d points in the second image. One per column. + * @param[in] K intrinsic camera parameter + */ + ResectionKernel(const Mat& x2d, const Mat& x3d, int w, int h) + : _x2d(x2d.rows(), x2d.cols()) + , KernelBase(_x2d, x3d) + , _logalpha0(log10(M_PI)) + , _N1(3, 3) + { + assert(2 == x2d.rows()); + assert(3 == x3d.rows()); + assert(x2d.cols() == x3d.cols()); + + robustEstimation::normalizePointsFromImageSize(x2d, &_x2d, &_N1, w, h); + } + + void unnormalize(ModelT_& model) const override + { + // unnormalize model from the computed conditioning. + Mat34 P = model.getMatrix(); + UnnormalizerT_::unnormalize(_N1, Mat3::Identity(), &P); + model.setMatrix(P); + } + + double logalpha0() const override {return _logalpha0; } + double multError() const override {return 1.0;} // point to point error + Mat3 normalizer1() const override {return Mat3::Identity();} + Mat3 normalizer2() const override {return _N1;} + double unormalizeError(double val) const override {return sqrt(val) / _N1(0,0);} + +protected: + /// Normalized input data + Mat _x2d; + /// Matrix used to normalize data + Mat3 _N1; + /// Alpha0 is used to make the error adaptive to the image size + double _logalpha0; +}; + + +/** + * @brief The kernel for the resection with known intrinsics (PnP) to be used with + * the ACRANSAC / LORANSAC framework. + * + * @ref [1] "Robust and accurate calibration of camera networks". PhD. Pierre MOULON + * + * @tparam SolverT The minimal solver able to find a solution from a minimum set of points, usually any PnP solver. + * @tparam ErrorT The functor computing the error for each data sample with respect to the estimated model, usually a reprojection error functor. + * @tparam UnnormalizerT The functor used to normalize the data before the estimation of the model, usually a functor that normalize the point in camera + * coordinates (ie multiply by the inverse of the calibration matrix). + * @tparam SolverLsT = SolverT The least square solver that is used to find a solution from any set of data larger than the minimum required, + * usually the 6 point algorithm which solves the resection problem by means of LS. + * @tparam ModelT = Mat34Model The type of the model to estimate, the projection matrix. + */ +template> +class ResectionKernel_K + : public robustEstimation::PointFittingRansacKernel +{ +public: + + using KernelBase = robustEstimation::PointFittingRansacKernel; + + /** + * @brief ResectionKernel constructor with known intrinsic + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d 3d points in the second image. One per column. + * @param[in] K intrinsic camera parameter + */ + ResectionKernel_K(const Mat& x2d, const Mat& x3d, const Mat3& K) + : _x2d(x2d.rows(), x2d.cols()) + , KernelBase(_x2d, x3d) + , _logalpha0(log10(M_PI)) + , _N1(K.inverse()) + , _K(K) + { + assert(2 == x2d.rows()); + assert(3 == x3d.rows()); + assert(x2d.cols() == x3d.cols()); + + // normalize points by inverse K + robustEstimation::applyTransformationToPoints(x2d, _N1, &_x2d); + } + + void unnormalize(ModelT_& model) const override + { + // unnormalize model from the computed conditioning. + model.setMatrix(_K * model.getMatrix()); + } + + double logalpha0() const override {return _logalpha0; } + double multError() const override {return 1.0;} // point to point error + Mat3 normalizer1() const override {return Mat3::Identity();} + Mat3 normalizer2() const override {return _N1;} + double unormalizeError(double val) const override {return sqrt(val) / _N1(0,0);} + +protected: + /// Normalized input data + Mat _x2d; + /// Matrix used to normalize data + Mat3 _N1; + /// Alpha0 is used to make the error adaptive to the image size + double _logalpha0; + /// intrinsic camera parameter + Mat3 _K; +}; + +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/Unnormalizer.cpp b/src/aliceVision/multiview/Unnormalizer.cpp new file mode 100644 index 0000000000..30135227b2 --- /dev/null +++ b/src/aliceVision/multiview/Unnormalizer.cpp @@ -0,0 +1,32 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 "Unnormalizer.hpp" + +namespace aliceVision { +namespace multiview { + +// Denormalize the results. See HZ page 109. +void UnnormalizerT::unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) +{ + *H = T2.transpose() * (*H) * T1; +} + +// Denormalize the results. See HZ page 109. +void UnnormalizerI::unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) +{ + *H = T2.inverse() * (*H) * T1; +} + +void UnnormalizerResection::unnormalize(const Mat &T, const Mat &U, Mat34 *P) +{ + *P = T.inverse() * (*P); +} + +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/Unnormalizer.hpp b/src/aliceVision/multiview/Unnormalizer.hpp new file mode 100644 index 0000000000..c9f988b6c2 --- /dev/null +++ b/src/aliceVision/multiview/Unnormalizer.hpp @@ -0,0 +1,49 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { + +/** + * @brief Unnormalize using Inverse + */ +struct UnnormalizerI +{ + /** + * @brief Denormalize the results. + * @see HZ page 109. + */ + static void unnormalize(const Mat3& T1, const Mat3& T2, Mat3* H); +}; + +/** + * @brief Unnormalize using Transpose + */ +struct UnnormalizerT +{ + /** + * @brief Denormalize the results. + * @see HZ page 109. + */ + static void unnormalize(const Mat3& T1, const Mat3& T2, Mat3* H); +}; + +/** + * @brief Unnormalize for resection + */ +struct UnnormalizerResection +{ + static void unnormalize(const Mat& T, const Mat& U, Mat34* P); +}; + +} //namespace multiview +} //namespace aliceVision diff --git a/src/aliceVision/multiview/affineSolver.cpp b/src/aliceVision/multiview/affineSolver.cpp index dec9c3ec64..e2dd93ac82 100644 --- a/src/aliceVision/multiview/affineSolver.cpp +++ b/src/aliceVision/multiview/affineSolver.cpp @@ -9,6 +9,7 @@ #include "affineSolver.hpp" namespace aliceVision { +namespace multiview { // Parametrization // a b x @@ -22,7 +23,7 @@ namespace aliceVision { // | d | // | x | // | y | -bool Affine2DFromCorrespondencesLinear(const Mat &x1, const Mat &x2, +bool affine2DFromCorrespondencesLinear(const Mat &x1, const Mat &x2, Mat3 *M, double expected_precision) { assert(2 == x1.rows()); @@ -77,7 +78,7 @@ bool Affine2DFromCorrespondencesLinear(const Mat &x1, const Mat &x2, // | x | // | y | // | z | -bool Affine3DFromCorrespondencesLinear(const Mat &x1, +bool affine3DFromCorrespondencesLinear(const Mat &x1, const Mat &x2, Mat4 *M, double expected_precision) { @@ -124,4 +125,7 @@ bool Affine3DFromCorrespondencesLinear(const Mat &x1, return false; } } + +} // namespace multiview } // namespace aliceVision + diff --git a/src/aliceVision/multiview/affineSolver.hpp b/src/aliceVision/multiview/affineSolver.hpp index af93714dde..c217973b29 100644 --- a/src/aliceVision/multiview/affineSolver.hpp +++ b/src/aliceVision/multiview/affineSolver.hpp @@ -11,59 +11,55 @@ #include namespace aliceVision { +namespace multiview { -/** 2D Affine transformation estimation +/** * - * This function can be used in order to estimate the affine transformation - * between two sets of points with known 2D correspondences. + * @brief 2D Affine transformation estimation + * This function can be used in order to estimate the affine transformation + * between two sets of points with known 2D correspondences. * - * \param[in] x1 The first 2xN matrix of euclidean points - * \param[in] x2 The second 2xN matrix of euclidean points - * \param[out] M The 3x3 affine transformation matrix (6 dof) + * @param[in] x1 The first 2xN matrix of euclidean points + * @param[in] x2 The second 2xN matrix of euclidean points + * @param[out] M The 3x3 affine transformation matrix (6 dof) * with the following parametrization * |a b tx| * M = |c d ty| * |0 0 1 | * such that x2 = M * x1 - * \param[in] expected_precision The expected precision in order for instance + * @param[in] expected_precision The expected precision in order for instance * to accept almost affine matrices. * - * \return true if the transformation estimation has succeeded + * @return true if the transformation estimation has succeeded * - * \note Need at least 3 non aligned points - * \note Points coordinates must be normalized (euclidean) + * @note Need at least 3 non aligned points + * @note Points coordinates must be normalized (euclidean) */ -bool Affine2DFromCorrespondencesLinear(const Mat &x1, - const Mat &x2, - Mat3 *M, - double expected_precision = - EigenDoubleTraits::dummy_precision()); +bool affine2DFromCorrespondencesLinear(const Mat& x1, const Mat& x2, Mat3* M, double expected_precision = EigenDoubleTraits::dummy_precision()); -/** 3D Affine transformation estimation +/** + * @brief 3D Affine transformation estimation + * This function can be used in order to estimate the affine transformation + * between two sets of points with known 3D correspondences. * - * This function can be used in order to estimate the affine transformation - * between two sets of points with known 3D correspondences. - * - * \param[in] x1 The first 3xN matrix of euclidean points - * \param[in] x2 The second 3xN matrix of euclidean points - * \param[out] M The 3x3 affine transformation matrix (12 dof) - * with the following parametrization + * @param[in] x1 The first 3xN matrix of euclidean points + * @param[in] x2 The second 3xN matrix of euclidean points + * @param[out] M The 3x3 affine transformation matrix (12 dof) + * with the following parametrization * |a b c tx| * M = |d e f ty| * |g h i tz| * |0 0 0 1 | * such that x2 = M * x1 - * \param[in] expected_precision The expected precision in order for instance + * @param[in] expected_precision The expected precision in order for instance * to accept almost affine matrices. * - * \return true if the transformation estimation has succeeded + * @return true if the transformation estimation has succeeded * - * \note Need at least 4 non coplanar points - * \note Points coordinates must be normalized (euclidean) + * @note Need at least 4 non coplanar points + * @note Points coordinates must be normalized (euclidean) */ -bool Affine3DFromCorrespondencesLinear(const Mat &x1, - const Mat &x2, - Mat4 *M, - double expected_precision = - EigenDoubleTraits::dummy_precision()); +bool affine3DFromCorrespondencesLinear(const Mat& x1, const Mat& x2, Mat4* M, double expected_precision = EigenDoubleTraits::dummy_precision()); + +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/affineSolver_test.cpp b/src/aliceVision/multiview/affineSolver_test.cpp index 7dfe9012ca..2f64373b8a 100644 --- a/src/aliceVision/multiview/affineSolver_test.cpp +++ b/src/aliceVision/multiview/affineSolver_test.cpp @@ -16,6 +16,7 @@ #include using namespace aliceVision; +using namespace aliceVision::multiview; BOOST_AUTO_TEST_CASE(Affine2DTest_TranslationX) { Mat x1(2, 3); @@ -27,7 +28,7 @@ BOOST_AUTO_TEST_CASE(Affine2DTest_TranslationX) { 0, 1, 1; Mat3 AffineMat; - BOOST_CHECK(Affine2DFromCorrespondencesLinear(x1, x2, &AffineMat)); + BOOST_CHECK(affine2DFromCorrespondencesLinear(x1, x2, &AffineMat)); ALICEVISION_LOG_DEBUG("Mat Affine2D:\n" << AffineMat); Mat3 ground_truth; ground_truth << 1,0,1, @@ -46,7 +47,7 @@ BOOST_AUTO_TEST_CASE(Affine2DTest_TranslationXY) { 1, 2, 2; Mat3 affine_mat; - BOOST_CHECK(Affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine2D:\n" << affine_mat); Mat3 ground_truth; ground_truth << 1,0,1, @@ -71,7 +72,7 @@ BOOST_AUTO_TEST_CASE(Affine2DTest_Rotation45) { } Mat3 affine_mat; - BOOST_CHECK(Affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine2D " << affine_mat); EXPECT_MATRIX_NEAR(affine_mat, rot, 1e-8); } @@ -95,7 +96,7 @@ BOOST_AUTO_TEST_CASE(Affine2DTest_Rotation45AndTranslationXY) { } Mat3 affine_mat; - BOOST_CHECK(Affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine2D:\n" << affine_mat); EXPECT_MATRIX_NEAR(affine_mat, rot, 1e-8); } @@ -117,7 +118,7 @@ BOOST_AUTO_TEST_CASE(Affine2DTest_AffineGeneral) { } Mat3 affine_mat; - BOOST_CHECK(Affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine2DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine2D:\n" << affine_mat); EXPECT_MATRIX_NEAR(affine_mat, m, 1e-8); } @@ -134,7 +135,7 @@ BOOST_AUTO_TEST_CASE(Affine3DTest_TranslationZ) { 1, 2, 8, 4; Mat4 AffineMat; - BOOST_CHECK(Affine3DFromCorrespondencesLinear(x1, x2, &AffineMat)); + BOOST_CHECK(affine3DFromCorrespondencesLinear(x1, x2, &AffineMat)); ALICEVISION_LOG_DEBUG("Mat Affine3D:\n" << AffineMat); Mat4 ground_truth; ground_truth << 1,0,0,0, @@ -156,7 +157,7 @@ BOOST_AUTO_TEST_CASE(Affine3DTest_TranslationXYZ) { 1, 2, 8, 4; Mat4 affine_mat; - BOOST_CHECK(Affine3DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine3DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine3D:\n" << affine_mat); Mat4 ground_truth; ground_truth << 1,0,0, 2, @@ -209,7 +210,7 @@ BOOST_AUTO_TEST_CASE(Affine3DTest_RotationAndTranslationXYZ) { } Mat4 affine_mat; - BOOST_CHECK(Affine3DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine3DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine3D:\n" << affine_mat); EXPECT_MATRIX_NEAR(affine_mat, M, 1e-8); } @@ -233,7 +234,7 @@ BOOST_AUTO_TEST_CASE(Affine3DTest_AffineGeneral) { } Mat4 affine_mat; - BOOST_CHECK(Affine3DFromCorrespondencesLinear(x1, x2, &affine_mat)); + BOOST_CHECK(affine3DFromCorrespondencesLinear(x1, x2, &affine_mat)); ALICEVISION_LOG_DEBUG("Mat Affine3D:\n" << affine_mat); EXPECT_MATRIX_NEAR(affine_mat, m, 1e-8); } diff --git a/src/aliceVision/multiview/conditioning.hpp b/src/aliceVision/multiview/conditioning.hpp deleted file mode 100644 index fc5efe2ec7..0000000000 --- a/src/aliceVision/multiview/conditioning.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - -//-- Implementation of normalized coordinates. -// Normalization improve accuracy of results and provide benefits -// that make scale and coordinate origin invariant. -// The implementation follows Algorithm 4.2 from HZ page 109. - -namespace aliceVision { - -// Point conditioning : -void PreconditionerFromPoints(const Mat &points, Mat3 *T); - -/// Normalize input point for a given T transform matrix -void ApplyTransformationToPoints(const Mat &points, - const Mat3 &T, - Mat *transformed_points); - -// Normalize point in [-.5, .5] and return transformation matrix -void NormalizePoints(const Mat &points, - Mat *normalized_points, - Mat3 *T); - -/// Point conditioning (compute Transformation matrix) -void PreconditionerFromImageSize(int width, int height, Mat3 *T); - -/// Normalize point rom image coordinates to [-.5, .5] -void NormalizePointsFromImageSize(const Mat &points, - Mat *normalized_points, - Mat3 *T, int width, int height); - - -/// Unnormalize using Inverse -struct UnnormalizerI { - // Denormalize the results. See HZ page 109. - static void Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H); -}; - -/// Unnormalize using Transpose -struct UnnormalizerT { - // Denormalize the results. See HZ page 109. - static void Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H); -}; - -} //namespace aliceVision diff --git a/src/aliceVision/multiview/epipolarEquation.hpp b/src/aliceVision/multiview/epipolarEquation.hpp new file mode 100644 index 0000000000..6ed4d9c995 --- /dev/null +++ b/src/aliceVision/multiview/epipolarEquation.hpp @@ -0,0 +1,67 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 multiview { + +/** + * @brief Build a 9 x n matrix from point matches, where each row is equivalent to the + * equation x'T*F*x = 0 for a single correspondence pair (x', x). The domain of + * the matrix is a 9 element vector corresponding to F. In other words, set up + * the linear system + * + * Af = 0, + * + * where f is the F matrix as a 9-vector rather than a 3x3 matrix (row + * major). If the points are well conditioned and there are 8 or more, then + * the nullspace should be rank one. If the nullspace is two dimensional, + * then the rank 2 constraint must be enforced to identify the appropriate F + * matrix. + * + * @note that this does not resize the matrix A; it is expected to have the + * appropriate size already. + */ +template +inline void encodeEpipolarEquation(const TMatX& x1, const TMatX& x2, TMatA* A, const std::vector *weights = nullptr) +{ + assert(x1.cols()==x2.cols()); + + if(weights != nullptr) + { + assert(x1.cols()==weights->size()); + } + + for(typename TMatX::Index i = 0; i < x1.cols(); ++i) + { + const Vec2 xx1 = x1.col(i); + const Vec2 xx2 = x2.col(i); + + A->row(i) << + xx2(0) * xx1(0), // 0 represents x coords, + xx2(0) * xx1(1), // 1 represents y coords. + xx2(0), + xx2(1) * xx1(0), + xx2(1) * xx1(1), + xx2(1), + xx1(0), + xx1(1), + 1.0; + + if(weights != nullptr) + A->row(i) *= (*weights)[i]; + } +} + +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/essential.cpp b/src/aliceVision/multiview/essential.cpp index 667a8a0685..b574253ecc 100644 --- a/src/aliceVision/multiview/essential.cpp +++ b/src/aliceVision/multiview/essential.cpp @@ -7,15 +7,14 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include -#include +#include #include #include - namespace aliceVision { // HZ 9.6 page 257 (formula 9.12) -void EssentialFromFundamental(const Mat3 &F, +void essentialFromFundamental(const Mat3 &F, const Mat3 &K1, const Mat3 &K2, Mat3 *E) { @@ -24,14 +23,14 @@ void EssentialFromFundamental(const Mat3 &F, // HZ 9.6 page 257 (formula 9.12) // Or http://ai.stanford.edu/~birch/projective/node20.html -void FundamentalFromEssential(const Mat3 &E, +void fundamentalFromEssential(const Mat3 &E, const Mat3 &K1, const Mat3 &K2, Mat3 *F) { *F = K2.inverse().transpose() * E * K1.inverse(); } -void RelativeCameraMotion(const Mat3 &R1, +void relativeCameraMotion(const Mat3 &R1, const Vec3 &t1, const Mat3 &R2, const Vec3 &t2, @@ -42,20 +41,20 @@ void RelativeCameraMotion(const Mat3 &R1, } // HZ 9.6 page 257 -void EssentialFromRt(const Mat3 &R1, +void essentialFromRt(const Mat3 &R1, const Vec3 &t1, const Mat3 &R2, const Vec3 &t2, Mat3 *E) { Mat3 R; Vec3 t; - RelativeCameraMotion(R1, t1, R2, t2, &R, &t); + relativeCameraMotion(R1, t1, R2, t2, &R, &t); Mat3 Tx = CrossProductMatrix(t); *E = Tx * R; } // HZ 9.7 page 259 (Result 9.19) -void MotionFromEssential(const Mat3 &E, +void motionFromEssential(const Mat3 &E, std::vector *Rs, std::vector *ts) { Eigen:: JacobiSVD USV(E, Eigen::ComputeFullU|Eigen::ComputeFullV); @@ -89,7 +88,7 @@ void MotionFromEssential(const Mat3 &E, } // HZ 9.6 pag 259 (9.6.3 Geometrical interpretation of the 4 solutions) -int MotionFromEssentialChooseSolution(const std::vector &Rs, +int motionFromEssentialChooseSolution(const std::vector &Rs, const std::vector &ts, const Mat3 &K1, const Vec2 &x1, @@ -102,14 +101,14 @@ int MotionFromEssentialChooseSolution(const std::vector &Rs, // Set P1 = K1 [Id|0] Mat3 R1 = Mat3::Identity(); Vec3 t1 = Vec3::Zero(); - P_From_KRt(K1, R1, t1, &P1); + P_from_KRt(K1, R1, t1, &P1); for (int i = 0; i < 4; ++i) { const Mat3 &R2 = Rs[i]; const Vec3 &t2 = ts[i]; - P_From_KRt(K2, R2, t2, &P2); + P_from_KRt(K2, R2, t2, &P2); Vec3 X; - TriangulateDLT(P1, x1, P2, x2, &X); + multiview::TriangulateDLT(P1, x1, P2, x2, &X); // Test if point is front to the two cameras (positive depth) if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) { return i; @@ -118,7 +117,7 @@ int MotionFromEssentialChooseSolution(const std::vector &Rs, return -1; } -bool MotionFromEssentialAndCorrespondence(const Mat3 &E, +bool motionFromEssentialAndCorrespondence(const Mat3 &E, const Mat3 &K1, const Vec2 &x1, const Mat3 &K2, @@ -127,8 +126,8 @@ bool MotionFromEssentialAndCorrespondence(const Mat3 &E, Vec3 *t) { std::vector Rs; std::vector ts; - MotionFromEssential(E, &Rs, &ts); - int solution = MotionFromEssentialChooseSolution(Rs, ts, K1, x1, K2, x2); + motionFromEssential(E, &Rs, &ts); + int solution = motionFromEssentialChooseSolution(Rs, ts, K1, x1, K2, x2); if (solution >= 0) { *R = Rs[solution]; *t = ts[solution]; diff --git a/src/aliceVision/multiview/essential.hpp b/src/aliceVision/multiview/essential.hpp index c9b66bcf1e..863abb5b78 100644 --- a/src/aliceVision/multiview/essential.hpp +++ b/src/aliceVision/multiview/essential.hpp @@ -14,64 +14,68 @@ namespace aliceVision { -// Compute the relative camera motion between two cameras. -// Given the motion parameters of two cameras, computes the motion parameters -// of the second one assuming the first one to be at the origin. -// If T1 and T2 are the camera motions, the computed relative motion is -// -// T = T2 T1^{-1} -// -void RelativeCameraMotion(const Mat3 &R1, - const Vec3 &t1, - const Mat3 &R2, - const Vec3 &t2, - Mat3 *R, - Vec3 *t); - -/// Given F, Left/Right K matrix it compute the Essential matrix -void EssentialFromFundamental(const Mat3 &F, - const Mat3 &K1, - const Mat3 &K2, - Mat3 *E); - -/// Compute E as E = [t12]x R12. -void EssentialFromRt(const Mat3 &R1, - const Vec3 &t1, - const Mat3 &R2, - const Vec3 &t2, - Mat3 *E); - -/// Given E, Left/Right K matrix it compute the Fundamental matrix -void FundamentalFromEssential(const Mat3 &E, - const Mat3 &K1, - const Mat3 &K2, - Mat3 *F); - -/// Test the possible R|t configuration to have point in front of the cameras -/// Return false if no possible configuration -bool MotionFromEssentialAndCorrespondence(const Mat3 &E, - const Mat3 &K1, - const Vec2 &x1, - const Mat3 &K2, - const Vec2 &x2, - Mat3 *R, - Vec3 *t); - -/// Choose one of the four possible motion solutions from an essential matrix. -/// Decides the right solution by checking that the triangulation of a match -/// x1--x2 lies in front of the cameras. -/// Return the index of the right solution or -1 if no solution. -int MotionFromEssentialChooseSolution(const std::vector &Rs, - const std::vector &ts, - const Mat3 &K1, - const Vec2 &x1, - const Mat3 &K2, - const Vec2 &x2); - -// HZ 9.7 page 259 (Result 9.19) -void MotionFromEssential(const Mat3 &E, - std::vector *Rs, - std::vector *ts); - +/** + * @brief Compute the relative camera motion between two cameras. + * Given the motion parameters of two cameras, computes the motion parameters + * of the second one assuming the first one to be at the origin. + * If T1 and T2 are the camera motions, the computed relative motion is + * + * T = T2 T1^{-1} + */ +void relativeCameraMotion(const Mat3& R1, + const Vec3& t1, + const Mat3& R2, + const Vec3& t2, + Mat3* R, + Vec3* t); +/** + * @brief Given F, Left/Right K matrix it compute the Essential matrix + */ +void essentialFromFundamental(const Mat3& F, + const Mat3& K1, + const Mat3& K2, + Mat3* E); +/** + * @brief Compute E as E = [t12]x R12. + */ +void essentialFromRt(const Mat3& R1, + const Vec3& t1, + const Mat3& R2, + const Vec3& t2, + Mat3* E); +/** + * @brief Given E, Left/Right K matrix it compute the Fundamental matrix + */ +void fundamentalFromEssential(const Mat3& E, + const Mat3& K1, + const Mat3& K2, + Mat3* F); +/** + * @brief Test the possible R|t configuration to have point in front of the cameras + * Return false if no possible configuration + */ +bool motionFromEssentialAndCorrespondence(const Mat3& E, + const Mat3& K1, + const Vec2& x1, + const Mat3& K2, + const Vec2& x2, + Mat3* R, + Vec3* t); +/** + * @brief Choose one of the four possible motion solutions from an essential matrix. + * Decides the right solution by checking that the triangulation of a match + * x1--x2 lies in front of the cameras. + * Return the index of the right solution or -1 if no solution. + */ +int motionFromEssentialChooseSolution(const std::vector& Rs, + const std::vector& ts, + const Mat3& K1, + const Vec2& x1, + const Mat3& K2, + const Vec2& x2); +/** + * @brief HZ 9.7 page 259 (Result 9.19) + */ +void motionFromEssential(const Mat3& E, std::vector* Rs, std::vector* ts); } // namespace aliceVision diff --git a/src/aliceVision/multiview/essentialFivePointSolver.cpp b/src/aliceVision/multiview/essentialFivePointSolver.cpp deleted file mode 100644 index 0d05cb8894..0000000000 --- a/src/aliceVision/multiview/essentialFivePointSolver.cpp +++ /dev/null @@ -1,195 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 "essentialFivePointSolver.hpp" -#include - -#include - -namespace aliceVision { - -Mat FivePointsNullspaceBasis(const Mat2X &x1, const Mat2X &x2) { - Eigen::Matrix A; - A.setZero(); // Make A square until Eigen supports rectangular SVD. - fundamental::kernel::EncodeEpipolarEquation(x1, x2, &A); - Eigen::JacobiSVD > svd(A,Eigen::ComputeFullV); - return svd.matrixV().topRightCorner<9,4>(); -} - -Vec o1(const Vec &a, const Vec &b) { - Vec res = Vec::Zero(20); - - res(coef_xx) = a(coef_x) * b(coef_x); - res(coef_xy) = a(coef_x) * b(coef_y) - + a(coef_y) * b(coef_x); - res(coef_xz) = a(coef_x) * b(coef_z) - + a(coef_z) * b(coef_x); - res(coef_yy) = a(coef_y) * b(coef_y); - res(coef_yz) = a(coef_y) * b(coef_z) - + a(coef_z) * b(coef_y); - res(coef_zz) = a(coef_z) * b(coef_z); - res(coef_x) = a(coef_x) * b(coef_1) - + a(coef_1) * b(coef_x); - res(coef_y) = a(coef_y) * b(coef_1) - + a(coef_1) * b(coef_y); - res(coef_z) = a(coef_z) * b(coef_1) - + a(coef_1) * b(coef_z); - res(coef_1) = a(coef_1) * b(coef_1); - - return res; -} - -Vec o2(const Vec &a, const Vec &b) { - Vec res(20); - - res(coef_xxx) = a(coef_xx) * b(coef_x); - res(coef_xxy) = a(coef_xx) * b(coef_y) - + a(coef_xy) * b(coef_x); - res(coef_xxz) = a(coef_xx) * b(coef_z) - + a(coef_xz) * b(coef_x); - res(coef_xyy) = a(coef_xy) * b(coef_y) - + a(coef_yy) * b(coef_x); - res(coef_xyz) = a(coef_xy) * b(coef_z) - + a(coef_yz) * b(coef_x) - + a(coef_xz) * b(coef_y); - res(coef_xzz) = a(coef_xz) * b(coef_z) - + a(coef_zz) * b(coef_x); - res(coef_yyy) = a(coef_yy) * b(coef_y); - res(coef_yyz) = a(coef_yy) * b(coef_z) - + a(coef_yz) * b(coef_y); - res(coef_yzz) = a(coef_yz) * b(coef_z) - + a(coef_zz) * b(coef_y); - res(coef_zzz) = a(coef_zz) * b(coef_z); - res(coef_xx) = a(coef_xx) * b(coef_1) - + a(coef_x) * b(coef_x); - res(coef_xy) = a(coef_xy) * b(coef_1) - + a(coef_x) * b(coef_y) - + a(coef_y) * b(coef_x); - res(coef_xz) = a(coef_xz) * b(coef_1) - + a(coef_x) * b(coef_z) - + a(coef_z) * b(coef_x); - res(coef_yy) = a(coef_yy) * b(coef_1) - + a(coef_y) * b(coef_y); - res(coef_yz) = a(coef_yz) * b(coef_1) - + a(coef_y) * b(coef_z) - + a(coef_z) * b(coef_y); - res(coef_zz) = a(coef_zz) * b(coef_1) - + a(coef_z) * b(coef_z); - res(coef_x) = a(coef_x) * b(coef_1) - + a(coef_1) * b(coef_x); - res(coef_y) = a(coef_y) * b(coef_1) - + a(coef_1) * b(coef_y); - res(coef_z) = a(coef_z) * b(coef_1) - + a(coef_1) * b(coef_z); - res(coef_1) = a(coef_1) * b(coef_1); - - return res; -} - -Mat FivePointsPolynomialConstraints(const Mat &E_basis) { - // Build the polynomial form of E (equation (8) in Stewenius et al. [1]) - Vec E[3][3]; - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - E[i][j] = Vec::Zero(20); - E[i][j](coef_x) = E_basis(3 * i + j, 0); - E[i][j](coef_y) = E_basis(3 * i + j, 1); - E[i][j](coef_z) = E_basis(3 * i + j, 2); - E[i][j](coef_1) = E_basis(3 * i + j, 3); - } - } - - // The constraint matrix. - Mat M(10, 20); - int mrow = 0; - - // Determinant constraint det(E) = 0; equation (19) of Nister [2]. - M.row(mrow++) = o2(o1(E[0][1], E[1][2]) - o1(E[0][2], E[1][1]), E[2][0]) + - o2(o1(E[0][2], E[1][0]) - o1(E[0][0], E[1][2]), E[2][1]) + - o2(o1(E[0][0], E[1][1]) - o1(E[0][1], E[1][0]), E[2][2]); - - // Cubic singular values constraint. - // Equation (20). - Vec EET[3][3]; - for (int i = 0; i < 3; ++i) { // Since EET is symmetric, we only compute - for (int j = 0; j < 3; ++j) { // its upper triangular part. - if (i <= j) { - EET[i][j] = o1(E[i][0], E[j][0]) - + o1(E[i][1], E[j][1]) - + o1(E[i][2], E[j][2]); - } else { - EET[i][j] = EET[j][i]; - } - } - } - - // Equation (21). - Vec (&L)[3][3] = EET; - Vec trace = 0.5 * (EET[0][0] + EET[1][1] + EET[2][2]); - for (int i = 0; i < 3; ++i) { - L[i][i] -= trace; - } - - // Equation (23). - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - Vec LEij = o2(L[i][0], E[0][j]) - + o2(L[i][1], E[1][j]) - + o2(L[i][2], E[2][j]); - M.row(mrow++) = LEij; - } - } - - return M; -} - -void FivePointsRelativePose(const Mat2X &x1, - const Mat2X &x2, - std::vector *Es) { - // Step 1: Nullspace Extraction. - const Eigen::Matrix E_basis = FivePointsNullspaceBasis(x1, x2); - - // Step 2: Constraint Expansion. - const Eigen::Matrix E_constraints = FivePointsPolynomialConstraints(E_basis); - - // Step 3: Gauss-Jordan Elimination (done thanks to a LU decomposition). - typedef Eigen::Matrix Mat10; - Eigen::FullPivLU c_lu(E_constraints.block<10, 10>(0, 0)); - const Mat10 M = c_lu.solve(E_constraints.block<10, 10>(0, 10)); - - // For next steps we follow the matlab code given in Stewenius et al [1]. - - // Build action matrix. - - const Mat10 & B = M.topRightCorner<10,10>(); - Mat10 At = Mat10::Zero(10,10); - At.block<3, 10>(0, 0) = B.block<3, 10>(0, 0); - At.row(3) = B.row(4); - At.row(4) = B.row(5); - At.row(5) = B.row(7); - At(6,0) = At(7,1) = At(8,3) = At(9,6) = -1; - - Eigen::EigenSolver eigensolver(At); - const auto& eigenvectors = eigensolver.eigenvectors(); - const auto& eigenvalues = eigensolver.eigenvalues(); - - // Build essential matrices for the real solutions. - Es->reserve(10); - for (int s = 0; s < 10; ++s) { - // Only consider real solutions. - if (eigenvalues(s).imag() != 0) { - continue; - } - Mat3 E; - Eigen::Map(E.data()) = - E_basis * eigenvectors.col(s).tail<4>().real(); - Es->emplace_back(E.transpose()); - } -} - -} // namespace aliceVision diff --git a/src/aliceVision/multiview/essentialFivePointSolver.hpp b/src/aliceVision/multiview/essentialFivePointSolver.hpp deleted file mode 100644 index d907c7acf1..0000000000 --- a/src/aliceVision/multiview/essentialFivePointSolver.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 { - -/** Computes the relative pose of two calibrated cameras from 5 correspondences. - * - * \param x1 Points in the first image. One per column. - * \param x2 Corresponding points in the second image. One per column. - * \param E A list of at most 10 candidate essential matrix solutions. - */ -void FivePointsRelativePose(const Mat2X &x1, const Mat2X &x2, - std::vector *E); - -// Compute the nullspace of the linear constraints given by the matches. -Mat FivePointsNullspaceBasis(const Mat2X &x1, const Mat2X &x2); - -// Multiply two polynomials of degree 1. -Vec o1(const Vec &a, const Vec &b); - -// Multiply a polynomial of degree 2, a, by a polynomial of degree 1, b. -Vec o2(const Vec &a, const Vec &b); - -// Builds the polynomial constraint matrix M. -Mat FivePointsPolynomialConstraints(const Mat &E_basis); - -// In the following code, polynomials are expressed as vectors containing -// their coeficients in the basis of monomials: -// -// [xxx xxy xyy yyy xxz xyz yyz xzz yzz zzz xx xy yy xz yz zz x y z 1] -// -// Note that there is an error in Stewenius' paper. In equation (9) they -// propose to use the basis: -// -// [xxx xxy xxz xyy xyz xzz yyy yyz yzz zzz xx xy xz yy yz zz x y z 1] -// -// But this is not the basis used in the rest of the paper, neither in -// the code they provide. I (pau) have spend 4 hours debugging and -// reverse engineering their code to find the problem. :( -enum { - coef_xxx, - coef_xxy, - coef_xyy, - coef_yyy, - coef_xxz, - coef_xyz, - coef_yyz, - coef_xzz, - coef_yzz, - coef_zzz, - coef_xx, - coef_xy, - coef_yy, - coef_xz, - coef_yz, - coef_zz, - coef_x, - coef_y, - coef_z, - coef_1 -}; - -} // namespace aliceVision diff --git a/src/aliceVision/multiview/essentialFivePointSolver_test.cpp b/src/aliceVision/multiview/essentialFivePointSolver_test.cpp deleted file mode 100644 index 7b6436177b..0000000000 --- a/src/aliceVision/multiview/essentialFivePointSolver_test.cpp +++ /dev/null @@ -1,270 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 "aliceVision/numeric/numeric.hpp" -#include "aliceVision/multiview/essential.hpp" -#include "aliceVision/multiview/projection.hpp" -#include "aliceVision/multiview/essentialFivePointSolver.hpp" -#include "aliceVision/multiview/NViewDataSet.hpp" - -#define BOOST_TEST_MODULE essentialFivePointSolver - -#include -#include -#include - -using namespace aliceVision; - -struct TestData { - //-- Dataset that encapsulate : - // 3D points and their projection given P1 and P2 - // Link between the two camera [R|t] - Mat3X X; - Mat3 R; - Vec3 t; - Mat3 E; - Mat34 P1, P2; - Mat2X x1, x2; -}; - -TestData SomeTestData() { - TestData d; - - // -- - // Modeling random 3D points, - // Consider first camera as [R=I|t=0], - // Second camera as [R=Rx*Ry*Rz|t=random], - // Compute projection of the 3D points onto image plane. - // -- - d.X = Mat3X::Random(3,5); - - //-- Make point in front to the cameras. - d.X.row(0).array() -= .5; - d.X.row(1).array() -= .5; - d.X.row(2).array() += 1.0; - - d.R = RotationAroundZ(0.3) * RotationAroundX(0.1) * RotationAroundY(0.2); - d.t.setRandom(); - //d.t(1) = 10; - - EssentialFromRt(Mat3::Identity(), Vec3::Zero(), d.R, d.t, &d.E); - - P_From_KRt(Mat3::Identity(), Mat3::Identity(), Vec3::Zero(), &d.P1); - P_From_KRt(Mat3::Identity(), d.R, d.t, &d.P2); - - Project(d.P1, d.X, &d.x1); - Project(d.P2, d.X, &d.x2); - - return d; -} - -double EvalPolynomial(Vec p, double x, double y, double z) { - return p(coef_xxx) * x * x * x - + p(coef_xxy) * x * x * y - + p(coef_xxz) * x * x * z - + p(coef_xyy) * x * y * y - + p(coef_xyz) * x * y * z - + p(coef_xzz) * x * z * z - + p(coef_yyy) * y * y * y - + p(coef_yyz) * y * y * z - + p(coef_yzz) * y * z * z - + p(coef_zzz) * z * z * z - + p(coef_xx) * x * x - + p(coef_xy) * x * y - + p(coef_xz) * x * z - + p(coef_yy) * y * y - + p(coef_yz) * y * z - + p(coef_zz) * z * z - + p(coef_x) * x - + p(coef_y) * y - + p(coef_z) * z - + p(coef_1) * 1; -} - -BOOST_AUTO_TEST_CASE(o1_Evaluation) { - - Vec p1 = Vec::Zero(20), p2 = Vec::Zero(20); - p1(coef_x) = double(rand()) / RAND_MAX; - p1(coef_y) = double(rand()) / RAND_MAX; - p1(coef_z) = double(rand()) / RAND_MAX; - p1(coef_1) = double(rand()) / RAND_MAX; - p2(coef_x) = double(rand()) / RAND_MAX; - p2(coef_y) = double(rand()) / RAND_MAX; - p2(coef_z) = double(rand()) / RAND_MAX; - p2(coef_1) = double(rand()) / RAND_MAX; - - Vec p3 = o1(p1, p2); - - for (double z = -5; z < 5; ++z) { - for (double y = -5; y < 5; ++y) { - for (double x = -5; x < 5; ++x) { - BOOST_CHECK_SMALL(EvalPolynomial(p3, x, y, z)- - EvalPolynomial(p1, x, y, z) * EvalPolynomial(p2, x, y, z), - 1e-8); - } - } - } -} - -BOOST_AUTO_TEST_CASE(o2_Evaluation) { - - Vec p1 = Vec::Zero(20), p2 = Vec::Zero(20); - p1(coef_xx) = double(rand()) / RAND_MAX; - p1(coef_xy) = double(rand()) / RAND_MAX; - p1(coef_xz) = double(rand()) / RAND_MAX; - p1(coef_yy) = double(rand()) / RAND_MAX; - p1(coef_yz) = double(rand()) / RAND_MAX; - p1(coef_zz) = double(rand()) / RAND_MAX; - p1(coef_x) = double(rand()) / RAND_MAX; - p1(coef_y) = double(rand()) / RAND_MAX; - p1(coef_z) = double(rand()) / RAND_MAX; - p1(coef_1) = double(rand()) / RAND_MAX; - p2(coef_x) = double(rand()) / RAND_MAX; - p2(coef_y) = double(rand()) / RAND_MAX; - p2(coef_z) = double(rand()) / RAND_MAX; - p2(coef_1) = double(rand()) / RAND_MAX; - - Vec p3 = o2(p1, p2); - - for (double z = -5; z < 5; ++z) { - for (double y = -5; y < 5; ++y) { - for (double x = -5; x < 5; ++x) { - BOOST_CHECK_SMALL(EvalPolynomial(p3, x, y, z)- - EvalPolynomial(p1, x, y, z) * EvalPolynomial(p2, x, y, z), - 1e-8); - } - } - } -} - -/// Check that the E matrix fit the Essential Matrix properties -/// Determinant is 0 -/// -#define EXPECT_ESSENTIAL_MATRIX_PROPERTIES(E, expectedPrecision) { \ - BOOST_CHECK_SMALL(E.determinant(), expectedPrecision); \ - Mat3 O = 2 * E * E.transpose() * E - (E * E.transpose()).trace() * E; \ - Mat3 zero3x3 = Mat3::Zero(); \ - EXPECT_MATRIX_NEAR(zero3x3, O, expectedPrecision);\ -} - -BOOST_AUTO_TEST_CASE(FivePointsRelativePose_Random) { - - TestData d = SomeTestData(); - - std::vector Es; - std::vector Rs; - std::vector ts; - FivePointsRelativePose(d.x1, d.x2, &Es); - - // Recover rotation and translation from E - Rs.resize(Es.size()); - ts.resize(Es.size()); - for (size_t s = 0; s < Es.size(); ++s) { - Vec2 x1Col = d.x1.col(0); - Vec2 x2Col = d.x2.col(0); - BOOST_CHECK( - MotionFromEssentialAndCorrespondence(Es[s], - Mat3::Identity(), - x1Col, - Mat3::Identity(), - x2Col, - &Rs[s], - &ts[s])); - } - - bool bsolution_found = false; - for (size_t i = 0; i < Es.size(); ++i) { - - std::cout << i << std::endl; - - // Check that we find the correct relative orientation. - if (FrobeniusDistance(d.R, Rs[i]) < 1e-3 - && (d.t / d.t.norm() - ts[i] / ts[i].norm()).norm() < 1e-3 ) { - bsolution_found = true; - // Check that E holds the essential matrix constraints. - EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es[i], 1e-8); - } - } - //-- Almost one solution must find the correct relative orientation - BOOST_CHECK(bsolution_found); -} - -BOOST_AUTO_TEST_CASE(FivePointsRelativePose_test_data_sets) { - - //-- Setup a circular camera rig and assert that 5PT relative pose works. - const int iNviews = 5; - NViewDataSet d = NRealisticCamerasRing(iNviews, 5, - NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - - // Compute pose [R|t] from 0 to [1;..;iNviews] - for(int i=1; i Es, Rs; // Essential, Rotation matrix. - std::vector ts; // Translation matrix. - FivePointsRelativePose(d._x[0], d._x[i], &Es); - - // Recover rotation and translation from E. - Rs.resize(Es.size()); - ts.resize(Es.size()); - for (size_t s = 0; s < Es.size(); ++s) { - Vec2 x1Col = d._x[0].col(0); - Vec2 x2Col = d._x[i].col(0); - BOOST_CHECK( - MotionFromEssentialAndCorrespondence(Es[s], - d._K[0], - x1Col, - d._K[i], - x2Col, - &Rs[s], - &ts[s])); - } - //-- Compute Ground Truth motion - Mat3 R; - Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(d._R[0], d._t[0], d._R[i], d._t[i], &R, &t); - - // Assert that found relative motion is correct for almost one model. - bool bsolution_found = false; - for (size_t nModel = 0; nModel < Es.size(); ++nModel) { - - // Check that E holds the essential matrix constraints. - EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es[nModel], 1e-8); - - // Check that we find the correct relative orientation. - if (FrobeniusDistance(R, Rs[nModel]) < 1e-3 - && (t / t.norm() - ts[nModel] / ts[nModel].norm()).norm() < 1e-3 ) { - bsolution_found = true; - } - } - //-- Almost one solution must find the correct relative orientation - BOOST_CHECK(bsolution_found); - } -} - -BOOST_AUTO_TEST_CASE(FivePointsNullspaceBasis_SatisfyEpipolarConstraint) { - - TestData d = SomeTestData(); - - Mat E_basis = FivePointsNullspaceBasis(d.x1, d.x2); - - for (int s = 0; s < 4; ++s) { - Mat3 E; - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - E(i, j) = E_basis(3 * i + j, s); - } - } - for (int i = 0; i < d.x1.cols(); ++i) { - Vec3 x1(d.x1(0,i), d.x1(1,i), 1); - Vec3 x2(d.x2(0,i), d.x2(1,i), 1); - BOOST_CHECK_SMALL(x2.dot(E * x1), 1e-6); - } - } -} diff --git a/src/aliceVision/multiview/essentialKernelSolver.hpp b/src/aliceVision/multiview/essentialKernelSolver.hpp deleted file mode 100644 index b1f2868d74..0000000000 --- a/src/aliceVision/multiview/essentialKernelSolver.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - -namespace aliceVision { -namespace essential { -namespace kernel { - -using namespace std; - -/** - * Eight-point algorithm for solving for the essential matrix from normalized - * image coordinates of point correspondences. - * See page 294 in HZ Result 11.1. - * - */ -struct EightPointRelativePoseSolver { - enum { MINIMUM_SAMPLES = 8 }; - enum { MAX_MODELS = 1 }; - static void Solve(const Mat &x1, const Mat &x2, vector *E); -}; - -/** - * Five-point algorithm to solve the Essential matrix from 5 points - * correspondences. It solves the relative pose problem. - * Input point must be normalized one. - */ -struct FivePointSolver { - enum { MINIMUM_SAMPLES = 5 }; - enum { MAX_MODELS = 10 }; - static void Solve(const Mat &x1, const Mat &x2, vector *E); -}; - -//-- Generic Solver for the 5pt Essential Matrix Estimation. -//-- Need a new Class that inherit of twoView::kernel::kernel. -// Error must be overwrite in order to compute F from E and K's. -//-- Fitting must normalize image values to camera values. -template -class EssentialKernel : - public twoView::kernel::Kernel -{ -public: - EssentialKernel(const Mat &x1, const Mat &x2, - const Mat3 &K1, const Mat3 &K2): - twoView::kernel::Kernel(x1,x2), - K1_(K1), K2_(K2) {} - void Fit(const vector &samples, vector *models) const { - const Mat x1 = ExtractColumns(this->x1_, samples); - const Mat x2 = ExtractColumns(this->x2_, samples); - - assert(2 == x1.rows()); - assert(SolverArg::MINIMUM_SAMPLES <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - // Normalize the data (image coords to camera coords). - const Mat3 K1Inverse = K1_.inverse(); - const Mat3 K2Inverse = K2_.inverse(); - Mat x1_normalized, x2_normalized; - ApplyTransformationToPoints(x1, K1Inverse, &x1_normalized); - ApplyTransformationToPoints(x2, K2Inverse, &x2_normalized); - SolverArg::Solve(x1_normalized, x2_normalized, models); - } - - double Error(size_t sample, const ModelArg &model) const { - Mat3 F; - FundamentalFromEssential(model, K1_, K2_, &F); - return ErrorArg::Error(F, this->x1_.col(sample), this->x2_.col(sample)); - } -protected: - Mat3 K1_, K2_; // The two camera calibrated camera matrix -}; - -//-- Solver kernel for the 8pt Essential Matrix Estimation -typedef essential::kernel::EssentialKernel EightPointKernel; - - -//-- Solver kernel for the 5pt Essential Matrix Estimation -typedef essential::kernel::EssentialKernel FivePointKernel; - - -} // namespace kernel -} // namespace essential -} // namespace aliceVision diff --git a/src/aliceVision/multiview/fundamentalKernelSolver.cpp b/src/aliceVision/multiview/fundamentalKernelSolver.cpp deleted file mode 100644 index 0b6b19e960..0000000000 --- a/src/aliceVision/multiview/fundamentalKernelSolver.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - -namespace aliceVision { -namespace fundamental { -namespace kernel { - -using namespace std; -void SevenPointSolver::Solve(const Mat &x1, const Mat &x2, vector *F) { - assert(2 == x1.rows()); - assert(7 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - Vec9 f1, f2; - if (x1.cols() == 7) { - // Set up the homogeneous system Af = 0 from the equations x'T*F*x = 0. - typedef Eigen::Matrix Mat9; - // In the minimal solution use fixed sized matrix to let Eigen and the - // compiler doing the maximum of optimization. - Mat9 A = Mat::Zero(9,9); - EncodeEpipolarEquation(x1, x2, &A); -// Eigen::FullPivLU luA(A); -// ALICEVISION_LOG_DEBUG("\n rank(A) = " << luA.rank()); -// Eigen::JacobiSVD svdA(A); -// cout << "Its singular values are:" << endl << svdA.singularValues() << endl; - // Find the two F matrices in the nullspace of A. - Nullspace2(&A, &f1, &f2); - //@fixme here there is a potential error, we should check that the size of - // null(A) is 2. Otherwise we have a family of possible solutions for the - // fundamental matrix (ie infinite solution). This happens, e.g., when matching - // the image against itself or in other degenerate configurations of the camera, - // such as pure rotation or correspondences all on the same plane (cf HZ pg296 table 11.1) - // This is not critical for just matching images with geometric validation, - // it becomes an issue if the estimated F has to be used for retrieving the - // motion of the camera. - } - else - { - // Set up the homogeneous system Af = 0 from the equations x'T*F*x = 0. - Mat A(x1.cols(), 9); - EncodeEpipolarEquation(x1, x2, &A); - // Find the two F matrices in the nullspace of A. - Nullspace2(&A, &f1, &f2); - } - - Mat3 F1 = Map(f1.data()); - Mat3 F2 = Map(f2.data()); - - // Then, use the condition det(F) = 0 to determine F. In other words, solve - // det(F1 + a*F2) = 0 for a. - double a = F1(0, 0), j = F2(0, 0), - b = F1(0, 1), k = F2(0, 1), - c = F1(0, 2), l = F2(0, 2), - d = F1(1, 0), m = F2(1, 0), - e = F1(1, 1), n = F2(1, 1), - f = F1(1, 2), o = F2(1, 2), - g = F1(2, 0), p = F2(2, 0), - h = F1(2, 1), q = F2(2, 1), - i = F1(2, 2), r = F2(2, 2); - - // Run fundamental_7point_coeffs.py to get the below coefficients. - // The coefficients are in ascending powers of alpha, i.e. P[N]*x^N. - double P[4] = { - a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g, - a*e*r + a*i*n + b*f*p + b*g*o + c*d*q + c*h*m + d*h*l + e*i*j + f*g*k - - a*f*q - a*h*o - b*d*r - b*i*m - c*e*p - c*g*n - d*i*k - e*g*l - f*h*j, - a*n*r + b*o*p + c*m*q + d*l*q + e*j*r + f*k*p + g*k*o + h*l*m + i*j*n - - a*o*q - b*m*r - c*n*p - d*k*r - e*l*p - f*j*q - g*l*n - h*j*o - i*k*m, - j*n*r + k*o*p + l*m*q - j*o*q - k*m*r - l*n*p}; - - // Solve for the roots of P[3]*x^3 + P[2]*x^2 + P[1]*x + P[0] = 0. - double roots[3]; - int num_roots = SolveCubicPolynomial(P, roots); - - // Build the fundamental matrix for each solution. - for (int kk = 0; kk < num_roots; ++kk) { - F->push_back(F1 + roots[kk] * F2); - } -} - -void EightPointSolver::Solve(const Mat &x1, const Mat &x2, vector *Fs, const vector *weights) -{ - assert(2 == x1.rows()); - assert(8 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - Vec9 f; - if (x1.cols() == 8) - { - typedef Eigen::Matrix Mat9; - // In the minimal solution use fixed sized matrix to let Eigen and the - // compiler doing the maximum of optimization. - Mat9 A = Mat::Zero(9,9); - EncodeEpipolarEquation(x1, x2, &A, weights); - Nullspace(&A, &f); - } - else - { - MatX9 A(x1.cols(), 9); - EncodeEpipolarEquation(x1, x2, &A, weights); - Nullspace(&A, &f); - } - - Mat3 F = Map(f.data()); - - // Force the fundamental property if the A matrix has full rank. - // HZ 11.1.1 pag.280 - if (x1.cols() > 8) - { - // Force fundamental matrix to have rank 2 - Eigen::JacobiSVD USV(F, Eigen::ComputeFullU | Eigen::ComputeFullV); - Vec3 d = USV.singularValues(); - d[2] = 0.0; - F = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose(); - } - Fs->push_back(F); -} - -} // namespace kernel -} // namespace fundamental -} // namespace aliceVision diff --git a/src/aliceVision/multiview/fundamentalKernelSolver.hpp b/src/aliceVision/multiview/fundamentalKernelSolver.hpp deleted file mode 100644 index 087506fd12..0000000000 --- a/src/aliceVision/multiview/fundamentalKernelSolver.hpp +++ /dev/null @@ -1,151 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - - -namespace aliceVision { -namespace fundamental { -namespace kernel { - -using namespace std; - -/** - * Seven-point algorithm for solving for the fundamental matrix from point - * correspondences. See page 281 in HZ, though oddly they use a different - * equation: \f$det(\alpha F_1 + (1-\alpha)F_2) = 0\f$. Since \f$F_1\f$ and - * \f$F2\f$ are projective, there's no need to balance the relative scale. - * Instead, here, the simpler equation is solved: \f$det(F_1 + \alpha F_2) = - * 0\f$. - * - * \see http://www.cs.unc.edu/~marc/tutorial/node55.html - */ -struct SevenPointSolver { - enum { MINIMUM_SAMPLES = 7 }; - enum { MAX_MODELS = 3 }; - static void Solve(const Mat &x1, const Mat &x2, vector *F); -}; - -struct EightPointSolver { - enum { MINIMUM_SAMPLES = 8 }; - enum { MAX_MODELS = 1 }; - static void Solve(const Mat &x1, const Mat &x2, vector *Fs, const vector *weights = nullptr); -}; - -/** - * Build a 9 x n matrix from point matches, where each row is equivalent to the - * equation x'T*F*x = 0 for a single correspondence pair (x', x). The domain of - * the matrix is a 9 element vector corresponding to F. In other words, set up - * the linear system - * - * Af = 0, - * - * where f is the F matrix as a 9-vector rather than a 3x3 matrix (row - * major). If the points are well conditioned and there are 8 or more, then - * the nullspace should be rank one. If the nullspace is two dimensional, - * then the rank 2 constraint must be enforced to identify the appropriate F - * matrix. - * - * Note that this does not resize the matrix A; it is expected to have the - * appropriate size already. - */ -template -inline void EncodeEpipolarEquation(const TMatX &x1, const TMatX &x2, TMatA *A, const vector *weights = nullptr) -{ - assert(x1.cols()==x2.cols()); - if(weights) - { - assert(x1.cols()==weights->size()); - } - for (typename TMatX::Index i = 0; i < x1.cols(); ++i) - { - const Vec2 xx1 = x1.col(i); - const Vec2 xx2 = x2.col(i); - A->row(i) << - xx2(0) * xx1(0), // 0 represents x coords, - xx2(0) * xx1(1), // 1 represents y coords. - xx2(0), - xx2(1) * xx1(0), - xx2(1) * xx1(1), - xx2(1), - xx1(0), - xx1(1), - 1.0; - if(weights) - A->row(i) *= (*weights)[i]; - } -} - -/// Compute SampsonError related to the Fundamental matrix and 2 correspondences -struct SampsonError { - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2) { - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); - // See page 287 equation (11.9) of HZ. - Vec3 F_x = F * x; - Vec3 Ft_y = F.transpose() * y; - return Square(y.dot(F_x)) / ( F_x.head<2>().squaredNorm() - + Ft_y.head<2>().squaredNorm()); - } -}; - -struct SymmetricEpipolarDistanceError { - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2) { - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); - // See page 288 equation (11.10) of HZ. - Vec3 F_x = F * x; - Vec3 Ft_y = F.transpose() * y; - return Square(y.dot(F_x)) * ( 1.0 / F_x.head<2>().squaredNorm() - + 1.0 / Ft_y.head<2>().squaredNorm()) - / 4.0; // The divide by 4 is to make this match the Sampson distance. - } -}; - -struct EpipolarDistanceError { - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2) { - // Transfer error in image 2 - // See page 287 equation (11.9) of HZ. - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); - Vec3 F_x = F * x; - return Square(F_x.dot(y)) / F_x.head<2>().squaredNorm(); - } -}; -typedef EpipolarDistanceError SimpleError; - -//-- Kernel solver for the 8pt Fundamental Matrix Estimation -typedef twoView::kernel::Kernel - SevenPointKernel; - -//-- Kernel solver for the 8pt Fundamental Matrix Estimation -typedef twoView::kernel::Kernel - EightPointKernel; - -//-- Normalized 7pt kernel -> conditioning from HZ (Algo 11.1) pag 282 -typedef twoView::kernel::Kernel< - twoView::kernel::NormalizedSolver, - SampsonError, - Mat3> - NormalizedSevenPointKernel; - -//-- Normalized 8pt kernel -> conditioning from HZ (Algo 11.1) pag 282 -typedef twoView::kernel::Kernel< - twoView::kernel::NormalizedSolver, - SampsonError, - Mat3> - NormalizedEightPointKernel; - -} // namespace kernel -} // namespace fundamental -} // namespace aliceVision diff --git a/src/aliceVision/multiview/fundamentalKernelSolver_test.cpp b/src/aliceVision/multiview/fundamentalKernelSolver_test.cpp deleted file mode 100644 index 5814a95df7..0000000000 --- a/src/aliceVision/multiview/fundamentalKernelSolver_test.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 "aliceVision/multiview/fundamentalKernelSolver.hpp" -#include "aliceVision/multiview/projection.hpp" - -#define BOOST_TEST_MODULE fundamentalKernelSolver - -#include -#include - -using namespace aliceVision; -using namespace std; - -// Check that sin(angle(a, b)) < tolerance. -template -bool Colinear(const A &a, const B &b, double tolerance) { - bool dims_match = (a.rows() == b.rows()) && (a.cols() == b.cols()); - if (!dims_match) { - return false; - } - double c = CosinusBetweenMatrices(a, b); - if (c * c < 1) { - double s = sqrt(1 - c * c); - return fabs(s) < tolerance; - } - return false; -} - -// Check the properties of a fundamental matrix: -// -// 1. The determinant is 0 (rank deficient) -// 2. The condition x'T*F*x = 0 is satisfied to precision. -// -template -bool ExpectFundamentalProperties(const TMat &F, - const Mat &ptsA, - const Mat &ptsB, - double precision) { - bool bOk = true; - bOk &= F.determinant() < precision; - assert(ptsA.cols() == ptsB.cols()); - Mat hptsA, hptsB; - EuclideanToHomogeneous(ptsA, &hptsA); - EuclideanToHomogeneous(ptsB, &hptsB); - for (int i = 0; i < ptsA.cols(); ++i) { - double residual = hptsB.col(i).dot(F * hptsA.col(i)); - bOk &= residual < precision; - } - return bOk; -} - -// Check the fundamental fitting: -// -// 1. Estimate the fundamental matrix -// 2. Check epipolar distance. -// -template -bool ExpectKernelProperties(const Mat &x1, - const Mat &x2, - Mat3 *F_expected = nullptr) { - bool bOk = true; - Kernel kernel(x1, x2); - vector samples; - for (size_t i = 0; i < x1.cols(); ++i) { - samples.push_back(i); - } - vector Fs; - kernel.Fit(samples, &Fs); - - bOk &= (!Fs.empty()); - for (int i = 0; i < Fs.size(); ++i) { - bOk &= ExpectFundamentalProperties(Fs[i], x1, x2, 1e-8); - if (F_expected) { - bOk &= Colinear(Fs[i], *F_expected, 1e-6); - } - } - return bOk; -} - -BOOST_AUTO_TEST_CASE(SevenPointTest_EasyCase) { - Mat x1(2, 7), x2(2, 7); - x1 << 0, 0, 0, 1, 1, 1, 2, - 0, 1, 2, 0, 1, 2, 0; - x2 << 0, 0, 0, 1, 1, 1, 2, - 1, 2, 3, 1, 2, 3, 1; - typedef fundamental::kernel::SevenPointKernel Kernel; - BOOST_CHECK(ExpectKernelProperties(x1, x2)); -} - -BOOST_AUTO_TEST_CASE(SevenPointTest_Normalized_EasyCase) { - Mat x1(2, 7), x2(2, 7); - x1 << 0, 0, 0, 1, 1, 1, 2, - 0, 1, 2, 0, 1, 2, 0; - x2 << 0, 0, 0, 1, 1, 1, 2, - 1, 2, 3, 1, 2, 3, 1; - typedef fundamental::kernel::NormalizedSevenPointKernel Kernel; - BOOST_CHECK(ExpectKernelProperties(x1, x2)); -} - -BOOST_AUTO_TEST_CASE(EightPointTest_EasyCase) { - Mat x1(2, 8), x2(2, 8); - x1 << 0, 0, 0, 1, 1, 1, 2, 2, - 0, 1, 2, 0, 1, 2, 0, 1; - x2 << 0, 0, 0, 1, 1, 1, 2, 2, - 1, 2, 3, 1, 2, 3, 1, 2; - typedef fundamental::kernel::EightPointKernel Kernel; - BOOST_CHECK(ExpectKernelProperties(x1, x2)); -} - -BOOST_AUTO_TEST_CASE(EightPointTest_Normalized_EasyCase) { - Mat x1(2, 8), x2(2, 8); - x1 << 0, 0, 0, 1, 1, 1, 2, 2, - 0, 1, 2, 0, 1, 2, 0, 1; - x2 << 0, 0, 0, 1, 1, 1, 2, 2, - 1, 2, 3, 1, 2, 3, 1, 2; - typedef fundamental::kernel::NormalizedEightPointKernel Kernel; - BOOST_CHECK(ExpectKernelProperties(x1, x2)); -} diff --git a/src/aliceVision/multiview/homographyKernelSolver.cpp b/src/aliceVision/multiview/homographyKernelSolver.cpp deleted file mode 100644 index 9d88b9fa22..0000000000 --- a/src/aliceVision/multiview/homographyKernelSolver.cpp +++ /dev/null @@ -1,118 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 "homographyKernelSolver.hpp" -#include - -namespace aliceVision { -namespace homography { -namespace kernel { - -/// Setup the Direct Linear Transform. -/// Use template in order to support fixed or dynamic sized matrix. -/// Allow solve H as homogeneous(p2) = H homogeneous(p1) -template -void BuildActionMatrix(Matrix & L, const Mat &p1, const Mat &p2) { - - const Mat::Index n = p1.cols(); - for (Mat::Index i = 0; i < n; ++i) { - Mat::Index j = 2 * i; - L(j, 0) = p1(0, i); - L(j, 1) = p1(1, i); - L(j, 2) = 1.0; - L(j, 6) = -p2(0, i) * p1(0, i); - L(j, 7) = -p2(0, i) * p1(1, i); - L(j, 8) = -p2(0, i); - - ++j; - L(j, 3) = p1(0, i); - L(j, 4) = p1(1, i); - L(j, 5) = 1.0; - L(j, 6) = -p2(1, i) * p1(0, i); - L(j, 7) = -p2(1, i) * p1(1, i); - L(j, 8) = -p2(1, i); - } -} - -void FourPointSolver::Solve(const Mat &p1, const Mat &p2, vector *Hs) { - assert(2 == p1.rows()); - assert(4 <= p2.cols()); - assert(p1.rows() == p2.rows()); - assert(p1.cols() == p2.cols()); - - Mat::Index n = p1.cols(); - - /* Normalize input */ - Mat p1_local = p1; - Mat p2_local = p2; - - auto p1x = p1_local.block(0, 0, 1, n); - auto p1y = p1_local.block(1, 0, 1, n); - auto p2x = p2_local.block(0, 0, 1, n); - auto p2y = p2_local.block(1, 0, 1, n); - - double p1x_mean = p1x.mean(); - double p1y_mean = p1y.mean(); - double p2x_mean = p2x.mean(); - double p2y_mean = p2y.mean(); - - p1x.array() -= p1x_mean; - p1y.array() -= p1y_mean; - p2x.array() -= p2x_mean; - p2y.array() -= p2y_mean; - - double p1_dist = sqrt((p1x * p1x.transpose() + p1y * p1y.transpose())(0,0)); - double p2_dist = sqrt((p2x * p2x.transpose() + p2y * p2y.transpose())(0,0)); - - p1_local /= p1_dist; - p2_local /= p2_dist; - - Vec9 h; - if (n == 4) { - // In the case of minimal configuration we use fixed sized matrix to let - // Eigen and the compiler doing the maximum of optimization. - typedef Eigen::Matrix Mat16_9; - Mat16_9 L = Mat::Zero(16, 9); - BuildActionMatrix(L, p1_local, p2_local); - Nullspace(&L, &h); - } - else { - MatX9 L = Mat::Zero(n * 2, 9); - BuildActionMatrix(L, p1_local, p2_local); - Nullspace(&L, &h); - } - - /*Build G matrix from vector*/ - Mat3 G = Map(h.data()); - - /* - p2_local = G * p1_local - Kp2 * p2 = G * Kp1 * p1 - p2 = Kp2^-1 * G * Kp1 * p1 - H = Kp2^-1 * G * Kp1 - */ - Eigen::Matrix3d Kp1 = Eigen::Matrix3d::Identity(); - Kp1(0, 0) = 1.0 / p1_dist; - Kp1(0, 2) = - p1x_mean / p1_dist; - Kp1(1, 1) = 1.0 / p1_dist; - Kp1(1, 2) = - p1y_mean / p1_dist; - - Eigen::Matrix3d Kp2 = Eigen::Matrix3d::Identity(); - Kp2(0, 0) = 1.0 / p2_dist; - Kp2(0, 2) = - p2x_mean / p2_dist; - Kp2(1, 1) = 1.0 / p2_dist; - Kp2(1, 2) = - p2y_mean / p2_dist; - - Eigen::Matrix3d H = Kp2.inverse() * G * Kp1; - - Hs->push_back(H); -} - -} // namespace kernel -} // namespace homography -} // namespace aliceVision diff --git a/src/aliceVision/multiview/homographyKernelSolver.hpp b/src/aliceVision/multiview/homographyKernelSolver.hpp deleted file mode 100644 index 5e91b5c600..0000000000 --- a/src/aliceVision/multiview/homographyKernelSolver.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - -namespace aliceVision { -namespace homography { -namespace kernel { - -using namespace std; - -struct FourPointSolver { - enum { MINIMUM_SAMPLES = 4 }; - enum { MAX_MODELS = 1 }; - /** - * Computes the homography that transforms p1 to p2 with the Direct Linear - * Transform (DLT). - * - * \param p1 A 2xN matrix of column vectors. - * \param p2 A 2xN matrix of column vectors. - * \param Hs A vector into which the computed homography is stored. - * - * The estimated homography should approximately hold the condition p2 = H p1. - */ - static void Solve(const Mat &p1, const Mat &p2, vector *Hs); -}; - -// Should be distributed as Chi-squared with k = 2. -struct AsymmetricError { - static double Error(const Mat &H, const Vec2 &p1, const Vec2 &p2) { - Vec3 p2h_est = H * EuclideanToHomogeneous(p1); - Vec2 p2_est = p2h_est.head<2>() / p2h_est[2]; - return (p2 - p2_est).squaredNorm(); - } -}; - -// Kernel that works on original data point -typedef twoView::kernel::Kernel - UnnormalizedKernel; - -// By default use the normalized version for increased robustness. -typedef twoView::kernel::Kernel< - twoView::kernel::NormalizedSolver, - AsymmetricError, - Mat3> - Kernel; - -} // namespace kernel -} // namespace homography -} // namespace aliceVision diff --git a/src/aliceVision/multiview/homographyKernelSolver_test.cpp b/src/aliceVision/multiview/homographyKernelSolver_test.cpp deleted file mode 100644 index 0931a69eed..0000000000 --- a/src/aliceVision/multiview/homographyKernelSolver_test.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 "aliceVision/multiview/homographyKernelSolver.hpp" - -#define BOOST_TEST_MODULE homographyKernelSolver - -#include -#include -#include - -using namespace std; -using namespace aliceVision; - -BOOST_AUTO_TEST_CASE(HomographyKernelTest_Fitting_Unnormalized) { - // Define 3 knows homographies (Use as GT). - vector H_gt(3); - - H_gt[0] = Mat3::Identity(); - H_gt[1] << 1, 0, -4, - 0, 1, 5, - 0, 0, 1; - H_gt[2] << 1, -2, 3, - 4, 5, -6, - -7, 8, 1; - - // Define a set of points. - Mat x(2, 9), xh; - x << 0, 0, 0, 1, 1, 1, 2, 2, 2, - 0, 1, 2, 0, 1, 2, 0, 1, 2; - EuclideanToHomogeneous(x, &xh); - - for (int i = 0; i < H_gt.size(); ++i) { - // Transform points by the ground truth homography. - Mat y, yh = H_gt[i] * xh; - HomogeneousToEuclidean(yh, &y); - - homography::kernel::UnnormalizedKernel kernel(x, y); - - size_t samples_[5]={0,1,2,3,4}; - vector samples(samples_,samples_+5); - for (size_t j = 4; samples.size() < x.cols(); samples.push_back(j++)) { - vector Hs; - kernel.Fit(samples, &Hs); - BOOST_CHECK_EQUAL(1, Hs.size()); - // Check that found matrix is equal to the GT - EXPECT_MATRIX_PROP(H_gt[i], Hs[0], 1e-6); - } - } -} - -BOOST_AUTO_TEST_CASE(HomographyKernelTest_Fitting_Normalized) { - // Define 3 knows homographies (Use as GT). - vector H_gt(3); - - H_gt[0] = Mat3::Identity(); - H_gt[1] << 1, 0, -4, - 0, 1, 5, - 0, 0, 1; - H_gt[2] << 1, -2, 3, - 4, 5, -6, - -7, 8, 1; - - // Define a set of points. - Mat x(2, 9), xh; - x << 0, 0, 0, 1, 1, 1, 2, 2, 2, - 0, 1, 2, 0, 1, 2, 0, 1, 2; - EuclideanToHomogeneous(x, &xh); - - for (int i = 0; i < H_gt.size(); ++i) { - // Transform points by the ground truth homography. - Mat y, yh = H_gt[i] * xh; - HomogeneousToEuclidean(yh, &y); - - homography::kernel::Kernel kernel(x, y); - - size_t samples_[5]={0,1,2,3,4}; - vector samples(samples_,samples_+5); - for (size_t j = 4; samples.size() < x.cols(); samples.push_back(j++)) { - vector Hs; - kernel.Fit(samples, &Hs); - BOOST_CHECK_EQUAL(1, Hs.size()); - // Check that found matrix is equal to the GT - EXPECT_MATRIX_PROP(H_gt[i], Hs[0], 1e-6); - } - } -} diff --git a/src/aliceVision/multiview/knownRotationTranslationKernel.hpp b/src/aliceVision/multiview/knownRotationTranslationKernel.hpp index 20f29af990..90e359de8c 100644 --- a/src/aliceVision/multiview/knownRotationTranslationKernel.hpp +++ b/src/aliceVision/multiview/knownRotationTranslationKernel.hpp @@ -7,101 +7,180 @@ #pragma once -#include -#include -#include - -#include +#include +#include namespace aliceVision { -namespace translation { -namespace kernel { +namespace multiview { /** - * Two-point translation estimation between two views from a known rotation - * Implementation based on [1] => 3.2 Selection of the right solution. + * @brief Two-point translation estimation solver between two views from a known rotation + * Implementation based on [1] => 3.2 Selection of the right solution. * - * [1] "Finding the Exact Rotation Between Two Images Independently of the Translation." - * Authors: L.Kneip and R.Siegwart and M.Pollefeys - * Date: October 2012 - * Conference: ECCV + * @ref [1] "Finding the Exact Rotation Between Two Images Independently of the Translation." + * Authors: L.Kneip and R.Siegwart and M.Pollefeys + * Date: October 2012 + * Conference: ECCV */ -template -struct TwoPointTranslationSolver { - enum { MINIMUM_SAMPLES = 2 }; - enum { MAX_MODELS = 1 }; +struct TwoPointTranslationSolver +{ + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const + { + return 2; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const + { + return 1; + } - // Solve the problem of camera translation. - static void Solve(const Mat &xA, const Mat &xB, const Mat3& R, std::vector *vec_t) + /** + * @brief Solve the problem of camera translation. + * @param[in] xA + * @param[in] xB + * @param[in] R + * @param[out] models t + */ + void solve(const Mat& xA, const Mat& xB, const Mat3& R, std::vector>& models) const { const Mat3 Rt = R.transpose(); + // A side bearing vectors const Vec3 f1 = Vec3(xA.col(0)(0), xA.col(0)(1), 1.); const Vec3 f2 = Vec3(xA.col(1)(0), xA.col(1)(1), 1.); + // B side bearing vectors const Vec3 f1prime = Rt * Vec3(xB.col(0)(0), xB.col(0)(1), 1.); const Vec3 f2prime = Rt * Vec3(xB.col(1)(0), xB.col(1)(1), 1.); - // Compute the translation of the camera - const Vec3 c = - ((f1.cross(f1prime)) - .cross( - f2.cross(f2prime))).normalized(); + // compute the translation of the camera + const Vec3 c = ((f1.cross(f1prime)).cross(f2.cross(f2prime))).normalized(); - // Ensure the translation make the points in front of the cameras + // ensure the translation make the points in front of the cameras const Vec3 opticalFlow = f1 - f1prime; Vec3 translation = c; if (opticalFlow.dot(translation) < 0) translation = -translation; - vec_t->push_back(-R * translation); - } - - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2){ - return EpipolarDistanceErrorFunctor::Error(F, x1, x2); + models.emplace_back(-R * translation); } }; -//-- Generic Solver to find the translation from a known Rotation. -template -class TranslationFromKnowRotation : - public twoView::kernel::Kernel +/** + * @brief Generic Solver to find the translation from a known Rotation. + */ +template> +class TranslationFromKnowRotationKernel { public: - // 2D / 2D points [camera coordinates] - TranslationFromKnowRotation(const Mat &pt2DA, const Mat &pt2DB, const Mat3 & R) : - twoView::kernel::Kernel(pt2DA, pt2DB), _R(R){} - void Fit(const std::vector &samples, std::vector *models) const { - const Mat ptA = ExtractColumns(this->x1_, samples); - const Mat ptB = ExtractColumns(this->x2_, samples); + using SolverT = SolverT_; + using ErrorT = ErrorT_; + using ModelT = ModelT_; + + /** + * @brief TranslationFromKnowRotation constructor + * @param[in] pt2DA 2D points [camera coordinates] + * @param[in] pt2DB 2D points [camera coordinates] + * @param[in] R rotation matrix + */ + TranslationFromKnowRotationKernel(const Mat& x1, const Mat& x2, const Mat3& R) + : _x1(x1) + , _x2(x2) + , _R(R) + {} + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const + { + return _kernelSolver.getMinimumNbRequiredSamples(); + } - assert(2 == ptA.rows()); - assert(2 == ptB.rows()); + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const + { + return _kernelSolver.getMaximumNbModels(); + } + + /** + * @brief Extract required sample and fit model(s) to the sample + * @param[in] samples + * @param[out] models + */ + inline void fit(const std::vector& samples, std::vector& models) const + { + assert(2 == _x1.rows()); + assert(2 == _x2.rows()); - SolverArg::Solve(ptA, ptB, _R, models); + const Mat x1 = ExtractColumns(_x1, samples); + const Mat x2 = ExtractColumns(_x2, samples); + + _kernelSolver.solve(x1, x2, _R, models); } - // Error: distance of the sample to the epipolar line - double Error(size_t sample, const ModelArg &model) const { + /** + * @brief error : distance of the sample to the epipolar line + * @param[in] sample + * @param[in] model + * @return error value + */ + inline double error(std::size_t sample, const ModelT& model) const + { Mat34 poseA, poseB; - P_From_KRt(Mat3::Identity(), Mat3::Identity(), Vec3::Zero(), &poseA); - P_From_KRt(Mat3::Identity(), _R, model, &poseB); - const Mat3 F = F_from_P(poseA, poseB); - return ErrorArg::Error(F, this->x1_.col(sample), this->x2_.col(sample)); + P_from_KRt(Mat3::Identity(), Mat3::Identity(), Vec3::Zero(), &poseA); + P_from_KRt(Mat3::Identity(), _R, model.getMatrix(), &poseB); + const robustEstimation::Mat3Model F(F_from_P(poseA, poseB)); + return _errorEstimator.error(F, _x1.col(sample), _x2.col(sample)); } + + /** + * @brief Return the errors associated to the model and each sample point + * @param[in] model + * @param[out] errors + */ + inline virtual void errors(const ModelT& model, std::vector& errors) const + { + errors.resize(_x1.cols()); + for(std::size_t sample = 0; sample < _x1.cols(); ++sample) + errors.at(sample) = error(sample, model); + } + + /** + * @brief get the number of putative points + * @return number of putative points + */ + inline std::size_t nbSamples() const + { + return _x1.cols(); + } + protected: + + /// left corresponding point + const Mat& _x1; + /// right corresponding point + const Mat& _x2; + /// two view solver + const SolverT _kernelSolver = SolverT(); + /// solver error estimation + const ErrorT _errorEstimator = ErrorT(); + /// R rotation matrix const Mat3 _R; }; -//-- Usable solver for the 2pt translation from known rotation estimation -typedef TranslationFromKnowRotation< - TwoPointTranslationSolver, // SolverFunctor - TwoPointTranslationSolver, // ErrorFunctor - Vec3> TranslationFromKnowRotationKernel; - -} // namespace kernel -} // namespace translation +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/knownRotationTranslationKernel_test.cpp b/src/aliceVision/multiview/knownRotationTranslationKernel_test.cpp index e9c75ac5e1..1e27711383 100644 --- a/src/aliceVision/multiview/knownRotationTranslationKernel_test.cpp +++ b/src/aliceVision/multiview/knownRotationTranslationKernel_test.cpp @@ -5,9 +5,9 @@ // 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 "aliceVision/multiview/NViewDataSet.hpp" -#include "aliceVision/multiview/essential.hpp" -#include "aliceVision/multiview/knownRotationTranslationKernel.hpp" +#include +#include +#include #include @@ -19,43 +19,43 @@ using namespace aliceVision; -// Estimate the translation for a pair of view for which the relative rotation is known -// Use a 2 correspondences based solver -BOOST_AUTO_TEST_CASE(Translation_knownRotation_Kernel_Multiview) { - +// estimate the translation for a pair of view for which the relative rotation is known +// use a 2 correspondences based solver +BOOST_AUTO_TEST_CASE(TranslationFromKnowRotationKernel_Multiview) +{ const int nViews = 10; const int nbPoints = 2; const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, - NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + NViewDatasetConfigurator(1,1,0,0,5,0)); // suppose a camera with Unit matrix as K - // Solve the problem and check that fitted value are good enough - for (int nCameraIndex = 2; nCameraIndex < nViews; ++nCameraIndex) + // solve the problem and check that fitted value are good enough + for(int nCameraIndex = 2; nCameraIndex < nViews; ++nCameraIndex) { const Mat x0 = d._x[0]; const Mat xCam = d._x[nCameraIndex]; // coordinates does not need to be normalized since we have used a unit K matrix. - // Compute GT (Ground Truth) motion + // compute GT (Ground Truth) motion Mat3 R_GT; Vec3 t_GT; - RelativeCameraMotion(d._R[0], d._t[0], d._R[nCameraIndex], d._t[nCameraIndex], &R_GT, &t_GT); + relativeCameraMotion(d._R[0], d._t[0], d._R[nCameraIndex], d._t[nCameraIndex], &R_GT, &t_GT); - aliceVision::translation::kernel::TranslationFromKnowRotationKernel kernel(x0, xCam, R_GT); + multiview::TranslationFromKnowRotationKernel<> kernel(x0, xCam, R_GT); std::size_t samples_[2]={0,1}; - std::vector samples(samples_,samples_+2); - std::vector vec_t; - kernel.Fit(samples, &vec_t); + std::vector samples(samples_, samples_ + 2); + std::vector> vec_t; + kernel.fit(samples, vec_t); BOOST_CHECK_EQUAL(1, vec_t.size()); - // Check that the fitted model is compatible with the data - // Here the distance to the epipolar line is used + // check that the fitted model is compatible with the data + // here the distance to the epipolar line is used for (std::size_t i = 0; i < x0.cols(); ++i) { - BOOST_CHECK_SMALL(kernel.Error(i, vec_t[0]), 1e-8); + BOOST_CHECK_SMALL(kernel.error(i, vec_t.at(0)), 1e-8); } - // Check that the GT translation and the estimated one are equal - EXPECT_MATRIX_NEAR(t_GT.normalized(), vec_t[0].normalized(), 1e-8); + // check that the GT translation and the estimated one are equal + EXPECT_MATRIX_NEAR(t_GT.normalized(), vec_t.at(0).getMatrix().normalized(), 1e-8); } } diff --git a/src/aliceVision/multiview/projection.hpp b/src/aliceVision/multiview/projection.hpp deleted file mode 100644 index 05526447a1..0000000000 --- a/src/aliceVision/multiview/projection.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - -/// Collection of function related to the classic Projection matrix used -/// in computer vision. P = K[R|t] with [t]=[-RC] Cf HZ -namespace aliceVision { - -/// Compute P = K[R|t] -void P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P); - -Mat34 P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t); - -/// Decompose using the RQ decomposition HZ A4.1.1 pag.579. -void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp); - -/// Compute a fundamental matrix from projection matrices -Mat3 F_from_P(const Mat34 & P1, const Mat34 & P2); - -// Compute the depth of the X point. R*X[2]+t[2]. -double Depth(const Mat3 &R, const Vec3 &t, const Vec3 &X); - -// Compute P*[X|1.0]. Transformed from homogeneous to euclidean coordinates. -Vec2 Project(const Mat34 &P, const Vec3 &X); - -// Compute P*[X|1.0] for the X list of point (3D point). -void Project(const Mat34 &P, const Mat3X &X, Mat2X *x); - -// Compute P*[X|1.0] for the X list of point (4D point). -void Project(const Mat34 &P, const Mat4X &X, Mat2X *x); - -// Return P*[X|1.0] for the X list of point (3D point). -Mat2X Project(const Mat34 &P, const Mat3X &X); - -// Return P*[X|1.0] for the X list of point (4D point). -Mat2X Project(const Mat34 &P, const Mat4X &X); - -// Change homogeneous coordinates to euclidean. -void HomogeneousToEuclidean(const Vec4 &H, Vec3 *X); - -// Change euclidean coordinates to homogeneous. -void EuclideanToHomogeneous(const Mat &X, Mat *H); - -// Change euclidean coordinates to homogeneous. -Vec3 EuclideanToHomogeneous(const Vec2 &x); - -void HomogeneousToEuclidean(const Mat &H, Mat *X); - -Mat3X EuclideanToHomogeneous(const Mat2X &x); - -void EuclideanToHomogeneous(const Mat2X &x, Mat3X *h); - -void HomogeneousToEuclidean(const Mat3X &h, Mat2X *e); - -/// Project x point in camera coordinates -void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n); - -/// Project x point in camera coordinates -void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n); - -/// Estimates the root mean square error (2D) -double reprojectionErrorRMSE(const Mat2X &x_image, - const Mat4X &X_world, - const Mat34 &P); - -/// Estimates the root mean square error (2D) -double reprojectionErrorRMSE(const Mat2X &x_image, - const Mat3X &X_world, - const Mat3 &K, - const Mat3 &R, - const Vec3 &t); - -} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/CMakeLists.txt b/src/aliceVision/multiview/relativePose/CMakeLists.txt new file mode 100644 index 0000000000..bcdbc63e20 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/CMakeLists.txt @@ -0,0 +1,10 @@ +# Kernels +alicevision_add_test(fundamentalKernel_test.cpp NAME "multiview_relativePose_fundamentalKernel" LINKS aliceVision_multiview) +alicevision_add_test(homographyKernel_test.cpp NAME "multiview_relativePose_homographyKernel" LINKS aliceVision_multiview aliceVision_multiview_test_data) +alicevision_add_test(essentialKernel_test.cpp NAME "multiview_relativePose_essentialKernel" LINKS aliceVision_multiview aliceVision_multiview_test_data) + +# Solvers +alicevision_add_test(fundamental10PSolver_test.cpp NAME "multiview_relativePose_fundamental10Solver" LINKS aliceVision_multiview aliceVision_multiview_test_data) +alicevision_add_test(essential5PSolver_test.cpp NAME "multiview_relativePose_essential5PSolver" LINKS aliceVision_multiview aliceVision_multiview_test_data) + + diff --git a/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp b/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp new file mode 100644 index 0000000000..a0f9a94dd1 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp @@ -0,0 +1,209 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 "Essential5PSolver.hpp" +#include + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +Vec o1(const Vec& a, const Vec& b) +{ + Vec res = Vec::Zero(20); + + res(Pc::coef_xx) = a(Pc::coef_x) * b(Pc::coef_x); + res(Pc::coef_xy) = a(Pc::coef_x) * b(Pc::coef_y) + + a(Pc::coef_y) * b(Pc::coef_x); + res(Pc::coef_xz) = a(Pc::coef_x) * b(Pc::coef_z) + + a(Pc::coef_z) * b(Pc::coef_x); + res(Pc::coef_yy) = a(Pc::coef_y) * b(Pc::coef_y); + res(Pc::coef_yz) = a(Pc::coef_y) * b(Pc::coef_z) + + a(Pc::coef_z) * b(Pc::coef_y); + res(Pc::coef_zz) = a(Pc::coef_z) * b(Pc::coef_z); + res(Pc::coef_x) = a(Pc::coef_x) * b(Pc::coef_1) + + a(Pc::coef_1) * b(Pc::coef_x); + res(Pc::coef_y) = a(Pc::coef_y) * b(Pc::coef_1) + + a(Pc::coef_1) * b(Pc::coef_y); + res(Pc::coef_z) = a(Pc::coef_z) * b(Pc::coef_1) + + a(Pc::coef_1) * b(Pc::coef_z); + res(Pc::coef_1) = a(Pc::coef_1) * b(Pc::coef_1); + + return res; +} + +Vec o2(const Vec& a, const Vec& b) +{ + Vec res(20); + + res(Pc::coef_xxx) = a(Pc::coef_xx) * b(Pc::coef_x); + res(Pc::coef_xxy) = a(Pc::coef_xx) * b(Pc::coef_y) + + a(Pc::coef_xy) * b(Pc::coef_x); + res(Pc::coef_xxz) = a(Pc::coef_xx) * b(Pc::coef_z) + + a(Pc::coef_xz) * b(Pc::coef_x); + res(Pc::coef_xyy) = a(Pc::coef_xy) * b(Pc::coef_y) + + a(Pc::coef_yy) * b(Pc::coef_x); + res(Pc::coef_xyz) = a(Pc::coef_xy) * b(Pc::coef_z) + + a(Pc::coef_yz) * b(Pc::coef_x) + + a(Pc::coef_xz) * b(Pc::coef_y); + res(Pc::coef_xzz) = a(Pc::coef_xz) * b(Pc::coef_z) + + a(Pc::coef_zz) * b(Pc::coef_x); + res(Pc::coef_yyy) = a(Pc::coef_yy) * b(Pc::coef_y); + res(Pc::coef_yyz) = a(Pc::coef_yy) * b(Pc::coef_z) + + a(Pc::coef_yz) * b(Pc::coef_y); + res(Pc::coef_yzz) = a(Pc::coef_yz) * b(Pc::coef_z) + + a(Pc::coef_zz) * b(Pc::coef_y); + res(Pc::coef_zzz) = a(Pc::coef_zz) * b(Pc::coef_z); + res(Pc::coef_xx) = a(Pc::coef_xx) * b(Pc::coef_1) + + a(Pc::coef_x) * b(Pc::coef_x); + res(Pc::coef_xy) = a(Pc::coef_xy) * b(Pc::coef_1) + + a(Pc::coef_x) * b(Pc::coef_y) + + a(Pc::coef_y) * b(Pc::coef_x); + res(Pc::coef_xz) = a(Pc::coef_xz) * b(Pc::coef_1) + + a(Pc::coef_x) * b(Pc::coef_z) + + a(Pc::coef_z) * b(Pc::coef_x); + res(Pc::coef_yy) = a(Pc::coef_yy) * b(Pc::coef_1) + + a(Pc::coef_y) * b(Pc::coef_y); + res(Pc::coef_yz) = a(Pc::coef_yz) * b(Pc::coef_1) + + a(Pc::coef_y) * b(Pc::coef_z) + + a(Pc::coef_z) * b(Pc::coef_y); + res(Pc::coef_zz) = a(Pc::coef_zz) * b(Pc::coef_1) + + a(Pc::coef_z) * b(Pc::coef_z); + res(Pc::coef_x) = a(Pc::coef_x) * b(Pc::coef_1) + + a(Pc::coef_1) * b(Pc::coef_x); + res(Pc::coef_y) = a(Pc::coef_y) * b(Pc::coef_1) + + a(Pc::coef_1) * b(Pc::coef_y); + res(Pc::coef_z) = a(Pc::coef_z) * b(Pc::coef_1) + + a(Pc::coef_1) * b(Pc::coef_z); + res(Pc::coef_1) = a(Pc::coef_1) * b(Pc::coef_1); + + return res; +} + +/** + * @brief Compute the nullspace of the linear constraints given by the matches. + */ +Mat fivePointsNullspaceBasis(const Mat2X& x1, const Mat2X& x2) +{ + Eigen::Matrix A; + A.setZero(); // make A square until Eigen supports rectangular SVD. + encodeEpipolarEquation(x1, x2, &A); + Eigen::JacobiSVD > svd(A,Eigen::ComputeFullV); + return svd.matrixV().topRightCorner<9,4>(); +} + +/** + * @brief Builds the polynomial constraint matrix M. + */ +Mat fivePointsPolynomialConstraints(const Mat& EBasis) +{ + // build the polynomial form of E (equation (8) in Stewenius et al. [1]) + Vec E[3][3]; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + E[i][j] = Vec::Zero(20); + E[i][j](Pc::coef_x) = EBasis(3 * i + j, 0); + E[i][j](Pc::coef_y) = EBasis(3 * i + j, 1); + E[i][j](Pc::coef_z) = EBasis(3 * i + j, 2); + E[i][j](Pc::coef_1) = EBasis(3 * i + j, 3); + } + } + + // the constraint matrix. + Mat M(10, 20); + int mrow = 0; + + // determinant constraint det(E) = 0; equation (19) of Nister [2]. + M.row(mrow++) = o2(o1(E[0][1], E[1][2]) - o1(E[0][2], E[1][1]), E[2][0]) + + o2(o1(E[0][2], E[1][0]) - o1(E[0][0], E[1][2]), E[2][1]) + + o2(o1(E[0][0], E[1][1]) - o1(E[0][1], E[1][0]), E[2][2]); + + // cubic singular values constraint. + // equation (20). + Vec EET[3][3]; + for (int i = 0; i < 3; ++i) { // since EET is symmetric, we only compute + for (int j = 0; j < 3; ++j) { // its upper triangular part. + if (i <= j) { + EET[i][j] = o1(E[i][0], E[j][0]) + + o1(E[i][1], E[j][1]) + + o1(E[i][2], E[j][2]); + } else { + EET[i][j] = EET[j][i]; + } + } + } + + // equation (21). + Vec (&L)[3][3] = EET; + Vec trace = 0.5 * (EET[0][0] + EET[1][1] + EET[2][2]); + for (int i = 0; i < 3; ++i) { + L[i][i] -= trace; + } + + // equation (23). + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + Vec LEij = o2(L[i][0], E[0][j]) + + o2(L[i][1], E[1][j]) + + o2(L[i][2], E[2][j]); + M.row(mrow++) = LEij; + } + } + + return M; +} + +void Essential5PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const +{ + assert(2 == x1.rows()); + assert(5 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // step 1: Nullspace Extraction. + const Eigen::Matrix EBasis = fivePointsNullspaceBasis(x1, x2); + + // step 2: Constraint Expansion. + const Eigen::Matrix EConstraints = fivePointsPolynomialConstraints(EBasis); + + // step 3: Gauss-Jordan Elimination (done thanks to a LU decomposition). + typedef Eigen::Matrix Mat10; + Eigen::FullPivLU c_lu(EConstraints.block<10, 10>(0, 0)); + const Mat10 M = c_lu.solve(EConstraints.block<10, 10>(0, 10)); + + // for next steps we follow the matlab code given in Stewenius et al [1]. + // build action matrix. + const Mat10& B = M.topRightCorner<10,10>(); + Mat10 At = Mat10::Zero(10,10); + At.block<3, 10>(0, 0) = B.block<3, 10>(0, 0); + At.row(3) = B.row(4); + At.row(4) = B.row(5); + At.row(5) = B.row(7); + At(6,0) = At(7,1) = At(8,3) = At(9,6) = -1; + + Eigen::EigenSolver eigensolver(At); + const auto& eigenvectors = eigensolver.eigenvectors(); + const auto& eigenvalues = eigensolver.eigenvalues(); + + // build essential matrices for the real solutions. + models.reserve(10); + for(int s = 0; s < 10; ++s) + { + // only consider real solutions. + if(eigenvalues(s).imag() != 0) + continue; + + Mat3 E; + Eigen::Map(E.data()) = EBasis * eigenvectors.col(s).tail<4>().real(); + models.emplace_back(E.transpose()); + } +} + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Essential5PSolver.hpp b/src/aliceVision/multiview/relativePose/Essential5PSolver.hpp new file mode 100644 index 0000000000..937ae01648 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Essential5PSolver.hpp @@ -0,0 +1,118 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class Essential5PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 5; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 10; + } + + /** + * @brief Computes the relative pose of two calibrated cameras from 5 correspondences. + * @param[in] x1 Points in the first image. One per column. + * @param[in] x2 Corresponding points in the second image. One per column. + * @param[out] models A list of at most 10 candidate essential matrix solutions. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models) const override; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("Essential5PSolver does not support problem solving with weights."); + } +}; + +/** + * In the following code, polynomials are expressed as vectors containing + * their coefficients in the basis of monomials: + * + * [xxx xxy xyy yyy xxz xyz yyz xzz yzz zzz xx xy yy xz yz zz x y z 1] + * + * Note that there is an error in Stewenius' paper. In equation (9) they + * propose to use the basis: + * + * [xxx xxy xxz xyy xyz xzz yyy yyz yzz zzz xx xy xz yy yz zz x y z 1] + * + * But this is not the basis used in the rest of the paper, neither in + * the code they provide. I (pau) have spend 4 hours debugging and + * reverse engineering their code to find the problem. :( + */ +enum polynomialCoefficient { + coef_xxx, + coef_xxy, + coef_xyy, + coef_yyy, + coef_xxz, + coef_xyz, + coef_yyz, + coef_xzz, + coef_yzz, + coef_zzz, + coef_xx, + coef_xy, + coef_yy, + coef_xz, + coef_yz, + coef_zz, + coef_x, + coef_y, + coef_z, + coef_1 +}; + +using Pc = polynomialCoefficient; + +/** + * @brief Multiply two polynomials of degree 1. + */ +Vec o1(const Vec& a, const Vec& b); + + +/** + * @brief Multiply a polynomial of degree 2, a, by a polynomial of degree 1, b. + */ +Vec o2(const Vec& a, const Vec& b); + +/** + * @brief Compute the nullspace of the linear constraints given by the matches. + */ +Mat fivePointsNullspaceBasis(const Mat2X& x1, const Mat2X& x2); + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/essentialKernelSolver.cpp b/src/aliceVision/multiview/relativePose/Essential8PSolver.cpp similarity index 56% rename from src/aliceVision/multiview/essentialKernelSolver.cpp rename to src/aliceVision/multiview/relativePose/Essential8PSolver.cpp index ef42fd9817..3f0e3de4cf 100644 --- a/src/aliceVision/multiview/essentialKernelSolver.cpp +++ b/src/aliceVision/multiview/relativePose/Essential8PSolver.cpp @@ -6,27 +6,22 @@ // 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 "essentialKernelSolver.hpp" -#include -#include -#include - -#include +#include "Essential8PSolver.hpp" +#include namespace aliceVision { -namespace essential { -namespace kernel { - -using namespace std; +namespace multiview { +namespace relativePose { -void EightPointRelativePoseSolver::Solve(const Mat &x1, const Mat &x2, vector *Es) { +void Essential8PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const +{ assert(2 == x1.rows()); assert(8 <= x1.cols()); assert(x1.rows() == x2.rows()); assert(x1.cols() == x2.cols()); MatX9 A(x1.cols(), 9); - fundamental::kernel::EncodeEpipolarEquation(x1, x2, &A); + encodeEpipolarEquation(x1, x2, &A); Vec9 e; Nullspace(&A, &e); @@ -34,7 +29,8 @@ void EightPointRelativePoseSolver::Solve(const Mat &x1, const Mat &x2, vector 8) { + if (x1.cols() > 8) + { Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); Vec3 d = USV.singularValues(); const double a = d[0]; @@ -42,18 +38,10 @@ void EightPointRelativePoseSolver::Solve(const Mat &x1, const Mat &x2, vectorpush_back(E); + models.emplace_back(E); } -void FivePointSolver::Solve(const Mat &x1, const Mat &x2, vector *E) { - assert(2 == x1.rows()); - assert(5 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - FivePointsRelativePose(x1, x2, E); -} - -} // namespace kernel -} // namespace essential +} // namespace relativePose +} // namespace multiview } // namespace aliceVision + diff --git a/src/aliceVision/multiview/relativePose/Essential8PSolver.hpp b/src/aliceVision/multiview/relativePose/Essential8PSolver.hpp new file mode 100644 index 0000000000..e817846d52 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Essential8PSolver.hpp @@ -0,0 +1,65 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class Essential8PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 8; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + /** + * @brief Eight-point algorithm for solving for the essential matrix from normalized + * image coordinates of point correspondences. + * See page 294 in HZ Result 11.1. + * + * @param[in] x1 Points in the first image. One per column. + * @param[in] x2 Corresponding points in the second image. One per column. + * @param[out] models A list of at most 10 candidate essential matrix solutions. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models) const override; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("Essential8PSolver does not support problem solving with weights."); + } +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/EssentialKernel.hpp b/src/aliceVision/multiview/relativePose/EssentialKernel.hpp new file mode 100644 index 0000000000..eb8ec33ce7 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/EssentialKernel.hpp @@ -0,0 +1,92 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 +#include + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/** + * @brief Generic Solver for the 5pt Essential Matrix Estimation. + * + * @todo Error must be overwrite in order to compute F from E and K's. + * Fitting must normalize image values to camera values. + */ +template +class EssentialKernel : public robustEstimation::PointFittingKernel +{ +public: + + using KernelBase = robustEstimation::PointFittingKernel; + + EssentialKernel(const Mat& x1, const Mat& x2, const Mat3& K1, const Mat3& K2) + : robustEstimation::PointFittingKernel(x1,x2) + , _K1(K1) + , _K2(K2) + {} + + void fit(const std::vector& samples, std::vector& models) const override + { + const Mat x1 = ExtractColumns(KernelBase::_x1, samples); + const Mat x2 = ExtractColumns(KernelBase::_x2, samples); + + assert(2 == x1.rows()); + assert(KernelBase::_kernelSolver.getMinimumNbRequiredSamples() <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // normalize the data (image coords to camera coords). + const Mat3 K1Inverse = _K1.inverse(); + const Mat3 K2Inverse = _K2.inverse(); + + Mat x1_normalized; + Mat x2_normalized; + + robustEstimation::applyTransformationToPoints(x1, K1Inverse, &x1_normalized); + robustEstimation::applyTransformationToPoints(x2, K2Inverse, &x2_normalized); + + KernelBase::_kernelSolver.solve(x1_normalized, x2_normalized, models); + } + + double error(std::size_t sample, const ModelT& model) const + { + Mat3 F; + fundamentalFromEssential(model.getMatrix(), _K1, _K2, &F); + robustEstimation::Mat3Model modelF(F); + return KernelBase::_errorEstimator.error(modelF, KernelBase::_x1.col(sample), KernelBase::_x2.col(sample)); + } + +protected: + + // The two camera calibrated camera matrix + Mat3 _K1, _K2; +}; + +/** + * @brief Solver kernel for the 8pt Essential Matrix Estimation + */ +typedef EssentialKernel Essential8PKernel; + +/** + * @brief Solver kernel for the 5pt Essential Matrix Estimation + */ +typedef EssentialKernel Essential5PKernel; + + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Fundamental10PSolver.cpp b/src/aliceVision/multiview/relativePose/Fundamental10PSolver.cpp new file mode 100644 index 0000000000..20f274c4b4 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Fundamental10PSolver.cpp @@ -0,0 +1,311 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2018 AliceVision contributors. +// Copyright (c) 2015 Jan Heller , Zuzana Kukelova +// 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 "Fundamental10PSolver.hpp" + +#include +#include + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/** + * @brief Computes column echelon form of a matrix + * @author Jan Heller, adapted to aliceVision by Michal Polic + */ +template +void colEchelonForm(Eigen::MatrixBase& M, double pivtol = 1e-12) +{ + using Scalar = typename Derived::Scalar; + + const int n = M.rows(); + const int m = M.cols(); + + Eigen::Index i = 0; + Eigen::Index j = 0; + + while((i < m) && (j < n)) + { + Scalar p = std::numeric_limits::min(); + Eigen::Index col = i; + + for(Eigen::Index k = i; k < m; ++k) + { + Scalar tp = std::abs(M(j, k)); + if(tp > p) + { + p = tp; + col = k; + } + } + + if(p < Scalar(pivtol)) + { + M.block(j, i, 1, m - i).setZero(); + ++j; + } + else + { + if(col != i) + M.block(j, i, n - j, 1).swap(M.block(j, col, n - j, 1)); + + M.block(j + 1, i, n - j - 1, 1) /= M(j, i); + M(j, i) = 1.0; + + for(Eigen::Index k = 0; k < m; ++k) + { + if(k == i) + continue; + + M.block(j, k, n - j, 1) -= M(j, k) * M.block(j, i, n - j, 1); + } + + ++i; + ++j; + } + } +} + +template +int f10e_gb(const Eigen::Matrix& params, Eigen::MatrixBase& sols, double pivtol = 1e-16) +{ + Eigen::Matrix c; + + c(0) = params(0) * params(14) - params(4) * params(10); + c(1) = params(0) * params(16) + params(2) * params(14) - params(4) * params(12) - params(6) * params(10); + c(2) = params(2) * params(18) - params(8) * params(12); + c(3) = params(1) * params(15) - params(5) * params(11); + c(4) = params(1) * params(17) + params(3) * params(15) - params(5) * params(13) - params(7) * params(11); + c(5) = params(0) * params(15) + params(1) * params(14) - params(4) * params(11) - params(5) * params(10); + c(6) = params(0) * params(17) + params(1) * params(16) + params(2) * params(15) + params(3) * params(14) - params(4) * params(13) - params(5) * params(12) - params(6) * params(11) - params(7) * params(10); + c(7) = params(0) * params(18) + params(2) * params(16) - params(6) * params(12) - params(8) * params(10); + c(8) = params(0) * params(19) + params(1) * params(18) + params(2) * params(17) + params(3) * params(16) - params(6) * params(13) - params(7) * params(12) - params(8) * params(11) - params(9) * params(10); + c(9) = params(2) * params(19) + params(3) * params(18) - params(8) * params(13) - params(9) * params(12); + c(10) = params(1) * params(19) + params(3) * params(17) - params(7) * params(13) - params(9) * params(11); + c(11) = params(3) * params(19) - params(9) * params(13); + c(12) = params(0) * params(23) - params(4) * params(20); + c(13) = params(1) * params(23) + params(0) * params(25) - params(4) * params(21) - params(5) * params(20); + c(14) = params(2) * params(24) - params(8) * params(20); + c(15) = params(3) * params(24) + params(2) * params(27) - params(8) * params(21) - params(9) * params(20); + c(16) = params(1) * params(26) - params(5) * params(22); + c(17) = params(0) * params(24) + params(2) * params(23) - params(6) * params(20); + c(18) = params(0) * params(26) + params(1) * params(25) - params(4) * params(22) - params(5) * params(21); + c(19) = params(1) * params(24) + params(3) * params(23) + params(0) * params(27) + params(2) * params(25) - params(6) * params(21) - params(7) * params(20); + c(20) = params(0) * params(28) + params(1) * params(27) + params(2) * params(26) + params(3) * params(25) - params(6) * params(22) - params(7) * params(21); + c(21) = params(2) * params(28) + params(3) * params(27) - params(8) * params(22) - params(9) * params(21); + c(22) = params(1) * params(28) + params(3) * params(26) - params(7) * params(22); + c(23) = params(3) * params(28) - params(9) * params(22); + c(24) = params(10) * params(23) - params(14) * params(20); + c(25) = params(11) * params(23) + params(10) * params(25) - params(14) * params(21) - params(15) * params(20); + c(26) = params(12) * params(24) - params(18) * params(20); + c(27) = params(13) * params(24) + params(12) * params(27) - params(18) * params(21) - params(19) * params(20); + c(28) = params(11) * params(26) - params(15) * params(22); + c(29) = params(10) * params(24) + params(12) * params(23) - params(16) * params(20); + c(30) = params(10) * params(26) + params(11) * params(25) - params(14) * params(22) - params(15) * params(21); + c(31) = params(11) * params(24) + params(13) * params(23) + params(10) * params(27) + params(12) * params(25) - params(16) * params(21) - params(17) * params(20); + c(32) = params(10) * params(28) + params(11) * params(27) + params(12) * params(26) + params(13) * params(25) - params(16) * params(22) - params(17) * params(21); + c(33) = params(12) * params(28) + params(13) * params(27) - params(18) * params(22) - params(19) * params(21); + c(34) = params(11) * params(28) + params(13) * params(26) - params(17) * params(22); + c(35) = params(13) * params(28) - params(19) * params(22); + + Eigen::Matrix M; + M.setZero(); + + M(0) = c(0); M(61) = c(0); M(82) = c(0); + M(144) = c(0); M(2) = c(1); M(64) = c(1); + M(85) = c(1); M(148) = c(1); M(9) = c(2); + M(72) = c(2); M(93) = c(2); M(156) = c(2); + M(3) = c(3); M(66) = c(3); M(87) = c(3); + M(150) = c(3); M(7) = c(4); M(70) = c(4); + M(91) = c(4); M(154) = c(4); M(1) = c(5); + M(63) = c(5); M(84) = c(5); M(147) = c(5); + M(4) = c(6); M(67) = c(6); M(88) = c(6); + M(151) = c(6); M(5) = c(7); M(68) = c(7); + M(89) = c(7); M(152) = c(7); M(8) = c(8); + M(71) = c(8); M(92) = c(8); M(155) = c(8); + M(12) = c(9); M(75) = c(9); M(96) = c(9); + M(158) = c(9); M(11) = c(10); M(74) = c(10); + M(95) = c(10); M(157) = c(10); M(15) = c(11); + M(77) = c(11); M(98) = c(11); M(159) = c(11); + M(20) = c(12); M(102) = c(12); M(165) = c(12); + M(21) = c(13); M(104) = c(13); M(168) = c(13); + M(25) = c(14); M(109) = c(14); M(173) = c(14); + M(28) = c(15); M(112) = c(15); M(176) = c(15); + M(26) = c(16); M(110) = c(16); M(174) = c(16); + M(22) = c(17); M(105) = c(17); M(169) = c(17); + M(23) = c(18); M(107) = c(18); M(171) = c(18); + M(24) = c(19); M(108) = c(19); M(172) = c(19); + M(27) = c(20); M(111) = c(20); M(175) = c(20); + M(31) = c(21); M(115) = c(21); M(178) = c(21); + M(30) = c(22); M(114) = c(22); M(177) = c(22); + M(34) = c(23); M(117) = c(23); M(179) = c(23); + M(40) = c(24); M(122) = c(24); M(185) = c(24); + M(41) = c(25); M(124) = c(25); M(188) = c(25); + M(45) = c(26); M(129) = c(26); M(193) = c(26); + M(48) = c(27); M(132) = c(27); M(196) = c(27); + M(46) = c(28); M(130) = c(28); M(194) = c(28); + M(42) = c(29); M(125) = c(29); M(189) = c(29); + M(43) = c(30); M(127) = c(30); M(191) = c(30); + M(44) = c(31); M(128) = c(31); M(192) = c(31); + M(47) = c(32); M(131) = c(32); M(195) = c(32); + M(51) = c(33); M(135) = c(33); M(198) = c(33); + M(50) = c(34); M(134) = c(34); M(197) = c(34); + M(54) = c(35); M(137) = c(35); M(199) = c(35); + + colEchelonForm(M, pivtol); + + Eigen::Matrix A; + A.setZero(); + + A(0, 2) = 1.000000; A(1, 4) = 1.000000; A(2, 5) = 1.000000; + A(3, 7) = 1.000000; A(4, 8) = 1.000000; A(5, 9) = 1.000000; + A(6, 0) = -M(19, 9); A(6, 1) = -M(18, 9); A(6, 2) = -M(17, 9); + A(6, 3) = -M(16, 9); A(6, 4) = -M(15, 9); A(6, 5) = -M(14, 9); + A(6, 6) = -M(13, 9); A(6, 7) = -M(12, 9); A(6, 8) = -M(11, 9); + A(6, 9) = -M(10, 9); A(7, 0) = -M(19, 8); A(7, 1) = -M(18, 8); + A(7, 2) = -M(17, 8); A(7, 3) = -M(16, 8); A(7, 4) = -M(15, 8); + A(7, 5) = -M(14, 8); A(7, 6) = -M(13, 8); A(7, 7) = -M(12, 8); + A(7, 8) = -M(11, 8); A(7, 9) = -M(10, 8); A(8, 0) = -M(19, 7); + A(8, 1) = -M(18, 7); A(8, 2) = -M(17, 7); A(8, 3) = -M(16, 7); + A(8, 4) = -M(15, 7); A(8, 5) = -M(14, 7); A(8, 6) = -M(13, 7); + A(8, 7) = -M(12, 7); A(8, 8) = -M(11, 7); A(8, 9) = -M(10, 7); + A(9, 0) = -M(19, 6); A(9, 1) = -M(18, 6); A(9, 2) = -M(17, 6); + A(9, 3) = -M(16, 6); A(9, 4) = -M(15, 6); A(9, 5) = -M(14, 6); + A(9, 6) = -M(13, 6); A(9, 7) = -M(12, 6); A(9, 8) = -M(11, 6); + A(9, 9) = -M(10, 6); + + const Eigen::EigenSolver > eig(A); + Eigen::Matrix, 10, 2> esols; + esols.col(0).array() = eig.eigenvectors().row(2).array() / eig.eigenvectors().row(0).array(); + esols.col(1).array() = eig.eigenvectors().row(1).array() / eig.eigenvectors().row(0).array(); + + int nsols = 0; + for(Eigen::Index i = 0; i < 10; ++i) + { + if(esols.row(i).imag().isZero(100.0 * std::numeric_limits::epsilon())) + sols.col(nsols++) = esols.row(i).real(); + } + + return nsols; +} + +/** + * @brief Computes the relative pose and two radial disortion coefficients for two cameras from 10 correspondences + * Epipolar geometry F + l1 + l2 solver F10e: + * @code{.unparsed} + * [F11 F12 F13] [u] + * [x, y, 1+l1*(x^2+y^2)] * [F21 F22 F23] [v] = 0 + * [F31 F32 F33] [1+l2*(u^2+v^2)] + * @endcode + * @param[in] X 10x2 matrix of 2D distorted measurements (X == [x y]') + * @param[in] U 10x2 matrix of 2D distorted measurements (U == [u v]') + * @param[out] F list of candidate fundamental matrices solutions + * @param[out] L list of candidate radial disortion solutions (L[i] = [l1 l2]'). + * @return nsols + * + * @author Zuzana Kukelova, Jan Heller, Martin Bujnak, Andrew Fitzgibbon, Tomas Pajdla, adapted to aliceVision by Michal Polic + * @ref [1] Zuzana Kukelova, Jan Heller, Martin Bujnak, Andrew Fitzgibbon, Tomas Pajdla: + * Efficient Solution to the Epipolar Geometry for Radially Distorted Cameras, + * The IEEE International Conference on Computer Vision (ICCV), + * December, 2015, Santiago, Chile. + */ +int F10RelativePose(const Mat& X, const Mat& U, std::vector& models) +{ + using Mat21 = Eigen::Matrix; + + assert((X.rows() == 10 && X.cols() == 2) && "The first parameter (x) must be a 10x2 matrix"); + assert((U.rows() == 10 && U.cols() == 2) && "The second parameter (u) must be a 10x2 matrix"); + + Eigen::Matrix Z1; + Eigen::Matrix Z2; + Eigen::Matrix A; + + Z1.array() = X.col(0).array() * X.col(0).array() + X.col(1).array() * X.col(1).array(); + Z2.array() = U.col(0).array() * U.col(0).array() + U.col(1).array() * U.col(1).array(); + + A.col(0).array() = X.col(0).array() * U.col(0).array(); + A.col(1).array() = X.col(0).array() * U.col(1).array(); + A.col(2).array() = X.col(1).array() * U.col(0).array(); + A.col(3).array() = X.col(1).array() * U.col(1).array(); + A.col(4).array() = U.col(0).array() * Z1.array(); + A.col(5).array() = U.col(0).array(); + A.col(6).array() = U.col(1).array() * Z1.array(); + A.col(7).array() = U.col(1).array(); + A.col(8).array() = X.col(0).array() * Z2.array(); + A.col(9).array() = X.col(0).array(); + A.col(10).array() = X.col(1).array() * Z2.array(); + A.col(11).array() = X.col(1).array(); + A.col(12).array() = Z1.array() * Z2.array(); + A.col(13).array() = Z1.array(); + A.col(14).array() = Z2.array(); + A.col(15).fill(1.0); + + const Eigen::Matrix mr = A.block<10, 10>(0, 0).lu().solve(A.block<10, 6>(0, 10)); + + Eigen::Matrix params; + + params << mr(5, 0), mr(5, 1), -mr(4, 0), -mr(4, 1), mr(5, 2), mr(5, 3), mr(5, 4) - mr(4, 2), + mr(5, 5) - mr(4, 3), -mr(4, 4), -mr(4, 5), + mr(7, 0), mr(7, 1), -mr(6, 0), -mr(6, 1), mr(7, 2), mr(7, 3), mr(7, 4) - mr(6, 2), + mr(7, 5) - mr(6, 3), -mr(6, 4), -mr(6, 5), + mr(9, 0), mr(9, 1) - mr(8, 0), -mr(8, 1), mr(9, 2), mr(9, 4), mr(9, 3) - mr(8, 2), + -mr(8, 3), mr(9, 5) - mr(8, 4), -mr(8, 5); + + Mat Ls(2, 10); + const int nsols = f10e_gb(params, Ls); + + if(nsols == 0) + return 0; + + Eigen::Matrix b; + + b << mr(5, 0), mr(5, 1), -mr(4, 0), -mr(4, 1), mr(5, 2), mr(5, 3), + mr(5, 4) - mr(4, 2), mr(5, 5) - mr(4, 3), -mr(4, 4), -mr(4, 5); + + models.reserve(nsols); + + for(Eigen::Index i = 0; i < nsols; ++i) + { + const double l1 = Ls(0, i); + const double l2 = Ls(1, i); + const double l1l1 = l1 * l1; + const double l1l2 = l1 * l2; + + const Eigen::Matrix m1 = (Eigen::Matrix() << l1l2, l1, l2, 1).finished(); + const Eigen::Matrix m2 = (Eigen::Matrix() << l1l2 * l1, l1l1, l1l2, l1, l2, 1).finished(); + + const double f23 = -b.block<6, 1>(4, 0).dot(m2) / b.block<4, 1>(0, 0).dot(m1); + + const Eigen::Matrix m3 = (Eigen::Matrix() << l2 * f23, f23, l1l2, l1, l2, 1).finished(); + + Mat3 F; + Mat21 L; + + L << l1, l2; + F << m3.dot(-mr.row(0)), m3.dot(-mr.row(1)), m3.dot(-mr.row(9)), + m3.dot(-mr.row(2)), m3.dot(-mr.row(3)), f23, + m3.dot(-mr.row(5)), m3.dot(-mr.row(7)), 1; + + models.emplace_back(F, L); + } + + return nsols; +} + +void Fundamental10PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const +{ + Mat x1_ = x1; + x1_.transposeInPlace(); + Mat x2_ = x2; + x2_.transposeInPlace(); + F10RelativePose(x1_, x2_, models); +} + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Fundamental10PSolver.hpp b/src/aliceVision/multiview/relativePose/Fundamental10PSolver.hpp new file mode 100644 index 0000000000..9cfe4a25cc --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Fundamental10PSolver.hpp @@ -0,0 +1,110 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2018 AliceVision contributors. +// Copyright (c) 2015 Jan Heller , Zuzana Kukelova +// 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/*** + * @brief Model for relative pose and two radial distortion coefficients. + */ +struct Fundamental10PModel : public robustEstimation::Mat3Model +{ + using Mat21 = Eigen::Matrix; + + Fundamental10PModel() = default; + + /** + * @brief Constructor + * @param[in] F the 3x3 fundamental matrix. + * @param[in] L a 2x1 matrix containing the two radial distortion parameters. + */ + Fundamental10PModel(const Mat3& F, const Mat21& L) + : robustEstimation::Mat3Model(F) , _L(L) + {} + + /** + * @brief Get the estimated radial distortion parameters. + * @return the radial distortion parameters as a 2x1 matrix. + */ + inline const Mat21& getRadialDistortion() const + { + return _L; + } + + /** + * @brief Get the estimated fundamental matrix. + * @return the fundamental matrix. + */ + inline const Mat3& getFundamentalMatrix() const + { + return getMatrix(); + } + +private: + /// the two radial distortion parameters + Mat21 _L; +}; + +/*** + * @brief Solver for the relative pose and two radial distortion coefficients for two cameras from 10 correspondences. + */ +class Fundamental10PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 10; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 10; + } + + /** + * @brief Computes the relative pose and two radial distortion coefficients for two cameras from 10 correspondences. + * @see Efficient Solution to the Epipolar Geometry for Radially Distorted Cameras, + * The IEEE International Conference on Computer Vision (ICCV), + * Zuzana Kukelova, Jan Heller, Martin Bujnak, Andrew Fitzgibbon, Tomas Pajdla + * December, 2015, Santiago, Chile. + * + * @param[in] x1 Points in the first image. One per column. + * @param[in] x2 Corresponding points in the second image. One per column. + * @param[out] models A list of at most 10 candidate essential matrix solutions. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models) const override; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("Fundamental10PModel does not support problem solving with weights."); + } +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Fundamental7PSolver.cpp b/src/aliceVision/multiview/relativePose/Fundamental7PSolver.cpp new file mode 100644 index 0000000000..3866ea2152 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Fundamental7PSolver.cpp @@ -0,0 +1,98 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 "Fundamental7PSolver.hpp" +#include +#include + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +void Fundamental7PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const +{ + assert(2 == x1.rows()); + assert(7 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + Vec9 f1, f2; + + if(x1.cols() == 7) + { + // set up the homogeneous system Af = 0 from the equations x'T*F*x = 0. + + // in the minimal solution use fixed sized matrix to let Eigen and the + // compiler doing the maximum of optimization. + Mat9 A = Mat::Zero(9,9); + encodeEpipolarEquation(x1, x2, &A); + + // Eigen::FullPivLU luA(A); + // ALICEVISION_LOG_DEBUG("rank(A) = " << luA.rank()); + // Eigen::JacobiSVD svdA(A); + // ALICEVISION_LOG_DEBUG("Its singular values are:\n" << svdA.singularValues()); + + // find the two F matrices in the nullspace of A. + Nullspace2(&A, &f1, &f2); + + // @fixme here there is a potential error, we should check that the size of + // null(A) is 2. Otherwise we have a family of possible solutions for the + // fundamental matrix (ie infinite solution). This happens, e.g., when matching + // the image against itself or in other degenerate configurations of the camera, + // such as pure rotation or correspondences all on the same plane (cf HZ pg296 table 11.1) + // This is not critical for just matching images with geometric validation, + // it becomes an issue if the estimated F has to be used for retrieving the + // motion of the camera. + } + else + { + // set up the homogeneous system Af = 0 from the equations x'T*F*x = 0. + Mat A(x1.cols(), 9); + encodeEpipolarEquation(x1, x2, &A); + + // find the two F matrices in the nullspace of A. + Nullspace2(&A, &f1, &f2); + } + + Mat3 F1 = Map(f1.data()); + Mat3 F2 = Map(f2.data()); + + // use the condition det(F) = 0 to determine F. + // in other words, solve: det(F1 + a*F2) = 0 for a. + const double a = F1(0, 0), j = F2(0, 0), + b = F1(0, 1), k = F2(0, 1), + c = F1(0, 2), l = F2(0, 2), + d = F1(1, 0), m = F2(1, 0), + e = F1(1, 1), n = F2(1, 1), + f = F1(1, 2), o = F2(1, 2), + g = F1(2, 0), p = F2(2, 0), + h = F1(2, 1), q = F2(2, 1), + i = F1(2, 2), r = F2(2, 2); + + // run fundamental_7point_coeffs.py to get the below coefficients. + // the coefficients are in ascending powers of alpha, i.e. P[N]*x^N. + const double P[4] = { + a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g, + a*e*r + a*i*n + b*f*p + b*g*o + c*d*q + c*h*m + d*h*l + e*i*j + f*g*k - + a*f*q - a*h*o - b*d*r - b*i*m - c*e*p - c*g*n - d*i*k - e*g*l - f*h*j, + a*n*r + b*o*p + c*m*q + d*l*q + e*j*r + f*k*p + g*k*o + h*l*m + i*j*n - + a*o*q - b*m*r - c*n*p - d*k*r - e*l*p - f*j*q - g*l*n - h*j*o - i*k*m, + j*n*r + k*o*p + l*m*q - j*o*q - k*m*r - l*n*p }; + + // solve for the roots of P[3]*x^3 + P[2]*x^2 + P[1]*x + P[0] = 0. + double roots[3]; + const int nbRoots = SolveCubicPolynomial(P, roots); + + // build the fundamental matrix for each solution. + for(int kk = 0; kk < nbRoots; ++kk) + models.emplace_back(F1 + roots[kk] * F2); +} + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Fundamental7PSolver.hpp b/src/aliceVision/multiview/relativePose/Fundamental7PSolver.hpp new file mode 100644 index 0000000000..e8f4e6a393 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Fundamental7PSolver.hpp @@ -0,0 +1,70 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class Fundamental7PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 7; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 3; + } + + /** + * @brief Seven-point algorithm for solving for the fundamental matrix from point + * correspondences. @see page 281 in HZ + * + * Though oddly they use a different equation: \f$det(\alpha F_1 + (1-\alpha)F_2) = 0\f$. + * Since \f$F_1\f$ and \f$F2\f$ are projective, there's no need to balance the relative scale. + * Instead, here, the simpler equation is solved: \f$det(F_1 + \alpha F_2) = 0\f$. + * + * @see http://www.cs.unc.edu/~marc/tutorial/node55.html + * + * @param[in] x1 Points in the first image. One per column. + * @param[in] x2 Corresponding points in the second image. One per column. + * @param[out] models A list of at most 10 candidate essential matrix solutions. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models) const override; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("Fundamental7PSolver does not support problem solving with weights."); + } +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Fundamental8PSolver.cpp b/src/aliceVision/multiview/relativePose/Fundamental8PSolver.cpp new file mode 100644 index 0000000000..1d5e4816ef --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Fundamental8PSolver.cpp @@ -0,0 +1,67 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 "Fundamental8PSolver.hpp" +#include + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +void solveProblem(const Mat& x1, const Mat& x2, std::vector& models, const std::vector* weights = nullptr) +{ + assert(2 == x1.rows()); + assert(8 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + Vec9 f; + + if(x1.cols() == 8) + { + // in the minimal solution use fixed sized matrix to let Eigen and the + // compiler doing the maximum of optimization. + Mat9 A = Mat::Zero(9,9); + encodeEpipolarEquation(x1, x2, &A, weights); + Nullspace(&A, &f); + } + else + { + MatX9 A(x1.cols(), 9); + encodeEpipolarEquation(x1, x2, &A, weights); + Nullspace(&A, &f); + } + + Mat3 F = Map(f.data()); + + // force the fundamental property if the A matrix has full rank. + // @see HZ 11.1.1 pag.280 + if(x1.cols() > 8) + { + // force fundamental matrix to have rank 2 + Eigen::JacobiSVD USV(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec3 d = USV.singularValues(); + d[2] = 0.0; + F = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose(); + } + models.emplace_back(F); +} + +void Fundamental8PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const +{ + solveProblem(x1, x2, models); +} + +void Fundamental8PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const +{ + solveProblem(x1, x2, models, &weights); +} + +} // namespace kernel +} // namespace fundamental +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Fundamental8PSolver.hpp b/src/aliceVision/multiview/relativePose/Fundamental8PSolver.hpp new file mode 100644 index 0000000000..6fcb6f4bad --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Fundamental8PSolver.hpp @@ -0,0 +1,59 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class Fundamental8PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 8; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + /** + * @brief + * @param[in] x1 Points in the first image. One per column. + * @param[in] x2 Corresponding points in the second image. One per column. + * @param[out] models A list of at most 10 candidate essential matrix solutions. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models) const override; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override; +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/FundamentalError.hpp b/src/aliceVision/multiview/relativePose/FundamentalError.hpp new file mode 100644 index 0000000000..f9056a25ec --- /dev/null +++ b/src/aliceVision/multiview/relativePose/FundamentalError.hpp @@ -0,0 +1,68 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 multiview { +namespace relativePose { + +/** + * @brief Compute FundamentalSampsonError related to the Fundamental matrix and 2 correspondences + */ +struct FundamentalSampsonError : public ISolverErrorRelativePose +{ + inline double error(const robustEstimation::Mat3Model& F, const Vec2& x1, const Vec2& x2) const override + { + const Vec3 x(x1(0), x1(1), 1.0); + const Vec3 y(x2(0), x2(1), 1.0); + + // @see page 287 equation (11.9) of HZ. + const Vec3 F_x = F.getMatrix() * x; + const Vec3 Ft_y = F.getMatrix().transpose() * y; + + return Square(y.dot(F_x)) / ( F_x.head<2>().squaredNorm() + Ft_y.head<2>().squaredNorm()); + } +}; + +struct FundamentalSymmetricEpipolarDistanceError: public ISolverErrorRelativePose +{ + inline double error(const robustEstimation::Mat3Model& F, const Vec2& x1, const Vec2& x2) const override + { + const Vec3 x(x1(0), x1(1), 1.0); + const Vec3 y(x2(0), x2(1), 1.0); + + // @see page 288 equation (11.10) of HZ. + const Vec3 F_x = F.getMatrix() * x; + const Vec3 Ft_y = F.getMatrix().transpose() * y; + + // @note the divide by 4 is to make this match the Sampson distance. + return Square(y.dot(F_x)) * ( 1.0 / F_x.head<2>().squaredNorm() + 1.0 / Ft_y.head<2>().squaredNorm()) / 4.0; + } +}; + +struct FundamentalEpipolarDistanceError : public ISolverErrorRelativePose +{ + inline double error(const robustEstimation::Mat3Model& F, const Vec2& x1, const Vec2& x2) const override + { + // transfer error in image 2 + // @see page 287 equation (11.9) of HZ. + const Vec3 x(x1(0), x1(1), 1.0); + const Vec3 y(x2(0), x2(1), 1.0); + const Vec3 F_x = F.getMatrix() * x; + + return Square(F_x.dot(y)) / F_x.head<2>().squaredNorm(); + } +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/FundamentalKernel.hpp b/src/aliceVision/multiview/relativePose/FundamentalKernel.hpp new file mode 100644 index 0000000000..1f357344c9 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/FundamentalKernel.hpp @@ -0,0 +1,45 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 multiview { +namespace relativePose { + +/** + * @brief Kernel solver for the 8pt Fundamental Matrix Estimation + */ +using Fundamental7PKernel = robustEstimation::PointFittingKernel; + +/** + * @brief Kernel solver for the 8pt Fundamental Matrix Estimation + */ +using Fundamental8PKernel = robustEstimation::PointFittingKernel; + +/** + * @brief Normalized 7pt kernel + * @see conditioning from HZ (Algo 11.1) pag 282 + */ +using NormalizedFundamental7PKernel = robustEstimation::NormalizedPointFittingKernel; + +/** + * @brief Normalized 8pt kernel + * @see conditioning from HZ (Algo 11.1) pag 282 + */ +using NormalizedFundamental8PKernel = robustEstimation::NormalizedPointFittingKernel; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Homography4PSolver.cpp b/src/aliceVision/multiview/relativePose/Homography4PSolver.cpp new file mode 100644 index 0000000000..ae7838e83f --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Homography4PSolver.cpp @@ -0,0 +1,77 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 "Homography4PSolver.hpp" + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/** + * @brief Setup the Direct Linear Transform. + * Use template in order to support fixed or dynamic sized matrix. + * Allow solve H as homogeneous(x2) = H homogeneous(x1) + */ +template +void buildActionMatrix(Matrix& L, const Mat& x1, const Mat& x2) +{ + const Mat::Index n = x1.cols(); + for(Mat::Index i = 0; i < n; ++i) + { + Mat::Index j = 2 * i; + L(j, 0) = x1(0, i); + L(j, 1) = x1(1, i); + L(j, 2) = 1.0; + L(j, 6) = -x2(0, i) * x1(0, i); + L(j, 7) = -x2(0, i) * x1(1, i); + L(j, 8) = -x2(0, i); + + ++j; + L(j, 3) = x1(0, i); + L(j, 4) = x1(1, i); + L(j, 5) = 1.0; + L(j, 6) = -x2(1, i) * x1(0, i); + L(j, 7) = -x2(1, i) * x1(1, i); + L(j, 8) = -x2(1, i); + } +} + +void Homography4PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const +{ + assert(2 == x1.rows()); + assert(4 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + const Mat::Index n = x1.cols(); + + Vec9 h; + if(n == 4) + { + // in the case of minimal configuration we use fixed sized matrix to let + // Eigen and the compiler doing the maximum of optimization. + typedef Eigen::Matrix Mat16_9; + Mat16_9 L = Mat::Zero(16, 9); + buildActionMatrix(L, x1, x2); + Nullspace(&L, &h); + } + else + { + MatX9 L = Mat::Zero(n * 2, 9); + buildActionMatrix(L, x1, x2); + Nullspace(&L, &h); + } + + // map the linear vector as the H matrix + Mat3 H = Map(h.data()); + models.emplace_back(H); +} + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Homography4PSolver.hpp b/src/aliceVision/multiview/relativePose/Homography4PSolver.hpp new file mode 100644 index 0000000000..85511a4841 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/Homography4PSolver.hpp @@ -0,0 +1,66 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class Homography4PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 4; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + /** + * @brief Computes the homography that transforms x to y with the Direct Linear + * Transform (DLT). + * + * @param[in] x A 2xN matrix of column vectors. + * @param[in] y A 2xN matrix of column vectors. + * @param[out] Hs A vector into which the computed homography is stored. + * + * The estimated homography should approximately hold the condition y = H x. + */ + void solve(const Mat& x, const Mat& y, std::vector& models) const override; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("Homography4PSolver does not support problem solving with weights."); + } +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/HomographyError.hpp b/src/aliceVision/multiview/relativePose/HomographyError.hpp new file mode 100644 index 0000000000..f258a03093 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/HomographyError.hpp @@ -0,0 +1,35 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/** + * @brief The HomographyAsymmetricError struct + * @note Should be distributed as Chi-squared with k = 2. + */ +struct HomographyAsymmetricError : ISolverErrorRelativePose +{ + inline double error(const robustEstimation::Mat3Model& H, const Vec2& x1, const Vec2& x2) const override + { + const Vec3 x2h_est = H.getMatrix() * euclideanToHomogeneous(x1); + const Vec2 x2_est = x2h_est.head<2>() / x2h_est[2]; + return (x2 - x2_est).squaredNorm(); + } +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/HomographyKernel.hpp b/src/aliceVision/multiview/relativePose/HomographyKernel.hpp new file mode 100644 index 0000000000..59de384696 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/HomographyKernel.hpp @@ -0,0 +1,32 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/** + * @brief Kernel that works on original data point + */ +typedef robustEstimation::PointFittingKernel Homography4PKernel; + +/** + * @brief By default use the normalized version for increased robustness. + */ +typedef robustEstimation::NormalizedPointFittingKernel NormalizedHomography4PKernel; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/ISolverErrorRelativePose.hpp b/src/aliceVision/multiview/relativePose/ISolverErrorRelativePose.hpp new file mode 100644 index 0000000000..9fdbc2de96 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/ISolverErrorRelativePose.hpp @@ -0,0 +1,24 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +/** + * @brief Relative pose solver error interface. + */ +template +struct ISolverErrorRelativePose +{ + virtual double error(const ModelT& model, const Vec2& x1, const Vec2& x2) const = 0; +}; + +} // namespace relativePose +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/essential5PSolver_test.cpp b/src/aliceVision/multiview/relativePose/essential5PSolver_test.cpp new file mode 100644 index 0000000000..b419fee1e1 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/essential5PSolver_test.cpp @@ -0,0 +1,288 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +#define BOOST_TEST_MODULE essentialFivePointSolver +#include +#include +#include + +using namespace aliceVision; + +struct TestData +{ + // dataset that encapsulate : + // 3D points and their projection given P1 and P2 + // link between the two camera [R|t] + + Mat3X X; + Mat3 R; + Vec3 t; + Mat3 E; + Mat34 P1, P2; + Mat2X x1, x2; +}; + +TestData SomeTestData() +{ + TestData d; + + // modeling random 3D points, + // consider first camera as [R=I|t=0], + // second camera as [R=Rx*Ry*Rz|t=random], + // compute projection of the 3D points onto image plane. + + d.X = Mat3X::Random(3,5); + + // make point in front to the cameras. + d.X.row(0).array() -= .5; + d.X.row(1).array() -= .5; + d.X.row(2).array() += 1.0; + + d.R = RotationAroundZ(0.3) * RotationAroundX(0.1) * RotationAroundY(0.2); + do + { + d.t.setRandom(); + }while(!cheiralityTestAll(d.R, d.t, d.X)); + + + essentialFromRt(Mat3::Identity(), Vec3::Zero(), d.R, d.t, &d.E); + + P_from_KRt(Mat3::Identity(), Mat3::Identity(), Vec3::Zero(), &d.P1); + P_from_KRt(Mat3::Identity(), d.R, d.t, &d.P2); + + project(d.P1, d.X, &d.x1); + project(d.P2, d.X, &d.x2); + + return d; +} + +double EvalPolynomial(Vec p, double x, double y, double z) +{ + using namespace aliceVision::multiview::relativePose; + + return p(Pc::coef_xxx) * x * x * x + + p(Pc::coef_xxy) * x * x * y + + p(Pc::coef_xxz) * x * x * z + + p(Pc::coef_xyy) * x * y * y + + p(Pc::coef_xyz) * x * y * z + + p(Pc::coef_xzz) * x * z * z + + p(Pc::coef_yyy) * y * y * y + + p(Pc::coef_yyz) * y * y * z + + p(Pc::coef_yzz) * y * z * z + + p(Pc::coef_zzz) * z * z * z + + p(Pc::coef_xx) * x * x + + p(Pc::coef_xy) * x * y + + p(Pc::coef_xz) * x * z + + p(Pc::coef_yy) * y * y + + p(Pc::coef_yz) * y * z + + p(Pc::coef_zz) * z * z + + p(Pc::coef_x) * x + + p(Pc::coef_y) * y + + p(Pc::coef_z) * z + + p(Pc::coef_1) * 1; +} + +BOOST_AUTO_TEST_CASE(o1_Evaluation) +{ + using namespace aliceVision::multiview::relativePose; + + Vec p1 = Vec::Zero(20), p2 = Vec::Zero(20); + + p1(Pc::coef_x) = double(rand()) / RAND_MAX; + p1(Pc::coef_y) = double(rand()) / RAND_MAX; + p1(Pc::coef_z) = double(rand()) / RAND_MAX; + p1(Pc::coef_1) = double(rand()) / RAND_MAX; + p2(Pc::coef_x) = double(rand()) / RAND_MAX; + p2(Pc::coef_y) = double(rand()) / RAND_MAX; + p2(Pc::coef_z) = double(rand()) / RAND_MAX; + p2(Pc::coef_1) = double(rand()) / RAND_MAX; + + Vec p3 = o1(p1, p2); + + for (double z = -5; z < 5; ++z) + for (double y = -5; y < 5; ++y) + for (double x = -5; x < 5; ++x) + BOOST_CHECK_SMALL(EvalPolynomial(p3, x, y, z) - EvalPolynomial(p1, x, y, z) * EvalPolynomial(p2, x, y, z), 1e-8); +} + +BOOST_AUTO_TEST_CASE(o2_Evaluation) +{ + using namespace aliceVision::multiview::relativePose; + + Vec p1 = Vec::Zero(20), p2 = Vec::Zero(20); + + p1(Pc::coef_xx) = double(rand()) / RAND_MAX; + p1(Pc::coef_xy) = double(rand()) / RAND_MAX; + p1(Pc::coef_xz) = double(rand()) / RAND_MAX; + p1(Pc::coef_yy) = double(rand()) / RAND_MAX; + p1(Pc::coef_yz) = double(rand()) / RAND_MAX; + p1(Pc::coef_zz) = double(rand()) / RAND_MAX; + p1(Pc::coef_x) = double(rand()) / RAND_MAX; + p1(Pc::coef_y) = double(rand()) / RAND_MAX; + p1(Pc::coef_z) = double(rand()) / RAND_MAX; + p1(Pc::coef_1) = double(rand()) / RAND_MAX; + p2(Pc::coef_x) = double(rand()) / RAND_MAX; + p2(Pc::coef_y) = double(rand()) / RAND_MAX; + p2(Pc::coef_z) = double(rand()) / RAND_MAX; + p2(Pc::coef_1) = double(rand()) / RAND_MAX; + + Vec p3 = o2(p1, p2); + + for (double z = -5; z < 5; ++z) + for (double y = -5; y < 5; ++y) + for (double x = -5; x < 5; ++x) + BOOST_CHECK_SMALL(EvalPolynomial(p3, x, y, z) - EvalPolynomial(p1, x, y, z) * EvalPolynomial(p2, x, y, z), 1e-8); +} + +/// Check that the E matrix fit the Essential Matrix properties +/// Determinant is 0 +/// +#define EXPECT_ESSENTIAL_MATRIX_PROPERTIES(E, expectedPrecision) { \ + BOOST_CHECK_SMALL(E.determinant(), expectedPrecision); \ + Mat3 O = 2 * E * E.transpose() * E - (E * E.transpose()).trace() * E; \ + Mat3 zero3x3 = Mat3::Zero(); \ + EXPECT_MATRIX_NEAR(zero3x3, O, expectedPrecision);\ +} + +BOOST_AUTO_TEST_CASE(FivePointsRelativePose_Random) +{ + for(std::size_t trial = 0; trial < 100; ++trial) + { + TestData d = SomeTestData(); + + std::vector Es; + std::vector Rs; + std::vector ts; + + multiview::relativePose::Essential5PSolver solver; + + solver.solve(d.x1, d.x2, Es); + + BOOST_CHECK(!Es.empty()); + + // Recover rotation and translation from E + Rs.resize(Es.size()); + ts.resize(Es.size()); + for(std::size_t s = 0; s < Es.size(); ++s) + { + for(Eigen::Index c = 0; c < d.x1.cols(); ++c) + { + const double v = + d.x2.col(c).homogeneous().transpose() * Es.at(s).getMatrix() * d.x1.col(c).homogeneous(); + BOOST_CHECK_SMALL(v, 1.0e-6); + } + + Vec2 x1Col = d.x1.col(0); + Vec2 x2Col = d.x2.col(0); + BOOST_CHECK(motionFromEssentialAndCorrespondence(Es.at(s).getMatrix(), Mat3::Identity(), x1Col, + Mat3::Identity(), x2Col, &Rs[s], &ts[s])); + } + + bool bsolution_found = false; + + for(std::size_t i = 0; i < Es.size(); ++i) + { +// std::cout << i << std::endl; + + // check that we find the correct relative orientation. + if(FrobeniusDistance(d.R, Rs[i]) < 1e-3 && (d.t / d.t.norm() - ts[i] / ts[i].norm()).norm() < 1e-3) + { + bsolution_found = true; + // Check that E holds the essential matrix constraints. + EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es.at(i).getMatrix(), 1e-8); + } + } + BOOST_CHECK(bsolution_found); + } +} + +BOOST_AUTO_TEST_CASE(FivePointsRelativePose_test_data_sets) +{ + + //-- Setup a circular camera rig and assert that 5PT relative pose works. + const int iNviews = 5; + NViewDataSet d = NRealisticCamerasRing(iNviews, 5, + NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + // Compute pose [R|t] from 0 to [1;..;iNviews] + for(int i=1; i Es; // Essential. + std::vector Rs; // Rotation matrix. + std::vector ts; // Translation matrix. + + multiview::relativePose::Essential5PSolver solver; + + solver.solve(d._x[0], d._x[i], Es); + + // Recover rotation and translation from E. + Rs.resize(Es.size()); + ts.resize(Es.size()); + for (size_t s = 0; s < Es.size(); ++s) { + Vec2 x1Col = d._x[0].col(0); + Vec2 x2Col = d._x[i].col(0); + BOOST_CHECK( + motionFromEssentialAndCorrespondence(Es.at(s).getMatrix(), + d._K[0], + x1Col, + d._K[i], + x2Col, + &Rs[s], + &ts[s])); + } + //-- Compute Ground Truth motion + Mat3 R; + Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); + relativeCameraMotion(d._R[0], d._t[0], d._R[i], d._t[i], &R, &t); + + // Assert that found relative motion is correct for almost one model. + bool bsolution_found = false; + for (size_t nModel = 0; nModel < Es.size(); ++nModel) { + + // Check that E holds the essential matrix constraints. + EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es.at(nModel).getMatrix(), 1e-8); + + // Check that we find the correct relative orientation. + if (FrobeniusDistance(R, Rs[nModel]) < 1e-3 + && (t / t.norm() - ts[nModel] / ts[nModel].norm()).norm() < 1e-3 ) { + bsolution_found = true; + } + } + //-- Almost one solution must find the correct relative orientation + BOOST_CHECK(bsolution_found); + } +} + +BOOST_AUTO_TEST_CASE(FivePointsNullspaceBasis_SatisfyEpipolarConstraint) { + + TestData d = SomeTestData(); + + Mat E_basis = multiview::relativePose::fivePointsNullspaceBasis(d.x1, d.x2); + + for (int s = 0; s < 4; ++s) { + Mat3 E; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + E(i, j) = E_basis(3 * i + j, s); + } + } + for (int i = 0; i < d.x1.cols(); ++i) { + Vec3 x1(d.x1(0,i), d.x1(1,i), 1); + Vec3 x2(d.x2(0,i), d.x2(1,i), 1); + BOOST_CHECK_SMALL(x2.dot(E * x1), 1e-6); + } + } +} diff --git a/src/aliceVision/multiview/essentialKernelSolver_test.cpp b/src/aliceVision/multiview/relativePose/essentialKernel_test.cpp similarity index 63% rename from src/aliceVision/multiview/essentialKernelSolver_test.cpp rename to src/aliceVision/multiview/relativePose/essentialKernel_test.cpp index 1994c4cb68..03f1113c3a 100644 --- a/src/aliceVision/multiview/essentialKernelSolver_test.cpp +++ b/src/aliceVision/multiview/relativePose/essentialKernel_test.cpp @@ -6,10 +6,11 @@ // 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 "aliceVision/multiview/essential.hpp" -#include "aliceVision/multiview/essentialKernelSolver.hpp" -#include "aliceVision/multiview/projection.hpp" -#include "aliceVision/multiview/NViewDataSet.hpp" +#include +#include +#include +#include + #define BOOST_TEST_MODULE essentialKernelSolver @@ -18,10 +19,10 @@ #include using namespace aliceVision; +using namespace aliceVision::multiview; -/// Check that the E matrix fit the Essential Matrix properties -/// Determinant is 0 -/// +/// check that the E matrix fit the Essential Matrix properties +/// determinant is 0 #define EXPECT_ESSENTIAL_MATRIX_PROPERTIES(E, expectedPrecision) { \ BOOST_CHECK_SMALL(E.determinant(), expectedPrecision); \ Mat3 O = 2 * E * E.transpose() * E - (E * E.transpose()).trace() * E; \ @@ -29,29 +30,33 @@ using namespace aliceVision; EXPECT_MATRIX_NEAR(zero3x3, O, expectedPrecision);\ } -BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel_IdFocal) { - - //-- Setup a circular camera rig and assert that 8PT relative pose works. +BOOST_AUTO_TEST_CASE(Essential8PSolver_IdFocal) +{ + // setup a circular camera rig and assert that 8PT relative pose works. const int iNviews = 5; - NViewDataSet d = NRealisticCamerasRing(iNviews, 8, - NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + NViewDataSet d = NRealisticCamerasRing(iNviews, 8, NViewDatasetConfigurator(1,1,0,0,5,0)); // suppose a camera with Unit matrix as K for(int i=0; i Es; // Essential, - std::vector Rs; // Rotation matrix. - std::vector ts; // Translation matrix. - essential::kernel::EightPointRelativePoseSolver::Solve(d._x[i], d._x[(i+1)%iNviews], &Es); + std::vector Es; // essential, + std::vector Rs; // rotation matrix. + std::vector ts; // translation matrix. + + relativePose::Essential8PSolver solver; + solver.solve(d._x[i], d._x[(i+1)%iNviews], Es); // Recover rotation and translation from E. Rs.resize(Es.size()); ts.resize(Es.size()); - for (int s = 0; s < Es.size(); ++s) { + + for(int s = 0; s < Es.size(); ++s) + { Vec2 x1Col, x2Col; x1Col << d._x[i].col(0); x2Col << d._x[(i+1)%iNviews].col(0); + BOOST_CHECK( - MotionFromEssentialAndCorrespondence(Es[s], + motionFromEssentialAndCorrespondence(Es.at(s).getMatrix(), d._K[i], x1Col, d._K[(i+1)%iNviews], x2Col, &Rs[s], @@ -60,14 +65,15 @@ BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel_IdFo //-- Compute Ground Truth motion Mat3 R; Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(d._R[i], d._t[i], d._R[(i+1)%iNviews], d._t[(i+1)%iNviews], &R, &t); + relativeCameraMotion(d._R[i], d._t[i], d._R[(i+1)%iNviews], d._t[(i+1)%iNviews], &R, &t); // Assert that found relative motion is correct for almost one model. bool bsolution_found = false; - for (std::size_t nModel = 0; nModel < Es.size(); ++nModel) { + for (std::size_t nModel = 0; nModel < Es.size(); ++nModel) + { // Check that E holds the essential matrix constraints. - EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es[nModel], 1e-8); + EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es.at(nModel).getMatrix(), 1e-8); // Check that we find the correct relative orientation. if (FrobeniusDistance(R, Rs[nModel]) < 1e-3 @@ -75,26 +81,27 @@ BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel_IdFo bsolution_found = true; } } - //-- Almost one solution must find the correct relative orientation + // almost one solution must find the correct relative orientation BOOST_CHECK(bsolution_found); } } -BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel) { - - typedef essential::kernel::EightPointKernel Kernel; +BOOST_AUTO_TEST_CASE(Essential8PKernel_EightPointsRelativePose) +{ + using Kernel = relativePose::Essential8PKernel; int focal = 1000; int principal_Point = 500; - //-- Setup a circular camera rig and assert that 8PT relative pose works. + // setup a circular camera rig and assert that 8PT relative pose works. const int iNviews = 5; - NViewDataSet d = NRealisticCamerasRing(iNviews, Kernel::MINIMUM_SAMPLES, + NViewDataSet d = NRealisticCamerasRing(iNviews, Kernel::SolverT().getMinimumNbRequiredSamples(), NViewDatasetConfigurator(focal,focal,principal_Point,principal_Point,5,0)); // Suppose a camera with Unit matrix as K for(int i=0; i Es, Rs; // Essential, Rotation matrix. + std::vector Es; // Essential + std::vector Rs; // Rotation matrix. std::vector ts; // Translation matrix. // Direct value do not work. @@ -104,20 +111,25 @@ BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel) { Kernel kernel(x0, x1, d._K[i], d._K[(i+1)%iNviews]); std::vector samples; - for (std::size_t k = 0; k < Kernel::MINIMUM_SAMPLES; ++k) { + + for (std::size_t k = 0; k < kernel.getMinimumNbRequiredSamples(); ++k) + { samples.push_back(k); } - kernel.Fit(samples, &Es); + + kernel.fit(samples, Es); // Recover rotation and translation from E. Rs.resize(Es.size()); ts.resize(Es.size()); - for (int s = 0; s < Es.size(); ++s) { + + for (int s = 0; s < Es.size(); ++s) + { Vec2 x1Col, x2Col; x1Col << d._x[i].col(0); x2Col << d._x[(i+1)%iNviews].col(0); BOOST_CHECK( - MotionFromEssentialAndCorrespondence(Es[s], + motionFromEssentialAndCorrespondence(Es.at(s).getMatrix(), d._K[i], x1Col, d._K[(i+1)%iNviews], x2Col, &Rs[s], @@ -126,14 +138,15 @@ BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel) { //-- Compute Ground Truth motion Mat3 R; Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(d._R[i], d._t[i], d._R[(i+1)%iNviews], d._t[(i+1)%iNviews], &R, &t); + relativeCameraMotion(d._R[i], d._t[i], d._R[(i+1)%iNviews], d._t[(i+1)%iNviews], &R, &t); // Assert that found relative motion is correct for almost one model. bool bsolution_found = false; - for (std::size_t nModel = 0; nModel < Es.size(); ++nModel) { + for (std::size_t nModel = 0; nModel < Es.size(); ++nModel) + { // Check that E holds the essential matrix constraints. - EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es[nModel], 1e-8); + EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es.at(nModel).getMatrix(), 1e-8); // Check that we find the correct relative orientation. if (FrobeniusDistance(R, Rs[nModel]) < 1e-3 @@ -146,47 +159,49 @@ BOOST_AUTO_TEST_CASE(EightPointsRelativePose_EightPointsRelativePose_Kernel) { } } -BOOST_AUTO_TEST_CASE(FivePointKernelTest_KernelError) { +BOOST_AUTO_TEST_CASE(Essential5PKernel_KernelError) +{ Mat x1(2, 5), x2(2, 5); x1 << 0, 0, 0, .8, .8, 0, -.5, .8, 0, .8; x2 << 0, 0, 0, .8, .8, .1, -.4, .9, .1, .9; // Y Translated camera. - typedef essential::kernel::FivePointKernel Kernel; + + using Kernel = relativePose::Essential5PKernel; + Kernel kernel(x1,x2, Mat3::Identity(), Mat3::Identity()); - bool bOk = true; std::vector samples; for (std::size_t i = 0; i < x1.cols(); ++i) { samples.push_back(i); } - std::vector Es; - kernel.Fit(samples, &Es); + std::vector Es; + kernel.fit(samples, Es); - bOk &= (!Es.empty()); for (int i = 0; i < Es.size(); ++i) { for(int j = 0; j < x1.cols(); ++j) - BOOST_CHECK_SMALL(kernel.Error(j,Es[i]), 1e-8); + BOOST_CHECK_SMALL(kernel.error(j, Es.at(i)), 1e-8); } } -BOOST_AUTO_TEST_CASE(FivePointKernelTest_FivePointsRelativePose_Kernel) { - - typedef essential::kernel::FivePointKernel Kernel; +BOOST_AUTO_TEST_CASE(Essential5PKernel_FivePointsRelativePose) +{ + using Kernel = relativePose::Essential5PKernel; int focal = 1000; int principal_Point = 500; //-- Setup a circular camera rig and assert that 5PT relative pose works. const int iNviews = 8; - NViewDataSet d = NRealisticCamerasRing(iNviews, Kernel::MINIMUM_SAMPLES, + NViewDataSet d = NRealisticCamerasRing(iNviews, Kernel::SolverT().getMinimumNbRequiredSamples(), NViewDatasetConfigurator(focal,focal,principal_Point,principal_Point,5,0)); // Suppose a camera with Unit matrix as K std::size_t found = 0; for(int i=1; i Es, Rs; // Essential, Rotation matrix. + std::vector Es; // Essential + std::vector Rs; // Rotation matrix. std::vector ts; // Translation matrix. // Direct value do not work. @@ -196,20 +211,23 @@ BOOST_AUTO_TEST_CASE(FivePointKernelTest_FivePointsRelativePose_Kernel) { Kernel kernel(x0, x1, d._K[0], d._K[1]); std::vector samples; - for (std::size_t k = 0; k < Kernel::MINIMUM_SAMPLES; ++k) { + for (std::size_t k = 0; k < kernel.getMinimumNbRequiredSamples(); ++k) + { samples.push_back(k); } - kernel.Fit(samples, &Es); + + kernel.fit(samples, Es); // Recover rotation and translation from E. Rs.resize(Es.size()); ts.resize(Es.size()); - for (int s = 0; s < Es.size(); ++s) { + for (int s = 0; s < Es.size(); ++s) + { Vec2 x1Col, x2Col; x1Col << d._x[0].col(0); x2Col << d._x[i].col(0); BOOST_CHECK( - MotionFromEssentialAndCorrespondence(Es[s], + motionFromEssentialAndCorrespondence(Es.at(s).getMatrix(), d._K[0], x1Col, d._K[i], x2Col, &Rs[s], @@ -218,14 +236,14 @@ BOOST_AUTO_TEST_CASE(FivePointKernelTest_FivePointsRelativePose_Kernel) { //-- Compute Ground Truth motion Mat3 R; Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(d._R[0], d._t[0], d._R[i], d._t[i], &R, &t); + relativeCameraMotion(d._R[0], d._t[0], d._R[i], d._t[i], &R, &t); // Assert that found relative motion is correct for almost one model. bool bsolution_found = false; - for (std::size_t nModel = 0; nModel < Es.size(); ++nModel) { - + for (std::size_t nModel = 0; nModel < Es.size(); ++nModel) + { // Check that E holds the essential matrix constraints. - EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es[nModel], 1e-4); + EXPECT_ESSENTIAL_MATRIX_PROPERTIES(Es.at(nModel).getMatrix(), 1e-4); // Check that we find the correct relative orientation. if (FrobeniusDistance(R, Rs[nModel]) < 1e-3 @@ -233,7 +251,7 @@ BOOST_AUTO_TEST_CASE(FivePointKernelTest_FivePointsRelativePose_Kernel) { bsolution_found = true; } } - //-- Almost one solution must find the correct relative orientation + // Almost one solution must find the correct relative orientation BOOST_CHECK(bsolution_found); if (bsolution_found) found++; diff --git a/src/aliceVision/multiview/relativePose/fundamental10PSolver_test.cpp b/src/aliceVision/multiview/relativePose/fundamental10PSolver_test.cpp new file mode 100644 index 0000000000..001c0f1077 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/fundamental10PSolver_test.cpp @@ -0,0 +1,156 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2018 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 + +#define BOOST_TEST_MODULE essentialF10Solver +#include +#include + +#include + +using namespace aliceVision; +using namespace aliceVision::multiview; + +BOOST_AUTO_TEST_CASE(Fundamental10PSolver_8_solutions) +{ + // input data + Mat X = Mat(10, 2); + X << 1.425203022203948e-01, 1.802107961554276e-02, + -2.467473080283717e-01, 1.134013928865132e-01, + -2.165429045024671e-01, 1.016561086554276e-01, + -1.779508570620888e-01, -5.443423622532895e-02, + -9.503401906866776e-02, -1.912260035464638e-01, + 4.285402960526316e-01, 8.614113255550987e-02, + -3.182964124177631e-02, -1.139683291786595e-01, + -8.882934570312501e-02, 9.867373817845394e-02, + 3.867745811060855e-01, 1.629066868832237e-02, + 4.764409436677632e-01, -8.015930175781250e-02; + + Mat U = Mat(10, 2); + U << 1.470885587993421e-01, -3.716578433388158e-02, + -3.384269955283717e-01, 3.505518863075658e-02, + -2.903711258737665e-01, 2.336130242598684e-02, + -2.017722360711349e-01, -1.733409359580592e-01, + -1.022310598273026e-01, -1.984814292506168e-01, + 4.136125745271382e-01, 5.560238486842106e-02, + -4.780700683593750e-02, -1.279896786338405e-01, + -1.000526829769737e-01, 3.618122301603619e-02, + 3.628858064350329e-01, -6.060566149259868e-03, + 4.513226639597039e-01, -1.025031481291119e-01; + + // transpose for aliceVision + X.transposeInPlace(); + U.transposeInPlace(); + + // expected result + std::vector resF; + Mat3 resF1; resF1 << -5.751087019565978e+02, -4.158188692023641e+02, -3.933617847277337e+01, 5.623077923604983e+02, -1.155037417506222e+03, -3.603293875557833e+01, -5.277848487166921e-01, -9.666992599768776e-02, 1.000000000000000e+00; resF.push_back(resF1); + Mat3 resF2; resF2 << -2.199589063941379e+04, -2.847966717222817e+04, -1.568596434350923e+02, 2.668165615957419e+04, 3.794695160371831e+04, 3.582097175710577e+02, -1.985243717996453e+03, 3.206256740219327e+03, 1.000000000000000e+00; resF.push_back(resF2); + Mat3 resF3; resF3 << -2.406164718504593e+02, 1.051858077445098e+03, 1.029257202497226e+02, -9.384606808289793e+02, -1.561769129182295e+00, -7.588146214289489e+00, -2.671999077624011e+01, 1.072299253095423e+00, 1.000000000000000e+00; resF.push_back(resF3); + Mat3 resF4; resF4 << 4.773291645793250e+02, -7.340046095681545e+03, -7.622433073293142e+01, 7.353659444399805e+03, -7.228144265103590e+02, 2.313924014459177e+01, -4.096068639840655e+02, 1.760210427160575e+02, 1.000000000000000e+00; resF.push_back(resF4); + Mat3 resF5; resF5 << -3.634107580978586e+00, -6.861915294293681e+01, -1.419442789733012e+00, 1.043103524635315e+02, -9.905128840695012e+01, -1.288978514515288e+01, -5.004364829690772e+00, 1.759690666207339e+01, 1.000000000000000e+00; resF.push_back(resF5); + Mat3 resF6; resF6 << 3.845891112065496e+02, -3.376624483974341e+02, 1.058628706262809e+00, 4.567090509397883e+02, -1.782032107508239e+01, 6.603245933354176e-01, -3.057044617411873e+01, -1.171693147510399e+01, 1.000000000000000e+00; resF.push_back(resF6); + Mat3 resF7; resF7 << 9.173705728965071e-01, 6.566802897577452e+02, -9.150842187178472e+01, -6.985651797488533e+02, 6.389672979364663e+01, -2.813972637838619e+01, 1.224499375450345e+02, 2.273467046355762e+00, 1.000000000000000e+00; resF.push_back(resF7); + Mat3 resF8; resF8 << 9.260243587671360e+00, 2.901973712182848e+01, -4.525967397412600e+00, 3.873064498047484e+00, -3.308114531934898e+01, -1.268705935682481e+01, 3.742963523739931e+00, 1.576531480674961e+01, 1.000000000000000e+00; resF.push_back(resF8); + + std::vector resL; + Mat2X resL1(2, 1); resL1 << 6.664890785418972e+02, 9.360374635703592e-01; resL.push_back(resL1); + Mat2X resL2(2, 1); resL2 << -1.401232955798790e+02, 1.190659194737138e+03; resL.push_back(resL2); + Mat2X resL3(2, 1); resL3 << 5.478399615715365e+01, 1.277075963855622e+01; resL.push_back(resL3); + Mat2X resL4(2, 1); resL4 << -3.004792949356860e+01, 1.442424098845748e+02; resL.push_back(resL4); + Mat2X resL5(2, 1); resL5 << 5.619131137084262e+00, 1.528997232119745e+01; resL.push_back(resL5); + Mat2X resL6(2, 1); resL6 << -1.359819306123174e+00, -3.194231629423156e+02; resL.push_back(resL6); + Mat2X resL7(2, 1); resL7 << -7.668121663596985e+00, -1.015843563423398e+01; resL.push_back(resL7); + Mat2X resL8(2, 1); resL8 << -8.478427431999348e+00, -4.469547181112368e+00; resL.push_back(resL8); + + // process + std::vector models; + relativePose::Fundamental10PSolver().solve(X, U, models); + + // test results + if(resF.size() != models.size()) + BOOST_CHECK(false); + + for(Eigen::Index i = 0; i < resF.size(); ++i) + { + relativePose::Fundamental10PModel model = models.at(i); + BOOST_CHECK(resF.at(i).isApprox(model.getMatrix(), 1e-1)); + BOOST_CHECK(resL.at(i).isApprox(model.getRadialDistortion(), 1e-1)); + } +} + +BOOST_AUTO_TEST_CASE(Fundamental10PSolver_2_solutions) +{ + // input data + Mat X = Mat(10, 2); + X << -3.229677381013569e-01, 9.796846088610198e-02, + -1.859613679584704e-01, -1.331652189555921e-02, + 3.993208393297698e-01, -4.309213738692434e-02, + 4.570184647409539e-01, 6.516200015419409e-02, + -2.922935084292763e-02, -3.059326252184416e-01, + -1.386923378392270e-01, -4.580717387952302e-02, + -7.286097476356908e-02, 6.164560418379934e-02, + 5.396587171052632e-02, 9.743896484375000e-02, + 4.628304893092105e-01, -1.323552021227385e-01, + 4.939758300781250e-01, -4.290067973889802e-02; + + Mat U = Mat(10, 2); + U << -4.438024018940173e-01, 3.003572162828947e-05, + -2.212387888055099e-01, -1.264103939658717e-01, + 3.731781327097040e-01, -6.214079204358552e-02, + 4.439694053248355e-01, 3.183979235197369e-02, + 2.125557026110198e-01, -1.950167364823191e-01, + -1.560874216180099e-01, -7.848533228824013e-02, + -7.585523103412829e-02, -1.291279039884868e-02, + -1.167459909539474e-01, 2.065792686060855e-02, + 4.302636076274671e-01, -1.466708052785773e-01, + 4.942827405427632e-01, -8.317700837787830e-02; + + // transpose for aliceVision + X.transposeInPlace(); + U.transposeInPlace(); + + // expected result + std::vector resF; + { + Mat3 resF1; + resF1 << 1.732473500041804e+02, 7.711017146713161e+00, 9.471075243833084e+00, -1.518330376101678e+01, 3.516937871974938e+02, 1.097973146625093e+01, -1.046922253151639e-01, 6.610316585565101e-03, 1.000000000000000e+00; + resF.push_back(resF1); + + Mat3 resF2; + resF2 << -1.716387900591730e+04, -3.791505579727971e+04, -4.981406222447205e+00, 6.621894661079650e+03, 4.767612634161247e+04, -1.134635374165869e+02, 1.131438262492466e+03, -1.429200024334792e+04, 1.000000000000000e+00; + resF.push_back(resF2); + } + + std::vector resL; + { + relativePose::Fundamental10PModel::Mat21 resL1; + relativePose::Fundamental10PModel::Mat21 resL2; + + resL1 << -1.904715904949555e+02, 4.147643173367164e-01; + resL.push_back(resL1); + + resL2 << 1.258548483732838e+01, -2.918230091342369e+03; + resL.push_back(resL2); + } + + // process + std::vector models; + relativePose::Fundamental10PSolver().solve(X, U, models); + + // test results + if(resF.size() != models.size()) + BOOST_CHECK(false); + + for(Eigen::Index i = 0; i < resF.size(); ++i) + { + relativePose::Fundamental10PModel model = models.at(i); + BOOST_CHECK(resF.at(i).isApprox(model.getMatrix(), 1e-1)); + BOOST_CHECK(resL.at(i).isApprox(model.getRadialDistortion(), 1e-1)); + } +} diff --git a/src/aliceVision/multiview/relativePose/fundamentalKernel_test.cpp b/src/aliceVision/multiview/relativePose/fundamentalKernel_test.cpp new file mode 100644 index 0000000000..441b4b50b2 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/fundamentalKernel_test.cpp @@ -0,0 +1,135 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +#define BOOST_TEST_MODULE fundamentalKernelSolver +#include +#include + +using namespace aliceVision; +using namespace aliceVision::multiview; + +// check that sin(angle(a, b)) < tolerance. +template +bool colinear(const A& a, const B& b, double tolerance) +{ + const bool dims_match = (a.rows() == b.rows()) && (a.cols() == b.cols()); + + if(!dims_match) + return false; + + const double c = CosinusBetweenMatrices(a, b); + + if(c * c < 1) + { + double s = sqrt(1 - c * c); + return fabs(s) < tolerance; + } + + return false; +} + +// check the properties of a fundamental matrix: +// 1. The determinant is 0 (rank deficient) +// 2. The condition x'T*F*x = 0 is satisfied to precision. +template +bool expectFundamentalProperties(const TMat& F, + const Mat& ptsA, + const Mat& ptsB, + double precision) +{ + bool bOk = true; + bOk &= F.determinant() < precision; + assert(ptsA.cols() == ptsB.cols()); + Mat hptsA, hptsB; + + euclideanToHomogeneous(ptsA, &hptsA); + euclideanToHomogeneous(ptsB, &hptsB); + + for(int i = 0; i < ptsA.cols(); ++i) + { + const double residual = hptsB.col(i).dot(F * hptsA.col(i)); + bOk &= residual < precision; + } + + return bOk; +} + +// check the fundamental fitting: +// 1. Estimate the fundamental matrix +// 2. Check epipolar distance. +template +bool expectKernelProperties(const Mat& x1, const Mat& x2, Mat3* F_expected = nullptr) +{ + bool bOk = true; + const Kernel kernel(x1, x2); + std::vector samples; + + for(std::size_t i = 0; i < x1.cols(); ++i) + samples.push_back(i); + + std::vector Fs; + kernel.fit(samples, Fs); + + bOk &= (!Fs.empty()); + + for(int i = 0; i < Fs.size(); ++i) + { + bOk &= expectFundamentalProperties(Fs.at(i).getMatrix(), x1, x2, 1e-8); + if(F_expected) + bOk &= colinear(Fs.at(i).getMatrix(), *F_expected, 1e-6); + } + return bOk; +} + +BOOST_AUTO_TEST_CASE(Fundamental7PKernel_EasyCase) +{ + Mat x1(2, 7), x2(2, 7); + x1 << 0, 0, 0, 1, 1, 1, 2, + 0, 1, 2, 0, 1, 2, 0; + x2 << 0, 0, 0, 1, 1, 1, 2, + 1, 2, 3, 1, 2, 3, 1; + + BOOST_CHECK(expectKernelProperties(x1, x2)); +} + +BOOST_AUTO_TEST_CASE(NormalizedFundamental7PKernel_EasyCase) +{ + Mat x1(2, 7), x2(2, 7); + x1 << 0, 0, 0, 1, 1, 1, 2, + 0, 1, 2, 0, 1, 2, 0; + x2 << 0, 0, 0, 1, 1, 1, 2, + 1, 2, 3, 1, 2, 3, 1; + + BOOST_CHECK(expectKernelProperties(x1, x2)); +} + +BOOST_AUTO_TEST_CASE(Fundamental8PKernel_EasyCase) +{ + Mat x1(2, 8), x2(2, 8); + x1 << 0, 0, 0, 1, 1, 1, 2, 2, + 0, 1, 2, 0, 1, 2, 0, 1; + x2 << 0, 0, 0, 1, 1, 1, 2, 2, + 1, 2, 3, 1, 2, 3, 1, 2; + + BOOST_CHECK(expectKernelProperties(x1, x2)); +} + +BOOST_AUTO_TEST_CASE(NormalizedFundamental8PKernel_EasyCase) +{ + Mat x1(2, 8), x2(2, 8); + x1 << 0, 0, 0, 1, 1, 1, 2, 2, + 0, 1, 2, 0, 1, 2, 0, 1; + x2 << 0, 0, 0, 1, 1, 1, 2, 2, + 1, 2, 3, 1, 2, 3, 1, 2; + + BOOST_CHECK(expectKernelProperties(x1, x2)); +} diff --git a/src/aliceVision/multiview/relativePose/homographyKernel_test.cpp b/src/aliceVision/multiview/relativePose/homographyKernel_test.cpp new file mode 100644 index 0000000000..0346e535f9 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/homographyKernel_test.cpp @@ -0,0 +1,102 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +#define BOOST_TEST_MODULE homographyKernelSolver +#include +#include +#include + +using namespace aliceVision; +using namespace aliceVision::multiview; + +BOOST_AUTO_TEST_CASE(NormalizedHomography4PKernel_Fitting) +{ + // define 3 knows homographies (Use as GT). + std::vector H_gt(3); + + H_gt[0] = Mat3::Identity(); + H_gt[1] << 1, 0, -4, + 0, 1, 5, + 0, 0, 1; + H_gt[2] << 1, -2, 3, + 4, 5, -6, + -7, 8, 1; + + // define a set of points. + Mat x(2, 9), xh; + x << 0, 0, 0, 1, 1, 1, 2, 2, 2, + 0, 1, 2, 0, 1, 2, 0, 1, 2; + + euclideanToHomogeneous(x, &xh); + + for(int i = 0; i < H_gt.size(); ++i) + { + // transform points by the ground truth homography. + Mat y, yh = H_gt[i] * xh; + homogeneousToEuclidean(yh, &y); + + const relativePose::NormalizedHomography4PKernel kernel(x, y); + + std::size_t samples_[5] = { 0, 1, 2, 3, 4 }; + std::vector samples(samples_,samples_+5); + for(std::size_t j = 4; samples.size() < x.cols(); samples.push_back(j++)) + { + std::vector Hs; + kernel.fit(samples, Hs); + BOOST_CHECK_EQUAL(1, Hs.size()); + + // check that found matrix is equal to the GT + EXPECT_MATRIX_PROP(H_gt[i], Hs.at(0).getMatrix(), 1e-6); + } + } +} + +BOOST_AUTO_TEST_CASE(Homography4PKernel_Fitting) +{ + // define 3 knows homographies (Use as GT). + std::vector H_gt(3); + + H_gt[0] = Mat3::Identity(); + H_gt[1] << 1, 0, -4, + 0, 1, 5, + 0, 0, 1; + H_gt[2] << 1, -2, 3, + 4, 5, -6, + -7, 8, 1; + + // define a set of points. + Mat x(2, 9), xh; + x << 0, 0, 0, 1, 1, 1, 2, 2, 2, + 0, 1, 2, 0, 1, 2, 0, 1, 2; + + euclideanToHomogeneous(x, &xh); + + for(int i = 0; i < H_gt.size(); ++i) + { + // transform points by the ground truth homography. + Mat y, yh = H_gt[i] * xh; + homogeneousToEuclidean(yh, &y); + + const relativePose::Homography4PKernel kernel(x, y); + + std::size_t samples_[5]={0,1,2,3,4}; + std::vector samples(samples_,samples_+5); + + for(std::size_t j = 4; samples.size() < x.cols(); samples.push_back(j++)) + { + std::vector Hs; + kernel.fit(samples, Hs); + BOOST_CHECK_EQUAL(1, Hs.size()); + + // check that found matrix is equal to the GT + EXPECT_MATRIX_PROP(H_gt[i], Hs.at(0).getMatrix(), 1e-6); + } + } +} diff --git a/src/aliceVision/multiview/resection/EPnPKernel.hpp b/src/aliceVision/multiview/resection/EPnPKernel.hpp new file mode 100644 index 0000000000..725e99012d --- /dev/null +++ b/src/aliceVision/multiview/resection/EPnPKernel.hpp @@ -0,0 +1,71 @@ +// 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 + +namespace aliceVision { +namespace multiview { +namespace resection { + +/** + * @brief Euclidean (EPnP) resection kernel + * @note Have K intrinsic helps + */ +class EPnPKernel : public robustEstimation::PointFittingKernel +{ + public: + + using KernelBase = robustEstimation::PointFittingKernel; + + EPnPKernel(const Mat2X& x2d, const Mat3X& x3d) + : KernelBase(x2d, x3d) + , x_camera_(x2d) + , X_(x3d) + { + assert(x2d.cols() == x3d.cols()); + K_ = Mat3::Identity(); + } + + EPnPKernel(const Mat2X& x2d, const Mat3X& x3d, const Mat3& K) + : KernelBase(x2d, x3d) + , X_(x3d), K_(K) + { + assert(x2d.cols() == x3d.cols()); + // Conversion from image coordinates to normalized camera coordinates + euclideanToNormalizedCamera(x2d, K, &x_camera_); + } + + void fit(const std::vector& samples, std::vector& models) const override + { + Mat2X x = ExtractColumns(x_camera_, samples); + Mat3X X = ExtractColumns(X_, samples); + Mat34 P; + Mat3 R; + Vec3 t; + if (_kernelSolver.resection(x, X, &R, &t)) + { + P_from_KRt(K_, R, t, &P); + models.emplace_back(P); + } + } + + private: + // x_camera_ contains the normalized camera coordinates + Mat2X x_camera_; + const Mat3X &X_; + Mat3 K_; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision + diff --git a/src/aliceVision/multiview/resection/EPnPSolver.cpp b/src/aliceVision/multiview/resection/EPnPSolver.cpp new file mode 100644 index 0000000000..5fb651027e --- /dev/null +++ b/src/aliceVision/multiview/resection/EPnPSolver.cpp @@ -0,0 +1,475 @@ +// 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 "EPnPSolver.hpp" +#include + +namespace aliceVision { +namespace multiview { +namespace resection { + +/** + * @brief Selects 4 virtual control points using mean and PCA. + * @param[in] x3d + * @param[out] xCentered + * @param[out] xControlPoints + */ +void selectControlPoints(const Mat3X& x3d, Mat* xCentered, Mat34* xControlPoints) +{ + const std::size_t nbPoints = x3d.cols(); + + // the first virtual control point, C0, is the centroid. + Vec mean, variance; + MeanAndVarianceAlongRows(x3d, &mean, &variance); + xControlPoints->col(0) = mean; + + // computes PCA + xCentered->resize (3, nbPoints); + for(std::size_t c = 0; c < nbPoints; ++c) + { + xCentered->col(c) = x3d.col (c) - mean; + } + + Mat3 xCenteredSQ = (*xCentered) * xCentered->transpose(); + Eigen::JacobiSVD xCenteredSQsvd(xCenteredSQ, Eigen::ComputeFullU); + const Vec3 w = xCenteredSQsvd.singularValues(); + const Mat3 u = xCenteredSQsvd.matrixU(); + + for(std::size_t c = 0; c < 3; ++c) + { + const double k = std::sqrt(w(c) / nbPoints); + xControlPoints->col(c + 1) = mean + k * u.col(c); + } +} + +/** + * @brief Computes the barycentric coordinates for all real points + * @param[in] xWorldCentered + * @param[in] xControlPoints + * @param[out] alphas + */ +void computeBarycentricCoordinates(const Mat3X& xWorldCentered, const Mat34& xControlPoints, Mat4X* alphas) +{ + const std::size_t nbPoints = xWorldCentered.cols(); + Mat3 C2; + + for(std::size_t c = 1; c < 4; ++c) + { + C2.col(c-1) = xControlPoints.col(c) - xControlPoints.col(0); + } + + Mat3 C2inv = C2.inverse(); + Mat3X a = C2inv * xWorldCentered; + + alphas->resize(4, nbPoints); + alphas->setZero(); + alphas->block(1, 0, 3, nbPoints) = a; + + for(std::size_t c = 0; c < nbPoints; ++c) + { + (*alphas)(0, c) = 1.0 - alphas->col(c).sum(); + } +} + +/** + * @brief Estimates the coordinates of all real points in the camera coordinate frame + * @param[in] alphas + * @param[in] betas + * @param[in] U + * @param[out] xCamera + */ +void computePointsCoordinatesInCameraFrame(const Mat4X& alphas, const Vec4& betas, const Eigen::Matrix& U, Mat3X* xCamera) +{ + const std::size_t nbPoints = alphas.cols(); + + // estimates the control points in the camera reference frame. + Mat34 C2b; + C2b.setZero(); + for(std::size_t cu = 0; cu < 4; ++cu) + { + for(std::size_t c = 0; c < 4; ++c) + { + C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose(); + } + } + + // estimates the 3D points in the camera reference frame + xCamera->resize(3, nbPoints); + for(std::size_t c = 0; c < nbPoints; ++c) + { + xCamera->col(c) = C2b * alphas.col(c); + } + + // check the sign of the z coordinate of the points (should be positive) + std::size_t num_z_neg = 0; + for(Mat3X::Index i = 0; i < xCamera->cols(); ++i) + { + if((*xCamera)(2,i) < 0) + { + num_z_neg++; + } + } + + // if more than 50% of z are negative, we change the signs + if(num_z_neg > 0.5 * xCamera->cols()) + { + C2b = -C2b; + *xCamera = -(*xCamera); + } +} + +/** + * @brief absoluteOrientation + * @param[in] X + * @param[in] Xp + * @param[out] R + * @param[out] t + */ +void absoluteOrientation(const Mat3X& X, const Mat3X& Xp, Mat3* R, Vec3* t) +{ + const int nbPoints = static_cast(X.cols()); + Vec3 C = X.rowwise().sum() / nbPoints; // centroid of X. + Vec3 Cp = Xp.rowwise().sum() / nbPoints; // centroid of Xp. + + // normalize the two point sets. + Mat3X Xn(3, nbPoints), Xpn(3, nbPoints); + + for(int i = 0; i < nbPoints; ++i) + { + Xn.col(i) = X.col(i) - C; + Xpn.col(i) = Xp.col(i) - Cp; + } + + // construct the N matrix (pg. 635). + const double Sxx = Xn.row(0).dot(Xpn.row(0)); + const double Syy = Xn.row(1).dot(Xpn.row(1)); + const double Szz = Xn.row(2).dot(Xpn.row(2)); + const double Sxy = Xn.row(0).dot(Xpn.row(1)); + const double Syx = Xn.row(1).dot(Xpn.row(0)); + const double Sxz = Xn.row(0).dot(Xpn.row(2)); + const double Szx = Xn.row(2).dot(Xpn.row(0)); + const double Syz = Xn.row(1).dot(Xpn.row(2)); + const double Szy = Xn.row(2).dot(Xpn.row(1)); + + Mat4 N; + N << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx, + Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz, + Szx - Sxz, Sxy + Syx, -Sxx + Syy - Szz, Syz + Szy, + Sxy - Syx, Szx + Sxz, Syz + Szy, -Sxx - Syy + Szz; + + // find the unit quaternion q that maximizes qNq. It is the eigenvector + // corresponding to the largest eigenvalue. + const Vec4 q = N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0); + + // retrieve the 3x3 rotation matrix. + const Vec4 qq = q.array() * q.array(); + + const double q0q1 = q(0) * q(1); + const double q0q2 = q(0) * q(2); + const double q0q3 = q(0) * q(3); + const double q1q2 = q(1) * q(2); + const double q1q3 = q(1) * q(3); + const double q2q3 = q(2) * q(3); + + (*R) << qq(0) + qq(1) - qq(2) - qq(3), + 2 * (q1q2 - q0q3), + 2 * (q1q3 + q0q2), + 2 * (q1q2+ q0q3), + qq(0) - qq(1) + qq(2) - qq(3), + 2 * (q2q3 - q0q1), + 2 * (q1q3 - q0q2), + 2 * (q2q3 + q0q1), + qq(0) - qq(1) - qq(2) + qq(3); + + // fix the handedness of the R matrix. + if(R->determinant() < 0) + { + R->row(2) = -R->row(2); + } + + // compute the final translation. + *t = Cp - *R * C; +} + +bool EPnPSolver::resection(const Mat2X& x2d, const Mat3X& x3d, Mat3* R, Vec3* t) const +{ + assert(x2d.cols() == x3d.cols()); + assert(x2d.cols() > 3); + + const std::size_t nbPoints = x3d.cols(); + + // select the control points. + Mat34 xControlPoints; + Mat xCentered; + + selectControlPoints(x3d, &xCentered, &xControlPoints); + + // compute the barycentric coordinates. + Mat4X alphas(4, nbPoints); + computeBarycentricCoordinates(xCentered, xControlPoints, &alphas); + + // estimates the M matrix with the barycentric coordinates + Mat M(2 * nbPoints, 12); + + for(std::size_t c = 0; c < nbPoints; ++c) + { + const double a0 = alphas(0, c); + const double a1 = alphas(1, c); + const double a2 = alphas(2, c); + const double a3 = alphas(3, c); + const double ui = x2d(0, c); + const double vi = x2d(1, c); + + M.block(2*c, 0, 2, 12) << a0, 0, + a0*(-ui), a1, 0, + a1*(-ui), a2, 0, + a2*(-ui), a3, 0, + a3*(-ui), 0, + a0, a0*(-vi), 0, + a1, a1*(-vi), 0, + a2, a2*(-vi), 0, + a3, a3*(-vi); + } + + // @todo: avoid the transpose by rewriting the u2.block() calls. + Eigen::JacobiSVD MtMsvd(M.transpose()*M, Eigen::ComputeFullU); + Eigen::Matrix u2 = MtMsvd.matrixU().transpose(); + + // estimate the L matrix. + Eigen::Matrix dv1; + Eigen::Matrix dv2; + Eigen::Matrix dv3; + Eigen::Matrix dv4; + + dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3); + dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3); + dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3); + dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3); + dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3); + dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3); + dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3); + dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3); + dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3); + dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3); + dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3); + dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3); + dv3.row(0) = u2.block( 9, 0, 1, 3) - u2.block( 9, 3, 1, 3); + dv3.row(1) = u2.block( 9, 0, 1, 3) - u2.block( 9, 6, 1, 3); + dv3.row(2) = u2.block( 9, 0, 1, 3) - u2.block( 9, 9, 1, 3); + dv3.row(3) = u2.block( 9, 3, 1, 3) - u2.block( 9, 6, 1, 3); + dv3.row(4) = u2.block( 9, 3, 1, 3) - u2.block( 9, 9, 1, 3); + dv3.row(5) = u2.block( 9, 6, 1, 3) - u2.block( 9, 9, 1, 3); + dv4.row(0) = u2.block( 8, 0, 1, 3) - u2.block( 8, 3, 1, 3); + dv4.row(1) = u2.block( 8, 0, 1, 3) - u2.block( 8, 6, 1, 3); + dv4.row(2) = u2.block( 8, 0, 1, 3) - u2.block( 8, 9, 1, 3); + dv4.row(3) = u2.block( 8, 3, 1, 3) - u2.block( 8, 6, 1, 3); + dv4.row(4) = u2.block( 8, 3, 1, 3) - u2.block( 8, 9, 1, 3); + dv4.row(5) = u2.block( 8, 6, 1, 3) - u2.block( 8, 9, 1, 3); + + Eigen::Matrix L; + for(std::size_t r = 0; r < 6; ++r) + { + L.row(r) << dv1.row(r).dot(dv1.row(r)), + 2.0 * dv1.row(r).dot(dv2.row(r)), + dv2.row(r).dot(dv2.row(r)), + 2.0 * dv1.row(r).dot(dv3.row(r)), + 2.0 * dv2.row(r).dot(dv3.row(r)), + dv3.row(r).dot(dv3.row(r)), + 2.0 * dv1.row(r).dot(dv4.row(r)), + 2.0 * dv2.row(r).dot(dv4.row(r)), + 2.0 * dv3.row(r).dot(dv4.row(r)), + dv4.row(r).dot(dv4.row(r)); + } + + Vec rho; + rho.resize(6); + rho << (xControlPoints.col(0) - xControlPoints.col(1)).squaredNorm(), + (xControlPoints.col(0) - xControlPoints.col(2)).squaredNorm(), + (xControlPoints.col(0) - xControlPoints.col(3)).squaredNorm(), + (xControlPoints.col(1) - xControlPoints.col(2)).squaredNorm(), + (xControlPoints.col(1) - xControlPoints.col(3)).squaredNorm(), + (xControlPoints.col(2) - xControlPoints.col(3)).squaredNorm(); + + // there are three possible solutions based on the three approximations of L + // (betas). below, each one is solved for then the best one is chosen. + Mat3X xCamera; + Mat3 K; K.setIdentity(); + std::vector Rs(3); + std::vector ts(3); + Vec rmse(3); + + bool bSol = false; + + // find the first possible solution for R, t corresponding to: + // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] + // Betas_approx_1 = [b00 b01 b02 b03] + Vec4 betas = Vec4::Zero(); + Eigen::Matrix l_6x4; + + for(std::size_t r = 0; r < 6; ++r) + { + l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6); + } + + Eigen::JacobiSVD svd_of_l4(l_6x4, Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec4 b4 = svd_of_l4.solve(rho); + + if((l_6x4 * b4).isApprox(rho, 1e-3)) + { + if(b4(0) < 0) + { + b4 = -b4; + } + + b4(0) = std::sqrt(b4(0)); + betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0); + computePointsCoordinatesInCameraFrame(alphas, betas, u2, &xCamera); + absoluteOrientation(x3d, xCamera, &Rs[0], &ts[0]); + rmse(0) = reprojectionErrorRMSE(x2d, x3d, K, Rs[0], ts[0]); + bSol = true; + } + else + { + // ALICEVISION_LOG_WARNING("First approximation of beta not good enough."); + ts[0].setZero(); + rmse(0) = std::numeric_limits::max(); + } + + // find the second possible solution for R, t corresponding to: + // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] + // Betas_approx_2 = [b00 b01 b11] + betas.setZero(); + Eigen::Matrix l_6x3; + l_6x3 = L.block(0, 0, 6, 3); + Eigen::JacobiSVD svdOfL3(l_6x3, Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec3 b3 = svdOfL3.solve(rho); + + if((l_6x3 * b3).isApprox(rho, 1e-3)) + { + if(b3(0) < 0) + { + betas(0) = std::sqrt(-b3(0)); + betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0; + } + else + { + betas(0) = std::sqrt(b3(0)); + betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0; + } + if (b3(1) < 0) { + betas(0) = -betas(0); + } + + betas(2) = 0; + betas(3) = 0; + computePointsCoordinatesInCameraFrame(alphas, betas, u2, &xCamera); + absoluteOrientation(x3d, xCamera, &Rs[1], &ts[1]); + rmse(1) = reprojectionErrorRMSE(x2d, x3d, K, Rs[1], ts[1]); + bSol = true; + } + else + { + // ALICEVISION_LOG_WARNING("Second approximation of beta not good enough."); + ts[1].setZero(); + rmse(1) = std::numeric_limits::max(); + } + + // find the third possible solution for R, t corresponding to: + // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] + // Betas_approx_3 = [b00 b01 b11 b02 b12] + betas.setZero(); + Eigen::Matrix l_6x5; + l_6x5 = L.block(0, 0, 6, 5); + Eigen::JacobiSVD svdOfL5(l_6x5, Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec b5 = svdOfL5.solve(rho); + + if((l_6x5 * b5).isApprox(rho, 1e-3)) + { + if (b5(0) < 0) + { + betas(0) = std::sqrt(-b5(0)); + + if(b5(2) < 0) + { + betas(1) = std::sqrt(-b5(2)); + } + else + { + b5(2) = 0; + } + } + else + { + betas(0) = std::sqrt(b5(0)); + + if (b5(2) > 0) + { + betas(1) = std::sqrt(b5(2)); + } + else + { + b5(2) = 0; + } + } + + if(b5(1) < 0) + { + betas(0) = -betas(0); + } + betas(2) = b5(3) / betas(0); + betas(3) = 0; + computePointsCoordinatesInCameraFrame(alphas, betas, u2, &xCamera); + absoluteOrientation(x3d, xCamera, &Rs[2], &ts[2]); + rmse(2) = reprojectionErrorRMSE(x2d, x3d, K, Rs[2], ts[2]); + bSol = true; + } + else + { + // ALICEVISION_LOG_WARNING("Third approximation of beta not good enough."); + ts[2].setZero(); + rmse(2) = std::numeric_limits::max(); + } + + // finally, with all three solutions, select the (R, t) with the best RMSE. + std::size_t n = 0; + if (rmse(1) < rmse(0)) + { + n = 1; + } + + if(rmse(2) < rmse(n)) + { + n = 2; + } + + if(bSol) + { + // If at least one solution have been found + *R = Rs[n]; + *t = ts[n]; + + return true; + } + return false; +} + +void EPnPSolver::solve(const Mat& x2d, const Mat& x3d, std::vector& models) const +{ + Mat3 R; + Vec3 t; + Mat34 P; + + if(resection(x2d, x3d, &R, &t)) + { + P_from_KRt(Mat3::Identity(), R, t, &P); // K = Id + models.emplace_back(P); + } +} + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/EPnPSolver.hpp b/src/aliceVision/multiview/resection/EPnPSolver.hpp new file mode 100644 index 0000000000..7ea30e62c6 --- /dev/null +++ b/src/aliceVision/multiview/resection/EPnPSolver.hpp @@ -0,0 +1,79 @@ +// 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 + +namespace aliceVision { +namespace multiview { +namespace resection { + +class EPnPSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return /*5*/ 6; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + /** + * @brief Solve the problem of camera pose. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A list of solutions. + */ + void solve(const Mat& x2d, const Mat& x3d, std::vector& models) const override; + + /** + * @brief Solve the problem. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x2d, const Mat& x3d, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("EPnPSolver does not support problem solving with weights."); + } + + /** + * @brief Computes the extrinsic parameters, R and t for a calibrated camera from 4 or + * more 3D points and their images. + * + * @param[in] x2d Image points in normalized camera coordinates, e.g. x2d = inv(K) * x_image + * @param[in] x3d 3D points in the world coordinate system + * @param[out] R Solution for the camera rotation matrix + * @param[out] t Solution for the camera translation vector + + * @see "{EPnP: An Accurate $O(n)$ Solution to the PnP Problem", by V. Lepetit + * and F. Moreno-Noguer and P. Fua, IJCV 2009. vol. 81, no. 2 + * + * @note: the non-linear optimization is not implemented here. + */ + bool resection(const Mat2X& x2d, const Mat3X& x3d, Mat3* R, Vec3* t) const; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/ISolverErrorResection.hpp b/src/aliceVision/multiview/resection/ISolverErrorResection.hpp new file mode 100644 index 0000000000..d887b05fd6 --- /dev/null +++ b/src/aliceVision/multiview/resection/ISolverErrorResection.hpp @@ -0,0 +1,24 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 + +namespace aliceVision { +namespace multiview { +namespace resection { + +/** + * @brief Resection solver error interface. + */ +template +struct ISolverErrorResection +{ + virtual double error(const ModelT& model, const Vec2& x2d, const Vec3& x3d) const = 0; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P3PSolver.cpp b/src/aliceVision/multiview/resection/P3PSolver.cpp index 7bb719f8a0..26168c4b5c 100644 --- a/src/aliceVision/multiview/resection/P3PSolver.cpp +++ b/src/aliceVision/multiview/resection/P3PSolver.cpp @@ -7,38 +7,39 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "P3PSolver.hpp" -#include +#include namespace aliceVision { +namespace multiview { namespace resection { -void solveQuartic(const Vec5 & factors, Vec4 & realRoots) +void solveQuartic(const Vec5& factors, Vec4& realRoots) { - double A = factors[0]; - double B = factors[1]; - double C = factors[2]; - double D = factors[3]; - double E = factors[4]; - - double A_pw2 = A*A; - double B_pw2 = B*B; - double A_pw3 = A_pw2*A; - double B_pw3 = B_pw2*B; - double A_pw4 = A_pw3*A; - double B_pw4 = B_pw3*B; - - double alpha = -3 * B_pw2 / (8 * A_pw2) + C / A; - double beta = B_pw3 / (8 * A_pw3) - B * C / (2 * A_pw2) + D / A; - double gamma = -3 * B_pw4 / (256 * A_pw4) + B_pw2 * C / (16 * A_pw3) - B * D / (4 * A_pw2) + E / A; - - double alpha_pw2 = alpha*alpha; - double alpha_pw3 = alpha_pw2*alpha; - - std::complex P(-alpha_pw2 / 12 - gamma, 0); - std::complex Q(-alpha_pw3 / 108 + alpha * gamma / 3 - pow(beta, 2) / 8, 0); - std::complex R = -Q / 2.0 + sqrt(pow(Q, 2.0) / 4.0 + pow(P, 3.0) / 27.0); - - std::complex U = pow(R, (1.0 / 3.0)); + const double A = factors[0]; + const double B = factors[1]; + const double C = factors[2]; + const double D = factors[3]; + const double E = factors[4]; + + const double A_pw2 = A*A; + const double B_pw2 = B*B; + const double A_pw3 = A_pw2*A; + const double B_pw3 = B_pw2*B; + const double A_pw4 = A_pw3*A; + const double B_pw4 = B_pw3*B; + + const double alpha = -3 * B_pw2 / (8 * A_pw2) + C / A; + const double beta = B_pw3 / (8 * A_pw3) - B * C / (2 * A_pw2) + D / A; + const double gamma = -3 * B_pw4 / (256 * A_pw4) + B_pw2 * C / (16 * A_pw3) - B * D / (4 * A_pw2) + E / A; + + const double alpha_pw2 = alpha * alpha; + const double alpha_pw3 = alpha_pw2 * alpha; + + const std::complex P(-alpha_pw2 / 12 - gamma, 0); + const std::complex Q(-alpha_pw3 / 108 + alpha * gamma / 3 - pow(beta, 2) / 8, 0); + const std::complex R = -Q / 2.0 + sqrt(pow(Q, 2.0) / 4.0 + pow(P, 3.0) / 27.0); + + const std::complex U = pow(R, (1.0 / 3.0)); std::complex y; if(U.real() == 0) @@ -46,7 +47,7 @@ void solveQuartic(const Vec5 & factors, Vec4 & realRoots) else y = -5.0 * alpha / 6.0 - P / (3.0 * U) + U; - std::complex w = sqrt(alpha + 2.0 * y); + const std::complex w = sqrt(alpha + 2.0 * y); std::complex temp; @@ -60,45 +61,44 @@ void solveQuartic(const Vec5 & factors, Vec4 & realRoots) realRoots[3] = temp.real(); } -/* - * Author: Laurent Kneip, adapted to aliceVision by Pierre Moulon - * Description: Compute the absolute pose of a camera using three 3D-to-2D correspondences - * Reference: [1] A Novel Parametrization of the P3P-Problem for a Direct Computation of +/** + * @brief Compute the absolute pose of a camera using three 3D-to-2D correspondences + * + * Reference: [1] A Novel Parametrization of the P3P-Problem for a Direct Computation of * Absolute Camera Position and Orientation * Kneip, L.; Scaramuzza, D. ; Siegwart, R. * CVPR 2011 * - * Input: featureVectors: 3x3 matrix with UNITARY feature vectors (each column is a vector) - * worldPoints: 3x3 matrix with corresponding 3D world points (each column is a point) - * solutions: 3x16 matrix that will contain the solutions - * form: [ C1,R1, C2,R2 ... ] - * the obtained orientation matrices are defined as transforming points from the cam to the world frame - * Output: bool: true if correct execution - * false if world points aligned + * @param[in] featureVectors: 3x3 matrix with UNITARY feature vectors (each column is a vector) + * @param[in] worldPoints: 3x3 matrix with corresponding 3D world points (each column is a point) + * @param[out] solutions: 3x16 matrix that will contain the solutions + * form: [ C1,R1, C2,R2 ... ] + * the obtained orientation matrices are defined as transforming points from the cam to the world frame + * @return true if correct execution, false if world points aligned + * @author: Laurent Kneip, adapted to the project by Pierre Moulon */ - -bool compute_P3P_Poses(const Mat3 & featureVectors, const Mat3 & worldPoints, Mat & solutions) +bool computeP3PPoses(const Mat3& featureVectors, const Mat3& worldPoints, Mat& solutions) { solutions = Mat(3, 4 * 4); - // Extraction of world points + // extraction of world points Vec3 P1 = worldPoints.col(0); Vec3 P2 = worldPoints.col(1); Vec3 P3 = worldPoints.col(2); - // Verification that world points are not colinear + // verification that world points are not colinear if(((P2 - P1).cross(P3 - Vec3(1, 1, 1))).norm() == 0) return false; - // Extraction of feature vectors + // extraction of feature vectors Vec3 f1 = featureVectors.col(0); Vec3 f2 = featureVectors.col(1); Vec3 f3 = featureVectors.col(2); - // Creation of intermediate camera frame + // creation of intermediate camera frame Vec3 e1 = f1; Vec3 e3 = (f1.cross(f2)).normalized(); @@ -111,7 +111,7 @@ bool compute_P3P_Poses(const Mat3 & featureVectors, const Mat3 & worldPoints, Ma f3 = T * f3; - // Reinforce that f3[2] > 0 for having theta in [0;pi] + // reinforce that f3[2] > 0 for having theta in [0;pi] if(f3[2] > 0) { @@ -134,49 +134,46 @@ bool compute_P3P_Poses(const Mat3 & featureVectors, const Mat3 & worldPoints, Ma P3 = worldPoints.col(2); } - // Creation of intermediate world frame + // creation of intermediate world frame - Vec3 n1 = (P2 - P1).normalized(); - Vec3 n3 = (n1.cross(P3 - P1)).normalized(); - Vec3 n2 = n3.cross(n1); + const Vec3 n1 = (P2 - P1).normalized(); + const Vec3 n3 = (n1.cross(P3 - P1)).normalized(); + const Vec3 n2 = n3.cross(n1); Mat3 N; N.row(0) = n1; N.row(1) = n2; N.row(2) = n3; - // Extraction of known parameters + // extraction of known parameters P3 = N * (P3 - P1); - double d_12 = (P2 - P1).norm(); - double f_1 = f3[0] / f3[2]; - double f_2 = f3[1] / f3[2]; - double p_1 = P3[0]; - double p_2 = P3[1]; + const double d_12 = (P2 - P1).norm(); + const double f_1 = f3[0] / f3[2]; + const double f_2 = f3[1] / f3[2]; + const double p_1 = P3[0]; + const double p_2 = P3[1]; - double cos_beta = f1.transpose() * f2; - double b = 1.0 / (1.0 - pow(cos_beta, 2)) - 1.0; + const double cos_beta = f1.transpose() * f2; - if(cos_beta < 0) - b = -sqrt(b); - else - b = sqrt(b); + const double sign = ((cos_beta < 0) ? -1.0 : 1.0) ; + const double b = sign * std::sqrt(1.0 / (1.0 - pow(cos_beta, 2)) - 1.0); - // Definition of temporary variables for avoiding multiple computation + // definition of temporary variables for avoiding multiple computation - double f_1_pw2 = pow(f_1, 2); - double f_2_pw2 = pow(f_2, 2); - double p_1_pw2 = pow(p_1, 2); - double p_1_pw3 = p_1_pw2 * p_1; - double p_1_pw4 = p_1_pw3 * p_1; - double p_2_pw2 = pow(p_2, 2); - double p_2_pw3 = p_2_pw2 * p_2; - double p_2_pw4 = p_2_pw3 * p_2; - double d_12_pw2 = pow(d_12, 2); - double b_pw2 = pow(b, 2); + const double f_1_pw2 = pow(f_1, 2); + const double f_2_pw2 = pow(f_2, 2); + const double p_1_pw2 = pow(p_1, 2); + const double p_1_pw3 = p_1_pw2 * p_1; + const double p_1_pw4 = p_1_pw3 * p_1; + const double p_2_pw2 = pow(p_2, 2); + const double p_2_pw3 = p_2_pw2 * p_2; + const double p_2_pw4 = p_2_pw3 * p_2; + const double d_12_pw2 = pow(d_12, 2); + const double b_pw2 = pow(b, 2); - // Computation of factors of 4th degree polynomial + // computation of factors of 4th degree polynomial Vec5 factors; factors << -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4, @@ -212,20 +209,19 @@ bool compute_P3P_Poses(const Mat3 & featureVectors, const Mat3 & worldPoints, Ma + p_2_pw2 * f_1_pw2 * p_1_pw2 + f_2_pw2 * p_2_pw2 * d_12_pw2*b_pw2; - // Computation of roots + // computation of roots Vec4 realRoots; solveQuartic(factors, realRoots); - // Backsubstitution of each solution + // backsubstitution of each solution for(int i = 0; i < 4; ++i) { - double cot_alpha = (-f_1 * p_1 / f_2 - realRoots[i] * p_2 + d_12 * b) / (-f_1 * realRoots[i] * p_2 / f_2 + p_1 - d_12); - - double cos_theta = realRoots[i]; + const double cot_alpha = (-f_1 * p_1 / f_2 - realRoots[i] * p_2 + d_12 * b) / (-f_1 * realRoots[i] * p_2 / f_2 + p_1 - d_12); + const double cos_theta = realRoots[i]; double sin_theta = sqrt(1 - pow(realRoots[i], 2)); - double sin_alpha = sqrt(1. / (pow(cot_alpha, 2) + 1)); + const double sin_alpha = sqrt(1. / (pow(cot_alpha, 2) + 1)); double cos_alpha = sqrt(1. - pow(sin_alpha, 2)); if(cot_alpha < 0) @@ -250,88 +246,44 @@ bool compute_P3P_Poses(const Mat3 & featureVectors, const Mat3 & worldPoints, Ma solutions.col(i * 4) = C; solutions.block<3, 3>(0, i * 4 + 1) = R.transpose(); } + return true; } -void P3PSolver::Solve(const Mat &pt2D, const Mat &pt3D, std::vector *models) +void P3PSolver::solve(const Mat& x2d, const Mat& x3d, std::vector& models) const { + assert(2 == x2d.rows()); + assert(3 == x3d.rows()); + assert(x2d.cols() == x3d.cols()); + Mat3 R; Vec3 t; Mat34 P; - assert(2 == pt2D.rows()); - assert(3 == pt3D.rows()); - assert(pt2D.cols() == pt3D.cols()); - Mat solutions = Mat(3, 4 * 4); + + Mat solutions = Mat(3, 16); + Mat3 pt2D_3x3; - pt2D_3x3.block<2, 3>(0, 0) = pt2D; + pt2D_3x3.block<2, 3>(0, 0) = x2d; pt2D_3x3.row(2).fill(1); pt2D_3x3.col(0).normalize(); pt2D_3x3.col(1).normalize(); pt2D_3x3.col(2).normalize(); - Mat3 pt3D_3x3 = pt3D; - if(compute_P3P_Poses(pt2D_3x3, pt3D_3x3, solutions)) - { - for(size_t i = 0; i < 4; ++i) - { - R = solutions.block<3, 3>(0, i * 4 + 1); - t = -R * solutions.col(i * 4); - P_From_KRt(Mat3::Identity(), R, t, &P); // K = Id - models->push_back(P); - } - } -} -// Compute the residual of the projection distance(pt2D, Project(P,pt3D)) + Mat3 pt3D_3x3 = x3d; -double P3PSolver::Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D) -{ - return (pt2D - Project(P, pt3D)).norm(); -} - -P3P_ResectionKernel_K::P3P_ResectionKernel_K(const Mat2X &x_camera, const Mat3X &X, const Mat3 &K /*= Mat3::Identity()*/) -: x_image_(x_camera), X_(X), K_(K) -{ - assert(x_camera.cols() == X.cols()); - // Conversion from image coordinates to normalized camera coordinates - Mat3X x_image_h; - EuclideanToHomogeneous(x_image_, &x_image_h); - x_camera_ = K_.inverse() * x_image_h; - for(size_t i = 0; i < x_camera_.cols(); ++i) - x_camera_.col(i).normalize(); -} - -void P3P_ResectionKernel_K::Fit(const std::vector &samples, std::vector *models) const -{ - const Mat3 pt2D_3x3(ExtractColumns(x_camera_, samples)); - const Mat3 pt3D_3x3(ExtractColumns(X_, samples)); - Mat solutions(3, 4 * 4); - if(compute_P3P_Poses(pt2D_3x3, pt3D_3x3, solutions)) + if(computeP3PPoses(pt2D_3x3, pt3D_3x3, solutions)) { - Mat34 P; - Mat3 R; - Vec3 t; for(size_t i = 0; i < 4; ++i) { R = solutions.block<3, 3>(0, i * 4 + 1); t = -R * solutions.col(i * 4); - P_From_KRt(K_, R, t, &P); - models->push_back(P); + P_from_KRt(Mat3::Identity(), R, t, &P); // K = Id + + models.emplace_back(P); } } } -double P3P_ResectionKernel_K::Error(size_t sample, const Model &model) const -{ - const Vec3 X = X_.col(sample); - const Mat2X error = Project(model, X) - x_image_.col(sample); - return error.col(0).norm(); -} - -size_t P3P_ResectionKernel_K::NumSamples() const -{ - return static_cast (x_camera_.cols()); -} - - -} // namespace resection -} // namespace aliceVision +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P3PSolver.hpp b/src/aliceVision/multiview/resection/P3PSolver.hpp index c174805c5e..bd9b58d81f 100644 --- a/src/aliceVision/multiview/resection/P3PSolver.hpp +++ b/src/aliceVision/multiview/resection/P3PSolver.hpp @@ -8,81 +8,59 @@ #pragma once -#include - -#include +#include namespace aliceVision { +namespace multiview { namespace resection { typedef Eigen::Matrix Vec5; -void solveQuartic(const Vec5 & factors, Vec4 & realRoots); - -/** - * @brief Compute the absolute pose of a camera using three 3D-to-2D correspondences - * - * Reference: [1] A Novel Parametrization of the P3P-Problem for a Direct Computation of - * Absolute Camera Position and Orientation - * Kneip, L.; Scaramuzza, D. ; Siegwart, R. - * CVPR 2011 - * - * @param featureVectors: 3x3 matrix with UNITARY feature vectors (each column is a vector) - * @param worldPoints: 3x3 matrix with corresponding 3D world points (each column is a point) - * @param solutions: 3x16 matrix that will contain the solutions - * form: [ C1,R1, C2,R2 ... ] - * the obtained orientation matrices are defined as transforming points from the cam to the world frame - * @return bool: true if correct execution - * false if world points aligned - * - * Author: Laurent Kneip, adapted to the project by Pierre Moulon - */ - -bool compute_P3P_Poses(const Mat3 & featureVectors, const Mat3 & worldPoints, Mat & solutions); - -struct P3PSolver -{ - - enum - { - MINIMUM_SAMPLES = 3 - }; - - enum - { - MAX_MODELS = 4 - }; - // Solve the problem of camera pose. - static void Solve(const Mat &pt2D, const Mat &pt3D, std::vector *models); - - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D); -}; - -class P3P_ResectionKernel_K +class P3PSolver : public robustEstimation::ISolver { public: - typedef Mat34 Model; - enum + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override { - MINIMUM_SAMPLES = 3 - }; - - P3P_ResectionKernel_K(const Mat2X &x_camera, const Mat3X &X, const Mat3 &K = Mat3::Identity()); - - void Fit(const std::vector &samples, std::vector *models) const; - - double Error(size_t sample, const Model &model) const; - - size_t NumSamples() const; - -private: - Mat2X x_image_; // camera coordinates - Mat3X x_camera_; // camera coordinates (normalized) - Mat3X X_; // 3D points - Mat3 K_; + return 3; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 4; + } + + /** + * @brief Solve the problem of camera pose. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A list of at most 4 candidate solutions. + */ + void solve(const Mat& x2d, const Mat& x3d, std::vector& models) const override; + + /** + * @brief Solve the problem. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x2d, const Mat& x3d, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("P3PSolver does not support problem solving with weights."); + } }; -} // namespace resection -} // namespace aliceVision +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P4PfSolver.cpp b/src/aliceVision/multiview/resection/P4PfSolver.cpp index 15b79ab5ed..8494142fb8 100644 --- a/src/aliceVision/multiview/resection/P4PfSolver.cpp +++ b/src/aliceVision/multiview/resection/P4PfSolver.cpp @@ -5,7 +5,7 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "P4PfSolver.hpp" -#include +#include #include #include @@ -14,8 +14,16 @@ #include namespace aliceVision { +namespace multiview { namespace resection { +/** + * @brief GJ elimination + * @param[in,out] A + * @param[in] rcnt + * @param[in] ccnt + * @param[in] tol + */ void GJ(double *A, int rcnt, int ccnt, double tol) { int row = 0; @@ -128,6 +136,15 @@ void GJ(double *A, int rcnt, int ccnt, double tol) } } +/** + * @brief Prepare polynomial coefficients. + * @param[in] src1 + * @param[in] src2 + * @param[in] src3 + * @param[in] src4 + * @param[in] src5 + * @param[out] dst1 + */ void computeCoefficients(const double *src1, const double *src2, const double *src3, const double *src4, const double *src5, double *dst1) { // symbolic names. @@ -223,6 +240,17 @@ void computeCoefficients(const double *src1, const double *src2, const double *s dst1[41] = t42 / 2.0 + t41 / 2.0 - t43 / 2.0 + glcd * t42 * t1 / 2.0 - t46 / 2.0 + t84 * t41 / 2.0; } +/** + * @brief Compute P4pf Poses + * [glab, glac, glad, glbc, glbd, glcd], [a1; a2], [b1; b2], [c1; c2], [d1;d2] + * + * @param[in] glab - glXY - ||X-Y||^2 - quadratic distances between 3D points X and Y + * @param[in] a1 (a2) = x (resp y) measurement of the first 2D point + * @param[in] b1 (b2) = x (resp y) measurement of the second 2D point + * @param[in] c1 (c2) = x (resp y) measurement of the third 2D point + * @param[in] d1 (d2) = x (resp y) measurement of the fourth 2D point + * @param[out] A 10 x 10 action matrix + */ void computeP4pfPoses(const double *glab, const double *a1, const double *b1, const double *c1, const double *d1, double *A) { // precalculate polynomial equations coefficients @@ -1034,6 +1062,10 @@ void computeP4pfPoses(const double *glab, const double *a1, const double *b1, co A[99] = -M[6238]; } +/** + * @brief isNan + * @param[in] A matrix + */ bool isNan(const Eigen::MatrixXcd &A) { const Mat B = A.real(); @@ -1044,6 +1076,11 @@ bool isNan(const Eigen::MatrixXcd &A) return false; } +/** + * @brief validSol + * @param[in] sol + * @param[out] vSol + */ bool validSol(const Eigen::MatrixXcd &sol, Mat &vSol) { assert(sol.cols() == 10 && sol.rows() == 4); @@ -1082,6 +1119,13 @@ bool validSol(const Eigen::MatrixXcd &sol, Mat &vSol) return true; } +/** + * @brief Get the rigid transformation + * @param[in] pp1 + * @param[in] pp2 + * @param[out] R + * @param[out] t + */ void getRigidTransform(const Mat &pp1, const Mat &pp2, Mat &R, Vec3 &t) { Mat p1(pp1); @@ -1116,10 +1160,10 @@ void getRigidTransform(const Mat &pp1, const Mat &pp2, Mat &R, Vec3 &t) t = -R * p1mean + p2mean; } -void P4PfSolver::solve(const Mat &pt2Dx, const Mat &pt3Dx, std::vector *models) +void P4PfSolver::solve(const Mat& x2d, const Mat& x3d, std::vector& models) const { - Mat pt2D(pt2Dx); - Mat pt3D(pt3Dx); + Mat pt2D(x2d); + Mat pt3D(x3d); assert(2 == pt2D.rows()); assert(3 == pt3D.rows()); @@ -1210,14 +1254,11 @@ void P4PfSolver::solve(const Mat &pt2Dx, const Mat &pt3Dx, std::vectoremplace_back(Rr, t, f * var2d); + models.emplace_back(Rr, t, f * var2d); } } -double P4PfSolver::error(const p4fSolution & model, const Vec2 & pt2D, const Vec3 & pt3D) -{ - return (pt2D - Project(model.getP(), pt3D)).norm(); -} } // namespace resection +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P4PfSolver.hpp b/src/aliceVision/multiview/resection/P4PfSolver.hpp index 9a14913639..38b94091c4 100644 --- a/src/aliceVision/multiview/resection/P4PfSolver.hpp +++ b/src/aliceVision/multiview/resection/P4PfSolver.hpp @@ -6,19 +6,20 @@ #pragma once -#include - -#include +#include +#include +#include namespace aliceVision { +namespace multiview { namespace resection { /** - * @brief The structure p4fSolution contain one output model + * @brief The structure P4PfModel contain one output model */ -struct p4fSolution +struct P4PfModel { - p4fSolution(Mat R, Vec3 t, double f) + P4PfModel(Mat R, Vec3 t, double f) : _R(R) , _t(t) , _f(f) @@ -33,17 +34,34 @@ struct p4fSolution 0, _f, 0, 0, 0, 1; - P.block(0, 0, 3, 3) = K*_R; - P.block(0, 3, 3, 1) = K*_t; + P.block(0, 0, 3, 3) = K * _R; + P.block(0, 3, 3, 1) = K * _t; return P; } + /// rotation matrix Mat _R; + /// translation vector Vec3 _t; + /// focal length double _f; }; +struct P4PfError : public ISolverErrorResection +{ + /** + * @brief Compute the residual of the projection distance(p2d, project(P,p3d)) + * @param[in] model solution + * @param[in] p2d feature vector + * @param[in] p3d corresponding 3D world point + */ + inline double error(const P4PfModel& model, const Vec2& p2d, const Vec3& p3d) const override + { + return (p2d - project(model.getP(), p3d)).norm(); + } +}; + /** * @brief Compute the absolute pose and focal length of a camera using three 3D-to-2D correspondences * @author Martin Bujnak, adapted to aliceVision by Michal Polic @@ -51,115 +69,52 @@ struct p4fSolution * Bujnak, M., Kukelova, Z., and Pajdla T. * CVPR 2008 */ -struct P4PfSolver +class P4PfSolver : public robustEstimation::ISolver { - enum +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override { - MINIMUM_SAMPLES = 4 - }; + return 4; + } - enum + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override { - MAX_MODELS = 10 - }; + return 10; + } /** * @brief Solve the problem of camera pose. - * - * @param[in] pt2Dx featureVectors: - * 2 x 4 matrix with feature vectors with subtracted principal point (each column is a vector) - * - * @param[in] pt3Dx worldPoints: - * 3 x 4 matrix with corresponding 3D world points (each column is a point) - * - * @param[out] solutions - * M x n vector that will contain the each solution in structure p4fSolution (rotation matrix p4fSolution._R, - * translation vector p4fSolution._t, focal length p4fSolution._f). Following equation holds for each solution: - * lambda*pt2D = diag([p4fSolution._f p4fSolution._f 1])*[p4fSolution._R p4fSolution._t] * pt3D + * @param[in] x2d featureVectors 2 x 4 matrix with feature vectors with subtracted principal point (each column is a vector) + * @param[in] x3d worldPoints 3 x 4 matrix with corresponding 3D world points (each column is a point) + * @param[out] models M x n vector that will contain the each solution in structure P4PfModel (rotation matrix P4PfModel._R, + * translation vector P4PfModel._t, focal length P4PfModel._f). Following equation holds for each solution: + * lambda*pt2D = diag([P4PfModel._f P4PfModel._f 1])*[P4PfModel._R P4PfModel._t] * pt3D */ - static void solve(const Mat &pt2Dx, - const Mat &pt3Dx, - std::vector *models); + void solve(const Mat& x2d, const Mat& x3d, std::vector& models) const override; /** - * @brief Compute the residual of the projection distance(pt2D, Project(P,pt3D)). - * @param[in] solution - * @param[in] pt2D feature vector - * @param[in] pt3D coresponding 3D world point + * @brief Solve the problem. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. */ - static double error(const p4fSolution &model, - const Vec2 &pt2D, - const Vec3 &pt3D); + void solve(const Mat& x2d, const Mat& x3d, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("P4PfSolver does not support problem solving with weights."); + } }; -/** - * @brief GJ elimination - * @param[in,out] A - * @param[in] rcnt - * @param[in] ccnt - * @param[in] tol - */ -void GJ(double *A, int rcnt, int ccnt, double tol); - -/** - * @brief Prepare polynomial coefficients. - * @param[in] src1 - * @param[in] src2 - * @param[in] src3 - * @param[in] src4 - * @param[in] src5 - * @param[out] dst1 - */ -void computeCoefficients(const double *src1, - const double *src2, - const double *src3, - const double *src4, - const double *src5, - double *dst1); - -/** - * @brief Compute P4pf Poses - * [glab, glac, glad, glbc, glbd, glcd], [a1; a2], [b1; b2], [c1; c2], [d1;d2] - * - * @param[in] glab - glXY - ||X-Y||^2 - quadratic distances between 3D points X and Y - * @param[in] a1 (a2) = x (resp y) measurement of the first 2D point - * @param[in] b1 (b2) = x (resp y) measurement of the second 2D point - * @param[in] c1 (c2) = x (resp y) measurement of the third 2D point - * @param[in] d1 (d2) = x (resp y) measurement of the fourth 2D point - * @param[out] A 10 x 10 action matrix - */ -void computeP4pfPoses(const double *glab, - const double *a1, - const double *b1, - const double *c1, - const double *d1, - double *A); - -/** - * @brief isNan - * @param[in] A matrix - */ -bool isNan(const Eigen::MatrixXcd &A); - -/** - * @brief validSol - * @param[in] sol - * @param[out] vSol - */ -bool validSol(const Eigen::MatrixXcd &sol, - Mat &vSol); - -/** - * @brief Get the rigid transformation - * @param[in] pp1 - * @param[in] pp2 - * @param[out] R - * @param[out] t - */ -void getRigidTransform(const Mat &pp1, - const Mat &pp2, - Mat &R, - Vec3 &t); - } // namespace resection +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P5PfrSolver.cpp b/src/aliceVision/multiview/resection/P5PfrSolver.cpp index 49f994941a..4298757db1 100644 --- a/src/aliceVision/multiview/resection/P5PfrSolver.cpp +++ b/src/aliceVision/multiview/resection/P5PfrSolver.cpp @@ -5,7 +5,7 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "P5PfrSolver.hpp" -#include +#include #include #include @@ -13,11 +13,12 @@ #include namespace aliceVision { +namespace multiview { namespace resection { /** * @brief Compute the nullspace, choose the algorithm based on input matrix size - * @param A matrix + * @param[in] A matrix */ Mat nullspace(const Mat &A) { @@ -37,25 +38,22 @@ Mat nullspace(const Mat &A) return N; } -Mat divisionToPolynomialModelDistortion(const p5pfrModel &divisionModel, - double maxRadius, - const Mat &points2d) +Mat P5PfrModel::divisionToPolynomialModelDistortion(const Mat& x2d /*,double maxRadius*/) const { - Vec r = divisionModel._r; Vec k(3); // make k of length 3 if shorter for(Vec::Index i = 0; i < 3; ++i) { - k(i) = (i < r.rows()) ? r(i) : 0; + k(i) = (i < _r.rows()) ? _r(i) : 0; } - Vec di(points2d.cols()); - Vec o(points2d.cols()); + Vec di(x2d.cols()); + Vec o(x2d.cols()); - for(Vec::Index i = 0; i < points2d.cols(); ++i) + for(Vec::Index i = 0; i < x2d.cols(); ++i) { - di(i) = points2d(0, i); + di(i) = x2d(0, i); o(i) = 1; } @@ -80,10 +78,7 @@ Mat divisionToPolynomialModelDistortion(const p5pfrModel &divisionModel, return A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); } -bool computeP5PfrPosesRD(const Mat &featureVectors, - const Mat &worldPoints, - int numOfRadialCoeff, - std::vector *solutions) +bool computePosesRD(const Mat& featureVectors, const Mat& worldPoints, int numOfRadialCoeff, std::vector* solutions) { // Eliminate all linear stuff Mat A = Mat(5, 8); @@ -299,43 +294,38 @@ bool computeP5PfrPosesRD(const Mat &featureVectors, return true; } -bool computeP5PfrPosesRP(const Mat &featureVectors, - const Mat &worldPoints, - int numOfRadialCoeff, - std::vector *solutions) +bool computePosesRP(const Mat& featureVectors, const Mat& worldPoints, int numOfRadialCoeff, std::vector* solutions) { - if(computeP5PfrPosesRD(featureVectors, worldPoints, numOfRadialCoeff, solutions)) + if(computePosesRD(featureVectors, worldPoints, numOfRadialCoeff, solutions)) { - const Mat pt2D_radius = featureVectors.colwise().norm(); + const Mat p2dRadius = featureVectors.colwise().norm(); for(std::size_t i = 0; i < solutions->size(); ++i) { - p5pfrModel &m = solutions->at(i); - m._r = divisionToPolynomialModelDistortion(m, pt2D_radius.maxCoeff(), (1 / m._f) * pt2D_radius); + P5PfrModel &m = solutions->at(i); + m._r = m.divisionToPolynomialModelDistortion((1 / m._f) * p2dRadius /*,p2dRadius.maxCoeff(),*/); } return true; } return false; } -double reprojectionErrorRD(const p5pfrModel &m, - const Vec2 &pt2D, - const Vec3 &pt3D) +double reprojectionErrorRD(const P5PfrModel& model, const Vec2& p2d, const Vec3& p3d) { - if(m._r.rows() > 1) + if(model._r.rows() > 1) { ALICEVISION_CERR("Projection function is not implemented for the radial division undistortion model for more than one parameter." << std::endl); throw std::invalid_argument("Projection function is not implemented for the radial division undistortion model for more than one parameter."); } - Vec3 v = m._R * pt3D + m._t; // from delta to epsilon + Vec3 v = model._R * p3d + model._t; // from delta to epsilon v *= 1.0 / v(2); // normalize to have v(3, :) = 1 // undistorted squared radius const double ru2 = v(0) * v(0) + v(1) * v(1); - // works for fish - eye, i.e.when distorte image gets smaller on the image plane - const double h1 = sqrt(-4 * m._r(0) * ru2 + 1); - const double h2 = 0.5 * ((-2 * m._r(0) * ru2 + 1) - h1) * (1 / (m._r(0) * m._r(0))); + // works for fish-eye, i.e. when distort image gets smaller on the image plane + const double h1 = sqrt(-4 * model._r(0) * ru2 + 1); + const double h2 = 0.5 * ((-2 * model._r(0) * ru2 + 1) - h1) * (1 / (model._r(0) * model._r(0))); const double rd = sqrt(h2 * (1 / ru2)); // distort in epsilon @@ -344,58 +334,28 @@ double reprojectionErrorRD(const p5pfrModel &m, u << v(0) * h3, v(1) * h3; // to alpha - u = m._f * u; - return (pt2D - u).norm(); + u = model._f * u; + return (p2d - u).norm(); } -double reprojectionErrorRP(const p5pfrModel &m, - const Vec2 &pt2D, - const Vec3 &pt3D) +double reprojectionErrorRP(const P5PfrModel& model, const Vec2& p2d, const Vec3& p3d) { - Vec3 v = m._R * pt3D + m._t; // from delta to epsilon + Vec3 v = model._R * p3d + model._t; // from delta to epsilon v *= 1.0 / v(2); // normalize to have v(3, :) = 1 double t = 1; // the final radius parameter const double r = std::hypot(v(0), v(1)); - for(Mat::Index i = 0; i < m._r.rows(); ++i) - t += m._r(i) * pow(r, 2 * (i + 1)); + for(Mat::Index i = 0; i < model._r.rows(); ++i) + t += model._r(i) * pow(r, 2 * (i + 1)); Vec2 u; u << v(0) * t, v(1) * t; // to alpha - u = m._f * u; - return (pt2D - u).norm(); -} - -void P5PfrSolver::solve(const Mat &pt2Dx, - const Mat &pt3Dx, - const int numR, - std::vector *models) -{ - assert(2 == pt2Dx.rows()); - assert(3 == pt3Dx.rows()); - assert(5 == pt3Dx.cols()); - assert(5 == pt2Dx.cols()); - - // The radial distorision is represented by: the radial division undistortion - if(!computeP5PfrPosesRD(pt2Dx, pt3Dx, numR, models)) - models->clear(); - - // The radial distorision is represented by: Brown polynomial distortion model - /*if (!compute_P5Pfr_Poses_RP(pt2Dx, pt3Dx, num_r, models)) - models->clear();*/ -} - -// Compute the residual of the projection distance(pt2D, Project(M,pt3D)) - -double P5PfrSolver::error(const p5pfrModel &m, - const Vec2 &pt2D, - const Vec3 &pt3D) -{ - return reprojectionErrorRD(m, pt2D, pt3D); - //return reproj_error_RP( m, pt2D, pt3D); + u = model._f * u; + return (p2d - u).norm(); } } // namespace resection +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P5PfrSolver.hpp b/src/aliceVision/multiview/resection/P5PfrSolver.hpp index ec7074f6fb..99f7a188db 100644 --- a/src/aliceVision/multiview/resection/P5PfrSolver.hpp +++ b/src/aliceVision/multiview/resection/P5PfrSolver.hpp @@ -6,140 +6,164 @@ #pragma once -#include - -#include +#include +#include namespace aliceVision { +namespace multiview { namespace resection { /** - * @brief The structure p5pfrModel contain one output model + * @brief The structure P5PfrModel contain one output model + * camera description with radial division undistortion parameters 'KRCrd' */ -struct p5pfrModel +struct P5PfrModel { - p5pfrModel(Mat R, Vec3 t, Vec r, double f) + P5PfrModel(const Mat& R, const Vec3& t, const Vec& r, double f) : _R(R) , _t(t) , _r(r) , _f(f) {} + /** + * @brief Inversion of the radial division undistortion to Brown polynomial distortion model conversion + * @param[in] x2d Points on which is the difference minimized, dmax//max(C.K([1 5]))*[0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0.95 1] implicit + * @param[in] maxRadius Maximal distorted radius, 1 implicit + * @return camera description with polynomial radial distoriton parameters 'KRCp' + */ + Mat divisionToPolynomialModelDistortion(const Mat& x2d /*,double maxRadius*/) const; + + /// rotation matrix Mat _R; + /// translation vector Vec3 _t; + /// radial division undistortion parameters Vec _r; + /// focal length double _f; }; /** - * @brief Compute the absolute pose, focal length and radial distorsion of a camera using three 3D-to-2D correspondences + * @brief computeP5PfrPosesRD + * @param[in] featureVectors + * @param[in] worldPoints + * @param[in] numOfRadialCoeff + * @param[out] solutions + */ +bool computePosesRD(const Mat& featureVectors, const Mat& worldPoints, int numOfRadialCoeff, std::vector* solutions); + +/** + * @brief Compute computePosesRD and transform the radial division undistortion to Brown polynomial distortion model + * @param[in] featureVectors + * @param[in] worldPoints + * @param[in] numOfRadialCoeff + * @param[out] solutions + */ +bool computePosesRP(const Mat& featureVectors, const Mat& worldPoints, int numOfRadialCoeff, std::vector* solutions); + +/** + * @brief Compute the reprojection error for the radial division undistortion model + * @param[in] P5PfrModel model + * @param[in] p2d feature vector + * @param[in] p3d corresponding 3D world point + * @return reprojection error for the radial division undistortion model + */ +double reprojectionErrorRD(const P5PfrModel& model, const Vec2& p2d, const Vec3& p3d); + +/** + * @brief Compute the reprojection error for Brown polynomial distortion model + * @param[in] P5PfrModel model + * @param[in] p2d feature vector + * @param[in] p3d corresponding 3D world point + * @return reprojection error for Brown polynomial distortion model + */ +double reprojectionErrorRP(const P5PfrModel& model, const Vec2 &p2d, const Vec3& p3d); + +struct P5PfrError : public ISolverErrorResection +{ + /** + * @brief Compute the residual of the projection distance(p2d, project(P,p3d)) + * @param[in] model solution + * @param[in] p2d feature vector + * @param[in] p3d corresponding 3D world point + */ + inline double error(const P5PfrModel& model, const Vec2& p2d, const Vec3& p3d) const override + { + return reprojectionErrorRD(model, p2d, p3d); + } +}; + + +/** + * @brief Compute the absolute pose, focal length and radial distortion of a camera using three 3D-to-2D correspondences * @author Tomas Pajdla, adapted to aliceVision by Michal Polic * @ref [1] Time solution to the absolute pose problem with unknown radial distortion and focal length * Kukelova, Z., Bujnak, M., and Pajdla T. * ICCV 2013 */ -struct P5PfrSolver +template +class P5PfrSolver : public robustEstimation::ISolver { - enum +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override { - MINIMUM_SAMPLES = 5 - }; + return 5; + } - enum + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override { - MAX_MODELS = 10 - }; + return 10; + } /** * @brief Solve the problem of camera pose. * - * @param pt2Dx featureVectors: - * 2 x 5 matrix with feature vectors with principal point at [0; 0] (each column is a vector) - * - * @param pt3Dx worldPoints: - * 3 x 5 matrix with corresponding 3D world points (each column is a point) - * - * @param num_r numOfRadialCoeff: - * integer which reperesents how many radial distorsion parameters should be computed [min 1, max 3] - * - * @param solutions: - * M x n vector that will contain the each solution in structure M (p5pfModel._R - rotation matrix, - * p5pfModel._t - translation vector, p5pfModel._r - the radial division undistortion parameters, p5pfModel._f - focal length). + * @param[in] x2d featureVectors 2 x 5 matrix with feature vectors with principal point at [0; 0] (each column is a vector) + * @param[in] x3d worldPoints 3 x 5 matrix with corresponding 3D world points (each column is a point) + * @param[in] numOfRadialCoeff number of radial distortion parameters to be computed [min 1, max 3] + * @param[out] models M x n vector that will contain the each solution in structure M */ - static void solve(const Mat &pt2Dx, - const Mat &pt3Dx, - const int num_r, - std::vector *models); + void solve(const Mat& x2d, const Mat& x3d, std::vector& models) const override + { + assert(2 == x2d.rows()); + assert(3 == x3d.rows()); + assert(5 == x3d.cols()); + assert(5 == x2d.cols()); + assert(numOfRadialCoeff_ >= 1 && numOfRadialCoeff_ <= 3 && "P5PfrSolver error: the number of radial parameters must be between 1 to 3 !"); + + // the radial distortion is represented by: the radial division undistortion + if(!computePosesRD(x2d, x3d, numOfRadialCoeff_, &models)) + models.clear(); + + // the radial distortion is represented by: Brown polynomial distortion model + //if(!P5PfrSolver::computePosesRP(x2d, x3d, numR, &models)) + // models.clear(); + } /** - * @brief Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - * @param model solution - * @param pt2D feature vector - * @param pt3D corresponding 3D world point + * @brief Solve the problem of camera pose.. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. */ - static double error(const p5pfrModel &model, - const Vec2 &pt2D, - const Vec3 &pt3D); + void solve(const Mat& x2d, const Mat& x3d, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("P5PfrSolver does not support problem solving with weights."); + } }; -/** - * @brief divisionToPolynomialModelDistortion - * inversion of the radial division undistortion to Brown polynomial distortion model conversion - * @author Tomas Pajdla, adapted to aliceVision by Michal Polic - * @param divisionModel camera description with radial division undistortion parameters 'KRCrd' - * @param maxRadius maximal distorted radius, 1 implicit - * @param points2d points on which is the difference minimized, dmax//max(C.K([1 5]))*[0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0.95 1] implicit - * @return camera description with polynomial radial distoriton parameters 'KRCp' - */ -Mat divisionToPolynomialModelDistortion(const p5pfrModel &divisionModel, - double maxRadius, - const Mat &points2d); - -/** - * @brief computeP5PfrPosesRD - * @param featureVectors - * @param worldPoints - * @param numOfRadialCoeff - * @param solutions - */ -bool computeP5PfrPosesRD(const Mat &featureVectors, - const Mat &worldPoints, - int numOfRadialCoeff, - std::vector *solutions); - -/** - * @brief Compute computeP5PfrPosesRD and transform the radial division undistortion to Brown polynomial distortion model - * @param featureVectors - * @param worldPoints - * @param numOfRadialCoeff - * @param solutions - */ -bool computeP5PfrPosesRP(const Mat &featureVectors, - const Mat &worldPoints, - int numOfRadialCoeff, - std::vector *solutions); - -/** - * @brief Compute the reprojection error for the radial division undistortion model - * @param m P5Pfr model - * @param pt2D feature vector - * @param pt3D corresponding 3D world point - * @return reprojection error for the radial division undistortion model - */ -double reprojectionErrorRD(const p5pfrModel &m, - const Vec2 &pt2D, - const Vec3 &pt3D); - -/** - * @brief Compute the reprojection error for Brown polynomial distortion model - * @param m P5Pfr model - * @param pt2D feature vector - * @param pt3D corresponding 3D world point - * @return reprojection error for Brown polynomial distortion model - */ -double reprojectionErrorRP(const p5pfrModel &m, - const Vec2 &pt2D, - const Vec3 &pt3D); - } // namespace resection +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/ProjectionDistanceError.hpp b/src/aliceVision/multiview/resection/ProjectionDistanceError.hpp new file mode 100644 index 0000000000..80d975e353 --- /dev/null +++ b/src/aliceVision/multiview/resection/ProjectionDistanceError.hpp @@ -0,0 +1,45 @@ +// 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 + + +namespace aliceVision { +namespace multiview { +namespace resection { + +/** + * @brief Compute the residual of the projection distance + * (pt2D, project(P,pt3D)) + */ +struct ProjectionDistanceError : public ISolverErrorResection +{ + inline double error(const robustEstimation::Mat34Model& P, const Vec2& p2d, const Vec3& p3d) const override + { + return (project(P.getMatrix(), p3d) - p2d).norm(); + } +}; + +/** + * @brief Compute the residual of the projection distance + * (pt2D, project(P,pt3D)) + */ +struct ProjectionDistanceSquaredError : public ISolverErrorResection +{ + inline double error(const robustEstimation::Mat34Model& P, const Vec2& p2d, const Vec3& p3d) const override + { + return (project(P.getMatrix(), p3d) - p2d).squaredNorm(); + } +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/Resection6PSolver.cpp b/src/aliceVision/multiview/resection/Resection6PSolver.cpp new file mode 100644 index 0000000000..b3c1f331b0 --- /dev/null +++ b/src/aliceVision/multiview/resection/Resection6PSolver.cpp @@ -0,0 +1,163 @@ +// 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 "Resection6PSolver.hpp" +#include + +namespace aliceVision { +namespace multiview { +namespace resection { + +void translate(const Mat3X& X, const Vec3& vecTranslation, Mat3X* XPoints) +{ + XPoints->resize(X.rows(), X.cols()); + for (int i=0; icol(i) = X.col(i) + vecTranslation; +} + +template +double NullspaceRatio(TMat *A, TVec *nullspace) +{ + if (A->rows() >= A->cols()) + { + Eigen::JacobiSVD svd(*A, Eigen::ComputeFullV); + (*nullspace) = svd.matrixV().col(A->cols()-1); + return svd.singularValues()(A->cols()-2) / svd.singularValues()(0); + } + + // extend A with rows of zeros to make it square. It's a hack, but is + // necessary until Eigen supports SVD with more columns than rows. + TMat A_extended(A->cols(), A->cols()); + A_extended.block(A->rows(), 0, A->cols() - A->rows(), A->cols()).setZero(); + A_extended.block(0,0, A->rows(), A->cols()) = (*A); + + return Nullspace(&A_extended, nullspace); +} + +/** + * @brief Setup the Direct Linear Transform. + * Use template in order to support fixed or dynamic sized matrix. + */ +template +void BuildActionMatrix(Matrix& A, const Mat& pt2D, const Mat& XPoints) +{ + const size_t n = pt2D.cols(); + for (size_t i = 0; i < n; ++i) { + size_t row_index = i * 2; + const Vec3 & X = XPoints.col(i); + const Vec2 & x = pt2D.col(i); + A(row_index, 0) = X(0); + A(row_index, 1) = X(1); + A(row_index, 2) = X(2); + A(row_index, 3) = 1.0; + A(row_index, 8) = -X(0) * x(0); + A(row_index, 9) = -X(1) * x(0); + A(row_index, 10) = -X(2) * x(0); + A(row_index, 11) = -1.0 * x(0); + + row_index = i * 2 + 1; + A(row_index, 4) = X(0); + A(row_index, 5) = X(1); + A(row_index, 6) = X(2); + A(row_index, 7) = 1.0; + A(row_index, 8) = -X(0) * x(1); + A(row_index, 9) = -X(1) * x(1); + A(row_index, 10) = -X(2) * x(1); + A(row_index, 11) = -1.0 * x(1); + } + // Normalize each row + for (size_t i = 0; i < static_cast(A.rows()); ++i) + A.row(i).normalize(); +} + +void solveProblem(const Mat& x2d, const Mat& x3d, std::vector& models, bool bcheck) +{ + assert(2 == x2d.rows()); + assert(3 == x3d.rows()); + assert(6 <= x2d.cols()); + assert(x2d.cols() == x3d.cols()); + + //-- Translate 3D points in order to have X0 = (0,0,0,1). + Vec3 vecTranslation = - x3d.col(0); + Mat4 translationMatrix = Mat4::Identity(); + translationMatrix << 1, 0, 0, vecTranslation(0), + 0, 1, 0, vecTranslation(1), + 0, 0, 1, vecTranslation(2), + 0, 0, 0, 1; + Mat3X XPoints; + translate(x3d, vecTranslation, &XPoints); + + const size_t n = x2d.cols(); + + typedef Eigen::Matrix Vec12; + Vec12 p; + double ratio = -1.0; + + if(n == 6) + { + // In the case of minimal configuration we use fixed sized matrix to let + // Eigen and the compiler doing the maximum of optimization. + typedef Eigen::Matrix Mat12; + Mat12 A = Mat12::Zero(12, 12); + BuildActionMatrix(A, x2d, XPoints); + ratio = NullspaceRatio(&A, &p); + } + else + { + Mat A = Mat::Zero(n*2, 12); + BuildActionMatrix(A, x2d, XPoints); + ratio = NullspaceRatio(&A, &p); + } + + if(bcheck) + { + if(ratio > 1e-5) // assert that at least only one solution if found by SVD + { + Mat34 P = Map(p.data(),4,3).transpose(); + P = P * translationMatrix; + P /= P(2,3); + + Mat3 K, R; + Vec3 t; + KRt_from_P(P,&K,&R,&t); + + // assert point in front of the cam + std::size_t cpt = 0; + + for(std::size_t i = 0; i < n; ++i) + { + cpt += (Depth(R, t, x3d.col(i))>0) ? 1 : 0; + } + + if(cpt == n) + { + models.emplace_back(P); + } + } + } + else + { + Mat34 P = Map(p.data(),4,3).transpose(); + P = P * translationMatrix; + P /= P(2,3); + models.emplace_back(P); + } +} + +void Resection6PSolver::solve(const Mat& x2d, const Mat& x3d, std::vector& models) const +{ + solveProblem(x2d, x3d, models, true); +} + +void Resection6PSolver::solve(const Mat& x2d, const Mat& x3d, std::vector& models, const std::vector& weights) const +{ + solveProblem(x2d, x3d, models, true); // TODO : no weights implementation +} + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/Resection6PSolver.hpp b/src/aliceVision/multiview/resection/Resection6PSolver.hpp new file mode 100644 index 0000000000..4a6dfeec14 --- /dev/null +++ b/src/aliceVision/multiview/resection/Resection6PSolver.hpp @@ -0,0 +1,66 @@ +// 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 + +namespace aliceVision { +namespace multiview { +namespace resection { + +class Resection6PSolver : public robustEstimation::ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 6; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + /** + * @brief Six-point resection + * P Matrix estimation (Pose estimation) + * Compute a projection matrix using linear least squares. + * Rely on Linear Resection algorithm. + * Work from 6 to N points. + * + * @note First 3d point will be translated in order to have X0 = (0,0,0,1) + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A list of solutions. + */ + void solve(const Mat& x2d, const Mat& x3d, std::vector& models) const override; + + /** + * @brief Solve the problem. + * + * @param[in] x2d 2d points in the first image. One per column. + * @param[in] x3d Corresponding 3d points in the second image. One per column. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + void solve(const Mat& x2d, const Mat& x3d, std::vector& models, const std::vector& weights) const override; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/ResectionKernel.cpp b/src/aliceVision/multiview/resection/ResectionKernel.cpp deleted file mode 100644 index 0b26ae7bdc..0000000000 --- a/src/aliceVision/multiview/resection/ResectionKernel.cpp +++ /dev/null @@ -1,524 +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 "ResectionKernel.hpp" -#include - -namespace aliceVision { -namespace resection { -namespace kernel { - -using namespace std; - -void translate( - const Mat3X & X, - const Vec3 & vecTranslation, - Mat3X * XPoints) -{ - XPoints->resize(X.rows(), X.cols()); - for (int i=0; icol(i) = X.col(i) + vecTranslation; -} - -template -double NullspaceRatio(TMat *A, TVec *nullspace) { - if (A->rows() >= A->cols()) { - Eigen::JacobiSVD svd(*A, Eigen::ComputeFullV); - (*nullspace) = svd.matrixV().col(A->cols()-1); - return svd.singularValues()(A->cols()-2) / svd.singularValues()(0); - } - // Extend A with rows of zeros to make it square. It's a hack, but is - // necessary until Eigen supports SVD with more columns than rows. - TMat A_extended(A->cols(), A->cols()); - A_extended.block(A->rows(), 0, A->cols() - A->rows(), A->cols()).setZero(); - A_extended.block(0,0, A->rows(), A->cols()) = (*A); - return Nullspace(&A_extended, nullspace); -} - -/// Setup the Direct Linear Transform. -/// Use template in order to support fixed or dynamic sized matrix. -template -void BuildActionMatrix(Matrix & A, const Mat &pt2D, const Mat &XPoints) { - - const size_t n = pt2D.cols(); - for (size_t i = 0; i < n; ++i) { - size_t row_index = i * 2; - const Vec3 & X = XPoints.col(i); - const Vec2 & x = pt2D.col(i); - A(row_index, 0) = X(0); - A(row_index, 1) = X(1); - A(row_index, 2) = X(2); - A(row_index, 3) = 1.0; - A(row_index, 8) = -X(0) * x(0); - A(row_index, 9) = -X(1) * x(0); - A(row_index, 10) = -X(2) * x(0); - A(row_index, 11) = -1.0 * x(0); - - row_index = i * 2 + 1; - A(row_index, 4) = X(0); - A(row_index, 5) = X(1); - A(row_index, 6) = X(2); - A(row_index, 7) = 1.0; - A(row_index, 8) = -X(0) * x(1); - A(row_index, 9) = -X(1) * x(1); - A(row_index, 10) = -X(2) * x(1); - A(row_index, 11) = -1.0 * x(1); - } - // Normalize each row - for (size_t i = 0; i < static_cast(A.rows()); ++i) - A.row(i).normalize(); -} - -void SixPointResectionSolver::Solve( - const Mat &pt2D, - const Mat &pt3d, - vector *Ps, - bool bcheck) -{ - assert(2 == pt2D.rows()); - assert(3 == pt3d.rows()); - assert(6 <= pt2D.cols()); - assert(pt2D.cols() == pt3d.cols()); - - //-- Translate 3D points in order to have X0 = (0,0,0,1). - Vec3 vecTranslation = - pt3d.col(0); - Mat4 translationMatrix = Mat4::Identity(); - translationMatrix << 1, 0, 0, vecTranslation(0), - 0, 1, 0, vecTranslation(1), - 0, 0, 1, vecTranslation(2), - 0, 0, 0, 1; - Mat3X XPoints; - translate(pt3d, vecTranslation, &XPoints); - - const size_t n = pt2D.cols(); - - typedef Eigen::Matrix Vec12; - Vec12 p; - double ratio = -1.0; - if (n==6) { - // In the case of minimal configuration we use fixed sized matrix to let - // Eigen and the compiler doing the maximum of optimization. - typedef Eigen::Matrix Mat12; - Mat12 A = Mat12::Zero(12, 12); - BuildActionMatrix(A, pt2D, XPoints); - ratio = NullspaceRatio(&A, &p); - } - else { - Mat A = Mat::Zero(n*2, 12); - BuildActionMatrix(A, pt2D, XPoints); - ratio = NullspaceRatio(&A, &p); - } - if (bcheck) { - if (ratio > 1e-5) //Assert that at least only one solution if found by SVD - { - Mat34 P = Map(p.data(),4,3).transpose(); - P = P * translationMatrix; - P /= P(2,3); - - Mat3 K, R; - Vec3 t; - KRt_From_P(P,&K,&R,&t); - - //Assert point in front of the cam - size_t cpt = 0; - for (size_t i = 0; i < n; ++i) { - cpt += (Depth(R, t, pt3d.col(i))>0) ? 1 : 0; - } - if (cpt == n) { - Ps->push_back(P); - } - } - } - else { - Mat34 P = Map(p.data(),4,3).transpose(); - P = P * translationMatrix; - P /= P(2,3); - Ps->push_back(P); - } -} - -} // namespace kernel -} // namespace resection -} // namespace aliceVision - -namespace aliceVision { -namespace resection { -namespace kernel { - -// Selects 4 virtual control points using mean and PCA. -void SelectControlPoints(const Mat3X &X_world, - Mat *X_centered, - Mat34 *X_control_points) { - size_t num_points = X_world.cols(); - - // The first virtual control point, C0, is the centroid. - Vec mean, variance; - MeanAndVarianceAlongRows(X_world, &mean, &variance); - X_control_points->col(0) = mean; - - // Computes PCA - X_centered->resize (3, num_points); - for (size_t c = 0; c < num_points; c++) { - X_centered->col(c) = X_world.col (c) - mean; - } - Mat3 X_centered_sq = (*X_centered) * X_centered->transpose(); - Eigen::JacobiSVD X_centered_sq_svd(X_centered_sq, Eigen::ComputeFullU); - Vec3 w = X_centered_sq_svd.singularValues(); - Mat3 u = X_centered_sq_svd.matrixU(); - for (size_t c = 0; c < 3; c++) { - double k = sqrt (w (c) / num_points); - X_control_points->col (c + 1) = mean + k * u.col (c); - } -} - -// Computes the barycentric coordinates for all real points -void ComputeBarycentricCoordinates(const Mat3X &X_world_centered, - const Mat34 &X_control_points, - Mat4X *alphas) { - size_t num_points = X_world_centered.cols(); - Mat3 C2 ; - for (size_t c = 1; c < 4; c++) { - C2.col(c-1) = X_control_points.col(c) - X_control_points.col(0); - } - - Mat3 C2inv = C2.inverse(); - Mat3X a = C2inv * X_world_centered; - - alphas->resize(4, num_points); - alphas->setZero(); - alphas->block(1, 0, 3, num_points) = a; - for (size_t c = 0; c < num_points; c++) { - (*alphas)(0, c) = 1.0 - alphas->col(c).sum(); - } -} - -// Estimates the coordinates of all real points in the camera coordinate frame -void ComputePointsCoordinatesInCameraFrame( - const Mat4X &alphas, - const Vec4 &betas, - const Eigen::Matrix &U, - Mat3X *X_camera) { - size_t num_points = alphas.cols(); - - // Estimates the control points in the camera reference frame. - Mat34 C2b; C2b.setZero(); - for (size_t cu = 0; cu < 4; cu++) { - for (size_t c = 0; c < 4; c++) { - C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose(); - } - } - - // Estimates the 3D points in the camera reference frame - X_camera->resize(3, num_points); - for (size_t c = 0; c < num_points; c++) { - X_camera->col(c) = C2b * alphas.col(c); - } - - // Check the sign of the z coordinate of the points (should be positive) - size_t num_z_neg = 0; - for (Mat3X::Index i = 0; i < X_camera->cols(); ++i) { - if ((*X_camera)(2,i) < 0) { - num_z_neg++; - } - } - - // If more than 50% of z are negative, we change the signs - if (num_z_neg > 0.5 * X_camera->cols()) { - C2b = -C2b; - *X_camera = -(*X_camera); - } -} - -void AbsoluteOrientation(const Mat3X &X, - const Mat3X &Xp, - Mat3 *R, - Vec3 *t) { - int num_points = static_cast(X.cols()); - Vec3 C = X.rowwise().sum() / num_points; // Centroid of X. - Vec3 Cp = Xp.rowwise().sum() / num_points; // Centroid of Xp. - - // Normalize the two point sets. - Mat3X Xn(3, num_points), Xpn(3, num_points); - for( int i = 0; i < num_points; ++i ){ - Xn.col(i) = X.col(i) - C; - Xpn.col(i) = Xp.col(i) - Cp; - } - - // Construct the N matrix (pg. 635). - double Sxx = Xn.row(0).dot(Xpn.row(0)); - double Syy = Xn.row(1).dot(Xpn.row(1)); - double Szz = Xn.row(2).dot(Xpn.row(2)); - double Sxy = Xn.row(0).dot(Xpn.row(1)); - double Syx = Xn.row(1).dot(Xpn.row(0)); - double Sxz = Xn.row(0).dot(Xpn.row(2)); - double Szx = Xn.row(2).dot(Xpn.row(0)); - double Syz = Xn.row(1).dot(Xpn.row(2)); - double Szy = Xn.row(2).dot(Xpn.row(1)); - - Mat4 N; - N << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx, - Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz, - Szx - Sxz, Sxy + Syx, -Sxx + Syy - Szz, Syz + Szy, - Sxy - Syx, Szx + Sxz, Syz + Szy, -Sxx - Syy + Szz; - - // Find the unit quaternion q that maximizes qNq. It is the eigenvector - // corresponding to the largest eigenvalue. - Vec4 q = N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0); - - // Retrieve the 3x3 rotation matrix. - Vec4 qq = q.array() * q.array(); - double q0q1 = q(0) * q(1); - double q0q2 = q(0) * q(2); - double q0q3 = q(0) * q(3); - double q1q2 = q(1) * q(2); - double q1q3 = q(1) * q(3); - double q2q3 = q(2) * q(3); - - (*R) << qq(0) + qq(1) - qq(2) - qq(3), - 2 * (q1q2 - q0q3), - 2 * (q1q3 + q0q2), - 2 * (q1q2+ q0q3), - qq(0) - qq(1) + qq(2) - qq(3), - 2 * (q2q3 - q0q1), - 2 * (q1q3 - q0q2), - 2 * (q2q3 + q0q1), - qq(0) - qq(1) - qq(2) + qq(3); - - // Fix the handedness of the R matrix. - if (R->determinant() < 0) { - R->row(2) = -R->row(2); - } - // Compute the final translation. - *t = Cp - *R * C; -} - - -bool EuclideanResectionEPnP(const Mat2X &x_camera, - const Mat3X &X_world, - Mat3 *R, Vec3 *t) { - assert(x_camera.cols() == X_world.cols()); - assert(x_camera.cols() > 3); - size_t num_points = X_world.cols(); - - // Select the control points. - Mat34 X_control_points; - Mat X_centered; - SelectControlPoints(X_world, &X_centered, &X_control_points); - - // Compute the barycentric coordinates. - Mat4X alphas(4, num_points); - ComputeBarycentricCoordinates(X_centered, X_control_points, &alphas); - - // Estimates the M matrix with the barycentric coordinates - Mat M(2 * num_points, 12); - Eigen::Matrix sub_M; - for (size_t c = 0; c < num_points; c++) { - double a0 = alphas(0, c); - double a1 = alphas(1, c); - double a2 = alphas(2, c); - double a3 = alphas(3, c); - double ui = x_camera(0, c); - double vi = x_camera(1, c); - M.block(2*c, 0, 2, 12) << a0, 0, - a0*(-ui), a1, 0, - a1*(-ui), a2, 0, - a2*(-ui), a3, 0, - a3*(-ui), 0, - a0, a0*(-vi), 0, - a1, a1*(-vi), 0, - a2, a2*(-vi), 0, - a3, a3*(-vi); - } - - // TODO(julien): Avoid the transpose by rewriting the u2.block() calls. - Eigen::JacobiSVD MtMsvd(M.transpose()*M, Eigen::ComputeFullU); - Eigen::Matrix u2 = MtMsvd.matrixU().transpose(); - - // Estimate the L matrix. - Eigen::Matrix dv1; - Eigen::Matrix dv2; - Eigen::Matrix dv3; - Eigen::Matrix dv4; - - dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3); - dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3); - dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3); - dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3); - dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3); - dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3); - dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3); - dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3); - dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3); - dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3); - dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3); - dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3); - dv3.row(0) = u2.block( 9, 0, 1, 3) - u2.block( 9, 3, 1, 3); - dv3.row(1) = u2.block( 9, 0, 1, 3) - u2.block( 9, 6, 1, 3); - dv3.row(2) = u2.block( 9, 0, 1, 3) - u2.block( 9, 9, 1, 3); - dv3.row(3) = u2.block( 9, 3, 1, 3) - u2.block( 9, 6, 1, 3); - dv3.row(4) = u2.block( 9, 3, 1, 3) - u2.block( 9, 9, 1, 3); - dv3.row(5) = u2.block( 9, 6, 1, 3) - u2.block( 9, 9, 1, 3); - dv4.row(0) = u2.block( 8, 0, 1, 3) - u2.block( 8, 3, 1, 3); - dv4.row(1) = u2.block( 8, 0, 1, 3) - u2.block( 8, 6, 1, 3); - dv4.row(2) = u2.block( 8, 0, 1, 3) - u2.block( 8, 9, 1, 3); - dv4.row(3) = u2.block( 8, 3, 1, 3) - u2.block( 8, 6, 1, 3); - dv4.row(4) = u2.block( 8, 3, 1, 3) - u2.block( 8, 9, 1, 3); - dv4.row(5) = u2.block( 8, 6, 1, 3) - u2.block( 8, 9, 1, 3); - - Eigen::Matrix L; - for (size_t r = 0; r < 6; r++) { - L.row(r) << dv1.row(r).dot(dv1.row(r)), - 2.0 * dv1.row(r).dot(dv2.row(r)), - dv2.row(r).dot(dv2.row(r)), - 2.0 * dv1.row(r).dot(dv3.row(r)), - 2.0 * dv2.row(r).dot(dv3.row(r)), - dv3.row(r).dot(dv3.row(r)), - 2.0 * dv1.row(r).dot(dv4.row(r)), - 2.0 * dv2.row(r).dot(dv4.row(r)), - 2.0 * dv3.row(r).dot(dv4.row(r)), - dv4.row(r).dot(dv4.row(r)); - } - Vec rho; - rho.resize(6); - rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(), - (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(), - (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(), - (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(), - (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(), - (X_control_points.col(2) - X_control_points.col(3)).squaredNorm(); - - // There are three possible solutions based on the three approximations of L - // (betas). Below, each one is solved for then the best one is chosen. - Mat3X X_camera; - Mat3 K; K.setIdentity(); - std::vector Rs(3); - std::vector ts(3); - Vec rmse(3); - - bool bSol = false; - - // Find the first possible solution for R, t corresponding to: - // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] - // Betas_approx_1 = [b00 b01 b02 b03] - Vec4 betas = Vec4::Zero(); - Eigen::Matrix l_6x4; - for (size_t r = 0; r < 6; r++) { - l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6); - } - Eigen::JacobiSVD svd_of_l4(l_6x4, - Eigen::ComputeFullU | Eigen::ComputeFullV); - Vec4 b4 = svd_of_l4.solve(rho); - if ((l_6x4 * b4).isApprox(rho, 1e-3)) { - if (b4(0) < 0) { - b4 = -b4; - } - b4(0) = std::sqrt(b4(0)); - betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0); - ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera); - AbsoluteOrientation(X_world, X_camera, &Rs[0], &ts[0]); - rmse(0) = reprojectionErrorRMSE(x_camera, X_world, K, Rs[0], ts[0]); - bSol = true; - } else { - //ALICEVISION_LOG_WARNING("First approximation of beta not good enough."); - ts[0].setZero(); - rmse(0) = std::numeric_limits::max(); - } - - // Find the second possible solution for R, t corresponding to: - // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] - // Betas_approx_2 = [b00 b01 b11] - betas.setZero(); - Eigen::Matrix l_6x3; - l_6x3 = L.block(0, 0, 6, 3); - Eigen::JacobiSVD svdOfL3(l_6x3, - Eigen::ComputeFullU | Eigen::ComputeFullV); - Vec3 b3 = svdOfL3.solve(rho); - if ((l_6x3 * b3).isApprox(rho, 1e-3)) { - if (b3(0) < 0) { - betas(0) = std::sqrt(-b3(0)); - betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0; - } else { - betas(0) = std::sqrt(b3(0)); - betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0; - } - if (b3(1) < 0) { - betas(0) = -betas(0); - } - betas(2) = 0; - betas(3) = 0; - ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera); - AbsoluteOrientation(X_world, X_camera, &Rs[1], &ts[1]); - rmse(1) = reprojectionErrorRMSE(x_camera, X_world, K, Rs[1], ts[1]); - bSol = true; - } else { - //ALICEVISION_LOG_WARNING("Second approximation of beta not good enough."); - ts[1].setZero(); - rmse(1) = std::numeric_limits::max(); - } - - // Find the third possible solution for R, t corresponding to: - // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] - // Betas_approx_3 = [b00 b01 b11 b02 b12] - betas.setZero(); - Eigen::Matrix l_6x5; - l_6x5 = L.block(0, 0, 6, 5); - Eigen::JacobiSVD svdOfL5(l_6x5, - Eigen::ComputeFullU | Eigen::ComputeFullV); - Vec b5 = svdOfL5.solve(rho); - if ((l_6x5 * b5).isApprox(rho, 1e-3)) { - if (b5(0) < 0) { - betas(0) = std::sqrt(-b5(0)); - if (b5(2) < 0) { - betas(1) = std::sqrt(-b5(2)); - } else { - b5(2) = 0; - } - } else { - betas(0) = std::sqrt(b5(0)); - if (b5(2) > 0) { - betas(1) = std::sqrt(b5(2)); - } else { - b5(2) = 0; - } - } - if (b5(1) < 0) { - betas(0) = -betas(0); - } - betas(2) = b5(3) / betas(0); - betas(3) = 0; - ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera); - AbsoluteOrientation(X_world, X_camera, &Rs[2], &ts[2]); - rmse(2) = reprojectionErrorRMSE(x_camera, X_world, K, Rs[2], ts[2]); - bSol = true; - } else { - //ALICEVISION_LOG_WARNING("Third approximation of beta not good enough."); - ts[2].setZero(); - rmse(2) = std::numeric_limits::max(); - } - - // Finally, with all three solutions, select the (R, t) with the best RMSE. - size_t n = 0; - if (rmse(1) < rmse(0)) { - n = 1; - } - if (rmse(2) < rmse(n)) { - n = 2; - } - if (bSol) { //If at least one solution have been found - *R = Rs[n]; - *t = ts[n]; - return true; - } - - return false; -} - - -} // namespace kernel -} // namespace resection -} // namespace aliceVision - diff --git a/src/aliceVision/multiview/resection/ResectionKernel.hpp b/src/aliceVision/multiview/resection/ResectionKernel.hpp index 78926e8ce5..68e72be49d 100644 --- a/src/aliceVision/multiview/resection/ResectionKernel.hpp +++ b/src/aliceVision/multiview/resection/ResectionKernel.hpp @@ -7,171 +7,47 @@ #pragma once -#include -#include - -#include +#include +#include +#include namespace aliceVision { +namespace multiview { namespace resection { -namespace kernel { /** - * Six-point resection - * P Matrix estimation (Pose estimation) - * Compute a projection matrix using linear least squares. - * Rely on Linear Resection algorithm. - * Work from 6 to N points. + * @brief Generic solver for resection algorithm using linear least squares. */ -struct SixPointResectionSolver { - enum { MINIMUM_SAMPLES = 6 }; - enum { MAX_MODELS = 1 }; - // Solve the problem of camera pose. - // First 3d point will be translated in order to have X0 = (0,0,0,1) - static void Solve(const Mat &pt2D, const Mat &pt3D, std::vector *P, bool bcheck = true); - - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D){ - Vec2 x = Project(P, pt3D); - return (x-pt2D).norm(); - } -}; - -//-- Generic Solver for the 6pt Resection algorithm using linear least squares. -template -class ResectionKernel : - public twoView::kernel::Kernel +template +class ResectionKernel : public robustEstimation::PointFittingKernel { public: - // 2D / 3D points - ResectionKernel(const Mat &pt2D, const Mat &pt3D): - twoView::kernel::Kernel(pt2D,pt3D){} - void Fit(const std::vector &samples, std::vector *models) const { - Mat pt2d = ExtractColumns(this->x1_, samples); - Mat pt3d = ExtractColumns(this->x2_, samples); + using KernelBase = robustEstimation::PointFittingKernel; - assert(2 == pt2d.rows()); - assert(3 == pt3d.rows()); - assert(SolverArg::MINIMUM_SAMPLES <= pt2d.cols()); - assert(pt2d.cols() == pt3d.cols()); + ResectionKernel(const Mat& x2d, const Mat& x3d) + : robustEstimation::PointFittingKernel(x2d, x3d) + {} - SolverArg::Solve(pt2d, pt3d, models); - } + void fit(const std::vector& samples, std::vector& models) const override + { + const Mat x2d = ExtractColumns(KernelBase::_x1, samples); + const Mat x3d = ExtractColumns(KernelBase::_x2, samples); - // Error : re-projection error of the sample - double Error(size_t sample, const ModelArg &model) const { - return ErrorArg::Error(model, this->x1_.col(sample), this->x2_.col(sample)); + assert(2 == x2d.rows()); + assert(3 == x3d.rows()); + assert(KernelBase::_kernelSolver.getMinimumNbRequiredSamples() <= x2d.cols()); + assert(x2d.cols() == x3d.cols()); + + KernelBase::_kernelSolver.solve(x2d, x3d, models); } }; -//-- Usable solver for the 6pt Resection estimation -typedef twoView::kernel::Kernel PoseResectionKernel; - -} // namespace kernel -} // namespace resection -} // namespace aliceVision - -//-- -// Euclidean resection kernel (Have K intrinsic helps) -//-- - -namespace aliceVision { -namespace resection { -namespace kernel { - /** - * Computes the extrinsic parameters, R and t for a calibrated camera from 4 or - * more 3D points and their images. - * - * \param x_camera Image points in normalized camera coordinates, - * e.g. x_camera = inv(K) * x_image - * \param X_world 3D points in the world coordinate system - * \param R Solution for the camera rotation matrix - * \param t Solution for the camera translation vector - * - * This is the algorithm described in: - * "{EP$n$P: An Accurate $O(n)$ Solution to the P$n$P Problem", by V. Lepetit - * and F. Moreno-Noguer and P. Fua, IJCV 2009. vol. 81, no. 2 - * \note: the non-linear optimization is not implemented here. + * @brief Usable solver for the 6pt Resection estimation */ -bool EuclideanResectionEPnP(const Mat2X &x_camera, - const Mat3X &X_world, - Mat3 *R, Vec3 *t); - -struct EpnpSolver { - enum { MINIMUM_SAMPLES = /*5*/ 6 }; - enum { MAX_MODELS = 1 }; - // Solve the problem of camera pose. - static void Solve(const Mat &pt2D, const Mat &pt3D, std::vector *models) - { - Mat3 R; - Vec3 t; - Mat34 P; - if (EuclideanResectionEPnP(pt2D, pt3D, &R, &t)) { - P_From_KRt(Mat3::Identity(), R, t, &P); // K = Id - models->push_back(P); - } - } - - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D) { - return (pt2D - Project(P, pt3D)).norm(); - } -}; - -class ResectionKernel_K { - public: - typedef Mat34 Model; - enum { MINIMUM_SAMPLES = 6 }; - - ResectionKernel_K(const Mat2X &x_camera, const Mat3X &X) : x_camera_(x_camera), X_(X) { - assert(x_camera.cols() == X.cols()); - x_image_ = x_camera_; - K_ = Mat3::Identity(); - } - - ResectionKernel_K(const Mat2X &x_image, const Mat3X &X, const Mat3 &K) - : x_image_(x_image), X_(X), K_(K) - { - assert(x_image.cols() == X.cols()); - // Conversion from image coordinates to normalized camera coordinates - EuclideanToNormalizedCamera(x_image_, K, &x_camera_); - } - - void Fit(const std::vector &samples, std::vector *models) const { - Mat2X x = ExtractColumns(x_camera_, samples); - Mat3X X = ExtractColumns(X_, samples); - Mat34 P; - Mat3 R; - Vec3 t; - if (EuclideanResectionEPnP(x, X, &R, &t)) - { - P_From_KRt(K_, R, t, &P); - models->push_back(P); - } - } - - double Error(size_t sample, const Model &model) const { - Mat3X X = X_.col(sample); - Mat2X error = Project(model, X) - x_image_.col(sample); - return error.col(0).norm(); - } - - size_t NumSamples() const { - return static_cast(x_camera_.cols()); - } - - private: - // x_camera_ contains the normalized camera coordinates - Mat2X x_camera_, x_image_; - const Mat3X &X_; - Mat3 K_; -}; +typedef robustEstimation::PointFittingKernel Resection6PKernel; -} // namespace kernel } // namespace resection +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/p4pfSolver_test.cpp b/src/aliceVision/multiview/resection/p4pfSolver_test.cpp index 9aaf76eba4..7283b698d9 100644 --- a/src/aliceVision/multiview/resection/p4pfSolver_test.cpp +++ b/src/aliceVision/multiview/resection/p4pfSolver_test.cpp @@ -18,8 +18,9 @@ using namespace aliceVision; +using namespace aliceVision::multiview; -bool isEqual(const resection::p4fSolution first, const resection::p4fSolution second) +bool isEqual(const resection::P4PfModel first, const resection::P4PfModel second) { double eps = 1e-3; return ((first._R - second._R).norm() < first._R.maxCoeff() * eps && @@ -43,11 +44,12 @@ BOOST_AUTO_TEST_CASE(Resection_P4Pf_AssignmentWithOneResult) R_1 << -0.97189, 0.05884, -0.22797, -0.02068, -0.98586, -0.16631, -0.23454, -0.15692, 0.95936; Vec3 t_1; t_1 << 2.00322, -1.27420, 2.92685; - resection::p4fSolution sol_1(R_1, t_1, 887.17549); + resection::P4PfModel sol_1(R_1, t_1, 887.17549); // PROCESS THE RESECTION P4Pf - std::vector models_1; - resection::P4PfSolver::solve(pt2D_1, pt3D_1, &models_1); + std::vector models_1; + resection::P4PfSolver solver; + solver.solve(pt2D_1, pt3D_1, models_1); bool pass = true; if(!(models_1.size() == 1 && isEqual(models_1.at(0), sol_1))) @@ -79,13 +81,14 @@ BOOST_AUTO_TEST_CASE(Resection_P4Pf_AssignmentWithMoreResults) t_22 << 0.08257, 0.57753, 1.04335; Vec3 t_23; t_23 << 0.16029, 0.58720, 1.07571; - resection::p4fSolution sol_21(R_21, t_21, 4571.95746); - resection::p4fSolution sol_22(R_22, t_22, 1193.30606); - resection::p4fSolution sol_23(R_23, t_23, 1315.17564); + resection::P4PfModel sol_21(R_21, t_21, 4571.95746); + resection::P4PfModel sol_22(R_22, t_22, 1193.30606); + resection::P4PfModel sol_23(R_23, t_23, 1315.17564); // PROCESS - std::vector models_2; - resection::P4PfSolver::solve(pt2D_2, pt3D_2, &models_2); + std::vector models_2; + resection::P4PfSolver solver; + solver.solve(pt2D_2, pt3D_2, models_2); bool pass = true; if(!(models_2.size() == 3 @@ -108,8 +111,9 @@ BOOST_AUTO_TEST_CASE(Resection_P4Pf_AssignmentWithNoResults) -1.68077, 0.70813, 1.22217, -1.76850; // PROCESS - std::vector models_3; - resection::P4PfSolver::solve(pt2D_3, pt3D_3, &models_3); + std::vector models_3; + resection::P4PfSolver solver; + solver.solve(pt2D_3, pt3D_3, models_3); bool pass = true; if(models_3.size() != 0) diff --git a/src/aliceVision/multiview/resection/p5pfrSolver_test.cpp b/src/aliceVision/multiview/resection/p5pfrSolver_test.cpp index 71ea473948..e6165d5b3e 100644 --- a/src/aliceVision/multiview/resection/p5pfrSolver_test.cpp +++ b/src/aliceVision/multiview/resection/p5pfrSolver_test.cpp @@ -16,8 +16,8 @@ #include - using namespace aliceVision; +using namespace aliceVision::multiview; #define CHECK_SOLUTIONS(solutions, models, eps) \ { \ @@ -32,7 +32,7 @@ using namespace aliceVision; } -bool sortM(const resection::p5pfrModel &i, const resection::p5pfrModel &j) +bool sortM(const resection::P5PfrModel &i, const resection::P5PfrModel &j) { return (i._f < j._f); } @@ -51,7 +51,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_RandomRealExample) -8.605090000000000e-01, -1.507400000000000e+00, -1.828080000000000e+00, 1.208810000000000e+00, 2.980880000000000e-01; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << -9.430073299811607e-01, 2.928814581476584e-01, -1.579798312288744e-01, -3.071682103315301e-01, -5.834954588038342e-01, 7.517850358427121e-01, @@ -114,9 +114,10 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_RandomRealExample) std::sort(solutions.begin(), solutions.end(), sortM); - // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 1, &models); + // process + std::vector models; + resection::P5PfrSolver<1> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -138,7 +139,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test01) 1.100000000000000e+01, 9.000000000000000e+00, 1.000000000000000e+01, 1.100000000000000e+01, 7.000000000000000e+00; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << -9.866769718754666e-01, 8.864003655023664e-02, 1.364239608391149e-01, -8.610773932011366e-02, -9.959878698575466e-01, 2.436432485840338e-02, @@ -167,13 +168,14 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test01) Vec r2 = Vec(1); r2 << -1.000869226503413e-01; - resection::p5pfrModel m2(R2, t2, r2, f2); + resection::P5PfrModel m2(R2, t2, r2, f2); solutions.emplace_back(m2); std::sort(solutions.begin(), solutions.end(), sortM); // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 1, &models); + std::vector models; + resection::P5PfrSolver<1> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -195,7 +197,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test02) 1.100000000000000e+01, 9.000000000000000e+00, 1.000000000000000e+01, 1.100000000000000e+01, 7.000000000000000e+00; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << -9.866769718754656e-01, 8.864003655023635e-02, 1.364239608391232e-01, -8.610773932011311e-02, -9.959878698575468e-01, 2.436432485840404e-02, @@ -228,8 +230,9 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test02) std::sort(solutions.begin(), solutions.end(), sortM); // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 1, &models); + std::vector models; + resection::P5PfrSolver<1> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -251,7 +254,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test03) 1.100000000000000e+01, 9.000000000000000e+00, 1.000000000000000e+01, 1.100000000000000e+01, 7.000000000000000e+00; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << 9.866726171702454e-01, -8.863862515507148e-02, -1.364563690599919e-01, 8.610372986162385e-02, 9.959878340728819e-01, -2.437995247586850e-02, @@ -284,8 +287,9 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test03) std::sort(solutions.begin(), solutions.end(), sortM); // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 3, &models); + std::vector models; + resection::P5PfrSolver<3> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -307,7 +311,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test04) 1.100000000000000e+01, 9.000000000000000e+00, 1.000000000000000e+01, 1.100000000000000e+01, 7.000000000000000e+00; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << -9.866929474192679e-01, 8.865005182074735e-02, 1.363018555459043e-01, -8.612834136352524e-02, -9.959876607489899e-01, 2.429996810126685e-02, @@ -340,8 +344,9 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test04) std::sort(solutions.begin(), solutions.end(), sortM); // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 2, &models); + std::vector models; + resection::P5PfrSolver<2> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -363,7 +368,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test05) 2.975400000000000e+02, 2.715000000000000e+02, 2.743600000000000e+02, 2.852800000000000e+02, 2.909600000000000e+02; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << 1.155567671802503e-01, 8.662877974144153e-01, -4.859959728328315e-01, 9.628483489629153e-01, 2.253109491131032e-02, 2.691011086218072e-01, @@ -426,8 +431,9 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test05) std::sort(solutions.begin(), solutions.end(), sortM); // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 1, &models); + std::vector models; + resection::P5PfrSolver<1> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -449,7 +455,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test06) 2.975400000000000e+02, 2.715000000000000e+02, 2.743600000000000e+02, 2.852800000000000e+02, 2.909600000000000e+02; // SOLUTIONS - std::vector solutions; + std::vector solutions; Mat R1 = Mat(3, 3); R1 << 1.158093303645603e-01, 8.661302625338441e-01, -4.862165847886791e-01, 9.628499358654846e-01, 2.232902483922909e-02, 2.691122733239069e-01, @@ -512,8 +518,9 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_Test06) std::sort(solutions.begin(), solutions.end(), sortM); // PROCESS - std::vector models; - resection::P5PfrSolver::solve(pt2D, pt3D, 1, &models); + std::vector models; + resection::P5PfrSolver<1> solver; + solver.solve(pt2D, pt3D, models); std::sort(models.begin(), models.end(), sortM); // BOOST_AUTO_TEST_CASE @@ -537,7 +544,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_ReprojectionErrRD) Vec r1 = Vec(1); r1 << -1.915149736941400e-02; - resection::p5pfrModel m1(R1, t1, r1, f1); + resection::P5PfrModel m1(R1, t1, r1, f1); Mat X = Mat(3, 5); X << -4.780000000000000e+00, -5.662000000000001e+01, -3.109000000000000e+01, -1.337000000000000e+01, -1.220000000000000e+00, @@ -575,7 +582,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_ReprojectionErrRP) Vec r1 = Vec(2); r1 << 2.609980000000000e-01, -8.600540000000000e-01; - resection::p5pfrModel m1(R1, t1, r1, f1); + resection::P5PfrModel m1(R1, t1, r1, f1); Mat X = Mat(3, 5); X << -1.789980000000000e+00, 3.126420000000000e-02, -8.818300000000000e-01, 4.003490000000000e-01, -2.801120000000000e-01, @@ -617,7 +624,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_ConversionRD2RP) Vec r1 = Vec(1); r1 << -2.905907042217655e-01; - resection::p5pfrModel m1(R1, t1, r1, f1); + resection::P5PfrModel m1(R1, t1, r1, f1); Mat R2 = Mat(3, 3); R2 << -5.147789719583958e-01, -2.194669622313594e-01, -8.287562141657801e-01, @@ -632,7 +639,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_ConversionRD2RP) Vec r2 = Vec(1); r2 << -3.992521126906051e+00; - resection::p5pfrModel m2(R2, t2, r2, f2); + resection::P5PfrModel m2(R2, t2, r2, f2); // SOLUTIONS ( EXPECTED VALUES ) Vec e_r1 = Vec(3); @@ -644,10 +651,10 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_ConversionRD2RP) const double eps = 1e-4; Mat pt2D_radius = pt2D.colwise().norm(); - m1._r = divisionToPolynomialModelDistortion(m1, pt2D_radius.maxCoeff(), (1 / f1) * pt2D_radius); + m1._r = m1.divisionToPolynomialModelDistortion((1 / f1) * pt2D_radius /*, pt2D_radius.maxCoeff()*/); EXPECT_MATRIX_NEAR(m1._r, e_r1, eps); - m2._r = divisionToPolynomialModelDistortion(m2, pt2D_radius.maxCoeff(), (1 / f2) * pt2D_radius); + m2._r = m2.divisionToPolynomialModelDistortion((1 / f2) * pt2D_radius /*, pt2D_radius.maxCoeff()*/); EXPECT_MATRIX_NEAR(m2._r, e_r2, eps); } @@ -657,7 +664,7 @@ BOOST_AUTO_TEST_CASE(Resection_P5Pfr_ConversionRD2RP) // // // PROCESS // std::vector models; -// resection::P5PfrSolver::Solve(pt2D, pt3D, &models); +// resection::P5PfrSolver::Solve(pt2D, pt3D, models); // std::sort(models.begin(), models.end(), sortM); // // double eps = 1e-4; diff --git a/src/aliceVision/multiview/resection/resectionKernel_test.cpp b/src/aliceVision/multiview/resection/resectionKernel_test.cpp index addfbd9406..a626f00027 100644 --- a/src/aliceVision/multiview/resection/resectionKernel_test.cpp +++ b/src/aliceVision/multiview/resection/resectionKernel_test.cpp @@ -5,11 +5,11 @@ // 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 "aliceVision/multiview/NViewDataSet.hpp" -#include "aliceVision/multiview/resection/ResectionKernel.hpp" -#include "aliceVision/multiview/resection/P3PSolver.hpp" +#include -#include +#include +#include +#include #define BOOST_TEST_MODULE ResectionKernel @@ -18,8 +18,10 @@ #include using namespace aliceVision; +using namespace aliceVision::multiview; -BOOST_AUTO_TEST_CASE(Resection_Kernel_Multiview) { +BOOST_AUTO_TEST_CASE(Resection_Kernel_Multiview) +{ const int nViews = 3; const int nbPoints = 10; @@ -32,28 +34,31 @@ BOOST_AUTO_TEST_CASE(Resection_Kernel_Multiview) { { Mat x = d._x[nResectionCameraIndex]; Mat X = d._X; - aliceVision::resection::kernel::PoseResectionKernel kernel(x, X); + resection::Resection6PKernel kernel(x, X); std::size_t samples_[6]={0,1,2,3,4,5}; std::vector samples(samples_,samples_+6); - std::vector Ps; - kernel.Fit(samples, &Ps); - for (std::size_t i = 0; i < x.cols(); ++i) { - BOOST_CHECK_SMALL(kernel.Error(i, Ps[0]), 1e-8); + std::vector Ps; + + kernel.fit(samples, Ps); + + for (std::size_t i = 0; i < x.cols(); ++i) + { + BOOST_CHECK_SMALL(kernel.error(i, Ps.at(0)), 1e-8); } BOOST_CHECK_EQUAL(1, Ps.size()); // Check that Projection matrix is near to the GT : - Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() - / d.P(nResectionCameraIndex).norm(); - Mat34 COMPUTED_ProjectionMatrix = Ps[0].array() / Ps[0].norm(); + Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() / d.P(nResectionCameraIndex).norm(); + Mat34 COMPUTED_ProjectionMatrix = Ps.at(0).getMatrix().array() / Ps.at(0).getMatrix().norm(); EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-8); } } -BOOST_AUTO_TEST_CASE(P3P_Kneip_CVPR11_Multiview) { - +/* +BOOST_AUTO_TEST_CASE(P3P_Kneip_CVPR11_Multiview) +{ const int nViews = 3; const int nbPoints = 3; const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, @@ -65,12 +70,12 @@ BOOST_AUTO_TEST_CASE(P3P_Kneip_CVPR11_Multiview) { { Mat x = d._x[nResectionCameraIndex]; Mat X = d._X; - aliceVision::resection::P3P_ResectionKernel_K kernel(x, X, d._K[0]); + resection::P3PResectionKernel_K kernel(x, X, d._K[0]); std::size_t samples_[3]={0,1,2}; std::vector samples(samples_, samples_+3); - std::vector Ps; - kernel.Fit(samples, &Ps); + std::vector Ps; + kernel.fit(samples, Ps); bool bFound = false; char index = -1; @@ -78,7 +83,7 @@ BOOST_AUTO_TEST_CASE(P3P_Kneip_CVPR11_Multiview) { { Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() / d.P(nResectionCameraIndex).norm(); - Mat34 COMPUTED_ProjectionMatrix = Ps[i].array() / Ps[i].norm(); + Mat34 COMPUTED_ProjectionMatrix = Ps.at(i).getMatrix().array() / Ps.at(i).getMatrix().norm(); if ( NormLInfinity(GT_ProjectionMatrix - COMPUTED_ProjectionMatrix) < 1e-8 ) { bFound = true; @@ -90,10 +95,11 @@ BOOST_AUTO_TEST_CASE(P3P_Kneip_CVPR11_Multiview) { // Check that for the found matrix residual is small for (std::size_t i = 0; i < x.cols(); ++i) { - BOOST_CHECK_SMALL(kernel.Error(i,Ps[index]), 1e-8); + BOOST_CHECK_SMALL(kernel.error(i,Ps.at(index)), 1e-8); } } } +*/ // Create a new synthetic dataset for the EPnP implementation. // It seems it do not perform well on translation like t = (0,0,x) @@ -109,7 +115,7 @@ void CreateCameraSystem(const Mat3& KK, Mat3 *R_expected, Vec3 *T_expected) { - int num_points = x_image.cols(); + const auto num_points = x_image.cols(); Mat3X x_unit_cam(3, num_points); x_unit_cam = KK.inverse() * x_image; @@ -117,13 +123,16 @@ void CreateCameraSystem(const Mat3& KK, // Create normalized camera coordinates to be used as an input to the PnP // function, instead of using NormalizeColumnVectors(&x_unit_cam). *x_camera = x_unit_cam.block(0, 0, 2, num_points); - for (int i = 0; i < num_points; ++i){ + + for (int i = 0; i < num_points; ++i) + { x_unit_cam.col(i).normalize(); } // Create the 3D points in the camera system. Mat X_camera(3, num_points); - for (int i = 0; i < num_points; ++i) { + for (int i = 0; i < num_points; ++i) + { X_camera.col(i) = X_distances(i) * x_unit_cam.col(i); } @@ -138,7 +147,7 @@ void CreateCameraSystem(const Mat3& KK, // Create the expected result for comparison. *R_expected = R_input.transpose(); *T_expected = *R_expected * ( - T_input); -}; +} BOOST_AUTO_TEST_CASE(EuclideanResection_Points6AllRandomInput) { @@ -178,13 +187,13 @@ BOOST_AUTO_TEST_CASE(EuclideanResection_Points6AllRandomInput) { { - typedef aliceVision::resection::kernel::ResectionKernel_K Kernel; + using Kernel = resection::EPnPKernel; Kernel kernel(x_image.block(0, 0, 2, 6), X_world, KK); std::size_t samples_[6]={0,1,2,3,4,5}; - std::vector samples(samples_,samples_+6); - std::vector Ps; - kernel.Fit(samples, &Ps); + std::vector samples(samples_, samples_+6); + std::vector Ps; + kernel.fit(samples, Ps); BOOST_CHECK_EQUAL(1, Ps.size()); @@ -194,10 +203,9 @@ BOOST_AUTO_TEST_CASE(EuclideanResection_Points6AllRandomInput) { Mat3 R_output; Vec3 T_output; Mat3 K; - KRt_From_P(Ps[i], &K, &R_output, &T_output); - if ( NormLInfinity(T_output-T_expected) < 1e-8 && - NormLInfinity(R_output-R_expected) < 1e-8) - bFound = true; + KRt_from_P(Ps.at(i).getMatrix(), &K, &R_output, &T_output); + if(NormLInfinity(T_output-T_expected) < 1e-8 && NormLInfinity(R_output-R_expected) < 1e-8) + bFound = true; } BOOST_CHECK(bFound); } diff --git a/src/aliceVision/multiview/resection/resectionLORansac_test.cpp b/src/aliceVision/multiview/resection/resectionLORansac_test.cpp index 62d1b3b0e0..42d20bbeac 100644 --- a/src/aliceVision/multiview/resection/resectionLORansac_test.cpp +++ b/src/aliceVision/multiview/resection/resectionLORansac_test.cpp @@ -4,14 +4,15 @@ // 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 #include #include @@ -29,18 +30,6 @@ using namespace aliceVision; -struct ResectionSquaredResidualError -{ - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - // Return the squared error - - static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D) - { - const Vec2 x = Project(P, pt3D); - return (x - pt2D).squaredNorm(); - } -}; - bool refinePoseAsItShouldbe(const Mat & pt3D, const Mat & pt2D, const std::vector & vec_inliers, @@ -187,36 +176,36 @@ BOOST_AUTO_TEST_CASE(P3P_Ransac_noisyFromImagePoints) for(std::size_t trial = 0; trial < NUMTRIALS; ++trial) { ALICEVISION_LOG_DEBUG("Trial #" << trial); - typedef aliceVision::resection::P3PSolver SolverType; - typedef aliceVision::resection::kernel::SixPointResectionSolver SolverLSType; + typedef multiview::resection::P3PSolver SolverType; + typedef multiview::resection::Resection6PSolver SolverLSType; - typedef aliceVision::robustEstimation::KernelAdaptorResectionLORansac_K KernelType; + typedef aliceVision::multiview::ResectionKernel_K KernelType; // this is just to simplify and use image plane coordinates instead of camera // (pixel) coordinates Mat pts2Dnorm; - ApplyTransformationToPoints(pts2D, Kgt.inverse(), &pts2Dnorm); + robustEstimation::applyTransformationToPoints(pts2D, Kgt.inverse(), &pts2Dnorm); KernelType kernel(pts2Dnorm, pts3D, Mat3::Identity()); - std::vector vec_inliers; + std::vector inliers; const double threshold = 2*gaussianNoiseLevel; const double normalizedThreshold = Square(threshold / FOCAL); robustEstimation::ScoreEvaluator scorer(normalizedThreshold); - Mat34 Pest = robustEstimation::LO_RANSAC(kernel, scorer, &vec_inliers); - - const std::size_t numInliersFound = vec_inliers.size(); + robustEstimation::Mat34Model model = robustEstimation::LO_RANSAC(kernel, scorer, &inliers); + Mat34 Pest = model.getMatrix(); + const std::size_t numInliersFound = inliers.size(); const std::size_t numInliersExpected = nbPoints-vec_outliers.size(); - BOOST_CHECK(numInliersFound > KernelType::MINIMUM_SAMPLES *2.5); + BOOST_CHECK(numInliersFound > kernel.getMinimumNbRequiredSamples() *2.5); Mat3 Rest; Mat3 Kest; Vec3 Test; - KRt_From_P(Pest, &Kest, &Rest, &Test); + KRt_from_P(Pest, &Kest, &Rest, &Test); ALICEVISION_LOG_DEBUG("Est: Pest:\n" << Pest << "\nRest:\n" << Rest @@ -235,7 +224,7 @@ BOOST_AUTO_TEST_CASE(P3P_Ransac_noisyFromImagePoints) geometry::Pose3 pose = geometry::poseFromRT(Rest, Test); refinePoseAsItShouldbe(pts3D, pts2Dnorm, - vec_inliers, + inliers, new camera::Pinhole(WIDTH, HEIGHT, 1, 0, 0), pose, true, @@ -258,8 +247,8 @@ BOOST_AUTO_TEST_CASE(P3P_Ransac_noisyFromImagePoints) { // test if inliers found and outliers GT have a empty intersection std::vector inters(nbPoints); - std::sort(vec_inliers.begin(), vec_inliers.end()); - auto it = std::set_intersection(vec_inliers.begin(), vec_inliers.end(), + std::sort(inliers.begin(), inliers.end()); + auto it = std::set_intersection(inliers.begin(), inliers.end(), vec_outliers.begin(), vec_outliers.end(), inters.begin()); inters.resize(it-inters.begin()); diff --git a/src/aliceVision/multiview/rotationAveraging/rotationAveraging_test.cpp b/src/aliceVision/multiview/rotationAveraging/rotationAveraging_test.cpp index 2094805d5f..d32dde42ac 100644 --- a/src/aliceVision/multiview/rotationAveraging/rotationAveraging_test.cpp +++ b/src/aliceVision/multiview/rotationAveraging/rotationAveraging_test.cpp @@ -94,13 +94,13 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RotationLeastSquare_3_Camera) //-- Mat3 R; Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(vec_globalR[0], t0, vec_globalR[1], t1, &R, &t); + relativeCameraMotion(vec_globalR[0], t0, vec_globalR[1], t1, &R, &t); BOOST_CHECK_SMALL(FrobeniusDistance( R01, R), 1e-2); - RelativeCameraMotion(vec_globalR[1], t0, vec_globalR[2], t1, &R, &t); + relativeCameraMotion(vec_globalR[1], t0, vec_globalR[2], t1, &R, &t); BOOST_CHECK_SMALL(FrobeniusDistance( R12, R), 1e-2); - RelativeCameraMotion(vec_globalR[2], t0, vec_globalR[0], t1, &R, &t); + relativeCameraMotion(vec_globalR[2], t0, vec_globalR[0], t1, &R, &t); BOOST_CHECK_SMALL(FrobeniusDistance( R20, R), 1e-2); } @@ -139,13 +139,13 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_SimpleTriplet) //-- Mat3 R; Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(vec_globalR[0], t0, vec_globalR[1], t1, &R, &t); + relativeCameraMotion(vec_globalR[0], t0, vec_globalR[1], t1, &R, &t); BOOST_CHECK_SMALL(FrobeniusDistance( R01, R), 1e-8); - RelativeCameraMotion(vec_globalR[1], t0, vec_globalR[2], t1, &R, &t); + relativeCameraMotion(vec_globalR[1], t0, vec_globalR[2], t1, &R, &t); BOOST_CHECK_SMALL(FrobeniusDistance( R12, R), 1e-8); - RelativeCameraMotion(vec_globalR[2], t0, vec_globalR[0], t1, &R, &t); + relativeCameraMotion(vec_globalR[2], t0, vec_globalR[0], t1, &R, &t); BOOST_CHECK_SMALL(FrobeniusDistance( R20, R), 1e-8); } @@ -169,13 +169,13 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph) // (index0->index1), (index1,index2), (index0->index2) Mat3 Rrel; Vec3 trel; - RelativeCameraMotion(d._R[index0], d._t[index0], d._R[index1], d._t[index1], &Rrel, &trel); + relativeCameraMotion(d._R[index0], d._t[index0], d._R[index1], d._t[index1], &Rrel, &trel); vec_relativeRotEstimate.push_back(RelativeRotation(index0, index1, Rrel, 1)); - RelativeCameraMotion(d._R[index1], d._t[index1], d._R[index2], d._t[index2], &Rrel, &trel); + relativeCameraMotion(d._R[index1], d._t[index1], d._R[index2], d._t[index2], &Rrel, &trel); vec_relativeRotEstimate.push_back(RelativeRotation(index1, index2, Rrel, 1)); - RelativeCameraMotion(d._R[index0], d._t[index0], d._R[index2], d._t[index2], &Rrel, &trel); + relativeCameraMotion(d._R[index0], d._t[index0], d._R[index2], d._t[index2], &Rrel, &trel); vec_relativeRotEstimate.push_back(RelativeRotation(index0, index2, Rrel, 1)); } @@ -196,7 +196,7 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph) { Mat3 Rrel; Vec3 trel; - RelativeCameraMotion(vec_globalR[0], Vec3::Zero(), d._R[0], Vec3::Zero(), &Rrel, &trel); + relativeCameraMotion(vec_globalR[0], Vec3::Zero(), d._R[0], Vec3::Zero(), &Rrel, &trel); for (std::size_t i = 0; i < iNviews; ++i) vec_globalR[i] *= Rrel; } @@ -236,7 +236,7 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph_ if ( std::find(vec_unique.begin(), vec_unique.end(), std::make_pair(index0, index1)) == vec_unique.end() && std::find(vec_unique.begin(), vec_unique.end(), std::make_pair(index1, index0)) == vec_unique.end()) { - RelativeCameraMotion(d._R[index0], d._t[index0], d._R[index1], d._t[index1], &Rrel, &trel); + relativeCameraMotion(d._R[index0], d._t[index0], d._R[index1], d._t[index1], &Rrel, &trel); vec_relativeRotEstimate.push_back(RelativeRotation(index0, index1, Rrel, 1)); vec_unique.emplace_back(index0, index1); } @@ -244,7 +244,7 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph_ if ( std::find(vec_unique.begin(), vec_unique.end(), std::make_pair(index1, index2)) == vec_unique.end() && std::find(vec_unique.begin(), vec_unique.end(), std::make_pair(index2, index1)) == vec_unique.end()) { - RelativeCameraMotion(d._R[index1], d._t[index1], d._R[index2], d._t[index2], &Rrel, &trel); + relativeCameraMotion(d._R[index1], d._t[index1], d._R[index2], d._t[index2], &Rrel, &trel); vec_relativeRotEstimate.push_back(RelativeRotation(index1, index2, Rrel, 1)); vec_unique.emplace_back(index1, index2); } @@ -252,7 +252,7 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph_ if ( std::find(vec_unique.begin(), vec_unique.end(), std::make_pair(index0, index2)) == vec_unique.end() && std::find(vec_unique.begin(), vec_unique.end(), std::make_pair(index2, index0)) == vec_unique.end()) { - RelativeCameraMotion(d._R[index0], d._t[index0], d._R[index2], d._t[index2], &Rrel, &trel); + relativeCameraMotion(d._R[index0], d._t[index0], d._R[index2], d._t[index2], &Rrel, &trel); vec_relativeRotEstimate.push_back(RelativeRotation(index0, index2, Rrel, 1)); vec_unique.emplace_back(index0, index2); } @@ -272,27 +272,27 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph_ Matrix3x3Arr vec_globalR(iNviews); vec_globalR = d._R; std::size_t nMainViewID = 0; - std::vector vec_inliers; - bool bTest = GlobalRotationsRobust(vec_relativeRotEstimate, vec_globalR, nMainViewID, 0.0f, &vec_inliers); + std::vector inliers; + bool bTest = GlobalRotationsRobust(vec_relativeRotEstimate, vec_globalR, nMainViewID, 0.0f, &inliers); BOOST_CHECK(bTest); - ALICEVISION_LOG_DEBUG("Inliers: " << vec_inliers); + ALICEVISION_LOG_DEBUG("Inliers: " << inliers); // Check inlier list - BOOST_CHECK(std::accumulate(vec_inliers.begin(), vec_inliers.end(), 0) == 8); + BOOST_CHECK(std::accumulate(inliers.begin(), inliers.end(), 0) == 8); // Check outlier have been found - BOOST_CHECK(vec_inliers[0]== 0); - BOOST_CHECK(vec_inliers[3] == 0); + BOOST_CHECK(inliers[0]== 0); + BOOST_CHECK(inliers[3] == 0); // Remove outliers and refine RelativeRotations vec_relativeRotEstimateTemp; - for (std::size_t i = 0; i < vec_inliers.size(); ++i) + for (std::size_t i = 0; i < inliers.size(); ++i) { - if( vec_inliers[i] == 1) + if(inliers[i] == 1) vec_relativeRotEstimateTemp.push_back(vec_relativeRotEstimate[i]); } vec_relativeRotEstimate.swap(vec_relativeRotEstimateTemp); - BOOST_CHECK( GlobalRotationsRobust(vec_relativeRotEstimate, vec_globalR, nMainViewID, 0.0f, &vec_inliers)); + BOOST_CHECK( GlobalRotationsRobust(vec_relativeRotEstimate, vec_globalR, nMainViewID, 0.0f, &inliers)); // Check that the loop is closing Mat3 rotCumul = vec_globalR[0]; @@ -307,7 +307,7 @@ BOOST_AUTO_TEST_CASE ( rotationAveraging_RefineRotationsAvgL1IRLS_CompleteGraph_ { Mat3 Rrel; Vec3 trel; - RelativeCameraMotion(vec_globalR[0], Vec3::Zero(), d._R[0], Vec3::Zero(), &Rrel, &trel); + relativeCameraMotion(vec_globalR[0], Vec3::Zero(), d._R[0], Vec3::Zero(), &Rrel, &trel); for (std::size_t i = 0; i < iNviews; ++i) vec_globalR[i] *= Rrel; } diff --git a/src/aliceVision/multiview/translationAveraging/translationAveragingTest.hpp b/src/aliceVision/multiview/translationAveraging/translationAveragingTest.hpp index 99d55e965a..b89b4c6e5b 100644 --- a/src/aliceVision/multiview/translationAveraging/translationAveragingTest.hpp +++ b/src/aliceVision/multiview/translationAveraging/translationAveragingTest.hpp @@ -158,7 +158,7 @@ NViewDataSet Setup_RelativeTranslations_AndNviewDataset { Mat3 RijGt; Vec3 tij; - RelativeCameraMotion(RI, ti, RJ, tj, &RijGt, &tij); + relativeCameraMotion(RI, ti, RJ, tj, &RijGt, &tij); vec_relative_estimates.push_back( std::make_pair(std::make_pair(I, J), std::make_pair(RijGt, tij))); } diff --git a/src/aliceVision/multiview/translationAveraging/translationAveraging_test.cpp b/src/aliceVision/multiview/translationAveraging/translationAveraging_test.cpp index e30b94e2ae..5346d73ff0 100644 --- a/src/aliceVision/multiview/translationAveraging/translationAveraging_test.cpp +++ b/src/aliceVision/multiview/translationAveraging/translationAveraging_test.cpp @@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE(translation_averaging_globalTi_from_tijs_Triplets_softL1_Ce bCardiod, bRelative_Translation_PerTriplet ); - d.ExportToPLY("global_translations_from_triplets_GT.ply"); + d.exportToPLY("global_translations_from_triplets_GT.ply"); visibleCamPosToSVGSurface(d._C, "global_translations_from_triplets_GT.svg"); // Solve the translation averaging problem: @@ -93,7 +93,7 @@ BOOST_AUTO_TEST_CASE(translation_averaging_globalTi_from_tijs_softl1_Ceres) { bCardiod, bRelative_Translation_PerTriplet ); - d.ExportToPLY("global_translations_from_Tij_GT.ply"); + d.exportToPLY("global_translations_from_Tij_GT.ply"); visibleCamPosToSVGSurface(d._C, "global_translations_from_Tij_GT.svg"); // Solve the translation averaging problem: @@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(translation_averaging_globalTi_from_tijs_Triplets_l2_chorda bCardiod, bRelative_Translation_PerTriplet ); - d.ExportToPLY("global_translations_from_Tij_GT.ply"); + d.exportToPLY("global_translations_from_Tij_GT.ply"); visibleCamPosToSVGSurface(d._C, "global_translations_from_Tij_GT.svg"); //-- Compute the global translations from the triplets of heading directions diff --git a/src/aliceVision/multiview/triangulation/NViewsTriangulationLORansac.hpp b/src/aliceVision/multiview/triangulation/NViewsTriangulationLORansac.hpp index 0b05672127..05fdf99a8d 100644 --- a/src/aliceVision/multiview/triangulation/NViewsTriangulationLORansac.hpp +++ b/src/aliceVision/multiview/triangulation/NViewsTriangulationLORansac.hpp @@ -9,17 +9,19 @@ #pragma once #include -#include -#include -#include +#include #include -#include +#include +#include +#include +#include #include #include #include namespace aliceVision { +namespace multiview { /** * @brief The kernel for triangulating a point from N views to be used with @@ -36,39 +38,49 @@ namespace aliceVision { * a solution from any set of data larger than the minimum required, usually a * DLT algorithm. */ -template -class NViewsTriangulationLORansac : - public robustEstimation::LORansacGenericKernel<2, 3, ModelArg> +template , typename SolverLsT_ = robustEstimation::UndefinedSolver> +class NViewsTriangulationLORansac + : public robustEstimation::IRansacKernel { - - private: - const Mat2X& _pt2d; - const std::vector< Mat34 >& _projMatrices; - - public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - typedef SolverLSArg SolverLS; +public: - enum + using SolverT = SolverT_; + using SolverLsT = SolverLsT_; + using UnnormalizerT = UnnormalizerT_; + using ErrorT = ErrorT_; + using ModelT = ModelT_; + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return _kernelSolver.getMinimumNbRequiredSamples(); + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return _kernelSolver.getMaximumNbModels(); + } + + inline std::size_t getMinimumNbRequiredSamplesLS() const override { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES, - MINIMUM_LSSAMPLES = SolverLS::MINIMUM_SAMPLES - }; + return SolverLsT().getMinimumNbRequiredSamples(); + } /** * @brief Constructor. * @param[in] _pt2d The feature points, a 2xN matrix. * @param[in] projMatrices The N projection matrix for each view. */ - NViewsTriangulationLORansac(const Mat2X &_pt2d, const std::vector< Mat34 > &projMatrices) - : _pt2d(_pt2d), - _projMatrices(projMatrices) + NViewsTriangulationLORansac(const Mat2X& _pt2d, const std::vector& projMatrices) + : _pt2d(_pt2d) + , _projMatrices(projMatrices) { assert(_projMatrices.size() == _pt2d.cols()); } @@ -78,12 +90,12 @@ class NViewsTriangulationLORansac : * @param[in] samples The index of two points to triangulate. * @param[out] models The estimated 3D points. */ - void Fit(const std::vector &samples, std::vector *models) const override + void fit(const std::vector& samples, std::vector& models) const override { const Mat p2d = ExtractColumns(_pt2d, samples); - std::vector< Mat34 > sampledMats; + std::vector sampledMats; pick(sampledMats, _projMatrices, samples); - Solver::Solve(p2d, sampledMats, *models); + _kernelSolver.solve(p2d, sampledMats, models); } /** @@ -92,14 +104,14 @@ class NViewsTriangulationLORansac : * @param[out] models The estimated 3D point. * @param[in] weights The optional array of weight for each of the N points. */ - void FitLS(const std::vector &inliers, - std::vector *models, - const std::vector *weights = nullptr) const override + void fitLS(const std::vector& inliers, + std::vector& models, + const std::vector *weights = nullptr) const override { const Mat p2d = ExtractColumns(_pt2d, inliers); - std::vector< Mat34 > sampledMats; + std::vector sampledMats; pick(sampledMats, _projMatrices, inliers); - SolverLS::Solve(p2d, sampledMats, *models, *weights); + _kernelSolverLs.solve(p2d, sampledMats, models, *weights); } @@ -111,9 +123,9 @@ class NViewsTriangulationLORansac : * @param[in] eps An optional threshold to max out the value of the threshold (typically * to avoid division by zero or too small numbers). */ - void computeWeights(const Model & model, + void computeWeights(const ModelT_& model, const std::vector &inliers, - std::vector & vec_weights, + std::vector& vec_weights, const double eps = 0.001) const override { const auto numInliers = inliers.size(); @@ -121,7 +133,7 @@ class NViewsTriangulationLORansac : for(std::size_t sample = 0; sample < numInliers; ++sample) { const auto idx = inliers[sample]; - vec_weights[sample] = ErrorT::Error(_pt2d.col(idx), _projMatrices[idx], model); + vec_weights[sample] = _errorEstimator.error(_pt2d.col(idx), _projMatrices[idx], model); // avoid division by zero vec_weights[sample] = 1/std::pow(std::max(eps, vec_weights[sample]), 2); } @@ -133,9 +145,9 @@ class NViewsTriangulationLORansac : * @param[in] model The 3D point. * @return The estimation error for the given view and 3D point. */ - double Error(std::size_t sample, const Model &model) const override + double error(std::size_t sample, const ModelT_& model) const override { - return ErrorT::Error(_pt2d.col(sample), _projMatrices[sample], model); + return _errorEstimator.error(_pt2d.col(sample), _projMatrices[sample], model); } /** @@ -143,38 +155,72 @@ class NViewsTriangulationLORansac : * @param[in] model The 3D point. * @param[out] vec_errors The vector containing all the estimation errors for every view. */ - void Errors(const Model & model, std::vector & vec_errors) const override + void errors(const ModelT_& model, std::vector& errors) const override { - vec_errors.resize(NumSamples()); + errors.resize(nbSamples()); for(Mat::Index i = 0; i < _pt2d.cols(); ++i) - { - vec_errors[i] = Error(i, model); - } + errors[i] = error(i, model); } /** * @brief Unnormalize the model. (not used) * @param[in,out] model the 3D point. */ - void Unnormalize(Model * model) const override - { - - } + void unnormalize(ModelT_& model) const override + {} /** * @brief Return the number of view. * @return the number of view. */ - std::size_t NumSamples() const override + std::size_t nbSamples() const override { return _pt2d.cols(); } + double logalpha0() const override + { + std::runtime_error("Method 'logalpha0()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + + double multError() const override + { + std::runtime_error("Method 'multError()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + + double unormalizeError(double val) const override + { + std::runtime_error("Method 'unormalizeError()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + + Mat3 normalizer1() const override + { + std::runtime_error("Method 'normalizer1()' is not defined for 'NViewsTriangulationLORansac'."); + return Mat3(); + } + + Mat3 normalizer2() const override + { + std::runtime_error("Method 'normalizer2()' is not defined for 'NViewsTriangulationLORansac'."); + return Mat3(); + } + +private: + const Mat2X& _pt2d; + const std::vector& _projMatrices; + + const SolverT _kernelSolver = SolverT(); + const SolverLsT _kernelSolverLs = SolverLsT(); + const ErrorT _errorEstimator = ErrorT(); }; /** * @brief Functor used to compute the reprojection error as the pixel error. */ +template> struct ReprojectionError { /** @@ -184,9 +230,9 @@ struct ReprojectionError * @param[in] pt3d The 3d point. * @return the reprojection error. */ - static double Error(const Vec2 &pt2d, const Mat34 &projMatrix, const Vec4 &pt3d) + inline double error(const Vec2& pt2d, const Mat34& projMatrix, const ModelT_& model) const { - const Vec3 proj = projMatrix * pt3d; + const Vec3 proj = projMatrix * model.getMatrix(); return (pt2d - proj.hnormalized()).norm(); } }; @@ -194,6 +240,7 @@ struct ReprojectionError /** * @brief Functor used to compute the error as the angular error. */ +template> struct AngularError { /** @@ -204,10 +251,10 @@ struct AngularError * @return the error as the angular error between the direction of the * 3D point and the projection ray associated with the image point. */ - static double Error(const Vec2 &pt2d, const Mat34 &projMatrix, const Vec4 &pt3d) + inline double error(const Vec2& pt2d, const Mat34& projMatrix, const ModelT_& model) const { const Vec3 ray1 = pt2d.homogeneous(); - const Vec3 ray2 = projMatrix * pt3d; + const Vec3 ray2 = projMatrix * model.getMatrix(); return std::acos(ray1.normalized().transpose() * ray2.normalized()); } }; @@ -217,9 +264,7 @@ struct AngularError // ReprojectionError, // UnnormalizerT, // TriangulateNViewsSolver> LORansacTriangulationKernel; -template -using LORansacTriangulationKernel = NViewsTriangulationLORansac; -} +template>> +using LORansacTriangulationKernel = NViewsTriangulationLORansac, TriangulateNViewsSolver>; +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/Triangulation.cpp b/src/aliceVision/multiview/triangulation/Triangulation.cpp index 94a8d9f5f1..a332e2d35a 100644 --- a/src/aliceVision/multiview/triangulation/Triangulation.cpp +++ b/src/aliceVision/multiview/triangulation/Triangulation.cpp @@ -8,11 +8,12 @@ #include "Triangulation.hpp" #include "NViewsTriangulationLORansac.hpp" -#include +#include #include #include namespace aliceVision { +namespace multiview { void TriangulateNView(const Mat2X &x, const std::vector< Mat34 > &Ps, @@ -56,16 +57,18 @@ void TriangulateNViewAlgebraic(const Mat2X &x, Nullspace(&design, X); } -void TriangulateNViewLORANSAC(const Mat2X &x, - const std::vector< Mat34 > &Ps, - Vec4 *X, - std::vector *inliersIndex, - const double & thresholdError) +void TriangulateNViewLORANSAC(const Mat2X& x, + const std::vector& Ps, + Vec4* X, + std::vector* inliersIndex, + const double& thresholdError) { - using TriangulationKernel = LORansacTriangulationKernel<>; + using TriangulationKernel = multiview::LORansacTriangulationKernel<>; TriangulationKernel kernel(x, Ps); robustEstimation::ScoreEvaluator scorer(thresholdError); - *X = robustEstimation::LO_RANSAC(kernel, scorer, inliersIndex); + robustEstimation::MatrixModel model; + model = robustEstimation::LO_RANSAC(kernel, scorer, inliersIndex); + *X = model.getMatrix(); } double Triangulation::error(const Vec3 &X) const @@ -75,7 +78,7 @@ double Triangulation::error(const Vec3 &X) const { const Mat34& PMat = view.first; const Vec2 & xy = view.second; - const Vec2 p = Project(PMat, X); + const Vec2 p = project(PMat, X); squared_reproj_error += (xy - p).norm(); } return squared_reproj_error; @@ -142,21 +145,22 @@ Vec3 Triangulation::compute(int iter) const return X; } -void TriangulateNViewsSolver::Solve(const Mat2X& x, const std::vector& Ps, std::vector &X) +void TriangulateNViewsSolver::solve(const Mat2X& x, const std::vector& Ps, std::vector> &X) const { Vec4 pt3d; TriangulateNViewAlgebraic(x, Ps, &pt3d); - X.push_back(pt3d); + X.push_back(robustEstimation::MatrixModel(pt3d)); assert(X.size() == 1); } -void TriangulateNViewsSolver::Solve(const Mat2X& x, const std::vector& Ps, std::vector &X, const std::vector &weights) +void TriangulateNViewsSolver::solve(const Mat2X& x, const std::vector& Ps, std::vector> &X, const std::vector &weights) const { Vec4 pt3d; TriangulateNViewAlgebraic(x, Ps, &pt3d, &weights); - X.push_back(pt3d); + X.push_back(robustEstimation::MatrixModel(pt3d)); assert(X.size() == 1); } -} // namespace aliceCision +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/Triangulation.hpp b/src/aliceVision/multiview/triangulation/Triangulation.hpp index 9a3cbfdf05..0b976cc51f 100644 --- a/src/aliceVision/multiview/triangulation/Triangulation.hpp +++ b/src/aliceVision/multiview/triangulation/Triangulation.hpp @@ -9,10 +9,12 @@ #pragma once #include +#include #include namespace aliceVision { +namespace multiview { /** * @brief Compute a 3D position of a point from several images of it. In particular, @@ -106,13 +108,30 @@ class Triangulation struct TriangulateNViewsSolver { - enum { MINIMUM_SAMPLES = 2 }; - enum { MAX_MODELS = 1 }; - static void Solve(const Mat2X &x, const std::vector< Mat34 > &Ps, std::vector &X); + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const + { + return 2; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const + { + return 1; + } + + void solve(const Mat2X& x, const std::vector& Ps, std::vector>& X) const; - static void Solve(const Mat2X &x, const std::vector< Mat34 > &Ps, std::vector &X, const std::vector &weights); + void solve(const Mat2X& x, const std::vector& Ps, std::vector>& X, const std::vector& weights) const; }; +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/triangulationDLT.cpp b/src/aliceVision/multiview/triangulation/triangulationDLT.cpp index a6b1ad7805..a7ad1d72c8 100644 --- a/src/aliceVision/multiview/triangulation/triangulationDLT.cpp +++ b/src/aliceVision/multiview/triangulation/triangulationDLT.cpp @@ -6,10 +6,11 @@ // 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 namespace aliceVision { +namespace multiview { // HZ 12.2 pag.312 void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, @@ -30,7 +31,8 @@ void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, Vec3 *X_euclidean) { Vec4 X_homogeneous; TriangulateDLT(P1, x1, P2, x2, &X_homogeneous); - HomogeneousToEuclidean(X_homogeneous, X_euclidean); + homogeneousToEuclidean(X_homogeneous, X_euclidean); } -} // namespace aliceVision +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/triangulationDLT.hpp b/src/aliceVision/multiview/triangulation/triangulationDLT.hpp index d66740789d..093c8816eb 100644 --- a/src/aliceVision/multiview/triangulation/triangulationDLT.hpp +++ b/src/aliceVision/multiview/triangulation/triangulationDLT.hpp @@ -11,6 +11,7 @@ #include namespace aliceVision { +namespace multiview { /// Linear DLT triangulation: HZ 12.2 pag.312 void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, @@ -22,4 +23,5 @@ void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, const Mat34 &P2, const Vec2 &x2, Vec3 *X_euclidean); +} // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/triangulationDLT_test.cpp b/src/aliceVision/multiview/triangulation/triangulationDLT_test.cpp index 332ad37a2c..3ab797b049 100644 --- a/src/aliceVision/multiview/triangulation/triangulationDLT_test.cpp +++ b/src/aliceVision/multiview/triangulation/triangulationDLT_test.cpp @@ -6,9 +6,9 @@ // 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 "aliceVision/multiview/projection.hpp" -#include "aliceVision/multiview/NViewDataSet.hpp" -#include "aliceVision/multiview/triangulation/triangulationDLT.hpp" +#include +#include +#include #define BOOST_TEST_MODULE triangulationDLT @@ -16,19 +16,19 @@ #include using namespace aliceVision; -using namespace std; - -BOOST_AUTO_TEST_CASE(Triangulation_TriangulateDLT) { +BOOST_AUTO_TEST_CASE(Triangulation_TriangulateDLT) +{ NViewDataSet d = NRealisticCamerasRing(2, 12); - for (int i = 0; i < d._X.cols(); ++i) { + for (int i = 0; i < d._X.cols(); ++i) + { Vec2 x1, x2; x1 = d._x[0].col(i); x2 = d._x[1].col(i); Vec3 X_estimated, X_gt; X_gt = d._X.col(i); - TriangulateDLT(d.P(0), x1, d.P(1), x2, &X_estimated); + multiview::TriangulateDLT(d.P(0), x1, d.P(1), x2, &X_estimated); BOOST_CHECK_SMALL(DistanceLInfinity(X_estimated, X_gt), 1e-8); } } diff --git a/src/aliceVision/multiview/triangulation/triangulation_test.cpp b/src/aliceVision/multiview/triangulation/triangulation_test.cpp index bb221e3903..bacc7237a9 100644 --- a/src/aliceVision/multiview/triangulation/triangulation_test.cpp +++ b/src/aliceVision/multiview/triangulation/triangulation_test.cpp @@ -6,8 +6,8 @@ // 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 "aliceVision/multiview/triangulation/Triangulation.hpp" -#include "aliceVision/multiview/NViewDataSet.hpp" +#include +#include #define BOOST_TEST_MODULE Triangulation @@ -18,7 +18,7 @@ using namespace aliceVision; -BOOST_AUTO_TEST_CASE(Triangulate_NView_FiveViews) +BOOST_AUTO_TEST_CASE(TriangulateNView_FiveViews) { const int nviews = 5; const int npoints = 6; @@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(Triangulate_NView_FiveViews) xs.col(j) = d._x[j].col(i); } Vec4 X; - TriangulateNView(xs, Ps, &X); + multiview::TriangulateNView(xs, Ps, &X); // Check reprojection error. Should be nearly zero. for (int j = 0; j < nviews; ++j) @@ -53,7 +53,7 @@ BOOST_AUTO_TEST_CASE(Triangulate_NView_FiveViews) } } -BOOST_AUTO_TEST_CASE(Triangulate_NViewAlgebraic_FiveViews) { +BOOST_AUTO_TEST_CASE(TriangulateNViewAlgebraic_FiveViews) { const int nviews = 5; const int npoints = 6; const NViewDataSet d = NRealisticCamerasRing(nviews, npoints); @@ -74,7 +74,7 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewAlgebraic_FiveViews) { xs.col(j) = d._x[j].col(i); } Vec4 X; - TriangulateNViewAlgebraic(xs, Ps, &X); + multiview::TriangulateNViewAlgebraic(xs, Ps, &X); // Check reprojection error. Should be nearly zero. for (int j = 0; j < nviews; ++j) @@ -95,12 +95,12 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewAlgebraic_FiveViews) { // is considered). BOOST_AUTO_TEST_CASE(Triangulate_NViewAlgebraic_WithWeights) { - const std::size_t numviews = 20; - const std::size_t outliers = 8; + const std::size_t nbViews = 20; + const std::size_t nbOutliers = 8; // Collect random P matrices together. - std::vector Ps(numviews); - for(std::size_t j = 0; j < numviews; ++j) + std::vector Ps(nbViews); + for(std::size_t j = 0; j < nbViews; ++j) { Ps[j] = Mat34::Random(); } @@ -110,11 +110,11 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewAlgebraic_WithWeights) // project the 3D point and prepare weights const double w = 1e8; - std::vector weights(numviews, w); - Mat2X pt2d(2, numviews); - for(std::size_t j = 0; j < numviews; ++j) + std::vector weights(nbViews, w); + Mat2X pt2d(2, nbViews); + for(std::size_t j = 0; j < nbViews; ++j) { - if(j < numviews - outliers) + if(j < nbViews - nbOutliers) { // project the 3D point pt2d.col(j) = (Ps[j] * pt3d).hnormalized(); @@ -129,10 +129,10 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewAlgebraic_WithWeights) } Vec4 X; - TriangulateNViewAlgebraic(pt2d, Ps, &X, &weights); + multiview::TriangulateNViewAlgebraic(pt2d, Ps, &X, &weights); // Check the reprojection error is nearly zero for inliers. - for (std::size_t j = 0; j < numviews - outliers; ++j) + for (std::size_t j = 0; j < nbViews - nbOutliers; ++j) { const Vec2 x_reprojected = (Ps[j] * X).hnormalized(); const double error = (x_reprojected - pt2d.col(j)).norm(); @@ -146,10 +146,10 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewIterative_FiveViews) const int npoints = 6; const NViewDataSet d = NRealisticCamerasRing(nviews, npoints); - for (int i = 0; i < npoints; ++i) + for(int i = 0; i < npoints; ++i) { - Triangulation triangulationObj; + multiview::Triangulation triangulationObj; for (int j = 0; j < nviews; ++j) triangulationObj.add(d.P(j), d._x[j].col(i)); @@ -177,13 +177,13 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewIterative_LORANSAC) const std::size_t numTrials = 100; for(std::size_t trial = 0; trial < numTrials; ++ trial) { - const std::size_t numviews = 20; - const std::size_t outliers = 8; - const std::size_t inliers = numviews - outliers; + const std::size_t nbViews = 20; + const std::size_t nbOutliers = 8; + const std::size_t nbInliers = nbViews - nbOutliers; // Collect random P matrices together. - std::vector Ps(numviews); - for(std::size_t j = 0; j < numviews; ++j) + std::vector Ps(nbViews); + for(std::size_t j = 0; j < nbViews; ++j) { Ps[j] = Mat34::Random(); } @@ -192,10 +192,10 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewIterative_LORANSAC) Vec4 pt3d(Vec3::Random().homogeneous()); // project the 3D point and prepare weights - Mat2X pt2d(2, numviews); - for(std::size_t j = 0; j < numviews; ++j) + Mat2X pt2d(2, nbViews); + for(std::size_t j = 0; j < nbViews; ++j) { - if(j < numviews - outliers) + if(j < nbViews - nbOutliers) { // project the 3D point pt2d.col(j) = (Ps[j] * pt3d).hnormalized(); @@ -208,16 +208,16 @@ BOOST_AUTO_TEST_CASE(Triangulate_NViewIterative_LORANSAC) } } - std::vector vec_inliers; + std::vector inliers; Vec4 X; double const threshold = 0.01; // modify the default value: 4 pixels is too much in this configuration. - TriangulateNViewLORANSAC(pt2d, Ps, &X, &vec_inliers, threshold); + multiview::TriangulateNViewLORANSAC(pt2d, Ps, &X, &inliers, threshold); // check inliers are correct - BOOST_CHECK_EQUAL(vec_inliers.size(), inliers); + BOOST_CHECK_EQUAL(inliers.size(), nbInliers); // Check the reprojection error is nearly zero for inliers. - for (std::size_t j = 0; j < inliers; ++j) + for (std::size_t j = 0; j < nbInliers; ++j) { const Vec2 x_reprojected = (Ps[j] * X).hnormalized(); const double error = (x_reprojected - pt2d.col(j)).norm(); diff --git a/src/aliceVision/multiview/twoViewKernel.hpp b/src/aliceVision/multiview/twoViewKernel.hpp deleted file mode 100644 index 2be0726134..0000000000 --- a/src/aliceVision/multiview/twoViewKernel.hpp +++ /dev/null @@ -1,113 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// Copyright (c) 2010 libmv 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 - -namespace aliceVision { -namespace twoView { -namespace kernel { - -using namespace std; - -// This is one example (targeted at solvers that operate on correspondences -// between two views) that shows the "kernel" part of a robust fitting -// problem: -// -// 1. The model; Mat3 in the case of the F or H matrix. -// 2. The minimum number of samples needed to fit; 7 or 8 (or 4). -// 3. A way to convert samples to a model. -// 4. A way to convert a sample and a model to an error. -// -// Of particular note is that the kernel does not expose what the samples are. -// All the robust fitting algorithm sees is that there is some number of -// samples; it is able to fit subsets of them (via the kernel) and check their -// error, but can never access the samples themselves. -// -// The Kernel objects must follow the following concept so that the robust -// fitting alogrithm can fit this type of relation: -// -// 1. Kernel::MAX_MODELS -// 2. Kernel::MINIMUM_SAMPLES -// 3. Kernel::Fit(vector, vector *) -// 4. Kernel::Error(size_t, Model) -> error -// -// The fit routine must not clear existing entries in the vector of models; it -// should append new solutions to the end. -template -class Kernel { - public: - Kernel(const Mat &x1, const Mat &x2) : x1_(x1), x2_(x2) {} - typedef SolverArg Solver; - typedef ModelArg Model; - /// The minimal number of point required for the model estimation - enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; - /// The number of models that the minimal solver could return. - enum { MAX_MODELS = Solver::MAX_MODELS }; - - /// Extract required sample and fit model(s) to the sample - void Fit(const vector &samples, vector *models) const { - Mat x1 = ExtractColumns(x1_, samples), - x2 = ExtractColumns(x2_, samples); - Solver::Solve(x1, x2, models); - } - /// Return the error associated to the model and sample^nth point - double Error(size_t sample, const Model &model) const { - return ErrorArg::Error(model, x1_.col(sample), x2_.col(sample)); - } - /// Number of putative point - size_t NumSamples() const { - return x1_.cols(); - } - /// Compute a model on sampled point - static void Solve(const Mat &x1, const Mat &x2, vector *models) { - // By offering this, Kernel types can be passed to templates. - Solver::Solve(x1, x2, models); - } - protected: - const Mat & x1_, & x2_; // Left/Right corresponding point -}; - -// Analog Normalized version of the previous Kernel. -template -class NormalizedSolver { -public: - enum { MINIMUM_SAMPLES = SolverArg::MINIMUM_SAMPLES }; - enum { MAX_MODELS = SolverArg::MAX_MODELS }; - - static void Solve(const Mat &x1, const Mat &x2, vector *models) { - assert(2 == x1.rows()); - assert(MINIMUM_SAMPLES <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - // Normalize the data. - Mat3 T1, T2; - Mat x1_normalized, x2_normalized; - NormalizePoints(x1, &x1_normalized, &T1); - NormalizePoints(x2, &x2_normalized, &T2); - - SolverArg::Solve(x1_normalized, x2_normalized, models); - // Unormalize model from the computed conditioning. - for (int i = 0; i < models->size(); ++i) { - UnnormalizerArg::Unnormalize(T1, T2, &(*models)[i]); - } - } -}; - -} // namespace kernel -} // namespace twoView -} // namespace aliceVision diff --git a/src/aliceVision/mvsUtils/MultiViewParams.cpp b/src/aliceVision/mvsUtils/MultiViewParams.cpp index 6175c85b11..b850dac794 100644 --- a/src/aliceVision/mvsUtils/MultiViewParams.cpp +++ b/src/aliceVision/mvsUtils/MultiViewParams.cpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/aliceVision/numeric/CMakeLists.txt b/src/aliceVision/numeric/CMakeLists.txt index e15af28eb5..97bc1278b2 100644 --- a/src/aliceVision/numeric/CMakeLists.txt +++ b/src/aliceVision/numeric/CMakeLists.txt @@ -1,11 +1,13 @@ # Headers set(numeric_files_headers numeric.hpp + projection.hpp ) # Sources set(numeric_files_sources numeric.cpp + projection.cpp ) alicevision_add_library(aliceVision_numeric @@ -23,3 +25,4 @@ alicevision_add_library(aliceVision_numeric alicevision_add_test(numeric_test.cpp NAME "numeric" LINKS aliceVision_numeric) alicevision_add_test(polynomial_test.cpp NAME "numeric_polynomial" LINKS aliceVision_numeric) alicevision_add_test(lmFunctor_test.cpp NAME "numeric_lmFunctor" LINKS aliceVision_numeric) +alicevision_add_test(projection_test.cpp NAME "numeric_projection" LINKS aliceVision_numeric) diff --git a/src/aliceVision/numeric/numeric.hpp b/src/aliceVision/numeric/numeric.hpp index dc4d38737e..e76654c1cf 100644 --- a/src/aliceVision/numeric/numeric.hpp +++ b/src/aliceVision/numeric/numeric.hpp @@ -51,54 +51,56 @@ namespace aliceVision { using Eigen::Map; -typedef Eigen::NumTraits EigenDoubleTraits; +using EigenDoubleTraits = Eigen::NumTraits; -typedef Eigen::Vector3d Vec3; -typedef Eigen::Vector2i Vec2i; -typedef Eigen::Vector2f Vec2f; -typedef Eigen::Vector3f Vec3f; -typedef Eigen::Matrix Vec9; +using Vec3 = Eigen::Vector3d; +using Vec2i = Eigen::Vector2i; +using Vec2f = Eigen::Vector2f; +using Vec3f = Eigen::Vector3f; +using Vec9 = Eigen::Matrix; -typedef Eigen::Quaternion Quaternion; +using Quaternion = Eigen::Quaternion; -typedef Eigen::Matrix Mat3; +using Mat3 = Eigen::Matrix; #if defined(ENV32BIT) -typedef Eigen::Matrix Mat23; -typedef Eigen::Matrix Mat34; -typedef Eigen::Matrix Vec2; -typedef Eigen::Matrix Vec4; -typedef Eigen::Matrix Vec6; +using Mat23 = Eigen::Matrix; +using Mat34 = Eigen::Matrix; +using Vec2 = Eigen::Matrix; +using Vec4 = Eigen::Matrix; +using Vec6 = Eigen::Matrix; #else // 64 bits compiler -typedef Eigen::Matrix Mat23; -typedef Eigen::Matrix Mat34; -typedef Eigen::Vector2d Vec2; -typedef Eigen::Vector4d Vec4; -typedef Eigen::Matrix Vec6; +using Mat23 = Eigen::Matrix; +using Mat34 = Eigen::Matrix; +using Vec2 = Eigen::Vector2d; +using Vec4 = Eigen::Vector4d; +using Vec6 = Eigen::Matrix; #endif -typedef Eigen::Matrix Mat4; -typedef Eigen::Matrix Matu; +using Mat4 = Eigen::Matrix; +using Matu = Eigen::Matrix; -typedef Eigen::Matrix RMat3; +using RMat3 = Eigen::Matrix; //-- General purpose Matrix and Vector -typedef Eigen::MatrixXd Mat; -typedef Eigen::VectorXd Vec; -typedef Eigen::Matrix Vecu; -typedef Eigen::MatrixXf Matf; -typedef Eigen::VectorXf Vecf; +using Mat = Eigen::MatrixXd; +using Vec = Eigen::VectorXd; +using Vecu = Eigen::Matrix; +using Matf = Eigen::MatrixXf; +using Vecf = Eigen::VectorXf; +using Vecb = Eigen::Matrix; -typedef Eigen::Matrix Mat2X; -typedef Eigen::Matrix Mat3X; -typedef Eigen::Matrix Mat4X; +using Mat2X = Eigen::Matrix; +using Mat3X = Eigen::Matrix; +using Mat4X = Eigen::Matrix; -typedef Eigen::Matrix MatX9; +using MatX9 = Eigen::Matrix; +using Mat9 = Eigen::Matrix; //-- Sparse Matrix (Column major, and row major) -typedef Eigen::SparseMatrix sMat; -typedef Eigen::SparseMatrix sRMat; +using sMat = Eigen::SparseMatrix; +using sRMat = Eigen::SparseMatrix; //-------------- //-- Function -- @@ -312,7 +314,7 @@ Mat3 LookAt2(const Vec3 &eyePosition3D, template struct hstack_return { - typedef typename Derived1::Scalar Scalar; + using Scalar = typename Derived1::Scalar; enum { @@ -343,7 +345,7 @@ HStack(const Eigen::MatrixBase& lhs, const Eigen::MatrixBase template struct vstack_return { - typedef typename Derived1::Scalar Scalar; + using Scalar = typename Derived1::Scalar; enum { diff --git a/src/aliceVision/multiview/projection.cpp b/src/aliceVision/numeric/projection.cpp similarity index 75% rename from src/aliceVision/multiview/projection.cpp rename to src/aliceVision/numeric/projection.cpp index f11fd12b85..c615ee0a0a 100644 --- a/src/aliceVision/multiview/projection.cpp +++ b/src/aliceVision/numeric/projection.cpp @@ -6,25 +6,25 @@ // 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 "projection.hpp" #include namespace aliceVision { /// Compute P = K[R|t] -void P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P) +void P_from_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P) { assert(P != nullptr); - *P = P_From_KRt(K, R, t); + *P = P_from_KRt(K, R, t); } -Mat34 P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t) +Mat34 P_from_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t) { return K * HStack(R,t); } -void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) +void KRt_from_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) { assert(Kp != nullptr); assert(Rp != nullptr); @@ -167,40 +167,40 @@ Mat3 F_from_P(const Mat34 & P1, const Mat34 & P2) return F12; } -Vec2 Project(const Mat34 &P, const Vec3 &X) +Vec2 project(const Mat34 &P, const Vec3 &X) { return Vec3(P * X.homogeneous()).hnormalized(); } -void Project(const Mat34 &P, const Mat3X &X, Mat2X *x) +void project(const Mat34 &P, const Mat3X &X, Mat2X *x) { assert(x != nullptr); - Project(P, Mat4X(X.colwise().homogeneous()), x); + project(P, Mat4X(X.colwise().homogeneous()), x); } -void Project(const Mat34 &P, const Mat4X &X, Mat2X *x) +void project(const Mat34 &P, const Mat4X &X, Mat2X *x) { assert(x != nullptr); - *x = Project(P, X); + *x = project(P, X); } -Mat2X Project(const Mat34 &P, const Mat3X &X) +Mat2X project(const Mat34 &P, const Mat3X &X) { - return Project(P, Mat4X(X.colwise().homogeneous())); + return project(P, Mat4X(X.colwise().homogeneous())); } -Mat2X Project(const Mat34 &P, const Mat4X &X) +Mat2X project(const Mat34 &P, const Mat4X &X) { return Mat3X(P * X).colwise().hnormalized(); } -void HomogeneousToEuclidean(const Vec4 &H, Vec3 *X) +void homogeneousToEuclidean(const Vec4 &H, Vec3 *X) { assert(X != nullptr); *X = H.hnormalized(); } -void EuclideanToHomogeneous(const Mat &X, Mat *H) +void euclideanToHomogeneous(const Mat &X, Mat *H) { assert(H != nullptr); *H = X.colwise().homogeneous(); @@ -211,41 +211,51 @@ double Depth(const Mat3 &R, const Vec3 &t, const Vec3 &X) return (R*X)[2] + t[2]; } -Vec3 EuclideanToHomogeneous(const Vec2 &x) +Vecb cheiralityTest(const Mat3 &R, const Vec3 &t, const Mat3X &X) +{ + return ((R*X).row(2) + t[2]*Mat::Ones(1, X.cols())).array() >= .0; +} + +bool cheiralityTestAll(const Mat3 &R, const Vec3 &t, const Mat3X &X) +{ + return cheiralityTest(R, t, X).all(); +} + +Vec3 euclideanToHomogeneous(const Vec2 &x) { return x.homogeneous(); } -void HomogeneousToEuclidean(const Mat &H, Mat *X) +void homogeneousToEuclidean(const Mat &H, Mat *X) { assert(X != nullptr); *X = H.colwise().hnormalized(); } -Mat3X EuclideanToHomogeneous(const Mat2X &x) +Mat3X euclideanToHomogeneous(const Mat2X &x) { return x.colwise().homogeneous(); } -void EuclideanToHomogeneous(const Mat2X &x, Mat3X *h) +void euclideanToHomogeneous(const Mat2X &x, Mat3X *h) { assert(h != nullptr); *h = x.colwise().homogeneous(); } -void HomogeneousToEuclidean(const Mat3X &h, Mat2X *e) +void homogeneousToEuclidean(const Mat3X &h, Mat2X *e) { assert(e != nullptr); *e = h.colwise().hnormalized(); } -void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n) +void euclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n) { assert(n != nullptr); - HomogeneousToNormalizedCamera(x.colwise().homogeneous(), K, n); + homogeneousToNormalizedCamera(x.colwise().homogeneous(), K, n); } -void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n) +void homogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n) { assert(n != nullptr); *n = (K.inverse() * x).colwise().hnormalized(); @@ -257,7 +267,7 @@ double reprojectionErrorRMSE(const Mat2X &x_image, const Mat34 &P) { const std::size_t num_points = x_image.cols(); - const Mat2X dx = Project(P, X_world) - x_image; + const Mat2X dx = project(P, X_world) - x_image; return std::sqrt(dx.squaredNorm() / num_points); } @@ -269,7 +279,7 @@ double reprojectionErrorRMSE(const Mat2X &x_image, const Vec3 &t) { Mat34 P; - P_From_KRt(K, R, t, &P); + P_from_KRt(K, R, t, &P); return reprojectionErrorRMSE(x_image, X_world.colwise().homogeneous(), P); } diff --git a/src/aliceVision/numeric/projection.hpp b/src/aliceVision/numeric/projection.hpp new file mode 100644 index 0000000000..a12805773b --- /dev/null +++ b/src/aliceVision/numeric/projection.hpp @@ -0,0 +1,145 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +/** + * Collection of function related to the classic Projection matrix used + * in computer vision. P = K[R|t] with [t]=[-RC] Cf HZ + */ + +namespace aliceVision { + +/** + * @brief Compute P = K[R|t] + */ +void P_from_KRt(const Mat3& K, const Mat3& R, const Vec3& t, Mat34* P); + +/** + * @brief Compute P = K[R|t] + * @return P + */ +Mat34 P_from_KRt(const Mat3& K, const Mat3& R, const Vec3& t); + +/** + * @brief Decompose using the RQ decomposition HZ A4.1.1 pag.579. + */ +void KRt_from_P(const Mat34& P, Mat3* Kp, Mat3* Rp, Vec3* tp); + +/** + * @brief Compute a fundamental matrix from projection matrices + */ +Mat3 F_from_P(const Mat34& P1, const Mat34& P2); + +/** + * @brief Compute the depth of the X point. R*X[2]+t[2]. + */ +double Depth(const Mat3& R, const Vec3& t, const Vec3& X); + +/** + * @brief Test whether the given points are in front of the camera. + * @param[in] R the camera rotation. + * @param[in] t the camera translation. + * @param[in] X the 3D points to test. + * @return A vector of boolean of the same size as the number ot points. The corresponding value is true if the point + * is in front of the camera, false otherwise. + */ +Vecb cheiralityTest(const Mat3 &R, const Vec3 &t, const Mat3X &X); + +/** + * @brief Test whether all the given points are in front of the camera. + * @param[in] R the camera rotation. + * @param[in] t the camera translation. + * @param[in] X the 3D points to test. + * @return true if all the points is in front of the camera, false otherwise. + */ +bool cheiralityTestAll(const Mat3 &R, const Vec3 &t, const Mat3X &X); + +/** + * @brief Compute P*[X|1.0]. Transformed from homogeneous to euclidean coordinates. + */ +Vec2 project(const Mat34& P, const Vec3& X); + +/** + * @brief Compute P*[X|1.0] for the X list of point (3D point). + */ +void project(const Mat34& P, const Mat3X& X, Mat2X* x); + +/** + * @brief Compute P*[X|1.0] for the X list of point (4D point). + */ +void project(const Mat34& P, const Mat4X& X, Mat2X* x); + +/** + * @brief Return P*[X|1.0] for the X list of point (3D point). + */ +Mat2X project(const Mat34& P, const Mat3X& X); + +/** + * @brief Return P*[X|1.0] for the X list of point (4D point). + */ +Mat2X project(const Mat34& P, const Mat4X& X); + +/** + * @brief Change homogeneous coordinates to euclidean. + */ +void homogeneousToEuclidean(const Vec4& H, Vec3* X); + +/** + * @brief Change euclidean coordinates to homogeneous. + */ +void euclideanToHomogeneous(const Mat& X, Mat* H); + +/** + * @brief Change euclidean coordinates to homogeneous. + */ +Vec3 euclideanToHomogeneous(const Vec2& x); + +/** + * @brief Change homogeneous coordinates to euclidean. + */ +void homogeneousToEuclidean(const Mat& H, Mat* X); + +/** + * @brief Change euclidean coordinates to homogeneous. + */ +Mat3X euclideanToHomogeneous(const Mat2X& x); + +/** + * @brief Change euclidean coordinates to homogeneous. + */ +void euclideanToHomogeneous(const Mat2X& x, Mat3X* h); + +/** + * @brief Change homogeneous coordinates to euclidean. + */ +void homogeneousToEuclidean(const Mat3X& h, Mat2X* e); + +/** + * @brief Project x point in camera coordinates + */ +void euclideanToNormalizedCamera(const Mat2X& x, const Mat3& K, Mat2X* n); + +/** + * @brief Project x point in camera coordinates + */ +void homogeneousToNormalizedCamera(const Mat3X& x, const Mat3& K, Mat2X* n); + +/** + * @brief Estimates the root mean square error (2D) + */ +double reprojectionErrorRMSE(const Mat2X& x_image, const Mat4X& X_world, const Mat34& P); + +/** + * @brief Estimates the root mean square error (2D) + */ +double reprojectionErrorRMSE(const Mat2X& x_image, const Mat3X& X_world, const Mat3& K, const Mat3& R, const Vec3& t); + +} // namespace aliceVision diff --git a/src/aliceVision/numeric/projection_test.cpp b/src/aliceVision/numeric/projection_test.cpp new file mode 100644 index 0000000000..00fd3d35c2 --- /dev/null +++ b/src/aliceVision/numeric/projection_test.cpp @@ -0,0 +1,41 @@ +// 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 "aliceVision/numeric/numeric.hpp" +#include "aliceVision/numeric/projection.hpp" + +#define BOOST_TEST_MODULE projection + +#include +#include +#include + +using namespace aliceVision; + +BOOST_AUTO_TEST_CASE(Numeric_Cheirality) +{ + const Eigen::Index numPts{10}; + auto pt3d = Mat3X(3, numPts); + pt3d << 0.680375, 0.59688, -0.329554, 0.10794, -0.270431, 0.83239, -0.716795, -0.514226, -0.686642, -0.782382, + -0.211234, 0.823295, 0.536459, -0.0452059, 0.0268018, 0.271423, 0.213938, -0.725537, -0.198111, 0.997849, + 0.566198, -0.604897, -0.444451, 0.257742, 0.904459, 0.434594, -0.967399, 0.608354, -0.740419, -0.563486; + + const auto R = RotationAroundZ(0.3) * RotationAroundX(0.1) * RotationAroundY(0.2); + + const Vec3 t = {0.0258648, 0.678224, 0.22528}; + + Vecb expected = Vecb(numPts); + expected << true, false, false, true, true, true, false, true, false, false; + + const auto test = cheiralityTest(R, t, pt3d); + for(Eigen::Index i = 0; i < numPts; ++i) + { + BOOST_TEST(expected(i) == test(i)); + } + + BOOST_TEST(!cheiralityTestAll(R, t, pt3d)); +} \ No newline at end of file diff --git a/src/aliceVision/robustEstimation/ACRansac.hpp b/src/aliceVision/robustEstimation/ACRansac.hpp index d146201d27..818fc16b1a 100644 --- a/src/aliceVision/robustEstimation/ACRansac.hpp +++ b/src/aliceVision/robustEstimation/ACRansac.hpp @@ -9,31 +9,8 @@ #pragma once -//------------------- -// Generic implementation of ACRANSAC -//------------------- -// The A contrario parametrization have been first explained in [1] and -// later extended to generic model estimation in [2] (with a demonstration for -// the homography) and extended and use at large scale for Structure from -// Motion in [3]. -// -//-- -// [1] Lionel Moisan, Berenger Stival, -// A probalistic criterion to detect rigid point matches between -// two images and estimate the fundamental matrix. -// IJCV 04. -//-- -// [2] Lionel Moisan, Pierre Moulon, Pascal Monasse. -// Automatic Homographic Registration of a Pair of Images, -// with A Contrario Elimination of Outliers -// Image Processing On Line (IPOL), 2012. -// http://dx.doi.org/10.5201/ipol.2012.mmm-oh -//-- -// [3] Pierre Moulon, Pascal Monasse and Renaud Marlet. -// Adaptive Structure from Motion with a contrario mode estimation. -// In 11th Asian Conference on Computer Vision (ACCV 2012) -//-- - +#include +#include #include #include @@ -43,89 +20,97 @@ #include #include -#include -#include +/** + * @brief Generic implementation of ACRANSAC + * + * The A contrario parametrization have been first explained in [1] and + * later extended to generic model estimation in [2] (with a demonstration for + * the homography) and extended and use at large scale for Structure from Motion in [3]. + * + * @ref [1] Lionel Moisan, Berenger Stival. + * A probalistic criterion to detect rigid point matches between + * two images and estimate the fundamental matrix. + * IJCV 04. + * + * @ref [2] Lionel Moisan, Pierre Moulon, Pascal Monasse. + * Automatic Homographic Registration of a Pair of Images, + * with A Contrario Elimination of Outliers + * Image Processing On Line (IPOL), 2012. + * http://dx.doi.org/10.5201/ipol.2012.mmm-oh + * + * @ref [3] Pierre Moulon, Pascal Monasse and Renaud Marlet. + * Adaptive Structure from Motion with a contrario mode estimation. + * In 11th Asian Conference on Computer Vision (ACCV 2012) + */ namespace aliceVision { namespace robustEstimation{ -/// logarithm (base 10) of binomial coefficient +/** + * @brief Logarithm (base 10) of binomial coefficient + */ template -T logcombi -( - size_t k, - size_t n, - const std::vector & vec_log10 // lookuptable in [0,n+1] -) +T logcombi(std::size_t k, std::size_t n, const std::vector& vec_log10) // vec_log10: lookuptable in [0,n+1] + { if (k>=n || k<=0) return(0.0f); if (n-k -void makelogcombi_n -( - size_t n, - std::vector & l, - std::vector & vec_log10 // lookuptable [0,n+1] -) +void makelogcombi_n(std::size_t n, std::vector& l, std::vector& vec_log10) // vec_log10: lookuptable [0,n+1] { l.resize(n+1); - for (size_t k = 0; k <= n; ++k) + for (std::size_t k = 0; k <= n; ++k) l[k] = logcombi(k, n, vec_log10); } -/// tabulate logcombi(k,.) +/** + * @brief Tabulate logcombi(k,.) + */ template -void makelogcombi_k -( - size_t k, - size_t nmax, - std::vector & l, - std::vector & vec_log10 // lookuptable [0,n+1] -) +void makelogcombi_k(std::size_t k, std::size_t nmax, std::vector& l, std::vector& vec_log10) // vec_log10: lookuptable [0,n+1] { l.resize(nmax+1); - for (size_t n = 0; n <= nmax; ++n) + for (std::size_t n = 0; n <= nmax; ++n) l[n] = logcombi(k, n, vec_log10); } template -void makelogcombi -( - size_t k, - size_t n, - std::vector & vec_logc_k, - std::vector & vec_logc_n) +void makelogcombi(std::size_t k, std::size_t n, std::vector& vec_logc_k, std::vector& vec_logc_n) { // compute a lookuptable of log10 value for the range [0,n+1] std::vector vec_log10(n + 1); - for (size_t k = 0; k <= n; ++k) + for (std::size_t k = 0; k <= n; ++k) vec_log10[k] = log10((Type)k); makelogcombi_n(n, vec_logc_n, vec_log10); makelogcombi_k(k, n, vec_logc_k, vec_log10); } +/** + * @brief NFA and associated index + */ +using ErrorIndex = std::pair; -/// NFA and associated index -typedef std::pair ErrorIndex; - -/// Find best NFA and its index wrt square error threshold in e. -inline ErrorIndex bestNFA( - int startIndex, //number of point required for estimation - double logalpha0, - const std::vector& e, - double loge0, - double maxThreshold, - const std::vector &logc_n, - const std::vector &logc_k, - double multError = 1.0) +/** + * @brief Find best NFA and its index wrt square error threshold in e. + */ +inline ErrorIndex bestNFA(int startIndex, //number of point required for estimation + double logalpha0, + const std::vector& e, + double loge0, + double maxThreshold, + const std::vector &logc_n, + const std::vector &logc_k, + double multError = 1.0) { ErrorIndex bestIndex(std::numeric_limits::infinity(), startIndex); const size_t n = e.size(); @@ -157,17 +142,17 @@ inline ErrorIndex bestNFA( * @return (errorMax, minNFA) */ template -std::pair ACRANSAC(const Kernel &kernel, - std::vector & vec_inliers, - size_t nIter = 1024, - typename Kernel::Model * model = nullptr, - double precision = std::numeric_limits::infinity()) +std::pair ACRANSAC(const Kernel& kernel, + std::vector& vec_inliers, + std::size_t nIter = 1024, + typename Kernel::ModelT* model = nullptr, + double precision = std::numeric_limits::infinity()) { vec_inliers.clear(); - const size_t sizeSample = Kernel::MINIMUM_SAMPLES; - const size_t nData = kernel.NumSamples(); - if (nData <= (size_t)sizeSample) + const std::size_t sizeSample = kernel.getMinimumNbRequiredSamples(); + const std::size_t nData = kernel.nbSamples(); + if (nData <= (std::size_t)sizeSample) return std::make_pair(0.0,0.0); const double maxThreshold = (precision==std::numeric_limits::infinity()) ? @@ -182,7 +167,7 @@ std::pair ACRANSAC(const Kernel &kernel, std::iota(vec_index.begin(), vec_index.end(), 0); // Precompute log combi - const double loge0 = log10((double)Kernel::MAX_MODELS * (nData-sizeSample)); + const double loge0 = log10((double)kernel.getMaximumNbModels() * (nData-sizeSample)); std::vector vec_logc_n, vec_logc_k; makelogcombi(sizeSample, nData, vec_logc_k, vec_logc_n); @@ -197,28 +182,28 @@ std::pair ACRANSAC(const Kernel &kernel, bool bACRansacMode = (precision == std::numeric_limits::infinity()); // Main estimation loop. - for (size_t iter=0; iter < nIter; ++iter) + for(std::size_t iter = 0; iter < nIter; ++iter) { - std::vector< std::size_t> vec_sample(sizeSample); // Sample indices + std::vector vec_sample(sizeSample); // Sample indices if (bACRansacMode) - UniformSample(sizeSample, vec_index, vec_sample); // Get random sample + uniformSample(sizeSample, vec_index, vec_sample); // Get random sample else - UniformSample(sizeSample, nData, vec_sample); // Get random sample + uniformSample(sizeSample, nData, vec_sample); // Get random sample - std::vector vec_models; // Up to max_models solutions - kernel.Fit(vec_sample, &vec_models); + std::vector vec_models; // Up to max_models solutions + kernel.fit(vec_sample, vec_models); // Evaluate models bool better = false; - for (size_t k = 0; k < vec_models.size(); ++k) + for (std::size_t k = 0; k < vec_models.size(); ++k) { // Residuals computation and ordering - kernel.Errors(vec_models[k], vec_residuals_); + kernel.errors(vec_models[k], vec_residuals_); if (!bACRansacMode) { unsigned int nInlier = 0; - for (size_t i = 0; i < nData; ++i) + for (std::size_t i = 0; i < nData; ++i) { if (vec_residuals_[i] <= maxThreshold) ++nInlier; @@ -300,7 +285,7 @@ std::pair ACRANSAC(const Kernel &kernel, if (!vec_inliers.empty()) { if (model) - kernel.Unnormalize(model); + kernel.unnormalize(*model); errorMax = kernel.unormalizeError(errorMax); } diff --git a/src/aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp b/src/aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp deleted file mode 100644 index b8745f5a43..0000000000 --- a/src/aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp +++ /dev/null @@ -1,509 +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 - -// Here a collection of A contrario Kernel adaptor. -// - See // [1] "Robust and accurate calibration of camera networks". PhD. -// - Authors: Pierre MOULON -// -//ACKernelAdaptor -// i.e. general two view estimation (affine, homography, fundamental) -//ACKernelAdaptorResection -// For pose/resection estimation -//ACKernelAdaptorResection_K -// For pose/resection with known camera intrinsic -//ACKernelAdaptorEssential -// For essential matrix estimation -//ACKernelAdaptor_AngularRadianError -// i.e. essential matrix estimation between spherical camera -// -// Mainly it add correct data normalization and define the function required -// by the generic ACRANSAC routine. -// -#include -#include -#include -#include -#include -#include - -namespace aliceVision { -namespace robustEstimation { - -#define ALICEVISION_MINIMUM_SAMPLES_COEF 7 //TODO: TO REMOVE - -inline bool hasStrongSupport(const std::vector& inliers, const std::vector& descTypes, std::size_t minimumSamples) -{ - assert(inliers.size() <= descTypes.size()); - - float score = 0; - for(const std::size_t inlier : inliers) - { - score += feature::getStrongSupportCoeff(descTypes[inlier]); - } - return (score > minimumSamples); -} - -inline bool hasStrongSupport(const std::vector>& inliersPerCamera, const std::vector>& descTypesPerCamera, std::size_t minimumSamples) -{ - assert(inliersPerCamera.size() == descTypesPerCamera.size()); //same number of cameras - - float score = 0; - - for(std::size_t camIdx = 0; camIdx < inliersPerCamera.size(); ++camIdx) - { - const auto& inliers = inliersPerCamera.at(camIdx); - const auto& descTypes = descTypesPerCamera.at(camIdx); - - assert(inliers.size() <= descTypes.size()); - - for(const std::size_t inlier : inliers) - { - score += feature::getStrongSupportCoeff(descTypes[inlier]); - } - } - - return (score > minimumSamples); -} - -inline bool hasStrongSupport(const matching::MatchesPerDescType& matchesPerDesc, std::size_t minimumSamples) -{ - float score = 0; - for(const auto& matchesIt : matchesPerDesc) - { - const feature::EImageDescriberType descType = matchesIt.first; - const matching::IndMatches& descMatches = matchesIt.second; - - score += feature::getStrongSupportCoeff(descType) * descMatches.size(); - } - return (score > minimumSamples); -} - - -/// Two view Kernel adapter for the A contrario model estimator -/// Handle data normalization and compute the corresponding logalpha 0 -/// that depends of the error model (point to line, or point to point) -/// This kernel adapter is working for affine, homography, fundamental matrix -/// estimation. - -template -class ACKernelAdaptor -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - - ACKernelAdaptor( - const Mat &x1, int w1, int h1, - const Mat &x2, int w2, int h2, bool bPointToLine = true) - : x1_(x1.rows(), x1.cols()), x2_(x2.rows(), x2.cols()), - N1_(3, 3), N2_(3, 3), logalpha0_(0.0), bPointToLine_(bPointToLine) - { - assert(2 == x1_.rows()); - assert(x1_.rows() == x2_.rows()); - assert(x1_.cols() == x2_.cols()); - - NormalizePointsFromImageSize(x1, &x1_, &N1_, w1, h1); - NormalizePointsFromImageSize(x2, &x2_, &N2_, w2, h2); - - // LogAlpha0 is used to make error data scale invariant - if(bPointToLine) - { - // Ratio of containing diagonal image rectangle over image area - const double D = sqrt(w2 * (double) w2 + h2 * (double) h2); // diameter - const double A = w2 * (double) h2; // area - logalpha0_ = log10(2.0 * D / A / N2_(0, 0)); - } - else - { - // ratio of area : unit circle over image area - logalpha0_ = log10(M_PI / (w2 * (double) h2) / (N2_(0, 0) * N2_(0, 0))); - } - } - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; - - enum - { - MAX_MODELS = Solver::MAX_MODELS - }; - - void Fit(const std::vector &samples, std::vector *models) const - { - const Mat x1 = ExtractColumns(x1_, samples); - const Mat x2 = ExtractColumns(x2_, samples); - Solver::Solve(x1, x2, models); - } - - double Error(std::size_t sample, const Model &model) const - { - return ErrorT::Error(model, x1_.col(sample), x2_.col(sample)); - } - - void Errors(const Model & model, std::vector & vec_errors) const - { - vec_errors.resize(x1_.cols()); - for(std::size_t sample = 0; sample < x1_.cols(); ++sample) - vec_errors[sample] = ErrorT::Error(model, x1_.col(sample), x2_.col(sample)); - } - - std::size_t NumSamples() const - { - return static_cast (x1_.cols()); - } - - void Unnormalize(Model * model) const - { - // Unnormalize model from the computed conditioning. - UnnormalizerArg::Unnormalize(N1_, N2_, &(*model)); - } - - double logalpha0() const {return logalpha0_;} - - double multError() const {return (bPointToLine_)? 0.5 : 1.0;} - - Mat3 normalizer1() const {return N1_;} - Mat3 normalizer2() const {return N2_;} - double unormalizeError(double val) const {return sqrt(val) / N2_(0,0);} - -protected: - Mat x1_, x2_; // Normalized input data - Mat3 N1_, N2_; // Matrix used to normalize data - double logalpha0_; // Alpha0 is used to make the error adaptive to the image size - bool bPointToLine_; // Store if error model is pointToLine or point to point -}; - -struct UnnormalizerResection -{ - - static void Unnormalize(const Mat &T, const Mat &U, Mat34 *P) - { - *P = T.inverse() * (*P); - } -}; - -/// Pose/Resection Kernel adapter for the A contrario model estimator - -template -class ACKernelAdaptorResection -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - - ACKernelAdaptorResection(const Mat &x2d, int w, int h, const Mat &x3D) - : x2d_(x2d.rows(), x2d.cols()), x3D_(x3D), - N1_(3, 3), logalpha0_(log10(M_PI)) - { - assert(2 == x2d_.rows()); - assert(3 == x3D_.rows()); - assert(x2d_.cols() == x3D_.cols()); - - NormalizePointsFromImageSize(x2d, &x2d_, &N1_, w, h); - } - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; - - enum - { - MAX_MODELS = Solver::MAX_MODELS - }; - - void Fit(const std::vector &samples, std::vector *models) const - { - const Mat x1 = ExtractColumns(x2d_, samples); - const Mat x2 = ExtractColumns(x3D_, samples); - Solver::Solve(x1, x2, models); - } - - double Error(int sample, const Model &model) const - { - return ErrorT::Error(model, x2d_.col(sample), x3D_.col(sample)); - } - - void Errors(const Model & model, std::vector & vec_errors) const - { - vec_errors.resize(x2d_.cols()); - for(std::size_t sample = 0; sample < x2d_.cols(); ++sample) - vec_errors[sample] = ErrorT::Error(model, x2d_.col(sample), x3D_.col(sample)); - } - - std::size_t NumSamples() const - { - return x2d_.cols(); - } - - void Unnormalize(Model * model) const - { - // Unnormalize model from the computed conditioning. - UnnormalizerArg::Unnormalize(N1_, Mat3::Identity(), &(*model)); - } - - double logalpha0() const {return logalpha0_;} - double multError() const {return 1.0;} // point to point error - Mat3 normalizer1() const {return Mat3::Identity();} - Mat3 normalizer2() const {return N1_;} - double unormalizeError(double val) const {return sqrt(val) / N1_(0,0);} - -private: - Mat x2d_; - const Mat& x3D_; - Mat3 N1_; // Matrix used to normalize data - double logalpha0_; // Alpha0 is used to make the error adaptive to the image size -}; - -/// Pose/Resection Kernel adapter for the A contrario model estimator with -/// known Intrinsic - -template -class ACKernelAdaptorResection_K -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - - ACKernelAdaptorResection_K(const Mat &x2d, const Mat &x3D, const Mat3 & K) - : x2d_(x2d.rows(), x2d.cols()), x3D_(x3D), - N1_(K.inverse()), - logalpha0_(log10(M_PI)), K_(K) - { - assert(2 == x2d_.rows()); - assert(3 == x3D_.rows()); - assert(x2d_.cols() == x3D_.cols()); - - // Normalize points by inverse(K) - ApplyTransformationToPoints(x2d, N1_, &x2d_); - } - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; - - enum - { - MAX_MODELS = Solver::MAX_MODELS - }; - - void Fit(const std::vector &samples, std::vector *models) const - { - const Mat x1 = ExtractColumns(x2d_, samples); - const Mat x2 = ExtractColumns(x3D_, samples); - Solver::Solve(x1, x2, models); - } - - double Error(std::size_t sample, const Model &model) const - { - return ErrorT::Error(model, x2d_.col(sample), x3D_.col(sample)); - } - - void Errors(const Model & model, std::vector & vec_errors) const - { - vec_errors.resize(x2d_.cols()); - for(std::size_t sample = 0; sample < x2d_.cols(); ++sample) - vec_errors[sample] = ErrorT::Error(model, x2d_.col(sample), x3D_.col(sample)); - } - - std::size_t NumSamples() const { return x2d_.cols(); } - - void Unnormalize(Model * model) const - { - // Unnormalize model from the computed conditioning. - (*model) = K_ * (*model); - } - - double logalpha0() const {return logalpha0_;} - double multError() const {return 1.0;} // point to point error - Mat3 normalizer1() const {return Mat3::Identity();} - Mat3 normalizer2() const {return N1_;} - double unormalizeError(double val) const {return sqrt(val) / N1_(0,0);} - -protected: - Mat x2d_; - const Mat& x3D_; - Mat3 N1_; // Matrix used to normalize data - double logalpha0_; // Alpha0 is used to make the error adaptive to the image size - Mat3 K_; // Intrinsic camera parameter -}; - -/// Essential matrix Kernel adaptor for the A contrario model estimator - -template -class ACKernelAdaptorEssential -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - - ACKernelAdaptorEssential( - const Mat &x1, int w1, int h1, - const Mat &x2, int w2, int h2, - const Mat3 & K1, const Mat3 & K2) - : x1_(x1), x2_(x2), - N1_(Mat3::Identity()), N2_(Mat3::Identity()), logalpha0_(0.0), - K1_(K1), K2_(K2) - { - assert(2 == x1_.rows()); - assert(x1_.rows() == x2_.rows()); - assert(x1_.cols() == x2_.cols()); - - ApplyTransformationToPoints(x1_, K1_.inverse(), &x1k_); - ApplyTransformationToPoints(x2_, K2_.inverse(), &x2k_); - - //Point to line probability (line is the epipolar line) - const double D = sqrt(w2 * (double)w2 + h2 * (double)h2); // diameter - const double A = w2 * (double)h2; // area - logalpha0_ = log10(2.0 * D / A * .5); - } - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; - - enum - { - MAX_MODELS = Solver::MAX_MODELS - }; - - void Fit(const std::vector &samples, std::vector *models) const - { - const Mat x1 = ExtractColumns(x1k_, samples); - const Mat x2 = ExtractColumns(x2k_, samples); - Solver::Solve(x1, x2, models); - } - - double Error(std::size_t sample, const Model &model) const - { - Mat3 F; - FundamentalFromEssential(model, K1_, K2_, &F); - return ErrorT::Error(F, this->x1_.col(sample), this->x2_.col(sample)); - } - - void Errors(const Model & model, std::vector & vec_errors) const - { - Mat3 F; - FundamentalFromEssential(model, K1_, K2_, &F); - vec_errors.resize(x1_.cols()); - for(std::size_t sample = 0; sample < x1_.cols(); ++sample) - vec_errors[sample] = ErrorT::Error(F, this->x1_.col(sample), this->x2_.col(sample)); - } - - std::size_t NumSamples() const { return x1_.cols(); } - void Unnormalize(Model * model) const {} - double logalpha0() const {return logalpha0_;} - double multError() const {return 0.5;} // point to line error - Mat3 normalizer1() const {return N1_;} - Mat3 normalizer2() const {return N2_;} - double unormalizeError(double val) const { return val; } - -private: - Mat x1_, x2_, x1k_, x2k_; // image point and camera plane point. - Mat3 N1_, N2_; // Matrix used to normalize data - double logalpha0_; // Alpha0 is used to make the error adaptive to the image size - Mat3 K1_, K2_; // Intrinsic camera parameter -}; - -/// Two view Kernel adapter for the A contrario model estimator. -/// Specialization to handle radian angular residual error. - -template -class ACKernelAdaptor_AngularRadianError -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - - ACKernelAdaptor_AngularRadianError( - const Mat & xA, - const Mat & xB) : - x1_(xA), x2_(xB), - logalpha0_(log10(1.0 / 2.0)) - { - assert(3 == x1_.rows()); - assert(x1_.rows() == x2_.rows()); - assert(x1_.cols() == x2_.cols()); - } - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; - - enum - { - MAX_MODELS = Solver::MAX_MODELS - }; - - void Fit(const std::vector &samples, std::vector *models) const - { - const Mat x1 = ExtractColumns(x1_, samples); - const Mat x2 = ExtractColumns(x2_, samples); - Solver::Solve(x1, x2, models); - } - - double Error(std::size_t sample, const Model &model) const - { - return Square(ErrorT::Error(model, x1_.col(sample), x2_.col(sample))); - } - - void Errors(const Model & model, std::vector & vec_errors) const - { - vec_errors.resize(x1_.cols()); - for(std::size_t sample = 0; sample < x1_.cols(); ++sample) - vec_errors[sample] = Square(ErrorT::Error(model, x1_.col(sample), x2_.col(sample))); - } - - std::size_t NumSamples() const {return static_cast(x1_.cols());} - - void Unnormalize(Model * model) const - { - //-- Do nothing, no normalization in the angular case - } - - double logalpha0() const {return logalpha0_;} - - double multError() const {return 1./4.;} - - Mat3 normalizer1() const {return Mat3::Identity();} - Mat3 normalizer2() const {return Mat3::Identity();} - double unormalizeError(double val) const {return sqrt(val);} - -private: - Mat x1_, x2_; // Normalized input data - double logalpha0_; // Alpha0 is used to make the error scale invariant -}; - -} // namespace robustEstimation -} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/CMakeLists.txt b/src/aliceVision/robustEstimation/CMakeLists.txt index 1104c431fe..c3bfaaf9ca 100644 --- a/src/aliceVision/robustEstimation/CMakeLists.txt +++ b/src/aliceVision/robustEstimation/CMakeLists.txt @@ -1,27 +1,31 @@ # Headers -set(robustEstimation_files_headers - guidedMatching.hpp - lineTestGenerator.hpp - randSampling.hpp +set(robustEstimation_files_headers + conditioning.hpp + estimators.hpp LineKernel.hpp - Ransac.hpp + lineTestGenerator.hpp + PointFittingKernel.hpp + PointFittingRansacKernel.hpp + ISolver.hpp + IRansacKernel.hpp + Ransac.hpp ACRansac.hpp - ACRansacKernelAdaptator.hpp LORansac.hpp - LORansacKernelAdaptor.hpp ransacTools.hpp - estimators.hpp - maxConsensus.hpp + randSampling.hpp leastMedianOfSquares.hpp ScoreEvaluator.hpp + maxConsensus.hpp +) + +# Sources +set(robustEstimation_files_sources + conditioning.cpp ) -alicevision_add_interface(aliceVision_robustEstimation - SOURCES ${robustEstimation_files_headers} - LINKS - aliceVision_camera - aliceVision_feature - aliceVision_matching +alicevision_add_library(aliceVision_robustEstimation + SOURCES ${robustEstimation_files_headers} ${robustEstimation_files_sources} + PUBLIC_LINKS aliceVision_numeric aliceVision_system ) diff --git a/src/aliceVision/robustEstimation/IRansacKernel.hpp b/src/aliceVision/robustEstimation/IRansacKernel.hpp new file mode 100644 index 0000000000..3a6a4c6caf --- /dev/null +++ b/src/aliceVision/robustEstimation/IRansacKernel.hpp @@ -0,0 +1,114 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 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 robustEstimation{ + +/** + * @brief A generic kernel used for the ACRANSAC / LORANSAC framework. + * @tparam ModelT The class representing the model to estimate. + * + * @note Handle data normalization and compute the corresponding logalpha 0 + * that depends of the error model (point to line, or point to point) + */ +template +class IRansacKernel +{ + public: + + /** + * @brief Return the minimum number of required samples for the solver + * @return minimum number of required samples + */ + virtual std::size_t getMinimumNbRequiredSamples() const = 0; + + /** + * @brief Return the minimum number of required samples for the solver Ls + * @return minimum number of required samples + */ + virtual std::size_t getMinimumNbRequiredSamplesLS() const = 0; + + /** + * @brief Return the maximum number of models for the solver + * @return maximum number of models + */ + virtual std::size_t getMaximumNbModels() const = 0; + + /** + * @brief This function is called to estimate the model from the minimum number + * of sample \p minSample (i.e. minimal problem solver). + * @param[in] samples A vector containing the indices of the data to be used for + * the minimal estimation. + * @param[out] models The model(s) estimated by the minimal solver. + */ + virtual void fit(const std::vector& samples, std::vector& models) const = 0; + + /** + * @brief This function is called to estimate the model using a least squared + * algorithm from a minumum of \p minSampleLS. + * @param[in] inliers An array containing the indices of the data to use. + * @param[out] models The model(s) estimated using the least squared algorithm. + * @param[in] weights An optional array of weights, one for each sample + */ + virtual void fitLS(const std::vector& inliers, std::vector& models, const std::vector* weights = nullptr) const = 0; + + /** + * @brief Function used to estimate the weights, typically used by the least square algorithm. + * @param[in] model The model against which the weights are computed. + * @param[in] inliers The array of the indices of the data to be used. + * @param[out] vec_weights The array of weight of the same size as \p inliers. + * @param[in] eps An optional threshold to max out the value of the threshold (typically + * to avoid division by zero or too small numbers). + */ + virtual void computeWeights(const ModelT& model, const std::vector& inliers, std::vector& weights, const double eps = 0.001) const = 0; + + /** + * @brief Function that computes the estimation error for a given model and a given element. + * @param[in] sample The index of the element for which the error is computed. + * @param[in] model The model to consider. + * @return The estimation error for the given element and the given model. + */ + virtual double error(std::size_t sample, const ModelT& model) const = 0; + + /** + * @brief Function that computes the estimation error for a given model and all the elements. + * @param[in] model The model to consider. + * @param[out] vec_errors The vector containing all the estimation errors for every element. + */ + virtual void errors(const ModelT& model, std::vector& errors) const = 0; + + /** + * @brief Function used to unnormalize the model. + * @param[in,out] model The model to unnormalize. + */ + virtual void unnormalize(ModelT& model) const = 0; + + /** + * @brief The number of elements in the data. + * @return the number of elements in the data. + */ + virtual std::size_t nbSamples() const = 0; + + /** + * @brief Get logalpha0, Alpha0 is used to make the error adaptive to the image size + * @return logalpha0 + */ + virtual double logalpha0() const = 0; + + virtual double multError() const = 0; + virtual double unormalizeError(double val) const = 0; + virtual Mat3 normalizer1() const = 0; + virtual Mat3 normalizer2() const = 0; +}; + +} // namespace robustEstimation +} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/ISolver.hpp b/src/aliceVision/robustEstimation/ISolver.hpp new file mode 100644 index 0000000000..cb9ba10267 --- /dev/null +++ b/src/aliceVision/robustEstimation/ISolver.hpp @@ -0,0 +1,128 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 robustEstimation { + +/** + * @brief Generic solver interface. + */ +template +class ISolver +{ +public: + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + virtual std::size_t getMinimumNbRequiredSamples() const = 0; + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + virtual std::size_t getMaximumNbModels() const = 0; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + */ + virtual void solve(const Mat& x1, const Mat& x2, std::vector& models) const = 0; + + /** + * @brief Solve the problem. + * @param[in] x1 A 2xN matrix of column vectors. + * @param[in] x2 A 2xN (relative pose) or 3xN (resection) matrix of column vectors. + * @param[out] models A vector into which the computed models are stored. + * @param[in] weights. + */ + virtual void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const = 0; +}; + +/** + * @brief An Undefined Solver + */ +template +class UndefinedSolver : public ISolver +{ +public: + + std::size_t getMinimumNbRequiredSamples() const override + { + throw std::runtime_error("Undefined solver used in kernel."); + return 0; + } + + std::size_t getMaximumNbModels() const override + { + throw std::runtime_error("Undefined solver used in kernel."); + return 0; + } + + void solve(const Mat& x1, const Mat& x2, std::vector& models) const override + { + throw std::runtime_error("Undefined solver used in kernel."); + } + + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::runtime_error("Undefined solver used in kernel."); + } +}; + +/** + * @brief Matrix based model to be used in a solver. + */ +template +struct MatrixModel +{ + MatrixModel() = default; + + explicit MatrixModel(const MatrixT& matrix) + : _matrix(matrix) + {} + + inline const MatrixT& getMatrix() const + { + return _matrix; + } + + inline MatrixT& getMatrix() + { + return _matrix; + } + + inline void setMatrix(const MatrixT& matrix) + { + _matrix = matrix; + } + +protected: + MatrixT _matrix{}; +}; + +/** + * @brief explicit typename for MatrixModel with Mat3 matrix (fundamental, essential, homography, ...) + */ +using Mat3Model = MatrixModel; + +/** + * @brief explicit typename for MatrixModel with Mat34 matrix (projection, ...) + */ +using Mat34Model = MatrixModel; + + +} // namespace robustEstimation +} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/LORansac.hpp b/src/aliceVision/robustEstimation/LORansac.hpp index ab60fec227..7553063723 100644 --- a/src/aliceVision/robustEstimation/LORansac.hpp +++ b/src/aliceVision/robustEstimation/LORansac.hpp @@ -6,9 +6,10 @@ #pragma once -#include "aliceVision/robustEstimation/randSampling.hpp" -#include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ransacTools.hpp" +#include +#include +#include +#include #include #include #include @@ -18,99 +19,6 @@ namespace aliceVision { namespace robustEstimation{ -/** - * @brief A generic kernel used for the LORANSAC framework. - * - * @tparam minSample The minimum number of samples that allows to solve the problem - * @tparam minSampleLS The minimum number of samples that allows to solve the problem in a least squared manner. - * @tparam Model The class representing the model to estimate. - */ -template -class LORansacGenericKernel -{ - public: - - enum - { - MINIMUM_SAMPLES = minSample, - MINIMUM_LSSAMPLES = minSampleLS - }; - - /** - * @brief The constructor; - */ - LORansacGenericKernel() = default; - - /** - * @brief This function is called to estimate the model from the minimum number - * of sample \p minSample (i.e. minimal problem solver). - * @param[in] samples A vector containing the indices of the data to be used for - * the minimal estimation. - * @param[out] models The model(s) estimated by the minimal solver. - */ - virtual void Fit(const std::vector &samples, std::vector *models) const = 0; - - /** - * @brief This function is called to estimate the model using a least squared - * algorithm from a minumum of \p minSampleLS. - * @param[in] inliers An array containing the indices of the data to use. - * @param[out] models The model(s) estimated using the least squared algorithm. - * @param[in] weights An optional array of weights, one for each sample - */ - virtual void FitLS(const std::vector &inliers, - std::vector *models, - const std::vector *weights = nullptr) const = 0; - - /** - * @brief Function used to estimate the weights, typically used by the least square algorithm. - * @param[in] model The model against which the weights are computed. - * @param[in] inliers The array of the indices of the data to be used. - * @param[out] vec_weights The array of weight of the same size as \p inliers. - * @param[in] eps An optional threshold to max out the value of the threshold (typically - * to avoid division by zero or too small numbers). - */ - virtual void computeWeights(const Model & model, - const std::vector &inliers, - std::vector & vec_weights, - const double eps = 0.001) const = 0; - - /** - * @brief Function that computes the estimation error for a given model and a given element. - * @param[in] sample The index of the element for which the error is computed. - * @param[in] model The model to consider. - * @return The estimation error for the given element and the given model. - */ - virtual double Error(std::size_t sample, const Model &model) const = 0; - - /** - * @brief Function that computes the estimation error for a given model and all the elements. - * @param[in] model The model to consider. - * @param[out] vec_errors The vector containing all the estimation errors for every element. - */ - virtual void Errors(const Model & model, std::vector & vec_errors) const = 0; - - /** - * @brief Function used to unnormalize the model. - * @param[in,out] model The model to unnormalize. - */ - virtual void Unnormalize(Model * model) const = 0; - - /** - * @brief The number of elements in the data. - * @return the number of elements in the data. - */ - virtual std::size_t NumSamples() const = 0; - - /** - * @brief The destructor. - */ - virtual ~LORansacGenericKernel( ) = default; - -}; - - /** * @brief It performs an iterative reweighted least square (IRLS) estimation of the problem * defined by \p Kernel. At each step it perform a LS estimation using weights @@ -122,7 +30,7 @@ class LORansacGenericKernel * * @tparam Kernel The kernel used in the LORansac estimator which must provide a * minimum solver and a LS solver, the latter used here for the IRLS - * @see aliceVision/robustEstimation/LORansacKernelAdaptor.hpp + * @see aliceVision/robustEstimation/LORansacKernel.hpp * @tparam Scorer The scorer used in the LORansac estimator @see ScoreEvaluator * * @param[in] kernel The kernel used in the LORansac estimator. @@ -137,14 +45,14 @@ class LORansacGenericKernel template double iterativeReweightedLeastSquares(const Kernel &kernel, const Scorer &scorer, - typename Kernel::Model &best_model, + typename Kernel::ModelT &best_model, std::vector &inliers, double mtheta = std::sqrt(2), std::size_t numIter = 4, bool verbose = false) { - const std::size_t total_samples = kernel.NumSamples(); - const std::size_t min_samples = Kernel::MINIMUM_LSSAMPLES; + const std::size_t total_samples = kernel.nbSamples(); + const std::size_t min_samples = kernel.getMinimumNbRequiredSamplesLS(); double theta = scorer.getThreshold(); // used in the iterations to update (reduce) the threshold value const double deltaTetha = (mtheta*theta - theta) / (numIter-1); @@ -154,7 +62,7 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, // find inliers from best model with threshold theta inliers.clear(); - scorer.Score(kernel, best_model, all_samples, &inliers, theta); + scorer.score(kernel, best_model, all_samples, inliers, theta); if(inliers.size() < min_samples) { @@ -167,8 +75,8 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, } // LS model from the above inliers - std::vector models; - kernel.FitLS(inliers, &models); + std::vector models; + kernel.fitLS(inliers, models); assert(models.size()==1); // LS fitting must always return 1 model // change threshold for refinement @@ -180,7 +88,7 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, // find inliers on the best-so-far model // @todo maybe inliers instead of all samples to save some computation inliers.clear(); - scorer.Score(kernel, models[0], all_samples, &inliers, theta); + scorer.score(kernel, models[0], all_samples, inliers, theta); if(inliers.size() < min_samples) { @@ -201,7 +109,7 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, // LS with weights on inliers models.clear(); - kernel.FitLS(inliers, &models, &weights); + kernel.fitLS(inliers, models, &weights); if(models.size() != 1) // LS fitting must always return 1 model { if(verbose) @@ -218,7 +126,7 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, assert(models.size()==1); best_model = models[0]; inliers.clear(); - const double score = scorer.Score(kernel, best_model, all_samples, &inliers, theta); + const double score = scorer.score(kernel, best_model, all_samples, inliers, theta); if(verbose) { ALICEVISION_LOG_DEBUG("[IRLS] returning with num inliers: " << inliers.size() @@ -244,7 +152,7 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, * * @tparam Kernel The kernel used in the LORansac estimator which must provide a * minimum solver and a LS solver, the latter used here for the IRLS - * @see aliceVision/robustEstimation/LORansacKernelAdaptor.hpp + * @see aliceVision/robustEstimation/LORansacKernel.hpp * @tparam Scorer The scorer used in the LORansac estimator @see ScoreEvaluator * * @param[in] kernel The kernel used in the LORansac estimator. @@ -258,17 +166,17 @@ double iterativeReweightedLeastSquares(const Kernel &kernel, * @return the best score of the best model as computed by Scorer. */ template -double localOptimization(const Kernel &kernel, - const Scorer &scorer, - typename Kernel::Model &bestModel, - std::vector &bestInliers, +double localOptimization(const Kernel& kernel, + const Scorer& scorer, + typename Kernel::ModelT& bestModel, + std::vector& bestInliers, double mtheta = std::sqrt(2), std::size_t numRep = 10, std::size_t minSampleSize = 10, bool verbose = false) { - const std::size_t total_samples = kernel.NumSamples(); - const std::size_t min_samples = Kernel::MINIMUM_LSSAMPLES; + const std::size_t total_samples = kernel.nbSamples(); + const std::size_t min_samples = kernel.getMinimumNbRequiredSamplesLS(); assert((total_samples > min_samples) && "[localOptimization] not enough data to estimate the model!"); @@ -283,7 +191,7 @@ double localOptimization(const Kernel &kernel, debugInit = bestInliers.size(); bestInliers.clear(); } - double bestScore = scorer.Score(kernel, bestModel, all_samples, &bestInliers, theta); + double bestScore = scorer.score(kernel, bestModel, all_samples, bestInliers, theta); if(debugInit != 0) assert(debugInit == bestInliers.size()); // so far this is the best model @@ -291,30 +199,30 @@ double localOptimization(const Kernel &kernel, if(verbose) { ALICEVISION_LOG_DEBUG("[localOptim] so far best num inliers: " << bestNumInliers); - ALICEVISION_LOG_DEBUG("[localOptim] so far best model:\n" << bestModel); + ALICEVISION_LOG_DEBUG("[localOptim] so far best model:\n" << bestModel.getMatrix()); ALICEVISION_LOG_DEBUG("[localOptim] so far best score: " << bestScore); } // find inliers from best model with larger threshold t*m over all the samples std::vector inliersBase; - scorer.Score(kernel, bestModel, all_samples, &inliersBase, theta*mtheta); + scorer.score(kernel, bestModel, all_samples, inliersBase, theta*mtheta); assert((inliersBase.size() > min_samples) && "[localOptimization] not enough data in inliersBase to estimate the model!"); // LS model from the above inliers - std::vector models; + std::vector models; // ALICEVISION_LOG_DEBUG("[localOptim] before: "); - kernel.FitLS(inliersBase, &models); + kernel.fitLS(inliersBase, models); // ALICEVISION_LOG_DEBUG("[localOptim] after: "); assert(models.size()==1); // LS fitting must always return 1 model // find inliers with t again over all the samples inliersBase.clear(); - scorer.Score(kernel, models[0], all_samples, &inliersBase, theta); + scorer.score(kernel, models[0], all_samples, inliersBase, theta); // sample of size sampleSize from the last best inliers const std::size_t sampleSize = std::min(minSampleSize, inliersBase.size()/2); - if(sampleSize <= Kernel::MINIMUM_LSSAMPLES) + if(sampleSize <= kernel.getMinimumNbRequiredSamplesLS()) { if(verbose) { @@ -327,13 +235,13 @@ double localOptimization(const Kernel &kernel, for(std::size_t i = 0; i < numRep; ++i) { std::vector sample; - UniformSample(sampleSize, inliersBase, sample); - assert(sampleSize > Kernel::MINIMUM_LSSAMPLES); - assert(sample.size() > Kernel::MINIMUM_LSSAMPLES); + uniformSample(sampleSize, inliersBase, sample); + assert(sampleSize > kernel.getMinimumNbRequiredSamplesLS()); + assert(sample.size() > kernel.getMinimumNbRequiredSamplesLS()); // LS estimation from the sample models.clear(); - kernel.FitLS(sample, &models); + kernel.fitLS(sample, models); assert(models.size()==1); // LS fitting must always return 1 model // IRLS @@ -368,7 +276,7 @@ double localOptimization(const Kernel &kernel, * * @tparam Kernel The kernel used in the LORansac estimator which must provide a * minimum solver and a LS solver, the latter used here for the IRLS - * @see aliceVision/robustEstimation/LORansacKernelAdaptor.hpp + * @see aliceVision/robustEstimation/LORansacKernel.hpp * @tparam Scorer The scorer used in the LORansac estimator @see ScoreEvaluator * * @param[in] kernel The kernel containing the problem to solve. @@ -382,25 +290,25 @@ double localOptimization(const Kernel &kernel, * @return The best model found. */ template -typename Kernel::Model LO_RANSAC(const Kernel &kernel, - const Scorer &scorer, - std::vector *best_inliers = NULL, - double *best_score = NULL, - bool bVerbose = false, - std::size_t max_iterations = 100, - double outliers_probability = 1e-2) +typename Kernel::ModelT LO_RANSAC(const Kernel& kernel, + const Scorer& scorer, + std::vector* best_inliers = NULL, + double* best_score = NULL, + bool bVerbose = false, + std::size_t max_iterations = 100, + double outliers_probability = 1e-2) { assert(outliers_probability < 1.0); assert(outliers_probability > 0.0); std::size_t iteration = 0; - const std::size_t min_samples = Kernel::MINIMUM_SAMPLES; - const std::size_t total_samples = kernel.NumSamples(); + const std::size_t min_samples = kernel.getMinimumNbRequiredSamples(); + const std::size_t total_samples = kernel.nbSamples(); const std::size_t really_max_iterations = 4096; std::size_t bestNumInliers = 0; double bestInlierRatio = 0.0; - typename Kernel::Model bestModel; + typename Kernel::ModelT bestModel; // Test if we have sufficient points for the kernel. if (total_samples < min_samples) @@ -419,16 +327,16 @@ typename Kernel::Model LO_RANSAC(const Kernel &kernel, for(iteration = 0; iteration < max_iterations; ++iteration) { std::vector sample; - UniformSample(min_samples, total_samples, sample); + uniformSample(min_samples, total_samples, sample); - std::vector models; - kernel.Fit(sample, &models); + std::vector models; + kernel.fit(sample, models); // Compute the inlier list for each fit. for(std::size_t i = 0; i < models.size(); ++i) { std::vector inliers; - double score = scorer.Score(kernel, models[i], all_samples, &inliers); + double score = scorer.score(kernel, models.at(i), all_samples, inliers); if(bVerbose) { ALICEVISION_LOG_DEBUG("sample=" << sample); @@ -443,13 +351,13 @@ typename Kernel::Model LO_RANSAC(const Kernel &kernel, { ALICEVISION_LOG_DEBUG("Before Optim: num inliers: " << inliers.size() << " score: " << score - << " Kernel::MINIMUM_LSSAMPLES: " << Kernel::MINIMUM_LSSAMPLES + << " kernel minimum nb required samples LS: " << kernel.getMinimumNbRequiredSamplesLS() ); - ALICEVISION_LOG_DEBUG("Model:\n" << bestModel); + ALICEVISION_LOG_DEBUG("Model:\n" << bestModel.getMatrix()); } - if(inliers.size() > Kernel::MINIMUM_LSSAMPLES) + if(inliers.size() > kernel.getMinimumNbRequiredSamplesLS()) { score = localOptimization(kernel, scorer, bestModel, inliers); } @@ -458,7 +366,7 @@ typename Kernel::Model LO_RANSAC(const Kernel &kernel, { ALICEVISION_LOG_DEBUG("After Optim: num inliers: " << inliers.size() << " score: " << score); - ALICEVISION_LOG_DEBUG("Model:\n" << bestModel); + ALICEVISION_LOG_DEBUG("Model:\n" << bestModel.getMatrix()); } bestNumInliers = inliers.size(); @@ -479,7 +387,7 @@ typename Kernel::Model LO_RANSAC(const Kernel &kernel, } if (bestInlierRatio) { - max_iterations = IterationsRequired(min_samples, + max_iterations = iterationsRequired(min_samples, outliers_probability, bestInlierRatio); // safeguard to not get stuck in a big number of iterations @@ -494,7 +402,7 @@ typename Kernel::Model LO_RANSAC(const Kernel &kernel, *best_score = bestNumInliers; if(bestNumInliers) - kernel.Unnormalize(&bestModel); + kernel.unnormalize(bestModel); return bestModel; } diff --git a/src/aliceVision/robustEstimation/LORansacKernelAdaptor.hpp b/src/aliceVision/robustEstimation/LORansacKernelAdaptor.hpp deleted file mode 100644 index ff86eec64a..0000000000 --- a/src/aliceVision/robustEstimation/LORansacKernelAdaptor.hpp +++ /dev/null @@ -1,163 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 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 - -namespace aliceVision { -namespace robustEstimation { - - -/** - * @brief The generic kernel to be used for LORansac framework. - * - * @tparam SolverArg The minimal solver able to find a solution from a - * minimum set of points. - * @tparam ErrorArg The functor computing the error for each data sample with - * respect to the estimated model. - * @tparam UnnormalizerArg The functor used to normalize the data before the - * estimation of the model. - * @tparam ModelArg = Mat3 The type of the model to estimate. - * @tparam SolverLSArg = SolverArg The least square solver that is used to find - * a solution from any set of data larger than the minimum required. - */ -template -class KernelAdaptorLoRansac : public ACKernelAdaptor -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - typedef SolverLSArg SolverLS; - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES, - MINIMUM_LSSAMPLES = SolverLS::MINIMUM_SAMPLES - }; - - KernelAdaptorLoRansac(const Mat &x1, int w1, int h1, - const Mat &x2, int w2, int h2, - bool bPointToLine = true) - : ACKernelAdaptor(x1, w1, h1, x2, w2, h2, bPointToLine) - { - } - - void FitLS(const std::vector &inliers, - std::vector *models, - const std::vector *weights = nullptr) const - { - const Mat x1 = ExtractColumns(this->x1_, inliers); - const Mat x2 = ExtractColumns(this->x2_, inliers); - SolverLS::Solve(x1, x2, models, weights); - } - - /** - * @brief Given a model and the associated inliers, it computes the weight for - * each inlier as the squared inverse of the associated error. - * - * @param[in] model The model to against which to compute the weights. - * @param[in] inliers The inliers associated to the model. - * @param[out] vec_weights The weights associated to each inlier. - * @param[in] eps Each inlier having an error below this value will be assigned - * a weight of 1/eps^2 (to avoid division by zero). - */ - void computeWeights(const Model & model, - const std::vector &inliers, - std::vector & vec_weights, - const double eps = 0.001) const - { - const auto numInliers = inliers.size(); - vec_weights.resize(numInliers); - for(std::size_t sample = 0; sample < numInliers; ++sample) - { - const auto idx = inliers[sample]; - vec_weights[sample] = ErrorT::Error(model, this->x1_.col(idx), this->x2_.col(idx)); - // avoid division by zero - vec_weights[sample] = 1.0 / std::pow(std::max(eps, vec_weights[sample]), 2.0); - } - } - -}; - -/** - * @brief The kernel for the resection with known intrinsics (PnP) to be used with - * the LORansac framework. - * - * @tparam SolverArg The minimal solver able to find a solution from a - * minimum set of points, usually any PnP solver. - * @tparam ErrorArg The functor computing the error for each data sample with - * respect to the estimated model, usually a reprojection error functor. - * @tparam UnnormalizerArg The functor used to normalize the data before the - * estimation of the model, usually a functor that normalize the point in camera - * coordinates (ie multiply by the inverse of the calibration matrix). - * @tparam ModelArg = Mat34 The type of the model to estimate, the projection matrix. - * @tparam SolverLSArg = SolverArg The least square solver that is used to find - * a solution from any set of data larger than the minimum required, usually the - * 6 point algorithm which solves the resection problem by means of LS. - */ -template -class KernelAdaptorResectionLORansac_K : - public ACKernelAdaptorResection_K -{ - public: - typedef SolverArg Solver; - typedef ModelArg Model; - typedef ErrorArg ErrorT; - typedef SolverLSArg SolverLS; - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES, - MINIMUM_LSSAMPLES = SolverLS::MINIMUM_SAMPLES - }; - - KernelAdaptorResectionLORansac_K(const Mat &x2d, const Mat &x3D, const Mat3 & K) : - ACKernelAdaptorResection_K(x2d, x3D, K) - {} - - void FitLS(const std::vector &inliers, - std::vector *models, - const std::vector *weights = nullptr) const - { - const Mat x1 = ExtractColumns(this->x2d_, inliers); - const Mat x2 = ExtractColumns(this->x3D_, inliers); - SolverLS::Solve(x1, x2, models, weights); - } - - void computeWeights(const Model & model, - const std::vector &inliers, - std::vector & vec_weights, - const double eps = 0.001) const - { - const auto numInliers = inliers.size(); - vec_weights.resize(numInliers); - for(std::size_t sample = 0; sample < numInliers; ++sample) - { - const auto idx = inliers[sample]; - vec_weights[sample] = ErrorT::Error(model, this->x2d_.col(idx), this->x3D_.col(idx)); - // avoid division by zero - vec_weights[sample] = 1/std::pow(std::max(eps, vec_weights[sample]), 2); - } - } -}; - - - - -} // namespace robustEstimation -} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/LineKernel.hpp b/src/aliceVision/robustEstimation/LineKernel.hpp index d15d34eec1..8abc430363 100644 --- a/src/aliceVision/robustEstimation/LineKernel.hpp +++ b/src/aliceVision/robustEstimation/LineKernel.hpp @@ -8,90 +8,239 @@ #pragma once -#include "aliceVision/numeric/numeric.hpp" +#include +#include +#include namespace aliceVision { -namespace robustEstimation{ +namespace robustEstimation { -struct LineSolver +inline double pointToLineError(const Vec2& lineEq, const Vec2& xs) { - enum { MINIMUM_SAMPLES = 2 }; - enum { MAX_MODELS = 1 }; + const double b = lineEq[0]; + const double a = lineEq[1]; + const double x = xs[0]; + const double y = xs[1]; + const double e = y - (a * x + b); + return e*e; +} - static void Solve(const Mat &x, std::vector *lines) +/** + * @brief Embed the basic solver to fit from sampled point set + */ +class LineKernel : public IRansacKernel> +{ +public: + + using ModelT = robustEstimation::MatrixModel; + + explicit LineKernel(const Mat2X& xs) + : _xs(xs) + , _logalpha0(0) + {} + + LineKernel(const Mat2X& xs, int w, int h) + : _xs(xs) + { + // model error as point to line error + // ratio of containing diagonal image rectangle over image area + const double D = sqrt(w * w * 1.0 + h * h); // diameter + const double A = w * h; // area + _logalpha0 = log10(2.0 * D / A / 1.0); + } + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override { - Mat X(x.cols(), 2); + return 2; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + /** + * @brief Return the minimum number of required samples for the solver Ls + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamplesLS() const override + { + return 2; + } + + /** + * @brief The number of elements in the data. + * @return the number of elements in the data. + */ + inline std::size_t nbSamples() const override + { + return static_cast(_xs.cols()); + } + + /** + * @brief This function is called to estimate the model from the minimum number + * of sample \p minSample (i.e. minimal problem solver). + * @param[in] samples A vector containing the indices of the data to be used for + * the minimal estimation. + * @param[out] models The model(s) estimated by the minimal solver. + */ + void fit(const std::vector& samples, std::vector& models) const override + { + assert(samples.size() >= getMinimumNbRequiredSamples()); + + // standard least squares solution. + const Mat2X sampled_xs = ExtractColumns(_xs, samples); + + Mat X(sampled_xs.cols(), 2); X.col(0).setOnes(); - X.col(1) = x.row(0).transpose(); + X.col(1) = sampled_xs.row(0).transpose(); + const Mat A(X.transpose() * X); - const Vec b(X.transpose() * x.row(1).transpose()); + const Vec b(X.transpose() * sampled_xs.row(1).transpose()); + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + const Vec2 ba = svd.solve(b); - lines->push_back(ba); + + models.emplace_back(ba); } - - static void SolveWeightedLS(const Mat &x, std::vector *lines, const std::vector &weights) + + /** + * @brief This function is called to estimate the model using a least squared + * algorithm from a minumum of \p minSampleLS. + * @param[in] inliers An array containing the indices of the data to use. + * @param[out] models The model(s) estimated using the least squared algorithm. + * @param[in] weights An optional array of weights, one for each sample + */ + void fitLS(const std::vector& samples, std::vector& models, const std::vector* weights = nullptr) const override { - const std::size_t numPts = x.cols(); - assert(numPts == weights.size()); - + if(weights == nullptr) + { + fit(samples, models); + return; + } + + assert(samples.size() >= getMinimumNbRequiredSamples()); + + // standard least squares solution. + const Mat2X sampled_xs = ExtractColumns(_xs, samples); + + + const std::size_t numPts = sampled_xs.cols(); + + assert(numPts == weights->size()); + // create the weight matrix Mat W = Mat::Zero(numPts, numPts); for(std::size_t i = 0; i < numPts; ++i) - W(i,i) = weights[i]; + W(i,i) = (*weights)[i]; Mat X(numPts, 2); X.col(0).setOnes(); - X.col(1) = x.row(0).transpose(); + X.col(1) = sampled_xs.row(0).transpose(); const Mat A(X.transpose() * W * X); - const Vec b(X.transpose() * W * x.row(1).transpose()); + const Vec b(X.transpose() * W * sampled_xs.row(1).transpose()); Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); const Vec2 ba = svd.solve(b); - lines->push_back(ba); + models.emplace_back(ba); } -}; -struct pointToLineError -{ - static double Error(const Vec2 &lineEq, const Vec2 &xs) + /** + * @brief Function used to estimate the weights, typically used by the least square algorithm. + * @param[in] model The model against which the weights are computed. + * @param[in] inliers The array of the indices of the data to be used. + * @param[out] vec_weights The array of weight of the same size as \p inliers. + * @param[in] eps An optional threshold to max out the value of the threshold (typically + * to avoid division by zero or too small numbers). + */ + void computeWeights(const ModelT& model, const std::vector& inliers, std::vector& weights, const double eps = 0.001) const override { - const double b = lineEq[0]; - const double a = lineEq[1]; - const double x = xs[0]; - const double y = xs[1]; - const double e = y - (a * x + b); - return e*e; + const auto nbInliers = inliers.size(); + weights.resize(nbInliers); + for(std::size_t sample = 0; sample < nbInliers; ++sample) + { + const auto idx = inliers[sample]; + weights[sample] = error(idx, model); + + // avoid division by zero + weights[sample] = 1 / std::pow(std::max(eps, weights[sample]), 2); + } } -}; -// Embed the basic solver to fit from sampled point set -struct LineKernel -{ - typedef Vec2 Model; // line parametrization: a, b; - enum { MINIMUM_SAMPLES = 2 }; + /** + * @brief Function that computes the estimation error for a given model and a given element. + * @param[in] sample The index of the element for which the error is computed. + * @param[in] model The model to consider. + * @return The estimation error for the given element and the given model. + */ + inline double error(std::size_t sample, const ModelT& model) const override + { + // point to line error + const Vec2& xs = _xs.col(sample); + return pointToLineError(model.getMatrix(), xs); + } - LineKernel(const Mat2X &xs) : xs_(xs) {} + /** + * @brief Function that computes the estimation error for a given model and all the elements. + * @param[in] model The model to consider. + * @param[out] vec_errors The vector containing all the estimation errors for every element. + */ + inline void errors(const ModelT& model, std::vector& errors) const override + { + errors.resize(_xs.cols()); + for(std::size_t sample = 0; sample < _xs.cols(); ++sample) + errors.at(sample) = error(sample, model); + } + + /** + * @brief Function used to unnormalize the model. + * @param[in,out] model The model to unnormalize. + */ + inline void unnormalize(ModelT& model) const override + { + // nothing to do, model is left unchanged + } - size_t NumSamples() const { return static_cast (xs_.cols()); } + /** + * @brief Get logalpha0, Alpha0 is used to make the error adaptive to the image size + * @return logalpha0 + */ + inline double logalpha0() const override + { + return _logalpha0; + } - void Fit(const std::vector &samples, std::vector *lines) const + inline double multError() const override { - assert(samples.size() >= (unsigned int) MINIMUM_SAMPLES); - // Standard least squares solution. - const Mat2X sampled_xs = ExtractColumns(xs_, samples); + return 0.5; + } - LineSolver::Solve(sampled_xs, lines); + inline Mat3 normalizer1() const override + { + return Mat3::Identity(); } - double Error(size_t sample, const Vec2 &ba) const + inline Mat3 normalizer2() const override { - return pointToLineError::Error(ba, xs_.col(sample)); + return Mat3::Identity(); + } + + inline double unormalizeError(double val) const override + { + return std::sqrt(val); } - - void Unnormalize(Model * model) const - { } - const Mat2X &xs_; +private: + const Mat2X& _xs; + double _logalpha0; }; } // namespace robustEstimation diff --git a/src/aliceVision/robustEstimation/PointFittingKernel.hpp b/src/aliceVision/robustEstimation/PointFittingKernel.hpp new file mode 100644 index 0000000000..a992a377db --- /dev/null +++ b/src/aliceVision/robustEstimation/PointFittingKernel.hpp @@ -0,0 +1,179 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 robustEstimation { + +/** + * @brief This is one example (targeted at solvers that operate on correspondences + * between two views) that shows the "kernel" part of a robust fitting + * problem: + * + * 1. The model; Mat3Model in the case of the F or H matrix. + * 2. The minimum number of samples needed to fit; 7 or 8 (or 4). + * 3. A way to convert samples to a model. + * 4. A way to convert a sample and a model to an error. + * + * Of particular note is that the kernel does not expose what the samples are. + * All the robust fitting algorithm sees is that there is some number of + * samples; it is able to fit subsets of them (via the kernel) and check their + * error, but can never access the samples themselves. + * + * The Kernel objects must follow the following concept so that the robust + * fitting algorithm can fit this type of relation: + *ModelT + * 1. kernel.getMaximumNbModels() + * 2. kernel.getMinimumNbRequiredSamples() + * 3. kernel.fit(std::vector, std::vector&) + * 4. kernel.error(std::size_t, ModelT) -> error + * + * The fit routine must not clear existing entries in the vector of models; it + * should append new solutions to the end. + */ +template +class PointFittingKernel +{ +public: + + using SolverT = SolverT_; + using ErrorT = ErrorT_; + using ModelT = ModelT_; + + PointFittingKernel(const Mat& x1, const Mat& x2) + : _x1(x1) + , _x2(x2) + {} + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const + { + return _kernelSolver.getMinimumNbRequiredSamples(); + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const + { + return _kernelSolver.getMaximumNbModels(); + } + + /** + * @brief Extract required sample and fit model(s) to the sample + * @param[in] samples + * @param[out] models + */ + inline virtual void fit(const std::vector& samples, std::vector& models) const + { + const Mat x1 = ExtractColumns(_x1, samples); + const Mat x2 = ExtractColumns(_x2, samples); + _kernelSolver.solve(x1, x2, models); + } + + /** + * @brief Return the error associated to the model and a sample point + * @param[in] sample + * @param[in] model + * @return error value + */ + inline virtual double error(std::size_t sample, const ModelT& model) const + { + return _errorEstimator.error(model, _x1.col(sample), _x2.col(sample)); + } + + /** + * @brief Return the errors associated to the model and each sample point + * @param[in] model + * @param[out] errors + */ + inline virtual void errors(const ModelT& model, std::vector& errors) const + { + errors.resize(_x1.cols()); + for(std::size_t sample = 0; sample < _x1.cols(); ++sample) + errors.at(sample) = error(sample, model); + } + + /** + * @brief get the number of putative points + * @return number of putative points + */ + inline std::size_t nbSamples() const + { + return _x1.cols(); + } + +protected: + + /// left corresponding data + const Mat& _x1; + /// right corresponding data + const Mat& _x2; + /// two view solver + const SolverT _kernelSolver = SolverT(); + /// solver error estimation + const ErrorT _errorEstimator = ErrorT(); +}; + +template +class NormalizedPointFittingKernel : public PointFittingKernel +{ +public: + + using KernelBase = PointFittingKernel; + + NormalizedPointFittingKernel(const Mat& x1, const Mat& x2) + : KernelBase(x1, x2) + {} + + /** + * @brief Extract required sample and fit model(s) to the sample + * @param[in] samples + * @param[out] models + */ + inline void fit(const std::vector& samples, std::vector& models) const override + { + const Mat x1 = ExtractColumns(KernelBase::_x1, samples); + const Mat x2 = ExtractColumns(KernelBase::_x2, samples); + + assert(2 == x1.rows()); + assert(KernelBase::getMinimumNbRequiredSamples() <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // normalize the data. + Mat3 T1; + Mat3 T2; + Mat x1_normalized; + Mat x2_normalized; + + normalizePoints(x1, &x1_normalized, &T1); + normalizePoints(x2, &x2_normalized, &T2); + + KernelBase::_kernelSolver.solve(x1_normalized, x2_normalized, models); + + // unnormalize model from the computed conditioning. + for(int i = 0; i < models.size(); ++i) + UnnormalizerT_::unnormalize(T1, T2, &(models.at(i).getMatrix())); + } +}; + +} // namespace robustEstimation +} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/PointFittingRansacKernel.hpp b/src/aliceVision/robustEstimation/PointFittingRansacKernel.hpp new file mode 100644 index 0000000000..2a55c0956b --- /dev/null +++ b/src/aliceVision/robustEstimation/PointFittingRansacKernel.hpp @@ -0,0 +1,156 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2019 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 robustEstimation { + +/** + * @brief A virtual kernel used for the ACRANSAC / LORANSAC framework. + * @tparam SolverT_ The minimal solver able to find a solution from a minimum set of points. + * @tparam ErrorT_ The functor computing the error for each data sample with respect to the estimated model. + * @tparam UnnormalizerT_ The functor used to normalize the data before the estimation of the model. + * @tparam ModelT_ = Mat34Model The type of the model to estimate. + * @tparam SolverLsT_ = SolverT The least square solver that is used to find a solution from any set of + * data larger than the minimum required. + * + * @note Handle data normalization and compute the corresponding logalpha 0 + * that depends of the error model (point to line, or point to point) + */ +template > +class PointFittingRansacKernel + : public robustEstimation::IRansacKernel + , public robustEstimation::PointFittingKernel +{ +public: + + using KernelBase = robustEstimation::PointFittingKernel; + + PointFittingRansacKernel(const Mat& x1, const Mat& x2) + : KernelBase(x1, x2) + {} + + /** + * @brief Return the minimum number of required samples for the solver + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamples() const override + { + return KernelBase::getMinimumNbRequiredSamples(); + } + + std::size_t getMinimumNbRequiredSamplesLS() const override + { + return _solverLs.getMinimumNbRequiredSamples(); + } + + /** + * @brief Return the maximum number of models for the solver + * @return maximum number of models + */ + std::size_t getMaximumNbModels() const override + { + return KernelBase::getMaximumNbModels(); + } + + /** + * @brief This function is called to estimate the model from the minimum number + * of sample \p minSample (i.e. minimal problem solver). + * @param[in] samples A vector containing the indices of the data to be used for + * the minimal estimation. + * @param[out] models The model(s) estimated by the minimal solver. + */ + void fit(const std::vector& samples, std::vector& models) const override + { + KernelBase::fit(samples, models); + } + + + void fitLS(const std::vector& inliers, std::vector& models, const std::vector* weights = nullptr) const override + { + const Mat x1 = ExtractColumns(KernelBase::_x1, inliers); + const Mat x2 = ExtractColumns(KernelBase::_x2, inliers); + + if(weights == nullptr) + _solverLs.solve(x1, x2, models); + else + _solverLs.solve(x1, x2, models, *weights); + } + + void computeWeights(const ModelT_& model, const std::vector& inliers, std::vector& weights, const double eps = 0.001) const override + { + const auto numInliers = inliers.size(); + weights.resize(numInliers); + for(std::size_t sample = 0; sample < numInliers; ++sample) + { + const auto idx = inliers[sample]; + weights[sample] = KernelBase::_errorEstimator.error(model, KernelBase::_x1.col(idx), KernelBase::_x2.col(idx)); + // avoid division by zero + weights[sample] = 1/std::pow(std::max(eps, weights[sample]), 2); + } + } + + /** + * @brief Function that computes the estimation error for a given model and a given element. + * @param[in] sample The index of the element for which the error is computed. + * @param[in] model The model to consider. + * @return The estimation error for the given element and the given model. + */ + double error(std::size_t sample, const ModelT_& model) const override + { + return KernelBase::error(sample, model); + } + + /** + * @brief Function that computes the estimation error for a given model and all the elements. + * @param[in] model The model to consider. + * @param[out] vec_errors The vector containing all the estimation errors for every element. + */ + void errors(const ModelT_& model, std::vector& errors) const override + { + KernelBase::errors(model, errors); + } + + /** + * @brief Function used to unnormalize the model. + * @param[in,out] model The model to unnormalize. + */ + virtual void unnormalize(ModelT_& model) const = 0; + + /** + * @brief The number of elements in the data. + * @return the number of elements in the data. + */ + std::size_t nbSamples() const override + { + return KernelBase::nbSamples(); + } + + /** + * @brief Get logalpha0, Alpha0 is used to make the error adaptive to the image size + * @return logalpha0 + */ + virtual double logalpha0() const = 0; + + virtual double multError() const = 0; + virtual double unormalizeError(double val) const = 0; + virtual Mat3 normalizer1() const = 0; + virtual Mat3 normalizer2() const = 0; + +private: + const SolverLsT_ _solverLs = SolverLsT_(); +}; + +} // namespace robustEstimation +} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/Ransac.hpp b/src/aliceVision/robustEstimation/Ransac.hpp index c420a10bc4..26ce6aa2ee 100644 --- a/src/aliceVision/robustEstimation/Ransac.hpp +++ b/src/aliceVision/robustEstimation/Ransac.hpp @@ -27,31 +27,31 @@ namespace robustEstimation{ // 3. A way to convert samples to a model. // 4. A way to convert samples and a model to an error. // -// 1. Kernel::Model -// 2. Kernel::MINIMUM_SAMPLES -// 3. Kernel::Fit(vector, vector *) -// 4. Kernel::Error(Model, int) -> error +// 1. Kernel::ModelT +// 2. Kernel::getMinimumNbRequiredSamples() +// 3. Kernel::fit(vector, vector *) +// 4. Kernel::error(Model, int) -> error template -typename Kernel::Model RANSAC( - const Kernel &kernel, - const Scorer &scorer, - std::vector *best_inliers = nullptr, - double *best_score = nullptr, +typename Kernel::ModelT RANSAC( + const Kernel& kernel, + const Scorer& scorer, + std::vector* best_inliers = nullptr, + double* best_score = nullptr, bool bVerbose = true, double outliers_probability = 1e-2) { assert(outliers_probability < 1.0); assert(outliers_probability > 0.0); size_t iteration = 0; - const size_t min_samples = Kernel::MINIMUM_SAMPLES; - const size_t total_samples = kernel.NumSamples(); + const size_t min_samples = kernel.getMinimumNbRequiredSamples(); + const size_t total_samples = kernel.nbSamples(); size_t max_iterations = 100; const size_t really_max_iterations = 4096; size_t best_num_inliers = 0; double best_inlier_ratio = 0.0; - typename Kernel::Model best_model; + typename Kernel::ModelT best_model; // Test if we have sufficient points for the kernel. if (total_samples < min_samples) @@ -72,16 +72,16 @@ typename Kernel::Model RANSAC( iteration < really_max_iterations; ++iteration) { std::vector sample; - UniformSample(min_samples, total_samples, sample); + uniformSample(min_samples, total_samples, sample); - std::vector models; - kernel.Fit(sample, &models); + std::vector models; + kernel.fit(sample, models); // Compute the inlier list for each fit. for (size_t i = 0; i < models.size(); ++i) { std::vector inliers; - scorer.Score(kernel, models[i], all_samples, &inliers); + scorer.score(kernel, models[i], all_samples, inliers); if (best_num_inliers < inliers.size()) { @@ -101,7 +101,7 @@ typename Kernel::Model RANSAC( } if (best_inlier_ratio) { - max_iterations = IterationsRequired(min_samples, + max_iterations = iterationsRequired(min_samples, outliers_probability, best_inlier_ratio); if(bVerbose) @@ -114,7 +114,7 @@ typename Kernel::Model RANSAC( *best_score = best_num_inliers; if(best_num_inliers) - kernel.Unnormalize(&best_model); + kernel.unnormalize(best_model); return best_model; } diff --git a/src/aliceVision/robustEstimation/ScoreEvaluator.hpp b/src/aliceVision/robustEstimation/ScoreEvaluator.hpp index 4e071140be..1e5c5839d1 100644 --- a/src/aliceVision/robustEstimation/ScoreEvaluator.hpp +++ b/src/aliceVision/robustEstimation/ScoreEvaluator.hpp @@ -12,27 +12,32 @@ namespace robustEstimation{ using namespace std; -/// Templated Functor class to evaluate a given model over a set of samples. +/** + * @brief Templated Functor class to evaluate a given model over a set of samples. + */ template -class ScoreEvaluator { +class ScoreEvaluator +{ public: - ScoreEvaluator(double threshold) : threshold_(threshold) {} + explicit ScoreEvaluator(double threshold) + : _threshold(threshold) + {} template - double Score(const Kernel &kernel, - const typename Kernel::Model &model, - const std::vector &samples, - std::vector *inliers, + double score(const Kernel& kernel, + const typename Kernel::ModelT& model, + const std::vector& samples, + std::vector& inliers, double threshold) const { double cost = 0.0; - for (size_t j = 0; j < samples.size(); ++j) + for(std::size_t j = 0; j < samples.size(); ++j) { - double error = kernel.Error(samples[j], model); + double error = kernel.error(samples.at(j), model); if (error < threshold) { cost += error; - inliers->push_back(samples[j]); + inliers.push_back(samples[j]); } else { @@ -44,18 +49,18 @@ class ScoreEvaluator { } template - double Score(const Kernel &kernel, - const typename Kernel::Model &model, - const std::vector &samples, - std::vector *inliers) const + double score(const Kernel &kernel, + const typename Kernel::ModelT& model, + const std::vector& samples, + std::vector& inliers) const { - return Score(kernel, model, samples, inliers, threshold_); + return score(kernel, model, samples, inliers, _threshold); } - double getThreshold() const {return threshold_;} + double getThreshold() const {return _threshold;} private: - double threshold_; + double _threshold; }; } // namespace robustEstimation diff --git a/src/aliceVision/robustEstimation/acRansac_test.cpp b/src/aliceVision/robustEstimation/acRansac_test.cpp index accdc57f7e..a15b41a6ae 100644 --- a/src/aliceVision/robustEstimation/acRansac_test.cpp +++ b/src/aliceVision/robustEstimation/acRansac_test.cpp @@ -5,145 +5,51 @@ // 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 "lineTestGenerator.hpp" -#include "dependencies/vectorGraphics/svgDrawer.hpp" +#include -#define BOOST_TEST_MODULE ACRansac +#define BOOST_TEST_MODULE robustEstimation_AC_Ransac #include #include +#include +#include + using namespace svg; using namespace aliceVision; using namespace aliceVision::robustEstimation; -using namespace std; - -/// ACRansac Kernel for line estimation - -template -class ACRANSACOneViewKernel -{ -public: - typedef SolverArg Solver; - typedef ModelArg Model; - - ACRANSACOneViewKernel(const Mat &x1, int w1, int h1) - : x1_(x1), N1_(Mat3::Identity()), logalpha0_(0.0) - { - assert(2 == x1_.rows()); - - // Model error as point to line error - // Ratio of containing diagonal image rectangle over image area - const double D = sqrt(w1 * w1 * 1.0 + h1 * h1); // diameter - const double A = w1 * h1; // area - logalpha0_ = log10(2.0 * D / A / 1.0); - } - - enum - { - MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES - }; - - enum - { - MAX_MODELS = Solver::MAX_MODELS - }; - - void Fit(const std::vector &samples, std::vector *models) const - { - const Mat sampled_xs = ExtractColumns(x1_, samples); - Solver::Solve(sampled_xs, models); - } - - double Error(std::size_t sample, const Model &model) const - { - return ErrorArg::Error(model, x1_.col(sample)); - } - - void Errors(const Model &model, std::vector & vec_errors) const - { - for(std::size_t sample = 0; sample < x1_.cols(); ++sample) - vec_errors[sample] = ErrorArg::Error(model, x1_.col(sample)); - } - - std::size_t NumSamples() const - { - return x1_.cols(); - } - - void Unnormalize(Model * model) const - { - // Model is left unchanged - } - - double logalpha0() const - { - return logalpha0_; - } - - double multError() const - { - return 0.5; - } - - Mat3 normalizer1() const - { - return Mat3::Identity(); - } - - Mat3 normalizer2() const - { - return Mat3::Identity(); - } - - double unormalizeError(double val) const - { - return sqrt(val); - } - -private: - Mat x1_; - Mat3 N1_; - double logalpha0_; -}; - -// Test ACRANSAC with the AC-adapted Line kernel in a noise/outlier free dataset +// test ACRANSAC with the AC-adapted Line kernel in a noise/outlier free dataset BOOST_AUTO_TEST_CASE(RansacLineFitter_OutlierFree) { - Mat2X xy(2, 5); + // y = 2x + 1 xy << 1, 2, 3, 4, 5, - 3, 5, 7, 9, 11; + 3, 5, 7, 9, 11; // The base estimator - ACRANSACOneViewKernel lineKernel(xy, 12, 12); + LineKernel lineKernel(xy, 12, 12); - // Check the best model that fit the most of the data - // in a robust framework (ACRANSAC). - std::vector vec_inliers; - Vec2 line; - ACRANSAC(lineKernel, vec_inliers, 300, &line); + // check the best model that fit the most of the data in a robust framework (ACRANSAC). + std::vector inliers; - BOOST_CHECK_SMALL(2.0-line[1], 1e-9); - BOOST_CHECK_SMALL(1.0-line[0], 1e-9); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); -} + robustEstimation::MatrixModel model; + ACRANSAC(lineKernel, inliers, 300, &model); -// Simple test without getting back the model + BOOST_CHECK_SMALL(2.0 - model.getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - model.getMatrix()[0], 1e-9); + BOOST_CHECK_EQUAL(5, inliers.size()); +} +// test without getting back the model BOOST_AUTO_TEST_CASE(RansacLineFitter_OutlierFree_DoNotGetBackModel) { @@ -152,11 +58,12 @@ BOOST_AUTO_TEST_CASE(RansacLineFitter_OutlierFree_DoNotGetBackModel) xy << 1, 2, 3, 4, 5, 3, 5, 7, 9, 11; - ACRANSACOneViewKernel lineKernel(xy, 12, 12); - std::vector vec_inliers; - ACRANSAC(lineKernel, vec_inliers); + LineKernel lineKernel(xy, 12, 12); + std::vector inliers; - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + ACRANSAC(lineKernel, inliers); + + BOOST_CHECK_EQUAL(5, inliers.size()); } BOOST_AUTO_TEST_CASE(RansacLineFitter_OneOutlier) @@ -168,41 +75,41 @@ BOOST_AUTO_TEST_CASE(RansacLineFitter_OneOutlier) 3, 5, 7, 9, 11, -123; // The base estimator - ACRANSACOneViewKernel lineKernel(xy, 12, 12); + LineKernel lineKernel(xy, 12, 12); // Check the best model that fit the most of the data // in a robust framework (ACRANSAC). - std::vector vec_inliers; - Vec2 line; - ACRANSAC(lineKernel, vec_inliers, 300, &line); + std::vector inliers; + robustEstimation::MatrixModel model; + + ACRANSAC(lineKernel, inliers, 300, &model); - BOOST_CHECK_SMALL(2.0-line[1], 1e-9); - BOOST_CHECK_SMALL(1.0-line[0], 1e-9); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + BOOST_CHECK_SMALL(2.0 - model.getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - model.getMatrix()[0], 1e-9); + BOOST_CHECK_EQUAL(5, inliers.size()); } -// Test if the robust estimator do not return inlier if too few point +// test if the robust estimator do not return inlier if too few point // was given for an estimation. - BOOST_AUTO_TEST_CASE(RansacLineFitter_TooFewPoints) { Vec2 xy; // y = 2x + 1 xy << 1, 2; - ACRANSACOneViewKernel lineKernel(xy, 12, 12); - std::vector vec_inliers; - ACRANSAC(lineKernel, vec_inliers); + LineKernel lineKernel(xy, 12, 12); + std::vector inliers; + + ACRANSAC(lineKernel, inliers); - BOOST_CHECK_EQUAL(0, vec_inliers.size()); + BOOST_CHECK_EQUAL(0, inliers.size()); } -// From a GT model : +// from a GT model : // Compute a list of point that fit the model. // Add white noise to given amount of points in this list. // Check that the number of inliers and the model are correct. - BOOST_AUTO_TEST_CASE(RansacLineFitter_RealisticCase) { @@ -236,22 +143,22 @@ BOOST_AUTO_TEST_CASE(RansacLineFitter_RealisticCase) } // The base estimator - ACRANSACOneViewKernel lineKernel(xy, 12, 12); + LineKernel lineKernel(xy, 12, 12); // Check the best model that fit the most of the data // in a robust framework (ACRANSAC). - std::vector vec_inliers; - Vec2 line; - ACRANSAC(lineKernel, vec_inliers, 300, &line); + std::vector inliers; + robustEstimation::MatrixModel model; - BOOST_CHECK_EQUAL(NbPoints - nbPtToNoise, vec_inliers.size()); - BOOST_CHECK_SMALL(GTModel(0)-line[0], 1e-9); - BOOST_CHECK_SMALL(GTModel(1)-line[1], 1e-9); -} + ACRANSAC(lineKernel, inliers, 300, &model); -// Generate nbPoints along a line and add gaussian noise. -// Move some point in the dataset to create outlier contamined data + BOOST_CHECK_EQUAL(NbPoints - nbPtToNoise, inliers.size()); + BOOST_CHECK_SMALL(GTModel(0) - model.getMatrix()[0], 1e-9); + BOOST_CHECK_SMALL(GTModel(1) - model.getMatrix()[1], 1e-9); +} +// generate nbPoints along a line and add gaussian noise. +// move some point in the dataset to create outlier contamined data void generateLine(Mat & points, std::size_t nbPoints, int W, int H, float noise, float outlierRatio) { points = Mat(2, nbPoints); @@ -311,11 +218,10 @@ struct IndMatchd double _i, _j; }; -// Test ACRANSAC adaptability to noise -// Set a line with a increasing gaussian noise -// See if the AContrario RANSAC is able to label the good point as inlier +// test ACRANSAC adaptability to noise +// set a line with a increasing gaussian noise +// see if the AContrario RANSAC is able to label the good point as inlier // by having it's estimated confidence threshold growing. - BOOST_AUTO_TEST_CASE(RansacLineFitter_ACRANSACSimu) { @@ -342,19 +248,19 @@ BOOST_AUTO_TEST_CASE(RansacLineFitter_ACRANSACSimu) std::vector vec_inliersGT; generateLine(numPoints, outlierRatio, gaussianNoiseLevel, GTModel, gen, points, vec_inliersGT); // robust line estimation - Vec2 line; + robustEstimation::MatrixModel model; // The base estimator - ACRANSACOneViewKernel lineKernel(points, W, H); + LineKernel lineKernel(points, W, H); // Check the best model that fit the most of the data // in a robust framework (ACRANSAC). std::vector vec_inliers; - const std::pair ret = ACRANSAC(lineKernel, vec_inliers, 1000, &line); + const std::pair ret = ACRANSAC(lineKernel, vec_inliers, 1000, &model); const double errorMax = ret.first; - cout << "gaussianNoiseLevel " << gaussianNoiseLevel << " \tsqrt(errorMax) " << sqrt(errorMax) << std::endl; - cout << "line " << line << std::endl; + std::cout << "gaussianNoiseLevel " << gaussianNoiseLevel << " \tsqrt(errorMax) " << sqrt(errorMax) << std::endl; + std::cout << "line " << model.getMatrix() << std::endl; const std::size_t expectedInliers = vec_inliersGT.size(); // ACRansac is not really repeatable so we can check at least it does not diff --git a/src/aliceVision/multiview/conditioning.cpp b/src/aliceVision/robustEstimation/conditioning.cpp similarity index 68% rename from src/aliceVision/multiview/conditioning.cpp rename to src/aliceVision/robustEstimation/conditioning.cpp index 2bdc735931..e15981ca56 100644 --- a/src/aliceVision/multiview/conditioning.cpp +++ b/src/aliceVision/robustEstimation/conditioning.cpp @@ -9,9 +9,10 @@ #include "conditioning.hpp" namespace aliceVision { +namespace robustEstimation { // HZ 4.4.4 pag.109 -void PreconditionerFromPoints(const Mat &points, Mat3 *T) { +void preconditionerFromPoints(const Mat &points, Mat3 *T) { Vec mean, variance; MeanAndVarianceAlongRows(points, &mean, &variance); @@ -30,7 +31,7 @@ void PreconditionerFromPoints(const Mat &points, Mat3 *T) { 0, 0, 1; } -void PreconditionerFromImageSize(int width, int height, Mat3 *T) { +void preconditionerFromImageSize(int width, int height, Mat3 *T) { // Build the normalization matrix double dNorm = 1.0 / sqrt( static_cast(width*height) ); @@ -40,9 +41,10 @@ void PreconditionerFromImageSize(int width, int height, Mat3 *T) { (*T)(1,2) = -.5*height*dNorm; } -void ApplyTransformationToPoints(const Mat &points, +void applyTransformationToPoints(const Mat &points, const Mat3 &T, - Mat *transformed_points) { + Mat *transformed_points) +{ const Mat::Index n = points.cols(); transformed_points->resize(2,n); for (Mat::Index i = 0; i < n; ++i) { @@ -54,31 +56,22 @@ void ApplyTransformationToPoints(const Mat &points, } } -void NormalizePointsFromImageSize(const Mat &points, +void normalizePointsFromImageSize(const Mat &points, Mat *normalized_points, Mat3 *T, int width, int height) { - PreconditionerFromImageSize(width, height, T); - ApplyTransformationToPoints(points, *T, normalized_points); + preconditionerFromImageSize(width, height, T); + applyTransformationToPoints(points, *T, normalized_points); } -void NormalizePoints(const Mat &points, +void normalizePoints(const Mat &points, Mat *normalized_points, Mat3 *T) { - PreconditionerFromPoints(points, T); - ApplyTransformationToPoints(points, *T, normalized_points); -} - -// Denormalize the results. See HZ page 109. -void UnnormalizerT::Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) { - *H = T2.transpose() * (*H) * T1; -} - -// Denormalize the results. See HZ page 109. -void UnnormalizerI::Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) { - *H = T2.inverse() * (*H) * T1; + preconditionerFromPoints(points, T); + applyTransformationToPoints(points, *T, normalized_points); } +} // namespace robustEstimation } // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/conditioning.hpp b/src/aliceVision/robustEstimation/conditioning.hpp new file mode 100644 index 0000000000..053da3ccd8 --- /dev/null +++ b/src/aliceVision/robustEstimation/conditioning.hpp @@ -0,0 +1,49 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2016 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2010 libmv 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 + +/** + * @brief Implementation of normalized coordinates. + * Normalization improve accuracy of results and provide benefits + * that make scale and coordinate origin invariant. + * The implementation follows @see Algorithm 4.2 from HZ page 109. + */ + +namespace aliceVision { +namespace robustEstimation { + +/** + * @brief Point conditioning + */ +void preconditionerFromPoints(const Mat& points, Mat3* T); + +/** + * @brief Normalize input point for a given T transform matrix + */ +void applyTransformationToPoints(const Mat& points, const Mat3& T, Mat* transformed_points); + +/** + * @brief Normalize point in [-.5, .5] and return transformation matrix + */ +void normalizePoints(const Mat& points, Mat* normalized_points, Mat3* T); + +/** + * @brief Point conditioning (compute Transformation matrix) + */ +void preconditionerFromImageSize(int width, int height, Mat3 *T); + +/** + * @brief Normalize point rom image coordinates to [-.5, .5] + */ +void normalizePointsFromImageSize(const Mat& points, Mat* normalized_points, Mat3* T, int width, int height); + +} //namespace robustEstimation +} //namespace aliceVision diff --git a/src/aliceVision/robustEstimation/estimators.hpp b/src/aliceVision/robustEstimation/estimators.hpp index aecb1846c7..4190b2505e 100644 --- a/src/aliceVision/robustEstimation/estimators.hpp +++ b/src/aliceVision/robustEstimation/estimators.hpp @@ -7,9 +7,10 @@ #pragma once +#include + #include #include -#include namespace aliceVision { namespace robustEstimation { diff --git a/src/aliceVision/robustEstimation/guidedMatching.hpp b/src/aliceVision/robustEstimation/guidedMatching.hpp deleted file mode 100644 index 181b6a102d..0000000000 --- a/src/aliceVision/robustEstimation/guidedMatching.hpp +++ /dev/null @@ -1,460 +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 "aliceVision/numeric/numeric.hpp" -#include "aliceVision/matching/IndMatch.hpp" -#include "aliceVision/feature/RegionsPerView.hpp" - -#include "aliceVision/feature/Regions.hpp" -#include "aliceVision/camera/IntrinsicBase.hpp" - -#include - -namespace aliceVision { -namespace robustEstimation { - -/** - * @brief Guided Matching (features only): - * Use a model to find valid correspondences: - * Keep the best corresponding points for the given model under the - * user specified distance. - */ -template< - typename ModelArg, // The used model type - typename ErrorArg> // The metric to compute distance to the model -void GuidedMatching( - const ModelArg & mod, // The model - const Mat & xLeft, // The left data points - const Mat & xRight, // The right data points - double errorTh, // Maximal authorized error threshold - matching::IndMatches & out_validMatches) // Ouput corresponding index -{ - assert(xLeft.rows() == xRight.rows()); - - // Looking for the corresponding points that have - // the smallest distance (smaller than the provided Threshold) - for (Mat::Index i = 0; i < xLeft.cols(); ++i) - { - double min = std::numeric_limits::max(); - matching::IndMatch match; - for(Mat::Index j = 0; j < xRight.cols(); ++j) - { - // Compute the geometric error: error to the model - const double err = ErrorArg::Error( - mod, // The model - xLeft.col(i), xRight.col(j)); // The corresponding points - // if smaller error update corresponding index - if(err < errorTh && err < min) - { - min = err; - match = matching::IndMatch(i, j); - } - } - if(min < errorTh) - { - // save the best corresponding index - out_validMatches.push_back(match); - } - } - - // Remove duplicates (when multiple points at same position exist) - matching::IndMatch::getDeduplicated(out_validMatches); -} - - -// Struct to help filtering of correspondence according update of -// two smallest distance. -// -> useful for descriptor distance ratio filtering -template -struct distanceRatio -{ - DistT bd, sbd; // best and second best distance - std::size_t idx; // best corresponding index - - distanceRatio(): - bd(std::numeric_limits::max()), - sbd(std::numeric_limits::max()), - idx(0) - { } - - // Update match according the provided distance - inline bool update(std::size_t index, DistT dist) - { - if(dist < bd) // best than any previous - { - idx = index; - // update and swap - sbd = dist; - std::swap(bd, sbd); - return true; - } - else if(dist < sbd) - { - sbd = dist; - return true; - } - return false; - } - - // Return if the ratio of distance is ok or not - - inline bool isValid(const double distRatio) const - { - // check: - // - that two best distance have been found - // - the distance ratio - return (sbd != std::numeric_limits::max() - && bd < distRatio * sbd); - } -}; - -/// Guided Matching (features + descriptors with distance ratio): -/// Use a model to find valid correspondences: -/// Keep the best corresponding points for the given model under the -/// user specified distance ratio. -template< - typename ModelArg, // The used model type - typename ErrorArg, // The metric to compute distance to the model - typename DescriptorT, // The descriptor type - typename MetricT > // The metric to compare two descriptors -void GuidedMatching( - const ModelArg & mod, // The model - const Mat & xLeft, // The left data points - const std::vector & lDescriptors, - const Mat & xRight, // The right data points - const std::vector & rDescriptors, - double errorTh, // Maximal authorized error threshold - double distRatio, // Maximal authorized distance ratio - matching::IndMatches & vec_corresponding_index) // Ouput corresponding index -{ - assert(xLeft.rows() == xRight.rows()); - assert(xLeft.cols() == lDescriptors.size()); - assert(xRight.cols() == rDescriptors.size()); - - MetricT metric; - - // Looking for the corresponding points that have to satisfy: - // 1. a geometric distance below the provided Threshold - // 2. a distance ratio between descriptors of valid geometric correspondencess - - for(Mat::Index i = 0; i < xLeft.cols(); ++i) - { - - distanceRatio dR; - for(Mat::Index j = 0; j < xRight.cols(); ++j) - { - // Compute the geometric error: error to the model - const double geomErr = ErrorArg::Error(mod, // The model - xLeft.col(i), - xRight.col(j)); // The corresponding points - if(geomErr < errorTh) - { - const typename MetricT::ResultType descDist = - metric(lDescriptors[i].getData(), rDescriptors[j].getData(), DescriptorT::static_size); - // Update the corresponding points & distance (if required) - dR.update(j, descDist); - } - } - // Add correspondence only iff the distance ratio is valid - if(dR.isValid(distRatio)) - { - // save the best corresponding index - vec_corresponding_index.emplace_back(i, dR.idx); - } - } - - // Remove duplicates (when multiple points at same position exist) - matching::IndMatch::getDeduplicated(vec_corresponding_index); -} - -/** - * @brief Guided Matching (features + descriptors with distance ratio): - * Use a model to find valid correspondences: - * Keep the best corresponding points for the given model under the - * user specified distance ratio. - */ - -template< -typename ModelArg, // The used model type -typename ErrorArg // The metric to compute distance to the model -> -void GuidedMatching( - const ModelArg & mod, // The model - const camera::IntrinsicBase * camL, // Optional camera (in order to undistord on the fly feature positions, can be NULL) - const feature::Regions & lRegions, // regions (point features & corresponding descriptors) - const camera::IntrinsicBase * camR, // Optional camera (in order to undistord on the fly feature positions, can be NULL) - const feature::Regions & rRegions, // regions (point features & corresponding descriptors) - double errorTh, // Maximal authorized error threshold - double distRatio, // Maximal authorized distance ratio - matching::IndMatches & out_matches) // Ouput corresponding index -{ - // Looking for the corresponding points that have to satisfy: - // 1. a geometric distance below the provided Threshold - // 2. a distance ratio between descriptors of valid geometric correspondencess - - // Build region positions arrays (in order to un-distord on-demand point position once) - std::vector lRegionsPos(lRegions.RegionCount()); - std::vector rRegionsPos(rRegions.RegionCount()); - - if(camL && camL->isValid()) - { - for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) - lRegionsPos[i] = camL->get_ud_pixel(lRegions.GetRegionPosition(i)); - } - else - { - for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) - lRegionsPos[i] = lRegions.GetRegionPosition(i); - } - if(camR && camR->isValid()) - { - for(std::size_t i = 0; i < rRegions.RegionCount(); ++i) - rRegionsPos[i] = camR->get_ud_pixel(rRegions.GetRegionPosition(i)); - } - else - { - for(std::size_t i = 0; i < rRegions.RegionCount(); ++i) - rRegionsPos[i] = rRegions.GetRegionPosition(i); - } - - for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) - { - distanceRatio dR; - for(std::size_t j = 0; j < rRegions.RegionCount(); ++j) - { - // Compute the geometric error: error to the model - const double geomErr = ErrorArg::Error(mod, // The model - // The corresponding points - lRegionsPos[i], - rRegionsPos[j]); - if(geomErr < errorTh) - { - // Update the corresponding points & distance (if required) - dR.update(j, lRegions.SquaredDescriptorDistance(i, &rRegions, j)); - } - } - // Add correspondence only iff the distance ratio is valid - if(dR.isValid(distRatio)) - { - // save the best corresponding index - out_matches.emplace_back(i, dR.idx); - } - } - - // Remove duplicates (when multiple points at same position exist) - matching::IndMatch::getDeduplicated(out_matches); -} - - -/** - * @brief Guided Matching (features + descriptors with distance ratio): - * Use a model to find valid correspondences: - * Keep the best corresponding points for the given model under the - * user specified distance ratio. - */ -template< - typename ModelArg, // The used model type - typename ErrorArg // The metric to compute distance to the model - > -void GuidedMatching( - const ModelArg & mod, // The model - const camera::IntrinsicBase * camL, // Optional camera (in order to undistord on the fly feature positions, can be NULL) - const feature::MapRegionsPerDesc & lRegions, // regions (point features & corresponding descriptors) - const camera::IntrinsicBase * camR, // Optional camera (in order to undistord on the fly feature positions, can be NULL) - const feature::MapRegionsPerDesc & rRegions, // regions (point features & corresponding descriptors) - double errorTh, // Maximal authorized error threshold - double distRatio, // Maximal authorized distance ratio - matching::MatchesPerDescType & out_matchesPerDesc) // Ouput corresponding index -{ - const std::vector descTypes = getCommonDescTypes(lRegions, rRegions); - if(descTypes.empty()) - return; - - for(const feature::EImageDescriberType descType: descTypes) - { - GuidedMatching(mod, camL, *lRegions.at(descType), camR, *rRegions.at(descType), errorTh, distRatio, out_matchesPerDesc[descType]); - } -} - -/// Compute a bucket index from an epipolar point -/// (the one that is closer to image border intersection) -inline unsigned int pix_to_bucket(const Vec2i &x, int W, int H) -{ - if(x(1) == 0) return x(0); // Top border - if(x(0) == W - 1) return W - 1 + x(1); // Right border - if(x(1) == H - 1) return 2 * W + H - 3 - x(0); // Bottom border - return 2 * (W + H - 2) - x(1); // Left border -} - -/// Compute intersection of the epipolar line with the image border -inline bool line_to_endPoints(const Vec3 & line, int W, int H, Vec2 & x0, Vec2 & x1) -{ - const double a = line(0), b = line(1), c = line(2); - - float r1, r2; - // Intersection with Y axis (0 or W-1) - if(b != 0) - { - double x = (b < 0) ? 0 : W - 1; - double y = -(a * x + c) / b; - if(y < 0) y = 0.; - else if(y >= H) y = H - 1; - r1 = fabs(a * x + b * y + c); - x0 << x, y; - } - else - { - return false; - } - - // Intersection with X axis (0 or H-1) - if(a != 0) - { - double y = (a < 0) ? H - 1 : 0; - double x = -(b * y + c) / a; - if(x < 0) x = 0.; - else if(x >= W) x = W - 1; - r2 = fabs(a * x + b * y + c); - x1 << x, y; - } - else - { - return false; - } - - // Choose x0 to be as close as the intersection axis - if(r1 > r2) - std::swap(x0, x1); - - return true; -} - -/// Guided Matching (features + descriptors with distance ratio): -/// Cluster correspondences per epipolar line (faster than exhaustive search). -/// Keep the best corresponding points for the given model under the -/// user specified distance ratio. -/// Can be seen as a variant of robustEstimation method [1]. -/// Note that implementation done here use a pixel grid limited to image border. -/// -/// [1] Rajvi Shah, Vanshika Shrivastava, and P J Narayanan -/// Geometry-aware Feature Matching for Structure from Motion Applications. -/// WACV 2015. -template // The used model type -void GuidedMatching_Fundamental_Fast( - const Mat3 & FMat, // The fundamental matrix - const Vec3 & epipole2,// Epipole2 (camera center1 in image plane2; must not be normalized) - const camera::IntrinsicBase * camL, // Optional camera (in order to undistord on the fly feature positions, can be NULL) - const feature::Regions & lRegions, // regions (point features & corresponding descriptors) - const camera::IntrinsicBase * camR, // Optional camera (in order to undistord on the fly feature positions, can be NULL) - const feature::Regions & rRegions, // regions (point features & corresponding descriptors) - const int widthR, const int heightR, - double errorTh, // Maximal authorized error threshold (consider it's a square threshold) - double distRatio, // Maximal authorized distance ratio - matching::IndMatches & vec_corresponding_index) // Ouput corresponding index -{ - // Looking for the corresponding points that have to satisfy: - // 1. a geometric distance below the provided Threshold - // 2. a distance ratio between descriptors of valid geometric correspondencess - // - // - Cluster left point according their epipolar line border intersection. - // - For each right point, compute threshold limited bandwidth and compare only - // points that belong to this range (limited buckets). - - // Normalize F and epipole for (ep2->p2) line adequation - Mat3 F = FMat; - Vec3 ep2 = epipole2; - if(ep2(2) > 0.0) - { - F = -F; - ep2 = -ep2; - } - ep2 = ep2 / ep2(2); - - //-- - //-- Store point in the corresponding epipolar line bucket - //-- - typedef std::vector Bucket_vec; - typedef std::vector Buckets_vec; - const int nb_buckets = 2 * (widthR + heightR - 2); - - Buckets_vec buckets(nb_buckets); - for(std::size_t i = 0; i < lRegions.RegionCount(); ++i) - { - // Compute epipolar line - const Vec2 l_pt = (camL && camL->isValid()) ? camL->get_ud_pixel(lRegions.GetRegionPosition(i)) : lRegions.GetRegionPosition(i); - const Vec3 line = F * Vec3(l_pt(0), l_pt(1), 1.); - // If the epipolar line exists in Right image - Vec2 x0, x1; - if(line_to_endPoints(line, widthR, heightR, x0, x1)) - { - // Find in which cluster the point belongs - const int bucket = pix_to_bucket(x0.cast(), widthR, heightR); - buckets[bucket].push_back(i); - } - } - - // For each point in right image, find if there is good candidates. - std::vector > dR(lRegions.RegionCount()); - for(std::size_t j = 0; j < rRegions.RegionCount(); ++j) - { - // According the point: - // - Compute it's epipolar line from the epipole - // - compute the range of possible bucket by computing - // the epipolar line gauge limitation introduced by the tolerated pixel error - - const Vec2 xR = (camR && camR->isValid()) ? camR->get_ud_pixel(rRegions.GetRegionPosition(j)) : rRegions.GetRegionPosition(j); - const Vec3 l2 = ep2.cross(Vec3(xR(0), xR(1), 1.)); - const Vec2 n = l2.head<2>() * (sqrt(errorTh) / l2.head<2>().norm()); - - const Vec3 l2min = ep2.cross(Vec3(xR(0) - n(0), xR(1) - n(1), 1.)); - const Vec3 l2max = ep2.cross(Vec3(xR(0) + n(0), xR(1) + n(1), 1.)); - - // Compute corresponding buckets - Vec2 x0, x1; - if(!line_to_endPoints(l2min, widthR, heightR, x0, x1)) - continue; - - const int bucket_start = pix_to_bucket(x0.cast(), widthR, heightR); - - if(!line_to_endPoints(l2max, widthR, heightR, x0, x1)) - continue; - - const int bucket_stop = pix_to_bucket(x0.cast(), widthR, heightR); - - if(bucket_stop - bucket_start > 0) - { - // test candidate buckets - for(Buckets_vec::const_iterator itBs = buckets.begin() + bucket_start; - itBs != buckets.begin() + bucket_stop; ++itBs) - { - const Bucket_vec & bucket = *itBs; - for(Bucket_vec::const_iterator itB = bucket.begin(); itB != bucket.end(); ++itB) - { - const IndexT i = *itB; - // Compute descriptor distance - const double descDist = lRegions.SquaredDescriptorDistance(i, &rRegions, j); - // Update the corresponding points & distance (if required) - dR[i].update(j, descDist); - } - } - } - } - // Check distance ratio validity - for(std::size_t i = 0; i < dR.size(); ++i) - { - if(dR[i].isValid(distRatio)) - { - // save the best corresponding index - vec_corresponding_index.emplace_back(i, dR[i].idx); - } - } -} - -} // namespace robustEstimation -} // namespace aliceVision diff --git a/src/aliceVision/robustEstimation/leastMedianOfSquares.hpp b/src/aliceVision/robustEstimation/leastMedianOfSquares.hpp index 1c57f67ee2..57d2ab609f 100644 --- a/src/aliceVision/robustEstimation/leastMedianOfSquares.hpp +++ b/src/aliceVision/robustEstimation/leastMedianOfSquares.hpp @@ -7,8 +7,8 @@ #pragma once -#include "aliceVision/robustEstimation/randSampling.hpp" -#include "aliceVision/robustEstimation/ransacTools.hpp" +#include +#include #include #include @@ -17,24 +17,27 @@ namespace aliceVision { namespace robustEstimation { -/// \brief Variant of RANSAC using Least Median of Squares. -/// \details Instead of using a fixed threshold to distinguish inlier/outlier, -/// find the threshold at 1-\a outlierRatio quantile of residuals and keep the -/// parameters minimizing this threshold. The final threshold -/// returned in \a outlierThreshold is a multiple of this and can be used to -/// filter out outliers. -/// LMedS : Z. Zhang. Determining The Epipolar Geometry And Its Uncertainty. A Review -/// IJCV 1998 - +/** + * @brief Variant of RANSAC using Least Median of Squares. + * + * @details Instead of using a fixed threshold to distinguish inlier/outlier, + * find the threshold at 1-\a outlierRatio quantile of residuals and keep the + * parameters minimizing this threshold. The final threshold + * returned in \a outlierThreshold is a multiple of this and can be used to + * filter out outliers. + * + * @ref LMedS : Z. Zhang. Determining The Epipolar Geometry And Its Uncertainty. + * A Review IJCV 1998 + */ template -double LeastMedianOfSquares(const Kernel &kernel, - typename Kernel::Model * model = nullptr, +double leastMedianOfSquares(const Kernel& kernel, + typename Kernel::ModelT* model = nullptr, double* outlierThreshold = nullptr, double outlierRatio = 0.5, double minProba = 0.99) { - const std::size_t min_samples = Kernel::MINIMUM_SAMPLES; - const std::size_t total_samples = kernel.NumSamples(); + const std::size_t min_samples = kernel.getMinimumNbRequiredSamples(); + const std::size_t total_samples = kernel.nbSamples(); std::vector residuals(total_samples); // Array for storing residuals @@ -49,10 +52,10 @@ double LeastMedianOfSquares(const Kernel &kernel, std::vector vec_sample(min_samples); // Get Samples indexes - UniformSample(min_samples, total_samples, vec_sample); + uniformSample(min_samples, total_samples, vec_sample); // Estimate parameters: the solutions are stored in a vector - std::vector models; + std::vector models; kernel.Fit(vec_sample, &models); // Now test the solutions on the whole data diff --git a/src/aliceVision/robustEstimation/leastMedianOfSquares_test.cpp b/src/aliceVision/robustEstimation/leastMedianOfSquares_test.cpp index 633902845f..d43f5d8219 100644 --- a/src/aliceVision/robustEstimation/leastMedianOfSquares_test.cpp +++ b/src/aliceVision/robustEstimation/leastMedianOfSquares_test.cpp @@ -49,7 +49,7 @@ BOOST_AUTO_TEST_CASE(LMedsLineFitter_OutlierFree) { // in a robust framework (LMeds). Vec2 model; double dThreshold = std::numeric_limits::infinity(); - double dBestMedian = LeastMedianOfSquares(kernel, &model, &dThreshold); + double dBestMedian = leastMedianOfSquares(kernel, &model, &dThreshold); BOOST_CHECK_CLOSE(2.0, model[1], dExpectedPrecision); BOOST_CHECK_CLOSE(1.0, model[0], dExpectedPrecision); BOOST_CHECK_CLOSE(0.0, dBestMedian, dExpectedPrecision); @@ -72,7 +72,7 @@ BOOST_AUTO_TEST_CASE(LMedsLineFitter_OneOutlier) { Vec2 model; double dThreshold = std::numeric_limits::infinity(); - double dBestMedian = LeastMedianOfSquares(kernel, &model, &dThreshold); + double dBestMedian = leastMedianOfSquares(kernel, &model, &dThreshold); BOOST_CHECK_CLOSE(2.0, model[1], dExpectedPrecision); BOOST_CHECK_CLOSE(1.0, model[0], dExpectedPrecision); BOOST_CHECK_CLOSE(0.0, dBestMedian, dExpectedPrecision); @@ -95,7 +95,7 @@ BOOST_AUTO_TEST_CASE(LMedsLineFitter_TooFewPoints) { Vec2 model; double dThreshold = std::numeric_limits::infinity(); - double dBestMedian = LeastMedianOfSquares(kernel, &model, &dThreshold); + double dBestMedian = leastMedianOfSquares(kernel, &model, &dThreshold); //No inliers BOOST_CHECK_EQUAL( dBestMedian, std::numeric_limits::max()); } @@ -122,7 +122,7 @@ BOOST_AUTO_TEST_CASE(LMedsLineFitter_RealisticCase) { //-- Add some noise (for the asked percentage amount) int nbPtToNoise = (int) NbPoints * outlierRatio; vector vec_samples; // Fit with unique random index - UniformSample(nbPtToNoise, NbPoints, vec_samples); + uniformSample(nbPtToNoise, NbPoints, vec_samples); for(size_t i = 0; i ::infinity(); - double dBestMedian = LeastMedianOfSquares(kernel, &model, &dThreshold); + double dBestMedian = leastMedianOfSquares(kernel, &model, &dThreshold); BOOST_CHECK_CLOSE(-2.0, model[0], dExpectedPrecision); BOOST_CHECK_CLOSE(6.3, model[1], dExpectedPrecision); //Compute which point are inliers (error below dThreshold) diff --git a/src/aliceVision/robustEstimation/lineKernel_test.cpp b/src/aliceVision/robustEstimation/lineKernel_test.cpp index baa1df069f..dd4156af79 100644 --- a/src/aliceVision/robustEstimation/lineKernel_test.cpp +++ b/src/aliceVision/robustEstimation/lineKernel_test.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 "aliceVision/robustEstimation/LineKernel.hpp" +#include #include @@ -17,21 +17,23 @@ using namespace aliceVision; using namespace aliceVision::robustEstimation; -// Since the line fitter isn't so simple, test it in isolation. -BOOST_AUTO_TEST_CASE(LineFitter_ItWorks) { - +// since the line fitter isn't so simple, test it in isolation. +BOOST_AUTO_TEST_CASE(LineFitter_ItWorks) +{ Mat2X xy(2, 5); // y = 2x + 1 xy << 1, 2, 3, 4, 5, 3, 5, 7, 9, 11; - std::vector models; + + std::vector models; LineKernel kernel(xy); - std::vector samples; - for (size_t i = 0; i < xy.cols(); ++i) { + std::vector samples; + + for(size_t i = 0; i < xy.cols(); ++i) samples.push_back(i); - } - kernel.Fit(samples, &models); + + kernel.fit(samples, models); BOOST_CHECK_EQUAL(1, models.size()); - BOOST_CHECK_SMALL(2.0-models[0][1], 1e-9); - BOOST_CHECK_SMALL(1.0-models[0][0], 1e-9); + BOOST_CHECK_SMALL(2.0 - models.at(0).getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - models.at(0).getMatrix()[0], 1e-9); } diff --git a/src/aliceVision/robustEstimation/lineTestGenerator.hpp b/src/aliceVision/robustEstimation/lineTestGenerator.hpp index c213830d81..1d1b49af30 100644 --- a/src/aliceVision/robustEstimation/lineTestGenerator.hpp +++ b/src/aliceVision/robustEstimation/lineTestGenerator.hpp @@ -138,7 +138,7 @@ void generateLine(std::size_t numPoints, assert(timeToStop < 200); pt(0) = realDist(gen) * W; pt(1) = realDist(gen) * H; - distance = aliceVision::robustEstimation::pointToLineError::Error(GTModel, pt); + distance = aliceVision::robustEstimation::pointToLineError(GTModel, pt); ++timeToStop; } // total += timeToStop; diff --git a/src/aliceVision/robustEstimation/loRansac_test.cpp b/src/aliceVision/robustEstimation/loRansac_test.cpp index 1b08356af8..aeebc6bd25 100644 --- a/src/aliceVision/robustEstimation/loRansac_test.cpp +++ b/src/aliceVision/robustEstimation/loRansac_test.cpp @@ -5,13 +5,11 @@ // 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 "aliceVision/robustEstimation/LineKernel.hpp" -#include "aliceVision/robustEstimation/LORansac.hpp" -#include "aliceVision/robustEstimation/ScoreEvaluator.hpp" - -#include "lineTestGenerator.hpp" - -#include "aliceVision/numeric/numeric.hpp" +#include +#include +#include +#include +#include #include #include @@ -26,50 +24,6 @@ using namespace aliceVision; using namespace aliceVision::robustEstimation; - - -struct LineKernelLoRansac : public LineKernel -{ - typedef Vec2 Model; // line parametrization: a, b; - - enum - { - MINIMUM_SAMPLES = 2, - MINIMUM_LSSAMPLES = 2 - }; - - LineKernelLoRansac(const Mat2X &xs) : LineKernel(xs) - { - } - - void FitLS(const std::vector &samples, std::vector *lines, const std::vector *weights = nullptr) const - { - - assert(samples.size() >= (unsigned int) MINIMUM_SAMPLES); - // Standard least squares solution. - const Mat2X sampled_xs = ExtractColumns(xs_, samples); - if(weights) - LineSolver::SolveWeightedLS(sampled_xs, lines, *weights); - else - LineSolver::Solve(sampled_xs, lines); - } - - void computeWeights(const Model & model, - const std::vector &inliers, - std::vector & vec_weights, - const double eps = 0.001) const - { - const auto numInliers = inliers.size(); - vec_weights.resize(numInliers); - for(std::size_t sample = 0; sample < numInliers; ++sample) - { - const auto idx = inliers[sample]; - vec_weights[sample] = Error(idx, model); - // avoid division by zero - vec_weights[sample] = 1 / std::pow(std::max(eps, vec_weights[sample]), 2); - } - } -}; void lineFittingTest(std::size_t numPoints, double outlierRatio, @@ -89,15 +43,13 @@ void lineFittingTest(std::size_t numPoints, const bool withNoise = (gaussianNoiseLevel > std::numeric_limits::epsilon()); const std::size_t expectedInliers = numPoints - (std::size_t) numPoints * outlierRatio; const double threshold = (withNoise) ? 3 * gaussianNoiseLevel : 0.3; - LineKernelLoRansac kernel(xy); + LineKernel kernel(xy); + LineKernel::ModelT model = LO_RANSAC(kernel, ScoreEvaluator(threshold), &vec_inliers); + estimatedModel = model.getMatrix(); - estimatedModel = LO_RANSAC(kernel, ScoreEvaluator(threshold), &vec_inliers); - ALICEVISION_LOG_DEBUG("#inliers found : " << vec_inliers.size() - << " expected: " << numPoints - expectedInliers); - ALICEVISION_LOG_DEBUG("model[0] found : " << estimatedModel[0] - << " expected: " << GTModel[0]); - ALICEVISION_LOG_DEBUG("model[1] found : " << estimatedModel[1] - << " expected: " << GTModel[1]); + ALICEVISION_LOG_DEBUG("#inliers found : " << vec_inliers.size() << " expected: " << numPoints - expectedInliers); + ALICEVISION_LOG_DEBUG("model[0] found : " << estimatedModel[0] << " expected: " << GTModel[0]); + ALICEVISION_LOG_DEBUG("model[1] found : " << estimatedModel[1] << " expected: " << GTModel[1]); const std::string base = "testRansac_line_t" + std::to_string(threshold) + "_n" + std::to_string(gaussianNoiseLevel); const int W = std::abs(xy(0, 0) - xy(0, numPoints - 1)); @@ -127,11 +79,11 @@ BOOST_AUTO_TEST_CASE(LoRansacLineFitter_IdealCaseLoRansac) for(std::size_t trial = 0; trial < numTrials; ++trial) { Vec2 model; - std::vector vec_inliers; - lineFittingTest(numPoints, outlierRatio, gaussianNoiseLevel, GTModel, gen, model, vec_inliers); + std::vector inliers; + lineFittingTest(numPoints, outlierRatio, gaussianNoiseLevel, GTModel, gen, model, inliers); const std::size_t expectedInliers = numPoints - (std::size_t) numPoints * outlierRatio; - BOOST_CHECK_EQUAL(expectedInliers, vec_inliers.size()); + BOOST_CHECK_EQUAL(expectedInliers, inliers.size()); BOOST_CHECK_SMALL(GTModel[0]-model[0], 1e-2); BOOST_CHECK_SMALL(GTModel[1]-model[1], 1e-2); } @@ -156,9 +108,9 @@ BOOST_AUTO_TEST_CASE(LoRansacLineFitter_RealCaseLoRansac) for(std::size_t trial = 0; trial < numTrials; ++trial) { Vec2 model; - std::vector vec_inliers; - lineFittingTest(numPoints, outlierRatio, gaussianNoiseLevel, GTModel, gen, model, vec_inliers); + std::vector inliers; + lineFittingTest(numPoints, outlierRatio, gaussianNoiseLevel, GTModel, gen, model, inliers); const std::size_t expectedInliers = numPoints - (std::size_t) numPoints * outlierRatio; - BOOST_CHECK_EQUAL(expectedInliers, vec_inliers.size()); + BOOST_CHECK_EQUAL(expectedInliers, inliers.size()); } } diff --git a/src/aliceVision/robustEstimation/maxConsensus.hpp b/src/aliceVision/robustEstimation/maxConsensus.hpp index e1cf6bfdcc..dd1638f87e 100644 --- a/src/aliceVision/robustEstimation/maxConsensus.hpp +++ b/src/aliceVision/robustEstimation/maxConsensus.hpp @@ -26,19 +26,20 @@ namespace robustEstimation{ /// 4. A way to convert samples and a model to an error. /// /// 1. Kernel::Model -/// 2. Kernel::MINIMUM_SAMPLES -/// 3. Kernel::Fit(vector, vector *) -/// 4. Kernel::Error(Model, int) -> error +/// 2. Kernel::getMinimumNbRequiredSamples() +/// 3. Kernel::fit(vector, vector *) +/// 4. Kernel::error(Model, int) -> error template -typename Kernel::Model MaxConsensus(const Kernel &kernel, - const Scorer &scorer, - std::vector *best_inliers = nullptr, std::size_t max_iteration = 1024) { +typename Kernel::ModelT maxConsensus(const Kernel& kernel, + const Scorer& scorer, + std::vector* best_inliers = nullptr, + std::size_t max_iteration = 1024) { - const std::size_t min_samples = Kernel::MINIMUM_SAMPLES; - const std::size_t total_samples = kernel.NumSamples(); + const std::size_t min_samples = kernel.getMinimumNbRequiredSamples(); + const std::size_t total_samples = kernel.nbSamples(); std::size_t best_num_inliers = 0; - typename Kernel::Model best_model; + typename Kernel::ModelT best_model; // Test if we have sufficient points to for the kernel. if (total_samples < min_samples) @@ -61,16 +62,16 @@ typename Kernel::Model MaxConsensus(const Kernel &kernel, for(std::size_t iteration = 0; iteration < max_iteration; ++iteration) { std::vector sample; - UniformSample(min_samples, total_samples, sample); + uniformSample(min_samples, total_samples, sample); - std::vector models; - kernel.Fit(sample, &models); + std::vector models; + kernel.fit(sample, models); // Compute costs for each fit. for(std::size_t i = 0; i < models.size(); ++i) { std::vector inliers; - scorer.Score(kernel, models[i], all_samples, &inliers); + scorer.score(kernel, models[i], all_samples, inliers); if (best_num_inliers < inliers.size()) { diff --git a/src/aliceVision/robustEstimation/maxConsensus_test.cpp b/src/aliceVision/robustEstimation/maxConsensus_test.cpp index 850ff67ca8..225bb5b923 100644 --- a/src/aliceVision/robustEstimation/maxConsensus_test.cpp +++ b/src/aliceVision/robustEstimation/maxConsensus_test.cpp @@ -20,8 +20,8 @@ using namespace aliceVision; using namespace aliceVision::robustEstimation; // Test without outlier -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree) +{ Mat2X xy(2, 5); // y = 2x + 1 xy << 1, 2, 3, 4, 5, @@ -32,32 +32,30 @@ BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree) { // Check the best model that fit the most of the data // in a robust framework (Max-consensus). - std::vector vec_inliers; - Vec2 model = MaxConsensus(kernel, - ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_SMALL(2.0-model[1], 1e-9); - BOOST_CHECK_SMALL(1.0-model[0], 1e-9); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + std::vector inliers; + LineKernel::ModelT model = maxConsensus(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_SMALL(2.0 - model.getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - model.getMatrix()[0], 1e-9); + BOOST_CHECK_EQUAL(5, inliers.size()); } // Test without getting back the model -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree_DoNotGetBackModel) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree_DoNotGetBackModel) +{ Mat2X xy(2, 5); // y = 2x + 1 xy << 1, 2, 3, 4, 5, 3, 5, 7, 9, 11; LineKernel kernel(xy); - std::vector vec_inliers; - Vec2 model = MaxConsensus(kernel, - ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + std::vector inliers; + LineKernel::ModelT model = maxConsensus(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_EQUAL(5, inliers.size()); } // Test efficiency of MaxConsensus to find (inlier/outlier) in contamined data -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OneOutlier) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OneOutlier) +{ Mat2X xy(2, 6); // y = 2x + 1 with an outlier xy << 1, 2, 3, 4, 5, 100, // outlier! @@ -65,35 +63,33 @@ BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OneOutlier) { LineKernel kernel(xy); - std::vector vec_inliers; - Vec2 model = MaxConsensus(kernel, - ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_SMALL(2.0-model[1], 1e-9); - BOOST_CHECK_SMALL(1.0-model[0], 1e-9); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + std::vector inliers; + LineKernel::ModelT model = maxConsensus(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_SMALL(2.0 - model.getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - model.getMatrix()[0], 1e-9); + BOOST_CHECK_EQUAL(5, inliers.size()); } // Critical test: // Test if the robust estimator do not return inlier if too few point // was given for an estimation. -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_TooFewPoints) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_TooFewPoints) +{ Mat2X xy(2, 1); xy << 1, 3; // y = 2x + 1 with x = 1 LineKernel kernel(xy); - std::vector vec_inliers; - Vec2 model = MaxConsensus(kernel, - ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_EQUAL(0, vec_inliers.size()); + std::vector inliers; + const LineKernel::ModelT model = maxConsensus(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_EQUAL(0, inliers.size()); } // From a GT model : // Compute a list of point that fit the model. // Add white noise to given amount of points in this list. // Check that the number of inliers and the model are correct. -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_RealisticCase) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_RealisticCase) +{ const int numPoints = 30; const float outlierRatio = .3; //works with .4 Mat2X xy(2, numPoints); @@ -109,21 +105,20 @@ BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_RealisticCase) { //-- Add some noise (for the asked percentage amount) int nbPtToNoise = (int) numPoints * outlierRatio; - vector vec_samples; // Fit with unique random index - UniformSample(nbPtToNoise, numPoints, vec_samples); - for(size_t i = 0; i vec_samples; // Fit with unique random index + uniformSample(nbPtToNoise, numPoints, vec_samples); + for(std::size_t i = 0; i vec_inliers; - Vec2 model = MaxConsensus(kernel, - ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_EQUAL(numPoints-nbPtToNoise, vec_inliers.size()); - BOOST_CHECK_SMALL((-2.0)-model[0], 1e-9); - BOOST_CHECK_SMALL( 6.3-model[1], 1e-9); + std::vector inliers; + LineKernel::ModelT model = maxConsensus(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_EQUAL(numPoints-nbPtToNoise, inliers.size()); + BOOST_CHECK_SMALL((-2.0) - model.getMatrix()[0], 1e-9); + BOOST_CHECK_SMALL( 6.3 - model.getMatrix()[1], 1e-9); } diff --git a/src/aliceVision/robustEstimation/randSampling.hpp b/src/aliceVision/robustEstimation/randSampling.hpp index ac3c046141..4fc250d788 100644 --- a/src/aliceVision/robustEstimation/randSampling.hpp +++ b/src/aliceVision/robustEstimation/randSampling.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include namespace aliceVision { @@ -89,7 +90,7 @@ inline std::vector randSample(IntT lowerBound, * @param[out] samples The set containing the random numbers in the range [0, upperBound) */ template -inline void UniformSample(std::size_t numSamples, +inline void uniformSample(std::size_t numSamples, std::size_t upperBound, std::set &samples) { @@ -113,7 +114,7 @@ inline void UniformSample(std::size_t numSamples, * @param[out] samples The vector containing the samples. */ template -inline void UniformSample(std::size_t lowerBound, +inline void uniformSample(std::size_t lowerBound, std::size_t upperBound, std::size_t numSamples, std::vector &samples) @@ -129,11 +130,11 @@ inline void UniformSample(std::size_t lowerBound, * @param[out] samples The vector containing the samples. */ template -inline void UniformSample(std::size_t numSamples, +inline void uniformSample(std::size_t numSamples, std::size_t upperBound, std::vector &samples) { - UniformSample(0, upperBound, numSamples, samples); + uniformSample(0, upperBound, numSamples, samples); } /** @@ -144,7 +145,7 @@ inline void UniformSample(std::size_t numSamples, * @param[in] elements The possible data indices. * @param[out] sample The random sample of sizeSample indices. */ -inline void UniformSample(std::size_t sampleSize, +inline void uniformSample(std::size_t sampleSize, const std::vector& elements, std::vector& sample) { diff --git a/src/aliceVision/robustEstimation/randSampling_test.cpp b/src/aliceVision/robustEstimation/randSampling_test.cpp index 97c1ee267a..bbfd91cc2c 100644 --- a/src/aliceVision/robustEstimation/randSampling_test.cpp +++ b/src/aliceVision/robustEstimation/randSampling_test.cpp @@ -29,7 +29,7 @@ BOOST_AUTO_TEST_CASE(UniformSampleTest_NoRepetions) { //Size of the consensus set std::vector samples; std::cout << "Upper " << upperBound << " Lower " << 0 << " numSamples " << numSamples << "\n"; - UniformSample(numSamples, upperBound, samples); + uniformSample(numSamples, upperBound, samples); std::set myset; for(const auto& s : samples) { @@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(UniformSampleTest_UniformSampleSet) { //Size of the consensus set std::cout << "Upper " << upperBound << " Lower " << 0 << " numSamples " << numSample << "\n"; std::set samples; - UniformSample(numSample, upperBound, samples); + uniformSample(numSample, upperBound, samples); BOOST_CHECK_EQUAL(numSample, samples.size()); for(const auto& s : samples) { @@ -75,7 +75,7 @@ BOOST_AUTO_TEST_CASE(UniformSampleTest_NoRepetionsBeginEnd) { const std::size_t begin = upperBound-numSamples; std::cout << "Upper " << upperBound << " Lower " << begin << " numSamples " << numSamples << "\n"; std::vector samples; - UniformSample(begin, upperBound, numSamples, samples); + uniformSample(begin, upperBound, numSamples, samples); std::set myset; for(const auto& s : samples) { diff --git a/src/aliceVision/robustEstimation/ransacTools.hpp b/src/aliceVision/robustEstimation/ransacTools.hpp index c0773ce64e..b9a67f05ce 100644 --- a/src/aliceVision/robustEstimation/ransacTools.hpp +++ b/src/aliceVision/robustEstimation/ransacTools.hpp @@ -13,25 +13,22 @@ namespace aliceVision { namespace robustEstimation{ -/// Number of samplings to have at least \a minProba probability of absence of -/// outlier in a sample of \a SampleSize elements. -inline size_t getNumSamples( - double minProba, - double outlierRatio, - std::size_t SampleSize) +/** + * @brief Number of samplings to have at least \a minProba probability of absence of + * outlier in a sample of \a sampleSize elements. + * @param minProba + * @param outlierRatio + * @param SampleSize + * @return + */ +inline std::size_t getNumSamples(double minProba, double outlierRatio, std::size_t sampleSize) { - return static_cast( std::log(1.-minProba) / - std::log(1.-std::pow(1.-outlierRatio, static_cast(SampleSize)))); + return static_cast(std::log(1.-minProba) / std::log(1.-std::pow(1.-outlierRatio, static_cast(sampleSize)))); } -inline size_t IterationsRequired( - std::size_t min_samples, - double outliers_probability, - double inlier_ratio) +inline std::size_t iterationsRequired(std::size_t min_samples, double outliersProbability, double inlierRatio) { - return static_cast( - std::log(outliers_probability) / - std::log(1.0 - std::pow(inlier_ratio, static_cast(min_samples)))); + return static_cast(std::log(outliersProbability) / std::log(1.0 - std::pow(inlierRatio, static_cast(min_samples)))); } } // namespace robustEstimation diff --git a/src/aliceVision/robustEstimation/ransac_test.cpp b/src/aliceVision/robustEstimation/ransac_test.cpp index 1ff343d333..d983149cd3 100644 --- a/src/aliceVision/robustEstimation/ransac_test.cpp +++ b/src/aliceVision/robustEstimation/ransac_test.cpp @@ -5,13 +5,11 @@ // 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 "aliceVision/robustEstimation/LineKernel.hpp" -#include "aliceVision/robustEstimation/Ransac.hpp" -#include "aliceVision/robustEstimation/ScoreEvaluator.hpp" - -#include "aliceVision/numeric/numeric.hpp" - -#include "lineTestGenerator.hpp" +#include +#include +#include +#include +#include #define BOOST_TEST_MODULE robustEstimationRansac @@ -22,8 +20,8 @@ using namespace aliceVision; using namespace aliceVision::robustEstimation; // Test without outlier -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree) +{ Mat2X xy(2, 5); // y = 2x + 1 xy << 1, 2, 3, 4, 5, @@ -34,50 +32,50 @@ BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OutlierFree) { // Check the best model that fit the most of the data // in a robust framework (Ransac). - std::vector vec_inliers; - Vec2 model = RANSAC(kernel, ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_SMALL(2.0-model[1], 1e-9); - BOOST_CHECK_SMALL(1.0-model[0], 1e-9); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + std::vector inliers; + LineKernel::ModelT model = RANSAC(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_SMALL(2.0 - model.getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - model.getMatrix()[0], 1e-9); + BOOST_CHECK_EQUAL(5, inliers.size()); } // Test efficiency of MaxConsensus to find (inlier/outlier) in contamined data -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OneOutlier) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_OneOutlier) +{ Mat2X xy(2, 6); // y = 2x + 1 with an outlier - xy << 1, 2, 3, 4, 5, 100, // outlier! + xy << 1, 2, 3, 4, 5, 100, // outlier! 3, 5, 7, 9, 11, -123; // outlier! LineKernel kernel(xy); - std::vector vec_inliers; - Vec2 model = RANSAC(kernel, ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_SMALL(2.0-model[1], 1e-9); - BOOST_CHECK_SMALL(1.0-model[0], 1e-9); - BOOST_CHECK_EQUAL(5, vec_inliers.size()); + std::vector inliers; + LineKernel::ModelT model = RANSAC(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_SMALL(2.0 - model.getMatrix()[1], 1e-9); + BOOST_CHECK_SMALL(1.0 - model.getMatrix()[0], 1e-9); + BOOST_CHECK_EQUAL(5, inliers.size()); } // Critical test: // Test if the robust estimator do not return inlier if too few point // was given for an estimation. -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_TooFewPoints) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_TooFewPoints) +{ Mat2X xy(2, 1); xy << 1, 3; // y = 2x + 1 with x = 1 LineKernel kernel(xy); - std::vector vec_inliers; - Vec2 model = RANSAC(kernel, ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_EQUAL(0, vec_inliers.size()); + std::vector inliers; + const LineKernel::ModelT model = RANSAC(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_EQUAL(0, inliers.size()); } // From a GT model : // Compute a list of point that fit the model. // Add white noise to given amount of points in this list. // Check that the number of inliers and the model are correct. -BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_RealisticCase) { - +BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_RealisticCase) +{ const int NbPoints = 30; const double outlierRatio = .3; //works with 40 Mat2X xy(2, NbPoints); @@ -87,15 +85,15 @@ BOOST_AUTO_TEST_CASE(MaxConsensusLineFitter_RealisticCase) { GTModel << -2.0, 6.3; //-- Add some noise (for the asked percentage amount) - const std::size_t nbPtToNoise = (size_t) NbPoints*outlierRatio; + const std::size_t nbPtToNoise = (std::size_t) NbPoints*outlierRatio; std::vector vec_inliersGT; generateLine(NbPoints, outlierRatio, 0.0, GTModel, gen, xy, vec_inliersGT); LineKernel kernel(xy); - std::vector vec_inliers; - Vec2 model = RANSAC(kernel, ScoreEvaluator(0.3), &vec_inliers); - BOOST_CHECK_EQUAL(NbPoints-nbPtToNoise, vec_inliers.size()); - BOOST_CHECK_SMALL((-2.0)-model[0], 1e-9); - BOOST_CHECK_SMALL( 6.3-model[1], 1e-9); + std::vector inliers; + LineKernel::ModelT model = RANSAC(kernel, ScoreEvaluator(0.3), &inliers); + BOOST_CHECK_EQUAL(NbPoints-nbPtToNoise, inliers.size()); + BOOST_CHECK_SMALL((-2.0) - model.getMatrix()[0], 1e-9); + BOOST_CHECK_SMALL( 6.3 - model.getMatrix()[1], 1e-9); } diff --git a/src/aliceVision/sensorDB/cameraSensors.db b/src/aliceVision/sensorDB/cameraSensors.db index 73e22b5de3..bc166d8ee0 100644 --- a/src/aliceVision/sensorDB/cameraSensors.db +++ b/src/aliceVision/sensorDB/cameraSensors.db @@ -561,6 +561,7 @@ Canon;Canon EOS 30D;22.5;dpreview,digicamdb,dxomark Canon;Canon EOS 350D;22.2;dpreview,digicamdb,dxomark Canon;Canon EOS 4000D;22.3;dpreview,digicamdb,dxomark Canon;Canon EOS 400D;22.2;dpreview,digicamdb,dxomark +Canon;Canon EOS 400D DIGITAL;22.2;dpreview,digicamdb,dxomark Canon;Canon EOS 400D Rebel XTi;22.2;imaging-resource Canon;Canon EOS 40D;22.2;dpreview,digicamdb Canon;Canon EOS 450D;22.2;dpreview,digicamdb,dxomark diff --git a/src/aliceVision/sfm/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentCeres.cpp index 41747112a2..1f3625bd75 100644 --- a/src/aliceVision/sfm/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/BundleAdjustmentCeres.cpp @@ -275,7 +275,6 @@ void BundleAdjustmentCeres::setSolverOptions(ceres::Solver::Options& solverOptio solverOptions.logging_type = ceres::SILENT; solverOptions.num_threads = _ceresOptions.nbThreads; - #if CERES_VERSION_MAJOR < 2 solverOptions.num_linear_solver_threads = _ceresOptions.nbThreads; #endif @@ -521,7 +520,7 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat // 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(4.0)); // TODO: make the LOSS function and the parameter an option + ceres::LossFunction* lossFunction = _ceresOptions.lossFunction.get(); // build the residual blocks corresponding to the track observations for(const auto& landmarkPair: sfmData.getLandmarks()) @@ -611,7 +610,7 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf { // 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(4.0)); // TODO: make the LOSS function and the parameter an option + ceres::LossFunction* lossFunction = _ceresOptions.lossFunction.get(); for (const auto & constraint : sfmData.getConstraints2D()) { const sfmData::View& view_1 = sfmData.getView(constraint.ViewFirst); @@ -772,7 +771,9 @@ void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, ceres::CRSMatrix& jacobian) { // create problem - ceres::Problem problem; + ceres::Problem::Options problemOptions; + problemOptions.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + ceres::Problem problem(problemOptions); createProblem(sfmData, refineOptions, problem); // configure Jacobian engine @@ -789,7 +790,9 @@ void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, bool BundleAdjustmentCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions refineOptions) { // create problem - ceres::Problem problem; + ceres::Problem::Options problemOptions; + problemOptions.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + ceres::Problem problem(problemOptions); createProblem(sfmData, refineOptions, problem); // configure a Bundle Adjustment engine and run it diff --git a/src/aliceVision/sfm/BundleAdjustmentCeres.hpp b/src/aliceVision/sfm/BundleAdjustmentCeres.hpp index b8bab64c3c..06ed090bcd 100644 --- a/src/aliceVision/sfm/BundleAdjustmentCeres.hpp +++ b/src/aliceVision/sfm/BundleAdjustmentCeres.hpp @@ -11,9 +11,13 @@ #include #include #include +#include #include +#include + + namespace aliceVision { namespace sfmData { @@ -36,6 +40,7 @@ class BundleAdjustmentCeres : public BundleAdjustment , nbThreads(multithreaded ? omp_get_max_threads() : 1) // set number of threads, 1 if OpenMP is not enabled { setDenseBA(); // use dense BA by default + lossFunction.reset(new ceres::HuberLoss(Square(4.0))); } void setDenseBA(); @@ -45,6 +50,7 @@ class BundleAdjustmentCeres : public BundleAdjustment ceres::PreconditionerType preconditionerType; ceres::SparseLinearAlgebraLibraryType sparseLinearAlgebraLibraryType; ceres::ParameterBlockOrdering linearSolverOrdering; + std::shared_ptr lossFunction; unsigned int nbThreads; bool useParametersOrdering = true; bool summary = false; diff --git a/src/aliceVision/sfm/pipeline/RelativePoseInfo.cpp b/src/aliceVision/sfm/pipeline/RelativePoseInfo.cpp index 12676625ad..694e6cf027 100644 --- a/src/aliceVision/sfm/pipeline/RelativePoseInfo.cpp +++ b/src/aliceVision/sfm/pipeline/RelativePoseInfo.cpp @@ -6,13 +6,14 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "RelativePoseInfo.hpp" - -#include -#include -#include - +#include #include -#include +#include +#include +#include +#include +#include +#include namespace aliceVision { namespace sfm { @@ -29,7 +30,7 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, std::vector ts; // Translation matrix. // Recover best rotation and translation from E. - MotionFromEssential(E, &Rs, &ts); + motionFromEssential(E, &Rs, &ts); //-> Test the 4 solutions will all the points assert(Rs.size() == 4); @@ -38,13 +39,13 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, Mat34 P1, P2; Mat3 R1 = Mat3::Identity(); Vec3 t1 = Vec3::Zero(); - P_From_KRt(K1, R1, t1, &P1); + P_from_KRt(K1, R1, t1, &P1); for (unsigned int i = 0; i < 4; ++i) { const Mat3 &R2 = Rs[i]; const Vec3 &t2 = ts[i]; - P_From_KRt(K2, R2, t2, &P2); + P_from_KRt(K2, R2, t2, &P2); Vec3 X; for (size_t k = 0; k < vec_inliers.size(); ++k) @@ -52,7 +53,7 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, const Vec2 & x1_ = x1.col(vec_inliers[k]), & x2_ = x2.col(vec_inliers[k]); - TriangulateDLT(P1, x1_, P2, x2_, &X); + multiview::TriangulateDLT(P1, x1_, P2, x2_, &X); // Test if point is front to the two cameras. if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) { @@ -74,35 +75,35 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, return true; } -using namespace aliceVision::robustEstimation; - -bool robustRelativePose( - const Mat3 & K1, const Mat3 & K2, - const Mat & x1, const Mat & x2, - RelativePoseInfo & relativePose_info, - const std::pair & size_ima1, - const std::pair & size_ima2, - const size_t max_iteration_count) +bool robustRelativePose(const Mat3& K1, const Mat3& K2, + const Mat& x1, const Mat& x2, + RelativePoseInfo & relativePose_info, + const std::pair & size_ima1, + const std::pair & size_ima2, + const size_t max_iteration_count) { - // Use the 5 point solver to estimate E - typedef aliceVision::essential::kernel::FivePointKernel SolverType; - // Define the AContrario adaptor - typedef ACKernelAdaptorEssential< - SolverType, - aliceVision::fundamental::kernel::EpipolarDistanceError, - UnnormalizerT, - Mat3> - KernelType; - - KernelType kernel(x1, size_ima1.first, size_ima1.second, - x2, size_ima2.first, size_ima2.second, K1, K2); - - // Robustly estimation of the Essential matrix and its precision - const std::pair acRansacOut = ACRANSAC(kernel, relativePose_info.vec_inliers, - max_iteration_count, &relativePose_info.essential_matrix, relativePose_info.initial_residual_tolerance); + // use the 5 point solver to estimate E + using SolverT = multiview::relativePose::Essential5PSolver; + + // define the kernel + using KernelT = multiview::RelativePoseKernel_K; + + KernelT kernel(x1, size_ima1.first, size_ima1.second, + x2, size_ima2.first, size_ima2.second, K1, K2); + + + robustEstimation::Mat3Model model; + + // robustly estimation of the Essential matrix and its precision + const std::pair acRansacOut = robustEstimation::ACRANSAC(kernel, + relativePose_info.vec_inliers, + max_iteration_count, + &model, + relativePose_info.initial_residual_tolerance); + relativePose_info.essential_matrix = model.getMatrix(); relativePose_info.found_residual_precision = acRansacOut.first; - if (relativePose_info.vec_inliers.size() < SolverType::MINIMUM_SAMPLES * ALICEVISION_MINIMUM_SAMPLES_COEF ) + if(relativePose_info.vec_inliers.size() < kernel.getMinimumNbRequiredSamples() * ALICEVISION_MINIMUM_SAMPLES_COEF) { ALICEVISION_LOG_INFO("robustRelativePose: no sufficient coverage (the model does not support enough samples): " << relativePose_info.vec_inliers.size()); return false; // no sufficient coverage (the model does not support enough samples) @@ -121,6 +122,7 @@ bool robustRelativePose( // Store [R|C] for the second camera, since the first camera is [Id|0] relativePose_info.relativePose = geometry::Pose3(R, -R.transpose() * t); + return true; } diff --git a/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp b/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp index fc4a7d00d3..1fd530297f 100644 --- a/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp +++ b/src/aliceVision/sfm/pipeline/global/GlobalSfMTranslationAveragingSolver.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -491,15 +491,15 @@ void GlobalSfMTranslationAveragingSolver::ComputePutativeTranslation_EdgesCovera Mat3 Rij; Vec3 tij; - RelativeCameraMotion(RI, ti, RJ, tj, &Rij, &tij); + relativeCameraMotion(RI, ti, RJ, tj, &Rij, &tij); Mat3 Rjk; Vec3 tjk; - RelativeCameraMotion(RJ, tj, RK, tk, &Rjk, &tjk); + relativeCameraMotion(RJ, tj, RK, tk, &Rjk, &tjk); Mat3 Rik; Vec3 tik; - RelativeCameraMotion(RI, ti, RK, tk, &Rik, &tik); + relativeCameraMotion(RI, ti, RK, tk, &Rik, &tik); // set number of threads, 1 if openMP is not enabled const int thread_id = omp_get_thread_num(); @@ -684,9 +684,9 @@ bool GlobalSfMTranslationAveragingSolver::Estimate_T_triplet( vec_tis.resize(3); Mat3 K, R; - KRt_From_P(T.P1, &K, &R, &vec_tis[0]); - KRt_From_P(T.P2, &K, &R, &vec_tis[1]); - KRt_From_P(T.P3, &K, &R, &vec_tis[2]); + KRt_from_P(T.P1, &K, &R, &vec_tis[0]); + KRt_from_P(T.P2, &K, &R, &vec_tis[1]); + KRt_from_P(T.P3, &K, &R, &vec_tis[2]); #ifdef DEBUG_TRIPLET // compute 3D scene base on motion estimation diff --git a/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp b/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp index 1eb3c4b7fe..4e3567c0a3 100644 --- a/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp +++ b/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp @@ -563,7 +563,7 @@ void ReconstructionEngine_globalSfM::Compute_Relative_Rotations(rotationAveragin const Vec2 x1_ = p1.coords().cast(); const Vec2 x2_ = p2.coords().cast(); Vec3 X; - TriangulateDLT(P1, x1_, P2, x2_, &X); + multiview::TriangulateDLT(P1, x1_, P2, x2_, &X); Observations obs; const double scaleI = (_featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : p1.scale(); const double scaleJ = (_featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : p2.scale(); @@ -597,7 +597,7 @@ void ReconstructionEngine_globalSfM::Compute_Relative_Rotations(rotationAveragin // Compute relative motion and save it Mat3 Rrel; Vec3 trel; - RelativeCameraMotion(R1, t1, R2, t2, &Rrel, &trel); + relativeCameraMotion(R1, t1, R2, t2, &Rrel, &trel); // Update found relative pose relativePose_info.relativePose = Pose3(Rrel, -Rrel.transpose() * trel); } diff --git a/src/aliceVision/sfm/pipeline/global/TranslationTripletKernelACRansac.hpp b/src/aliceVision/sfm/pipeline/global/TranslationTripletKernelACRansac.hpp index 997331cf22..a70ecb3edb 100644 --- a/src/aliceVision/sfm/pipeline/global/TranslationTripletKernelACRansac.hpp +++ b/src/aliceVision/sfm/pipeline/global/TranslationTripletKernelACRansac.hpp @@ -8,8 +8,9 @@ #pragma once #include -#include +#include #include +#include #include namespace aliceVision { @@ -18,19 +19,21 @@ namespace sfm { using namespace aliceVision::trifocal::kernel; /// AContrario Kernel to solve a translation triplet & structure problem -template +template class TranslationTripletKernelACRansac + : robustEstimation::IRansacKernel { public: - typedef SolverArg Solver; - typedef ModelArg Model; + using SolverT = SolverT_; + using ErrorT = SolverT_; + using ModelT = ModelT_; TranslationTripletKernelACRansac(const Mat& x1, - const Mat& x2, - const Mat& x3, - const std::vector& vec_KRi, - const Mat3& K, - const double ThresholdUpperBound) + const Mat& x2, + const Mat& x3, + const std::vector& vec_KRi, + const Mat3& K, + const double ThresholdUpperBound) : _x1(x1) , _x2(x2) , _x3(x3) @@ -41,77 +44,113 @@ class TranslationTripletKernelACRansac , _Kinv(K.inverse()) { // Normalize points by inverse(K) - ApplyTransformationToPoints(_x1, _Kinv, &_x1n); - ApplyTransformationToPoints(_x2, _Kinv, &_x2n); - ApplyTransformationToPoints(_x3, _Kinv, &_x3n); + robustEstimation::applyTransformationToPoints(_x1, _Kinv, &_x1n); + robustEstimation::applyTransformationToPoints(_x2, _Kinv, &_x2n); + robustEstimation::applyTransformationToPoints(_x3, _Kinv, &_x3n); _vecKR[0] = _Kinv * _vecKR[0]; _vecKR[1] = _Kinv * _vecKR[1]; _vecKR[2] = _Kinv * _vecKR[2]; } + /** + * @brief Return the minimum number of required samples for the solver + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return _kernelSolver.getMinimumNbRequiredSamples(); + } - enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; - enum { MAX_MODELS = Solver::MAX_MODELS }; + /** + * @brief Return the maximum number of models for the solver + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return _kernelSolver.getMaximumNbModels(); + } - void Fit(const std::vector& samples, std::vector* models) const + void fit(const std::vector& samples, std::vector& models) const override { - // Create a model from the points - Solver::Solve(ExtractColumns(_x1n, samples), - ExtractColumns(_x2n, samples), - ExtractColumns(_x3n, samples), - _vecKR, models, _thresholdUpperBound); + // create a model from the points + _kernelSolver.solve(ExtractColumns(_x1n, samples), + ExtractColumns(_x2n, samples), + ExtractColumns(_x3n, samples), + _vecKR, models, _thresholdUpperBound); } - double Error(std::size_t sample, const Model& model) const + /** + * @brief error + * @param sample + * @param model + * @return + */ + double error(std::size_t sample, const ModelT_& model) const override { - return ErrorArg::Error(model, _x1n.col(sample), _x2n.col(sample), _x3n.col(sample)); + return _errorEstimator.error(model, _x1n.col(sample), _x2n.col(sample), _x3n.col(sample)); } - void Errors(const Model& model, std::vector& vec_errors) const + void errors(const ModelT_& model, std::vector& errors) const override { for(std::size_t sample = 0; sample < _x1n.cols(); ++sample) - vec_errors[sample] = ErrorArg::Error(model, _x1n.col(sample), _x2n.col(sample), _x3n.col(sample)); + errors[sample] = error(sample, model); } - std::size_t NumSamples() const + std::size_t nbSamples() const override { return _x1n.cols(); } - void Unnormalize(Model* model) const + void unnormalize(ModelT_& model) const override { - // Unnormalize model from the computed conditioning. - model->P1 = _K * model->P1; - model->P2 = _K * model->P2; - model->P3 = _K * model->P3; + // unnormalize model from the computed conditioning. + model.P1 = _K * model.P1; + model.P2 = _K * model.P2; + model.P3 = _K * model.P3; } - double logalpha0() const + double logalpha0() const override { return _logalpha0; } - double multError() const + double multError() const override { return 1.0; } - Mat3 normalizer1() const + Mat3 normalizer1() const override { return _Kinv; } - Mat3 normalizer2() const + Mat3 normalizer2() const override { return Mat3::Identity(); } - double unormalizeError(double val) const + double unormalizeError(double val) const override { return std::sqrt(val) / _Kinv(0,0); } + std::size_t getMinimumNbRequiredSamplesLS() const + { + throw std::logic_error("TranslationTripletKernelACRansac cannot be used in LO_RANSAC."); + return 0; + } + + void fitLS(const std::vector& inliers, std::vector& models, const std::vector* weights = nullptr) const + { + throw std::logic_error("TranslationTripletKernelACRansac cannot be used in LO_RANSAC."); + } + + void computeWeights(const ModelT& model, const std::vector& inliers, std::vector & weights, const double eps = 0.001) const + { + throw std::logic_error("TranslationTripletKernelACRansac cannot be used in LO_RANSAC."); + } + private: const Mat& _x1; const Mat& _x2; @@ -123,6 +162,11 @@ class TranslationTripletKernelACRansac const double _logalpha0; const double _thresholdUpperBound; std::vector _vecKR; + + /// two view solver + const SolverT _kernelSolver = SolverT(); + /// solver error estimation + const ErrorT _errorEstimator = ErrorT(); }; } // namespace sfm diff --git a/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp b/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp index 1d77d26f84..4362ef3b5c 100644 --- a/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp +++ b/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp @@ -6,43 +6,34 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "SfMLocalizer.hpp" +#include #include -#include -#include #include -#include #include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include namespace aliceVision { namespace sfm { -struct ResectionSquaredResidualError -{ - // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) - // Return the squared error - static double Error(const Mat34& P, const Vec2& pt2D, const Vec3& pt3D) - { - const Vec2 x = Project(P, pt3D); - return (x - pt2D).squaredNorm(); - } -}; - bool SfMLocalizer::Localize(const Pair& imageSize, const camera::IntrinsicBase* optionalIntrinsics, ImageLocalizerMatchData& resectionData, geometry::Pose3& pose, robustEstimation::ERobustEstimator estimator) { - // -- - // Compute the camera pose (resectioning) - // -- + // compute the camera pose (resectioning) + Mat34 P; resectionData.vec_inliers.clear(); - // Setup the admissible upper bound residual error + // setup the admissible upper bound residual error const double precision = resectionData.error_max == std::numeric_limits::infinity() ? std::numeric_limits::infinity() : @@ -51,22 +42,21 @@ bool SfMLocalizer::Localize(const Pair& imageSize, std::size_t minimumSamples = 0; const camera::Pinhole* pinholeCam = dynamic_cast(optionalIntrinsics); - if (pinholeCam == nullptr || !pinholeCam->isValid()) + if(pinholeCam == nullptr || !pinholeCam->isValid()) { - //-- - // Classic resection (try to compute the entire P matrix) - typedef aliceVision::resection::kernel::SixPointResectionSolver SolverType; - minimumSamples = SolverType::MINIMUM_SAMPLES; - - typedef aliceVision::robustEstimation::ACKernelAdaptorResection< - SolverType, ResectionSquaredResidualError, aliceVision::robustEstimation::UnnormalizerResection, Mat34> - KernelType; - - KernelType kernel(resectionData.pt2D, imageSize.first, imageSize.second, resectionData.pt3D); - // Robust estimation of the Projection matrix and its precision - const std::pair ACRansacOut = - aliceVision::robustEstimation::ACRANSAC(kernel, resectionData.vec_inliers, resectionData.max_iteration, &P, precision); - // Update the upper bound precision of the model found by AC-RANSAC + // classic resection (try to compute the entire P matrix) + using SolverT = multiview::resection::Resection6PSolver; + using KernelT = multiview::ResectionKernel; + + const KernelT kernel(resectionData.pt2D, resectionData.pt3D, imageSize.first, imageSize.second); + + minimumSamples = kernel.getMinimumNbRequiredSamples(); + + // robust estimation of the Projection matrix and its precision + robustEstimation::Mat34Model model; + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, resectionData.vec_inliers, resectionData.max_iteration, &model, precision); + P = model.getMatrix(); + // update the upper bound precision of the model found by AC-RANSAC resectionData.error_max = ACRansacOut.first; } else @@ -88,22 +78,22 @@ bool SfMLocalizer::Localize(const Pair& imageSize, { case robustEstimation::ERobustEstimator::ACRANSAC: { - //-- - // Since K calibration matrix is known, compute only [R|t] - typedef aliceVision::resection::P3PSolver SolverType; - minimumSamples = SolverType::MINIMUM_SAMPLES; - - typedef aliceVision::robustEstimation::ACKernelAdaptorResection_K< - SolverType, ResectionSquaredResidualError, - aliceVision::robustEstimation::UnnormalizerResection, Mat34> KernelType; + // since K calibration matrix is known, compute only [R|t] + using SolverT = multiview::resection::P3PSolver; + using KernelT = multiview::ResectionKernel_K; // otherwise we just pass the input points - KernelType kernel = KernelType(hasDistortion ? pt2Dundistorted : resectionData.pt2D, resectionData.pt3D, pinholeCam->K()); + const KernelT kernel = KernelT(hasDistortion ? pt2Dundistorted : resectionData.pt2D, resectionData.pt3D, pinholeCam->K()); + + minimumSamples = kernel.getMinimumNbRequiredSamples(); + + // robust estimation of the Projection matrix and its precision + robustEstimation::Mat34Model model; + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, resectionData.vec_inliers, resectionData.max_iteration, &model, precision); + + P = model.getMatrix(); - // Robust estimation of the Projection matrix and its precision - const std::pair ACRansacOut = - aliceVision::robustEstimation::ACRANSAC(kernel, resectionData.vec_inliers, resectionData.max_iteration, &P, precision); - // Update the upper bound precision of the model found by AC-RANSAC + // update the upper bound precision of the model found by AC-RANSAC resectionData.error_max = ACRansacOut.first; break; } @@ -121,27 +111,27 @@ bool SfMLocalizer::Localize(const Pair& imageSize, } // use the P3P solver for generating the model - typedef aliceVision::resection::P3PSolver SolverType; - minimumSamples = SolverType::MINIMUM_SAMPLES; - // use the six point algorithm as Least square solution to refine the model - typedef aliceVision::resection::kernel::SixPointResectionSolver SolverLSType; + using SolverT = multiview::resection::P3PSolver; + using SolverLsT = multiview::resection::Resection6PSolver; - typedef aliceVision::robustEstimation::KernelAdaptorResectionLORansac_K KernelType; + // use the six point algorithm as Least square solution to refine the model + using KernelT = multiview::ResectionKernel_K; // otherwise we just pass the input points - KernelType kernel = KernelType(hasDistortion ? pt2Dundistorted : resectionData.pt2D, resectionData.pt3D, pinholeCam->K()); + const KernelT kernel = KernelT(hasDistortion ? pt2Dundistorted : resectionData.pt2D, resectionData.pt3D, pinholeCam->K()); + + minimumSamples = kernel.getMinimumNbRequiredSamples(); // this is just stupid and ugly, the threshold should be always give as pixel // value, the scorer should be not aware of the fact that we treat squared errors // and normalization inside the kernel // @todo refactor, maybe move scorer directly inside the kernel const double threshold = resectionData.error_max * resectionData.error_max * (kernel.normalizer2()(0, 0) * kernel.normalizer2()(0, 0)); - robustEstimation::ScoreEvaluator scorer(threshold); - P = robustEstimation::LO_RANSAC(kernel, scorer, &resectionData.vec_inliers); + robustEstimation::ScoreEvaluator scorer(threshold); + + const robustEstimation::Mat34Model model = robustEstimation::LO_RANSAC(kernel, scorer, &resectionData.vec_inliers); + P = model.getMatrix(); + break; } @@ -150,7 +140,7 @@ bool SfMLocalizer::Localize(const Pair& imageSize, } } - const bool resection = robustEstimation::hasStrongSupport(resectionData.vec_inliers, resectionData.vec_descType, minimumSamples); + const bool resection = matching::hasStrongSupport(resectionData.vec_inliers, resectionData.vec_descType, minimumSamples); if(!resection) { @@ -164,7 +154,7 @@ bool SfMLocalizer::Localize(const Pair& imageSize, resectionData.projection_matrix = P; Mat3 K, R; Vec3 t; - KRt_From_P(P, &K, &R, &t); + KRt_from_P(P, &K, &R, &t); pose = geometry::Pose3(R, -R.transpose() * t); } diff --git a/src/aliceVision/sfm/pipeline/panorama/CMakeLists.txt b/src/aliceVision/sfm/pipeline/panorama/CMakeLists.txt index 1f876755f4..c3339a6560 100644 --- a/src/aliceVision/sfm/pipeline/panorama/CMakeLists.txt +++ b/src/aliceVision/sfm/pipeline/panorama/CMakeLists.txt @@ -2,5 +2,6 @@ alicevision_add_test(panoramaSfM_test.cpp NAME "sfm_panorama" LINKS aliceVision_sfm aliceVision_feature + aliceVision_matching aliceVision_system ) diff --git a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp index 1b92fd2db6..6151f0b4be 100644 --- a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp +++ b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp @@ -17,14 +17,16 @@ #include #include - #include -#include -#include +#include +#include +#include +#include +#include #include -#include -#include -#include +#include +#include +#include #include @@ -43,10 +45,6 @@ using namespace aliceVision::geometry; using namespace aliceVision::feature; using namespace aliceVision::sfmData; - - -using namespace aliceVision::robustEstimation; - bool robustRelativeRotation_fromE( const Mat3 & K1, const Mat3 & K2, const Mat & x1, const Mat & x2, @@ -56,24 +54,23 @@ bool robustRelativeRotation_fromE( const size_t max_iteration_count) { // Use the 5 point solver to estimate E - typedef essential::kernel::FivePointKernel SolverType; // Define the AContrario adaptor - typedef ACKernelAdaptorEssential< - SolverType, - aliceVision::fundamental::kernel::EpipolarDistanceError, - UnnormalizerT, - Mat3> - KernelType; + using KernelType = multiview::RelativePoseKernel_K< + multiview::relativePose::Essential5PSolver, + multiview::relativePose::FundamentalSymmetricEpipolarDistanceError, + robustEstimation::Mat3Model>; KernelType kernel(x1, size_ima1.first, size_ima1.second, x2, size_ima2.first, size_ima2.second, K1, K2); // Robustly estimation of the Essential matrix and its precision - const std::pair acRansacOut = ACRANSAC(kernel, relativePose_info.vec_inliers, - max_iteration_count, &relativePose_info.essential_matrix, relativePose_info.initial_residual_tolerance); + robustEstimation::Mat3Model model; + const std::pair acRansacOut = robustEstimation::ACRANSAC(kernel, relativePose_info.vec_inliers, max_iteration_count, &model, + relativePose_info.initial_residual_tolerance); + relativePose_info.essential_matrix = model.getMatrix(); relativePose_info.found_residual_precision = acRansacOut.first; - if (relativePose_info.vec_inliers.size() < SolverType::MINIMUM_SAMPLES * ALICEVISION_MINIMUM_SAMPLES_COEF ) + if (relativePose_info.vec_inliers.size() < kernel.getMinimumNbRequiredSamples() * ALICEVISION_MINIMUM_SAMPLES_COEF) { ALICEVISION_LOG_INFO("robustRelativePose: no sufficient coverage (the model does not support enough samples): " << relativePose_info.vec_inliers.size()); return false; // no sufficient coverage (the model does not support enough samples) @@ -140,25 +137,26 @@ aliceVision::EstimationStatus robustHomographyEstimationAC(const Mat2X &x1, Mat3 &H, std::vector &vec_inliers) { - using KernelType = robustEstimation::ACKernelAdaptor< - homography::kernel::FourPointSolver, - homography::kernel::AsymmetricError, - UnnormalizerI, - Mat3>; + using KernelType = multiview::RelativePoseKernel< + multiview::relativePose::Homography4PSolver, + multiview::relativePose::HomographyAsymmetricError, + multiview::UnnormalizerI, + robustEstimation::Mat3Model>; KernelType kernel(x1, imgSize1.first, imgSize1.second, x2, imgSize2.first, imgSize2.second, false); // configure as point to point error model. - + robustEstimation::Mat3Model model; const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, vec_inliers, 1024, - &H, + &model, std::numeric_limits::infinity()); + H = model.getMatrix(); const bool valid{!vec_inliers.empty()}; //@fixme - const bool hasStrongSupport{vec_inliers.size() > KernelType::MINIMUM_SAMPLES * 2.5}; + const bool hasStrongSupport{vec_inliers.size() > kernel.getMinimumNbRequiredSamples() * ALICEVISION_MINIMUM_SAMPLES_COEF}; return {valid, hasStrongSupport}; } diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp index b2ec35b2c4..e40c2d053a 100644 --- a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp +++ b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp @@ -1294,7 +1294,7 @@ bool ReconstructionEngine_sequentialSfM::getBestInitialImagePairs(std::vectorgetFeatures(I, map_tracksCommon[trackId].descType)[iter->second].coords().cast(); @@ -1615,7 +1615,7 @@ bool ReconstructionEngine_sequentialSfM::computeResection(const IndexT viewId, R // setup a default camera model from the found projection matrix Mat3 K, R; Vec3 t; - KRt_From_P(resectionData.projection_matrix, &K, &R, &t); + KRt_from_P(resectionData.projection_matrix, &K, &R, &t); const double focal = (K(0,0) + K(1,1))/2.0; const Vec2 principal_point(K(0,2), K(1,2)); @@ -1797,7 +1797,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData& const Vec2 xJ = _featuresPerView->getFeatures(J, track.descType)[track.featPerView.at(J)].coords().cast(); // -- Triangulate: - TriangulateDLT(camI->get_projective_equivalent(poseI), + multiview::TriangulateDLT(camI->get_projective_equivalent(poseI), camI->get_ud_pixel(xI), camJ->get_projective_equivalent(poseJ), camI->get_ud_pixel(xJ), @@ -1849,9 +1849,9 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData& Vec4 X_homogeneous = Vec4::Zero(); std::vector inliersIndex; - TriangulateNViewLORANSAC(features, Ps, &X_homogeneous, &inliersIndex, 8.0); + multiview::TriangulateNViewLORANSAC(features, Ps, &X_homogeneous, &inliersIndex, 8.0); - HomogeneousToEuclidean(X_homogeneous, &X_euclidean); + homogeneousToEuclidean(X_homogeneous, &X_euclidean); // observations = {350, 380, 442} | inliersIndex = [0, 1] | inliers = {350, 380} for (const auto & id : inliersIndex) @@ -2006,7 +2006,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, cons const Mat34 pI = camI->get_projective_equivalent(poseI); const Mat34 pJ = camJ->get_projective_equivalent(poseJ); - TriangulateDLT(pI, xI_ud, pJ, xJ_ud, &X_euclidean); + multiview::TriangulateDLT(pI, xI_ud, pJ, xJ_ud, &X_euclidean); // Check triangulation results // - Check angle (small angle leads imprecise triangulation) diff --git a/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp b/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp index 5e804f39b8..3622b0ab9c 100644 --- a/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp +++ b/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp @@ -8,8 +8,8 @@ #include "StructureEstimationFromKnownPoses.hpp" #include #include -#include -#include +#include +#include #include #include #include @@ -112,9 +112,9 @@ void StructureEstimationFromKnownPoses::match(const SfMData& sfmData, for(feature::EImageDescriberType descType: commonDescTypes) { std::vector matches; - #ifdef ALICEVISION_EXHAUSTIVE_MATCHING - robustEstimation::GuidedMatching - +#ifdef ALICEVISION_EXHAUSTIVE_MATCHING + matching::guidedMatching + ( F_lr, iterIntrinsicL->second.get(), @@ -129,8 +129,7 @@ void StructureEstimationFromKnownPoses::match(const SfMData& sfmData, const Vec3 epipole2 = epipole_from_P(P_R, poseL); //const feature::Regions& regions = regionsPerView.getRegions(it->first); - robustEstimation::GuidedMatching_Fundamental_Fast - + matching::guidedMatchingFundamentalFast ( F_lr, epipole2, @@ -207,7 +206,7 @@ void StructureEstimationFromKnownPoses::filter( iterTracks != map_tracksCommon.end(); ++iterTracks) { { const track::Track & subTrack = iterTracks->second; - Triangulation trianObj; + multiview::Triangulation trianObj; for (auto iter = subTrack.featPerView.begin(); iter != subTrack.featPerView.end(); ++iter) { const size_t imaIndex = iter->first; diff --git a/src/aliceVision/sfm/sfmTriangulation.cpp b/src/aliceVision/sfm/sfmTriangulation.cpp index 3d0d2244fb..521f67efc2 100644 --- a/src/aliceVision/sfm/sfmTriangulation.cpp +++ b/src/aliceVision/sfm/sfmTriangulation.cpp @@ -51,7 +51,7 @@ void StructureComputation_blind::triangulate(sfmData::SfMData& sfmData) const ++(*my_progress_bar); } // Triangulate each landmark - Triangulation trianObj; + multiview::Triangulation trianObj; const sfmData::Observations & observations = iterTracks->second.observations; for(const auto& itObs : observations) { @@ -177,7 +177,7 @@ bool StructureComputation_robust::robust_triangulation(const sfmData::SfMData& s for(IndexT i = 0; i < nbIter; ++i) { std::set samples; - robustEstimation::UniformSample(std::min(std::size_t(min_sample_index), observations.size()), observations.size(), samples); + robustEstimation::uniformSample(std::min(std::size_t(min_sample_index), observations.size()), observations.size(), samples); // Hypothesis generation. const Vec3 current_model = track_sample_triangulation(sfmData, observations, samples); @@ -242,7 +242,7 @@ Vec3 StructureComputation_robust::track_sample_triangulation(const sfmData::SfMD const sfmData::Observations& observations, const std::set& samples) const { - Triangulation trianObj; + multiview::Triangulation trianObj; for (const IndexT idx : samples) { assert(idx < observations.size()); diff --git a/src/aliceVision/sfm/utils/syntheticScene.cpp b/src/aliceVision/sfm/utils/syntheticScene.cpp index 20670713a1..65b79e969b 100644 --- a/src/aliceVision/sfm/utils/syntheticScene.cpp +++ b/src/aliceVision/sfm/utils/syntheticScene.cpp @@ -189,7 +189,7 @@ sfmData::SfMData getInputRigScene(const NViewDataSet& d, { const sfmData::View& view = *sfmData.views.at(viewId); const geometry::Pose3 camPose = sfmData.getPose(view).getTransform(); - const Vec2 pt = Project(sfmData.intrinsics.at(0)->get_projective_equivalent(camPose), landmark.X); + const Vec2 pt = project(sfmData.intrinsics.at(0)->get_projective_equivalent(camPose), landmark.X); landmark.observations[viewId] = sfmData::Observation(pt, landmarkId, unknownScale); } sfmData.structure[landmarkId] = landmark; diff --git a/src/aliceVision/sfmDataIO/gtIO.cpp b/src/aliceVision/sfmDataIO/gtIO.cpp index 45983fed93..05299ec944 100644 --- a/src/aliceVision/sfmDataIO/gtIO.cpp +++ b/src/aliceVision/sfmDataIO/gtIO.cpp @@ -72,7 +72,7 @@ bool read_aliceVision_Camera(const std::string& camName, camera::Pinhole& cam, g } Mat3 K, R; Vec3 t; - KRt_From_P(P, &K, &R, &t); + KRt_from_P(P, &K, &R, &t); cam = camera::Pinhole(0,0,K); // K.transpose() is applied to give [R t] to the constructor instead of P = K [R t] pose = geometry::Pose3(K.transpose() * P); diff --git a/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp b/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp index f18eaa4333..63fb3c629b 100644 --- a/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp +++ b/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp @@ -6,7 +6,8 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include -#include +#include +#include #include diff --git a/src/aliceVision/unitTest.hpp b/src/aliceVision/unitTest.hpp index 2b59f5bf96..2b0f93a35b 100644 --- a/src/aliceVision/unitTest.hpp +++ b/src/aliceVision/unitTest.hpp @@ -8,7 +8,7 @@ #pragma once -#include "aliceVision/numeric/numeric.hpp" +#include #include diff --git a/src/samples/CMakeLists.txt b/src/samples/CMakeLists.txt index bb0da47377..8ac05b7c5b 100644 --- a/src/samples/CMakeLists.txt +++ b/src/samples/CMakeLists.txt @@ -14,6 +14,7 @@ add_subdirectory(robustEssential) add_subdirectory(robustEssentialBA) add_subdirectory(robustEssentialSpherical) add_subdirectory(robustFundamental) +add_subdirectory(robustFundamentalF10) add_subdirectory(robustFundamentalGuided) add_subdirectory(robustHomography) add_subdirectory(robustHomographyGrowing) diff --git a/src/samples/accv12Demo/main_openCVFitting.cpp b/src/samples/accv12Demo/main_openCVFitting.cpp index 550d596a7f..93ae73e5ab 100644 --- a/src/samples/accv12Demo/main_openCVFitting.cpp +++ b/src/samples/accv12Demo/main_openCVFitting.cpp @@ -7,7 +7,7 @@ #include "aliceVision/numeric/numeric.hpp" #include "aliceVision/multiview/homographyKernelSolver.hpp" -#include "aliceVision/multiview/conditioning.hpp" +#include "aliceVision/robustEstimation/conditioning.hpp" #include "aliceVision/robustEstimation/maxConsensus.hpp" #include "aliceVision/robustEstimation/ScoreEvaluator.hpp" #include "aliceVision/robustEstimation/ACRansac.hpp" @@ -189,7 +189,7 @@ int main(int, char**) double thresholdH = 4.0; double NFAH; { - typedef ACKernelAdaptor< + typedef AC_RelativePoseKernel< aliceVision::homography::kernel::FourPointSolver, aliceVision::homography::kernel::AsymmetricError, UnnormalizerI, @@ -211,7 +211,7 @@ int main(int, char**) typedef homography::kernel::Kernel KernelType; KernelType kernel(xA, xB); if (bDoRansac) - Hransac = MaxConsensus(kernel, ScoreEvaluator(Square(thresholdransac)), &vec_inliersRansac, 1024); + Hransac = maxConsensus(kernel, ScoreEvaluator(Square(thresholdransac)), &vec_inliersRansac, 1024); if (vec_inliersRansac.size()<12) thresholdransac = 900; } diff --git a/src/samples/featuresRepeatability/main_repeatabilityDataset.cpp b/src/samples/featuresRepeatability/main_repeatabilityDataset.cpp index 6f73575953..64a55cd0f4 100644 --- a/src/samples/featuresRepeatability/main_repeatabilityDataset.cpp +++ b/src/samples/featuresRepeatability/main_repeatabilityDataset.cpp @@ -9,8 +9,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -366,9 +366,8 @@ int main(int argc, char **argv) PointsToMat(pointsFeaturesI, xI); IndMatches matches_0I; - robustEstimation::GuidedMatching - ( - dataset.H(i).transpose(), x0, xI, Square(m_dPrecision_robust), matches_0I); + matching::guidedMatching( + robustEstimation::Mat3Model(dataset.H(i).transpose()), x0, xI, Square(m_dPrecision_robust), matches_0I); std::cout << "Feature repeatablity Results" << "\n" << "*******************************" << "\n" @@ -404,9 +403,8 @@ int main(int argc, char **argv) PointsToMat(putativesMatches, pointsFeatures0, pointsFeaturesI, x0, xI); IndMatches matches_0I; - robustEstimation::GuidedMatching - ( - dataset.H(i).transpose(), x0, xI, Square(m_dPrecision_robust), matches_0I); + matching::guidedMatching( + robustEstimation::Mat3Model(dataset.H(i).transpose()), x0, xI, Square(m_dPrecision_robust), matches_0I); std::cout << "Feature matching repeatability Results" << "\n" << "*******************************" << "\n" diff --git a/src/samples/kvldFilter/main_kvldFilter.cpp b/src/samples/kvldFilter/main_kvldFilter.cpp index 92084dc1c6..e2e2eb924c 100644 --- a/src/samples/kvldFilter/main_kvldFilter.cpp +++ b/src/samples/kvldFilter/main_kvldFilter.cpp @@ -9,12 +9,11 @@ #include #include #include -#include -#include +#include +#include #include #include #include -#include #include diff --git a/src/samples/robustEssential/main_robustEssential.cpp b/src/samples/robustEssential/main_robustEssential.cpp index 3fcb73a13e..6b4b329b01 100644 --- a/src/samples/robustEssential/main_robustEssential.cpp +++ b/src/samples/robustEssential/main_robustEssential.cpp @@ -207,7 +207,7 @@ int main() { const PointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j]; // Point triangulation Vec3 X; - TriangulateDLT(P1, LL.coords().cast(), P2, RR.coords().cast(), &X); + multiview::TriangulateDLT(P1, LL.coords().cast(), P2, RR.coords().cast(), &X); // Reject point that is behind the camera if (pose0.depth(X) < 0 && pose1.depth(X) < 0) continue; diff --git a/src/samples/robustEssentialBA/main_robustEssentialBA.cpp b/src/samples/robustEssentialBA/main_robustEssentialBA.cpp index 6a57599a05..0e32cc118b 100644 --- a/src/samples/robustEssentialBA/main_robustEssentialBA.cpp +++ b/src/samples/robustEssentialBA/main_robustEssentialBA.cpp @@ -251,7 +251,7 @@ int main() { const PointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j]; // Point triangulation Vec3 X; - TriangulateDLT(P1, LL.coords().cast(), P2, RR.coords().cast(), &X); + multiview::TriangulateDLT(P1, LL.coords().cast(), P2, RR.coords().cast(), &X); // Reject point that is behind the camera if (pose0.depth(X) < 0 && pose1.depth(X) < 0) continue; diff --git a/src/samples/robustEssentialSpherical/main_robustEssentialSpherical.cpp b/src/samples/robustEssentialSpherical/main_robustEssentialSpherical.cpp index d685537bbc..d0b04641d8 100644 --- a/src/samples/robustEssentialSpherical/main_robustEssentialSpherical.cpp +++ b/src/samples/robustEssentialSpherical/main_robustEssentialSpherical.cpp @@ -11,8 +11,8 @@ #include "aliceVision/matching/RegionsMatcher.hpp" #include "aliceVision/multiview/essential.hpp" #include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/multiview/conditioning.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" +#include "aliceVision/robustEstimation/conditioning.hpp" +#include "aliceVision/multiview/AngularRadianErrorKernel.hpp" #include "sphericalCam.hpp" @@ -153,19 +153,18 @@ int main() { typedef aliceVision::spherical_cam::EssentialKernel_spherical Kernel; // Define the AContrario angular error adaptor - typedef aliceVision::robustEstimation::ACKernelAdaptor_AngularRadianError< + typedef aliceVision::multiview::AngularRadianErrorKernel< aliceVision::spherical_cam::EightPointRelativePoseSolver, aliceVision::spherical_cam::AngularError, - Mat3> + robustEstimation::Mat3Model> KernelType; KernelType kernel(xL_spherical, xR_spherical); // Robust estimation of the Essential matrix and it's precision - Mat3 E; + robustEstimation::Mat3Model E; const double precision = std::numeric_limits::infinity(); - const std::pair ACRansacOut = - ACRANSAC(kernel, vec_inliers, 1024, &E, precision); + const std::pair ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &E, precision); const double & threshold = ACRansacOut.first; const double & NFA = ACRansacOut.second; @@ -182,17 +181,18 @@ int main() { // Accumulator to find the best solution std::vector f(4, 0); - std::vector Es; // Essential, - std::vector Rs; // Rotation matrix. - std::vector ts; // Translation matrix. + std::vector Es; // Essential, + std::vector Rs; // Rotation matrix. + std::vector ts; // Translation matrix. Es.push_back(E); + // Recover best rotation and translation from E. - MotionFromEssential(E, &Rs, &ts); + motionFromEssential(E.getMatrix(), &Rs, &ts); //-> Test the 4 solutions will all the point Mat34 P1; - P_From_KRt(Mat3::Identity(), Mat3::Identity(), Vec3::Zero(), &P1); + P_from_KRt(Mat3::Identity(), Mat3::Identity(), Vec3::Zero(), &P1); std::vector< std::vector > vec_newInliers(4); std::vector< std::vector > vec_3D(4); @@ -200,7 +200,7 @@ int main() { const Mat3 &R2 = Rs[kk]; const Vec3 &t2 = ts[kk]; Mat34 P2; - P_From_KRt(Mat3::Identity(), R2, t2, &P2); + P_from_KRt(Mat3::Identity(), R2, t2, &P2); //-- For each inlier: // - triangulate diff --git a/src/samples/robustEssentialSpherical/sphericalCam.hpp b/src/samples/robustEssentialSpherical/sphericalCam.hpp index 9f02c7dbdc..250ff11641 100644 --- a/src/samples/robustEssentialSpherical/sphericalCam.hpp +++ b/src/samples/robustEssentialSpherical/sphericalCam.hpp @@ -7,8 +7,10 @@ #pragma once -#include "aliceVision/numeric/numeric.hpp" -#include "aliceVision/multiview/projection.hpp" +#include +#include +#include +#include // [1] "Robust and accurate calibration of camera networks". PhD. // Authors: Pierre MOULON @@ -49,39 +51,65 @@ using namespace std; * See page 294 in HZ Result 11.1. * */ -struct EightPointRelativePoseSolver { - enum { MINIMUM_SAMPLES = 8 }; - enum { MAX_MODELS = 1 }; - static void Solve(const Mat &x1, const Mat &x2, std::vector *pvec_E) - { - assert(3 == x1.rows()); - assert(8 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - MatX9 A(x1.cols(), 9); - EncodeEpipolarEquation(x1, x2, &A); - - Vec9 e; - Nullspace(&A, &e); - Mat3 E = Map(e.data()); - - // Find the closest essential matrix to E in frobenius norm - // E = UD'VT - if (x1.cols() > 8) { - Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); - Vec3 d = USV.singularValues(); - double a = d[0]; - double b = d[1]; - d << (a+b)/2., (a+b)/2., 0.0; - E = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose(); +class EightPointRelativePoseSolver : public robustEstimation::ISolver +{ +public: + + using ModelT = robustEstimation::Mat3Model; + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 8; } - pvec_E->push_back(E); - } + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + + void solve(const Mat &x1, const Mat &x2, std::vector& models) const override + { + assert(3 == x1.rows()); + assert(8 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + MatX9 A(x1.cols(), 9); + encodeEpipolarEquation(x1, x2, &A); + + Vec9 e; + Nullspace(&A, &e); + Mat3 E = Map(e.data()); + + // Find the closest essential matrix to E in frobenius norm + // E = UD'VT + if (x1.cols() > 8) { + Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec3 d = USV.singularValues(); + double a = d[0]; + double b = d[1]; + d << (a+b)/2., (a+b)/2., 0.0; + E = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose(); + } + models.emplace_back(E); + } + + void solve(const Mat& x1, const Mat& x2, std::vector& models, const std::vector& weights) const override + { + throw std::logic_error("EightPointRelativePoseSolver does not support problem solving with weights."); + } template - static inline void EncodeEpipolarEquation(const TMatX &x1, const TMatX &x2, TMatA *A) { + static inline void encodeEpipolarEquation(const TMatX &x1, const TMatX &x2, TMatA *A) { for (int i = 0; i < x1.cols(); ++i) { (*A)(i, 0) = x2(0, i) * x1(0, i); // 0 represents x coords, (*A)(i, 1) = x2(0, i) * x1(1, i); // 1 represents y coords, @@ -97,9 +125,11 @@ struct EightPointRelativePoseSolver { }; // Return the angular error between [0; PI/2] -struct AngularError { - static double Error(const Mat3 &model, const Vec3 &x1, const Vec3 &x2) { - const Vec3 Em1 = (model * x1).normalized(); +struct AngularError +{ + double error(const robustEstimation::Mat3Model& model, const Vec3 &x1, const Vec3 &x2) const + { + const Vec3 Em1 = (model.getMatrix() * x1).normalized(); double angleVal = (x2.transpose() * Em1); angleVal /= (x2.norm() * Em1.norm()); return abs(asin(angleVal)); @@ -107,34 +137,26 @@ struct AngularError { }; class EssentialKernel_spherical + : public robustEstimation::PointFittingKernel { public: - typedef Mat3 Model; - enum { MINIMUM_SAMPLES = EightPointRelativePoseSolver::MINIMUM_SAMPLES }; - - EssentialKernel_spherical(const Mat &x1, const Mat &x2) : x1_(x1), x2_(x2) {} - void Fit(const vector &samples, std::vector *models) const { - const Mat x1 = ExtractColumns(x1_, samples); - const Mat x2 = ExtractColumns(x2_, samples); + using ModelT = robustEstimation::Mat3Model; + using KernelBase = robustEstimation::PointFittingKernel; - assert(3 == x1.rows()); - assert(MINIMUM_SAMPLES <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); + EssentialKernel_spherical(const Mat& x1, const Mat& x2) + : KernelBase(x1, x2) + {} - EightPointRelativePoseSolver::Solve(x1, x2, models); - } - - size_t NumSamples() const {return x1_.cols();} + void fit(const vector& samples, std::vector& models) const + { + assert(3 == KernelBase::_x1.rows()); + assert(KernelBase::getMinimumNbRequiredSamples() <= KernelBase::_x1.cols()); + assert(KernelBase::_x1.rows() == KernelBase::_x2.rows()); + assert(KernelBase::_x1.cols() == KernelBase::_x2.cols()); - /// Return the angular error (between 0 and PI/2) - double Error(size_t sample, const Model &model) const { - return AngularError::Error(model, x1_.col(sample), x2_.col(sample)); + KernelBase::_kernelSolver.solve(KernelBase::_x1, KernelBase::_x2, models); } - - protected: - const Mat & x1_, & x2_; }; // Solve: @@ -161,7 +183,7 @@ void TriangulateDLT(const Mat34 &P1, const Vec3 &x1, Vec3 *X_euclidean) { Vec4 X_homogeneous; TriangulateDLT(P1, x1, P2, x2, &X_homogeneous); - HomogeneousToEuclidean(X_homogeneous, X_euclidean); + homogeneousToEuclidean(X_homogeneous, X_euclidean); } } // namespace spherical_cam diff --git a/src/samples/robustFundamental/main_robustFundamental.cpp b/src/samples/robustFundamental/main_robustFundamental.cpp index 697919473e..748ff39630 100644 --- a/src/samples/robustFundamental/main_robustFundamental.cpp +++ b/src/samples/robustFundamental/main_robustFundamental.cpp @@ -9,10 +9,10 @@ #include "aliceVision/feature/feature.hpp" #include "aliceVision/feature/sift/ImageDescriber_SIFT.hpp" #include "aliceVision/matching/RegionsMatcher.hpp" -#include "aliceVision/multiview/fundamentalKernelSolver.hpp" -#include "aliceVision/multiview/conditioning.hpp" +#include "aliceVision/multiview/relativePose/FundamentalKernel.hpp" +#include "aliceVision/robustEstimation/conditioning.hpp" #include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" +#include "aliceVision/multiview/RelativePoseKernel.hpp" #include "dependencies/vectorGraphics/svgDrawer.hpp" @@ -172,11 +172,11 @@ int main(int argc, char **argv) //-- Fundamental robust estimation std::vector vec_inliers; - typedef ACKernelAdaptor< - aliceVision::fundamental::kernel::SevenPointSolver, - aliceVision::fundamental::kernel::SymmetricEpipolarDistanceError, - UnnormalizerT, - Mat3> + typedef multiview::RelativePoseKernel< + multiview::relativePose::Fundamental7PSolver, + multiview::relativePose::FundamentalSymmetricEpipolarDistanceError, + multiview::UnnormalizerT, + robustEstimation::Mat3Model> KernelType; KernelType kernel( @@ -184,14 +184,14 @@ int main(int argc, char **argv) xR, imageR.Width(), imageR.Height(), true); // configure as point to line error model. - Mat3 F; + robustEstimation::Mat3Model F; const std::pair ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &F, Square(4.0)); // Upper bound of authorized threshold const double & thresholdF = ACRansacOut.first; // Check the fundamental support some point to be considered as valid - if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) + if (vec_inliers.size() > kernel.getMinimumNbRequiredSamples() *2.5) { std::cout << "\nFound a fundamental under the confidence threshold of: " << thresholdF << " pixels\n\twith: " << vec_inliers.size() << " inliers" @@ -214,9 +214,7 @@ int main(int argc, char **argv) svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0)); svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0)); // residual computation - vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(F, - LL.coords().cast(), - RR.coords().cast())); + vec_residuals[i] = std::sqrt(KernelType::ErrorT().error(F, LL.coords().cast(), RR.coords().cast())); } const string out_filename = "04_ACRansacFundamental.svg"; ofstream svgFile( out_filename.c_str() ); diff --git a/src/samples/robustFundamentalF10/CMakeLists.txt b/src/samples/robustFundamentalF10/CMakeLists.txt new file mode 100644 index 0000000000..c010df8062 --- /dev/null +++ b/src/samples/robustFundamentalF10/CMakeLists.txt @@ -0,0 +1,11 @@ +alicevision_add_software(aliceVision_samples_robustFundamentalF10 + SOURCE main_robustFundamentalF10.cpp + FOLDER ${FOLDER_SAMPLES} + LINKS aliceVision_image + aliceVision_multiview + aliceVision_feature + aliceVision_matching + aliceVision_system + vlsift + ${Boost_LIBRARIES} +) diff --git a/src/samples/robustFundamentalF10/main_robustFundamentalF10.cpp b/src/samples/robustFundamentalF10/main_robustFundamentalF10.cpp new file mode 100644 index 0000000000..01a1277d7d --- /dev/null +++ b/src/samples/robustFundamentalF10/main_robustFundamentalF10.cpp @@ -0,0 +1,244 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2017 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 "dependencies/vectorGraphics/svgDrawer.hpp" + +#include + +#include +#include + +// These constants define the current software version. +// They must be updated when the command line is changed. +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 +#define ALICEVISION_SOFTWARE_VERSION_MINOR 0 + +using namespace svg; +using namespace std; +using namespace aliceVision; +using namespace aliceVision::image; +using namespace aliceVision::matching; +using namespace aliceVision::robustEstimation; + +namespace po = boost::program_options; + +int main(int argc, char **argv) +{ + std::string filenameLeft; + std::string filenameRight; + std::string describerPreset = feature::EImageDescriberPreset_enumToString(feature::EImageDescriberPreset::NORMAL); + std::string describerTypesName = feature::EImageDescriberType_enumToString(feature::EImageDescriberType::SIFT); + + po::options_description allParams("AliceVision Sample robustFundamental"); + allParams.add_options() + ("filenameLeft,l", po::value(&filenameLeft)->required(), + "Left image.") + ("filenameRight,r", po::value(&filenameRight)->required(), + "Right image.") + ("describerTypes,d", po::value(&describerTypesName)->default_value(describerTypesName), + feature::EImageDescriberType_informations().c_str()) + ("describerPreset,p", po::value(&describerPreset)->default_value(describerPreset), + "Control the ImageDescriber configuration (low, medium, normal, high, ultra).\n" + "Configuration 'ultra' can take long time !"); + + po::variables_map vm; + try + { + po::store(po::parse_command_line(argc, argv, allParams), vm); + + if(vm.count("help") || (argc == 1)) + { + ALICEVISION_COUT(allParams); + return EXIT_SUCCESS; + } + po::notify(vm); + } + catch(boost::program_options::required_option& e) + { + ALICEVISION_CERR("ERROR: " << e.what()); + ALICEVISION_COUT("Usage:\n\n" << allParams); + return EXIT_FAILURE; + } + catch(boost::program_options::error& e) + { + ALICEVISION_CERR("ERROR: " << e.what()); + ALICEVISION_COUT("Usage:\n\n" << allParams); + return EXIT_FAILURE; + } + + Image imageLeft; + readImage(filenameLeft, imageLeft, EImageColorSpace::LINEAR); + const auto imageLeftSize = std::make_pair(imageLeft.Width(), imageLeft.Height()); + Image imageRight; + readImage(filenameRight, imageRight, EImageColorSpace::LINEAR); + const auto imageRightSize = std::make_pair(imageRight.Width(), imageRight.Height()); + + //-- + // Detect regions thanks to an image_describer + //-- + using namespace aliceVision::feature; + std::unique_ptr image_describer(new ImageDescriber_SIFT); + if (!describerPreset.empty()) + { + image_describer->setConfigurationPreset(describerPreset); + } + + std::map > regions_perImage; + image_describer->describe(imageLeft, regions_perImage[0]); + image_describer->describe(imageRight, regions_perImage[1]); + + const SIFT_Regions* regionsL = dynamic_cast(regions_perImage.at(0).get()); + const SIFT_Regions* regionsR = dynamic_cast(regions_perImage.at(1).get()); + + const PointFeatures& featsL = regions_perImage.at(0)->Features(); + const PointFeatures& featsR = regions_perImage.at(1)->Features(); + + // Show both images side by side + { + const string out_filename = "01.features."+describerTypesName+".svg"; + drawKeypointsSideBySide(filenameLeft, + imageLeftSize, + featsL, + filenameRight, + imageRightSize, + featsR, + out_filename); + } + + std::vector vec_PutativeMatches; + //-- Perform matching -> find Nearest neighbor, filtered with Distance ratio + + // Find corresponding points + matching::DistanceRatioMatch( + 0.8, matching::BRUTE_FORCE_L2, + *regions_perImage.at(0).get(), + *regions_perImage.at(1).get(), + vec_PutativeMatches); + + // two ways to show the matches + { + // side by side + drawMatchesSideBySide(filenameLeft, imageLeftSize, featsL, + filenameRight, imageRightSize, featsR, + vec_PutativeMatches, + "02.putativeMatchesSideBySide." + describerTypesName + ".svg"); + } + { + // draw the matches over one of the images as they were tracks + + const bool isLeft = true; + const bool richKpts = false; + saveMatchesAsMotion(filenameLeft, imageLeftSize, featsL, featsR, + vec_PutativeMatches, + "03.putativeMatchesMotion."+describerTypesName+".svg", + isLeft, richKpts); + // Display some statistics + std::cout + << regions_perImage.at(0)->RegionCount() << " #Features on image A" << std::endl + << regions_perImage.at(1)->RegionCount() << " #Features on image B" << std::endl + << vec_PutativeMatches.size() << " #matches with Distance Ratio filter" << std::endl; + } + + + // Fundamental geometry filtering of putative matches using the F10 solver + { + //A. get back interest point and send it to the robust estimation framework + Mat xL(2, vec_PutativeMatches.size()); + Mat xR(2, vec_PutativeMatches.size()); + + for (size_t k = 0; k < vec_PutativeMatches.size(); ++k) + { + const PointFeature & imaL = featsL[vec_PutativeMatches[k]._i]; + const PointFeature & imaR = featsR[vec_PutativeMatches[k]._j]; + xL.col(k) = imaL.coords().cast(); + xR.col(k) = imaR.coords().cast(); + } + + //-- Fundamental robust estimation + std::vector vec_inliers; + typedef multiview::RelativePoseKernel< + multiview::relativePose::Fundamental10PSolver, + multiview::relativePose::FundamentalSymmetricEpipolarDistanceError, + multiview::UnnormalizerT, + multiview::relativePose::Fundamental10PModel> + KernelType; + + KernelType kernel( + xL, imageLeft.Width(), imageLeft.Height(), + xR, imageRight.Width(), imageRight.Height(), + true); // configure as point to line error model. + + multiview::relativePose::Fundamental10PModel F; + const std::pair ACRansacOut = robustEstimation::ACRANSAC(kernel, vec_inliers, 1024, &F, + Square(4.0)); // Upper bound of authorized threshold + + const double & thresholdF = ACRansacOut.first; + + // Check the fundamental support some point to be considered as valid + if(vec_inliers.size() > kernel.getMinimumNbRequiredSamples() * 2.5) + { + std::cout << "\nFound a fundamental under the confidence threshold of: " + << thresholdF << " pixels\n\twith: " << vec_inliers.size() << " inliers" + << " from: " << vec_PutativeMatches.size() + << " putatives correspondences" + << std::endl; + + //Show fundamental validated point and compute residuals + std::vector vec_residuals(vec_inliers.size(), 0.0); + svgDrawer svgStream( imageLeft.Width() + imageRight.Width(), max(imageLeft.Height(), imageRight.Height())); + svgStream.drawImage(filenameLeft, imageLeft.Width(), imageLeft.Height()); + svgStream.drawImage(filenameRight, imageRight.Width(), imageRight.Height(), imageLeft.Width()); + for ( size_t i = 0; i < vec_inliers.size(); ++i) + { + const PointFeature & LL = regionsL->Features()[vec_PutativeMatches[vec_inliers[i]]._i]; + const PointFeature & RR = regionsR->Features()[vec_PutativeMatches[vec_inliers[i]]._j]; + const Vec2f L = LL.coords(); + const Vec2f R = RR.coords(); + svgStream.drawLine(L.x(), L.y(), R.x()+imageLeft.Width(), R.y(), svgStyle().stroke("green", 2.0)); + svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0)); + svgStream.drawCircle(R.x()+imageLeft.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0)); + // residual computation + vec_residuals[i] = std::sqrt(KernelType::ErrorT().error(F, + LL.coords().cast(), + RR.coords().cast())); + } + const string out_filename = "04_ACRansacFundamental.svg"; + ofstream svgFile( out_filename.c_str() ); + svgFile << svgStream.closeSvgFile().str(); + svgFile.close(); + + // Display some statistics of reprojection errors + MinMaxMeanMedian stats(vec_residuals.begin(), vec_residuals.end()); + + std::cout << std::endl + << "Fundamental matrix estimation, residuals statistics:" << "\n" + << "\t-- Residual min:\t" << stats.min << std::endl + << "\t-- Residual median:\t" << stats.median << std::endl + << "\t-- Residual max:\t " << stats.max << std::endl + << "\t-- Residual mean:\t " << stats.mean << std::endl; + } + else + { + std::cout << "ACRANSAC was unable to estimate a rigid fundamental" + << std::endl; + } + } + return EXIT_SUCCESS; +} diff --git a/src/samples/robustFundamentalGuided/main_robustFundamentalGuided.cpp b/src/samples/robustFundamentalGuided/main_robustFundamentalGuided.cpp index 6959a0ae2d..96a5235738 100644 --- a/src/samples/robustFundamentalGuided/main_robustFundamentalGuided.cpp +++ b/src/samples/robustFundamentalGuided/main_robustFundamentalGuided.cpp @@ -9,11 +9,11 @@ #include #include #include -#include -#include +#include +#include #include -#include -#include +#include +#include #include @@ -128,25 +128,24 @@ int main() { //-- Fundamental robust estimation std::vector vec_inliers; - typedef ACKernelAdaptor< - aliceVision::fundamental::kernel::SevenPointSolver, - aliceVision::fundamental::kernel::SymmetricEpipolarDistanceError, - UnnormalizerT, - Mat3> - KernelType; + using KernelType = multiview::RelativePoseKernel< + multiview::relativePose::Fundamental7PSolver, + multiview::relativePose::FundamentalSymmetricEpipolarDistanceError, + multiview::UnnormalizerT, + robustEstimation::Mat3Model>; KernelType kernel( xL, imageL.Width(), imageL.Height(), xR, imageR.Width(), imageR.Height(), true); // configure as point to line error model. - Mat3 F; + robustEstimation::Mat3Model F; const std::pair ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &F, Square(4.0)); // Upper bound of authorized threshold const double & thresholdF = ACRansacOut.first; // Check the fundamental support some point to be considered as valid - if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) { + if (vec_inliers.size() > kernel.getMinimumNbRequiredSamples() *2.5) { std::cout << "\nFound a fundamental under the confidence threshold of: " << thresholdF << " pixels\n\twith: " << vec_inliers.size() << " inliers" @@ -168,9 +167,7 @@ int main() { svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0)); svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0)); // residual computation - vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(F, - LL.coords().cast(), - RR.coords().cast())); + vec_residuals[i] = std::sqrt(KernelType::ErrorT().error(F, LL.coords().cast(), RR.coords().cast())); } const string out_filename = "04_ACRansacFundamental.svg"; ofstream svgFile( out_filename.c_str() ); @@ -201,15 +198,14 @@ int main() { //a. by considering only the geometric error - robustEstimation::GuidedMatching( + matching::guidedMatching( F, xL, xR, Square(thresholdF), vec_corresponding_indexes[0]); std::cout << "\nGuided Fundamental matching (geometric error) found " << vec_corresponding_indexes[0].size() << " correspondences." << std::endl; // b. by considering geometric error and descriptor distance ratio - robustEstimation::GuidedMatching - ( + matching::guidedMatching( F, NULL, *regions_perImage.at(0), // Null since no Intrinsic is defined NULL, *regions_perImage.at(1), // Null since no Intrinsic is defined diff --git a/src/samples/robustHomography/main_robustHomography.cpp b/src/samples/robustHomography/main_robustHomography.cpp index 7712b3bb74..0aaec7ba19 100644 --- a/src/samples/robustHomography/main_robustHomography.cpp +++ b/src/samples/robustHomography/main_robustHomography.cpp @@ -10,10 +10,12 @@ #include "aliceVision/feature/feature.hpp" #include "aliceVision/feature/sift/ImageDescriber_SIFT.hpp" #include "aliceVision/matching/RegionsMatcher.hpp" -#include "aliceVision/multiview/homographyKernelSolver.hpp" -#include "aliceVision/multiview/conditioning.hpp" +#include "aliceVision/multiview/relativePose/Homography4PSolver.hpp" +#include "aliceVision/multiview/relativePose/HomographyError.hpp" +#include "aliceVision/multiview/Unnormalizer.hpp" +#include "aliceVision/robustEstimation/conditioning.hpp" #include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" +#include "aliceVision/multiview/RelativePoseKernel.hpp" #include "dependencies/vectorGraphics/svgDrawer.hpp" @@ -127,11 +129,11 @@ int main() { //-- Homography robust estimation std::vector vec_inliers; - typedef ACKernelAdaptor< - aliceVision::homography::kernel::FourPointSolver, - aliceVision::homography::kernel::AsymmetricError, - UnnormalizerI, - Mat3> + typedef multiview::RelativePoseKernel< + multiview::relativePose::Homography4PSolver, + multiview::relativePose::HomographyAsymmetricError, + multiview::UnnormalizerI, + robustEstimation::Mat3Model> KernelType; KernelType kernel( @@ -139,13 +141,13 @@ int main() { xR, imageR.Width(), imageR.Height(), false); // configure as point to point error model. - Mat3 H; + robustEstimation::Mat3Model H; const std::pair ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &H, std::numeric_limits::infinity()); const double & thresholdH = ACRansacOut.first; // Check the homography support some point to be considered as valid - if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) { + if (vec_inliers.size() > kernel.getMinimumNbRequiredSamples() *2.5) { std::cout << "\nFound a homography under the confidence threshold of: " << thresholdH << " pixels\n\twith: " << vec_inliers.size() << " inliers" @@ -167,9 +169,7 @@ int main() { svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0)); svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0)); // residual computation - vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(H, - LL.coords().cast(), - RR.coords().cast())); + vec_residuals[i] = std::sqrt(KernelType::ErrorT().error(H, LL.coords().cast(), RR.coords().cast())); } string out_filename = "04_ACRansacHomography.svg"; ofstream svgFile( out_filename.c_str() ); @@ -195,7 +195,7 @@ int main() { // Create and fill the output image Image imaOut(imageL.Width(), imageL.Height()); - image::Warp(image, H, imaOut); + image::Warp(image, H.getMatrix(), imaOut); const std::string imageNameOut = "query_warped.png"; writeImage(imageNameOut, imaOut, image::EImageColorSpace::NO_CONVERSION); } diff --git a/src/samples/robustHomographyGuided/main_robustHomographyGuided.cpp b/src/samples/robustHomographyGuided/main_robustHomographyGuided.cpp index 2919508df4..9c36521725 100644 --- a/src/samples/robustHomographyGuided/main_robustHomographyGuided.cpp +++ b/src/samples/robustHomographyGuided/main_robustHomographyGuided.cpp @@ -9,11 +9,11 @@ #include "aliceVision/feature/feature.hpp" #include "aliceVision/feature/sift/ImageDescriber_SIFT.hpp" #include "aliceVision/matching/RegionsMatcher.hpp" -#include "aliceVision/multiview/homographyKernelSolver.hpp" -#include "aliceVision/multiview/conditioning.hpp" +#include "aliceVision/multiview/relativePose/HomographyKernel.hpp" +#include "aliceVision/robustEstimation/conditioning.hpp" #include "aliceVision/robustEstimation/ACRansac.hpp" -#include "aliceVision/robustEstimation/ACRansacKernelAdaptator.hpp" -#include "aliceVision/robustEstimation/guidedMatching.hpp" +#include "aliceVision/multiview/RelativePoseKernel.hpp" +#include "aliceVision/matching/guidedMatching.hpp" #include "dependencies/vectorGraphics/svgDrawer.hpp" @@ -127,11 +127,11 @@ int main() { //-- Homography robust estimation std::vector vec_inliers; - typedef ACKernelAdaptor< - aliceVision::homography::kernel::FourPointSolver, - aliceVision::homography::kernel::AsymmetricError, - UnnormalizerI, - Mat3> + typedef multiview::RelativePoseKernel< + multiview::relativePose::Homography4PSolver, + multiview::relativePose::HomographyAsymmetricError, + multiview::UnnormalizerI, + robustEstimation::Mat3Model> KernelType; KernelType kernel( @@ -139,13 +139,13 @@ int main() { xR, imageR.Width(), imageR.Height(), false); // configure as point to point error model. - Mat3 H; + robustEstimation::Mat3Model H; const std::pair ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &H, std::numeric_limits::infinity()); const double & thresholdH = ACRansacOut.first; // Check the homography support some point to be considered as valid - if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) { + if (vec_inliers.size() > kernel.getMinimumNbRequiredSamples() *2.5) { std::cout << "\nFound a homography under the confidence threshold of: " << thresholdH << " pixels\n\twith: " << vec_inliers.size() << " inliers" @@ -167,9 +167,7 @@ int main() { svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0)); svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0)); // residual computation - vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(H, - LL.coords().cast(), - RR.coords().cast())); + vec_residuals[i] = std::sqrt(KernelType::ErrorT().error(H, LL.coords().cast(), RR.coords().cast())); } string out_filename = "04_ACRansacHomography.svg"; ofstream svgFile( out_filename.c_str() ); @@ -200,15 +198,14 @@ int main() { //a. by considering only the geometric error - robustEstimation::GuidedMatching( + matching::guidedMatching( H, xL, xR, Square(thresholdH), vec_corresponding_indexes[0]); std::cout << "\nGuided homography matching (geometric error) found " << vec_corresponding_indexes[0].size() << " correspondences." << std::endl; // b. by considering geometric error and descriptor distance ratio - robustEstimation::GuidedMatching - ( + matching::guidedMatching( H, NULL, *regions_perImage.at(0), // Null since no Intrinsic is defined NULL, *regions_perImage.at(1), // Null since no Intrinsic is defined diff --git a/src/software/export/main_exportMeshlab.cpp b/src/software/export/main_exportMeshlab.cpp index 71aa46d547..38e0e1698c 100644 --- a/src/software/export/main_exportMeshlab.cpp +++ b/src/software/export/main_exportMeshlab.cpp @@ -136,7 +136,7 @@ int main(int argc, char **argv) Mat3 R, K; Vec3 t; - KRt_From_P( P, &K, &R, &t); + KRt_from_P( P, &K, &R, &t); const Vec3 optical_center = R.transpose() * t; diff --git a/src/software/pipeline/main_featureMatching.cpp b/src/software/pipeline/main_featureMatching.cpp index a1831a84a4..2c46bc36da 100644 --- a/src/software/pipeline/main_featureMatching.cpp +++ b/src/software/pipeline/main_featureMatching.cpp @@ -465,12 +465,23 @@ int main(int argc, char **argv) } break; + case EGeometricFilterType::FUNDAMENTAL_WITH_DISTORTION: + { + matchingImageCollection::robustModelEstimation(geometricMatches, + &sfmData, + regionPerView, + GeometricFilterMatrix_F_AC(geometricErrorMax, maxIteration, geometricEstimator, true), + mapPutativesMatches, + guidedMatching); + } + break; + case EGeometricFilterType::ESSENTIAL_MATRIX: { matchingImageCollection::robustModelEstimation(geometricMatches, &sfmData, regionPerView, - GeometricFilterMatrix_E_AC(std::numeric_limits::infinity(), maxIteration), + GeometricFilterMatrix_E_AC(geometricErrorMax, maxIteration), mapPutativesMatches, guidedMatching); @@ -499,7 +510,7 @@ int main(int argc, char **argv) matchingImageCollection::robustModelEstimation(geometricMatches, &sfmData, regionPerView, - GeometricFilterMatrix_H_AC(std::numeric_limits::infinity(), maxIteration), + GeometricFilterMatrix_H_AC(geometricErrorMax, maxIteration), mapPutativesMatches, guidedMatching, onlyGuidedMatching ? -1.0 : 0.6); } @@ -510,7 +521,7 @@ int main(int argc, char **argv) matchingImageCollection::robustModelEstimation(geometricMatches, &sfmData, regionPerView, - GeometricFilterMatrix_HGrowing(std::numeric_limits::infinity(), maxIteration), + GeometricFilterMatrix_HGrowing(geometricErrorMax, maxIteration), mapPutativesMatches, guidedMatching); } diff --git a/src/software/utils/main_qualityEvaluation.cpp b/src/software/utils/main_qualityEvaluation.cpp index d23e224628..9af1d385b9 100644 --- a/src/software/utils/main_qualityEvaluation.cpp +++ b/src/software/utils/main_qualityEvaluation.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include diff --git a/src/software/utils/main_sfmLocalization.cpp b/src/software/utils/main_sfmLocalization.cpp index fbf743ea39..229e133518 100644 --- a/src/software/utils/main_sfmLocalization.cpp +++ b/src/software/utils/main_sfmLocalization.cpp @@ -211,7 +211,7 @@ int main(int argc, char **argv) // setup a default camera model from the found projection matrix Mat3 K, R; Vec3 t; - KRt_From_P(matching_data.projection_matrix, &K, &R, &t); + KRt_from_P(matching_data.projection_matrix, &K, &R, &t); const double focal = (K(0,0) + K(1,1))/2.0; const Vec2 principal_point(K(0,2), K(1,2));