pax_global_header00006660000000000000000000000064150445276640014527gustar00rootroot0000000000000052 comment=7e9f5f53372e43f89655040d4dfc4a00e5ace11c PoseLib-2.0.5/000077500000000000000000000000001504452766400130705ustar00rootroot00000000000000PoseLib-2.0.5/.clang-format000066400000000000000000000003451504452766400154450ustar00rootroot00000000000000--- BasedOnStyle: LLVM AlignAfterOpenBracket: Align ColumnLimit: '120' FixNamespaceComments: 'true' IndentWidth: '4' Language: Cpp PointerAlignment: Right TabWidth: '4' UseTab: Never SortIncludes: true IncludeBlocks: Regroup ... PoseLib-2.0.5/.github/000077500000000000000000000000001504452766400144305ustar00rootroot00000000000000PoseLib-2.0.5/.github/CODEOWNERS000066400000000000000000000001231504452766400160170ustar00rootroot00000000000000# These code reviewers should be added by default. * @vlarsson @pablospe @Parskatt PoseLib-2.0.5/.github/workflows/000077500000000000000000000000001504452766400164655ustar00rootroot00000000000000PoseLib-2.0.5/.github/workflows/build-pypi.yaml000066400000000000000000000030201504452766400214220ustar00rootroot00000000000000name: Build on: push: branches: - master release: types: [published] jobs: build_wheels: name: Build wheels on ${{ matrix.os }} runs-on: ${{ matrix.os }} strategy: matrix: os: [ubuntu-latest, windows-latest, macos-latest] steps: - uses: actions/checkout@v4 with: submodules: recursive # See https://cibuildwheel.pypa.io/en/stable/faq/#macos-building-cpython-38-wheels-on-arm64 - name: Install ARM64 Python 3.8 uses: actions/setup-python@v5 with: python-version: 3.8 if: runner.os == 'macOS' && runner.arch == 'ARM64' - name: Build wheels uses: pypa/cibuildwheel@v3.1.1 env: CIBW_PROJECT_REQUIRES_PYTHON: ">=3.8" CIBW_ENVIRONMENT: BUILD_SHARED_LIBS="OFF" CIBW_BEFORE_ALL_LINUX: yum install -y eigen3-devel CIBW_ARCHS_LINUX: "native" CIBW_BEFORE_ALL_WINDOWS: C:\vcpkg\vcpkg --triplet x64-windows install eigen3 CIBW_ENVIRONMENT_WINDOWS: CMAKE_TOOLCHAIN_FILE="C:\\vcpkg\\scripts\\buildsystems\\vcpkg.cmake" CIBW_ARCHS_WINDOWS: "native" CIBW_BEFORE_ALL_MACOS: brew install eigen CIBW_REPAIR_WHEEL_COMMAND_MACOS: "" # https://github.com/pypa/cibuildwheel/issues/1989 CIBW_SKIP: "*-musllinux_x86_64" - uses: actions/upload-artifact@v4 with: name: cibw-wheels-${{ matrix.os }}-${{ strategy.job-index }} path: ./wheelhouse/*.whlPoseLib-2.0.5/.github/workflows/main.yml000066400000000000000000000026211504452766400201350ustar00rootroot00000000000000name: Build PoseLib on: pull_request: branches: - '**' push: branches: - master env: BUILD_CONFIGURATION: Release LIBRARY_TYPE: OFF # Static libraries BUILD_DIR: _build INSTALL_DIR: _install jobs: CI-pipeline: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 with: # Fetches all submodules recursively. submodules: 'recursive' - name: Install Eigen3 Dependency run: | sudo apt-get update && \ sudo apt-get install -y libeigen3-dev clang-format - name: Check formatting using clang-format run: | git ls-files | grep -E '\.(cc|h)$' | xargs clang-format -i --verbose git diff --exit-code || (echo "Code format check failed, run clang-format to fix it." && exit 1) - name: Setup CMake Configuration run: | cmake \ -B ${{github.workspace}}/${{env.BUILD_DIR}} \ -DWITH_BENCHMARK=ON \ -DCMAKE_BUILD_TYPE=${{env.BUILD_CONFIGURATION}} \ -DBUILD_SHARED_LIBS=${{env.LIBRARY_TYPE}} \ -DCMAKE_INSTALL_PREFIX=${{github.workspace}}/${{env.INSTALL_DIR}} - name: Compile and Install run: | cmake \ --build ${{github.workspace}}/${{env.BUILD_DIR}} \ --target install \ -j $(nproc) - name: Execute Benchmark run: ${{github.workspace}}/${{env.INSTALL_DIR}}/bin/benchmark PoseLib-2.0.5/.gitignore000066400000000000000000000005661504452766400150670ustar00rootroot00000000000000# Compiled Object files *.slo *.lo *.o *.obj # Compiled Dynamic libraries *.so *.dylib *.dll # Compiled Static libraries *.lai *.la *.a *.lib # Executables *.exe *.out *.app # builds /build*/ /_build*/ # local installations /install*/ /_install*/ # kdevelop *.kdev4 # vs code .vscode .vs/* out/* # python package *.egg-info .venv/* wheelhouse/* *__pycache__* *.whl PoseLib-2.0.5/.gitmodules000066400000000000000000000001441504452766400152440ustar00rootroot00000000000000[submodule "pybind/pybind11"] path = pybind/pybind11 url = https://github.com/pybind/pybind11.git PoseLib-2.0.5/CMakeLists.txt000066400000000000000000000023201504452766400156250ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.10) project(PoseLib VERSION 2.0.4) # Compilation flags function function(set_compile_flags build_target) if(MSVC) target_compile_options(${build_target} PRIVATE /bigobj /fp:fast) else() target_compile_options(${build_target} PRIVATE -O3 -Wall -Werror -fPIC -Wno-sign-compare -Wfatal-errors) if(MARCH_NATIVE) target_compile_options(${build_target} PRIVATE -march=native) endif() if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") target_compile_options(${build_target} PRIVATE -Wno-maybe-uninitialized) endif() endif() target_compile_features(${build_target} PUBLIC cxx_std_17) endfunction() # Set variables set(LIBRARY_NAME ${PROJECT_NAME}) set(LIBRARY_FOLDER ${PROJECT_NAME}) include(${PROJECT_SOURCE_DIR}/cmake/SetEnv.cmake) # Eigen find_package(Eigen3 REQUIRED) # Library sources add_subdirectory(${LIBRARY_FOLDER}) # Benchmark option(WITH_BENCHMARK "Build benchmark example." OFF) if(WITH_BENCHMARK) add_subdirectory(benchmark) endif() # Compilation options option(MARCH_NATIVE "Enable CPU specific optimizations" OFF) set_compile_flags(${LIBRARY_NAME}) # python bindings option(PYTHON_PACKAGE "Build python package." OFF) if(PYTHON_PACKAGE) add_subdirectory(pybind) endif() PoseLib-2.0.5/LICENSE000066400000000000000000000027621504452766400141040ustar00rootroot00000000000000BSD 3-Clause License Copyright (c) 2020, Viktor Larsson All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. PoseLib-2.0.5/PoseLib/000077500000000000000000000000001504452766400144255ustar00rootroot00000000000000PoseLib-2.0.5/PoseLib/CMakeLists.txt000066400000000000000000000052311504452766400171660ustar00rootroot00000000000000# Set SOURCES variable set(SOURCES solvers/gp3p.cc solvers/gp4ps.cc solvers/p1p2ll.cc solvers/p2p1ll.cc solvers/p2p2pl.cc solvers/p3ll.cc solvers/p3p.cc solvers/p3p_lambdatwist.cc solvers/p4pf.cc solvers/p5lp_radial.cc solvers/p6lp.cc solvers/ugp2p.cc solvers/ugp3ps.cc solvers/up1p2pl.cc solvers/up2p.cc solvers/up1p1ll.cc solvers/up4pl.cc solvers/ugp4pl.cc solvers/relpose_upright_3pt.cc solvers/relpose_upright_planar_2pt.cc solvers/relpose_upright_planar_3pt.cc solvers/relpose_8pt.cc solvers/relpose_5pt.cc solvers/relpose_7pt.cc solvers/relpose_6pt_focal.cc solvers/gen_relpose_upright_4pt.cc solvers/gen_relpose_5p1pt.cc solvers/gen_relpose_6pt.cc solvers/homography_4pt.cc misc/qep.cc misc/univariate.cc misc/essential.cc misc/re3q3.cc misc/colmap_models.cc misc/decompositions.cc robust.cc robust/ransac.cc robust/bundle.cc robust/utils.cc robust/sampling.cc robust/estimators/absolute_pose.cc robust/estimators/relative_pose.cc robust/estimators/hybrid_pose.cc robust/estimators/homography.cc ) # Set HEADERS_PUBLIC variable set(HEADERS_PUBLIC alignment.h solvers/gp3p.h solvers/gp4ps.h solvers/p1p2ll.h solvers/p2p1ll.h solvers/p2p2pl.h solvers/p3ll.h solvers/p3p.h solvers/p3p_lambdatwist.h solvers/p4pf.h solvers/p5lp_radial.h solvers/p6lp.h solvers/ugp2p.h solvers/ugp3ps.h solvers/up1p2pl.h solvers/up2p.h solvers/up1p1ll.h solvers/up4pl.h solvers/ugp4pl.h solvers/relpose_upright_3pt.h solvers/relpose_upright_planar_2pt.h solvers/relpose_upright_planar_3pt.h solvers/relpose_8pt.h solvers/relpose_5pt.h solvers/relpose_7pt.h solvers/relpose_6pt_focal.h solvers/gen_relpose_upright_4pt.h solvers/gen_relpose_5p1pt.h solvers/gen_relpose_6pt.h solvers/homography_4pt.h misc/quaternion.h misc/colmap_models.h misc/qep.h misc/univariate.h misc/sturm.h misc/essential.h misc/re3q3.h misc/decompositions.h robust.h types.h camera_pose.h alignment.h robust/ransac.h robust/ransac_impl.h robust/bundle.h robust/utils.h robust/sampling.h robust/lm_impl.h robust/jacobian_impl.h robust/estimators/absolute_pose.h robust/estimators/relative_pose.h robust/estimators/hybrid_pose.h robust/estimators/homography.h ) # Set HEADERS_PRIVATE variable set(HEADERS_PRIVATE ) # library configuration include(${PROJECT_SOURCE_DIR}/cmake/LibraryConfig.cmake) # Eigen target_link_libraries(${LIBRARY_NAME} Eigen3::Eigen) PoseLib-2.0.5/PoseLib/alignment.h000066400000000000000000000151771504452766400165670ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ALIGNMENT_H_ #define POSELIB_ALIGNMENT_H_ // Wrapped header from COLMAP: // https://github.com/colmap/colmap/blob/dev/src/util/alignment.h // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) #ifndef COLMAP_SRC_UTIL_ALIGNMENT_H_ #define COLMAP_SRC_UTIL_ALIGNMENT_H_ #include #ifdef _MSVC_LANG #define CPP_VERSION _MSVC_LANG #else #define CPP_VERSION __cplusplus #endif #if !EIGEN_VERSION_AT_LEAST(3, 4, 0) || CPP_VERSION < 201703L #include #include #include #include #include #include #ifndef EIGEN_ALIGNED_ALLOCATOR #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator #endif // Equivalent to EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION but with support for // initializer lists, which is a C++11 feature and not supported by the Eigen. // The initializer list extension is inspired by Theia and StackOverflow code. #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(...) \ namespace std { \ template <> \ class vector<__VA_ARGS__, std::allocator<__VA_ARGS__>> \ : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> { \ typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> vector_base; \ \ public: \ typedef __VA_ARGS__ value_type; \ typedef vector_base::allocator_type allocator_type; \ typedef vector_base::size_type size_type; \ typedef vector_base::iterator iterator; \ explicit vector(const allocator_type &a = allocator_type()) : vector_base(a) {} \ template \ vector(InputIterator first, InputIterator last, const allocator_type &a = allocator_type()) \ : vector_base(first, last, a) {} \ vector(const vector &c) : vector_base(c) {} \ explicit vector(size_type num, const value_type &val = value_type()) : vector_base(num, val) {} \ vector(iterator start, iterator end) : vector_base(start, end) {} \ vector &operator=(const vector &x) { \ vector_base::operator=(x); \ return *this; \ } \ vector(initializer_list<__VA_ARGS__> list) : vector_base(list.begin(), list.end()) {} \ }; \ } // namespace std EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector2d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4f) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2f) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4f) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3f) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaterniond) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaternionf) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) #define EIGEN_STL_UMAP(KEY, VALUE) \ std::unordered_map, std::equal_to, \ Eigen::aligned_allocator>> #endif #undef CPP_VERSION #endif #endifPoseLib-2.0.5/PoseLib/camera_pose.h000066400000000000000000000077031504452766400170630ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_CAMERA_POSE_H_ #define POSELIB_CAMERA_POSE_H_ #include "PoseLib/misc/quaternion.h" #include "alignment.h" #include "misc/colmap_models.h" #include #include namespace poselib { struct alignas(32) CameraPose { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Rotation is represented as a unit quaternion // with real part first, i.e. QW, QX, QY, QZ Eigen::Vector4d q; Eigen::Vector3d t; // Constructors (Defaults to identity camera) CameraPose() : q(1.0, 0.0, 0.0, 0.0), t(0.0, 0.0, 0.0) {} CameraPose(const Eigen::Vector4d &qq, const Eigen::Vector3d &tt) : q(qq), t(tt) {} CameraPose(const Eigen::Matrix3d &R, const Eigen::Vector3d &tt) : q(rotmat_to_quat(R)), t(tt) {} // Helper functions inline Eigen::Matrix3d R() const { return quat_to_rotmat(q); } inline Eigen::Matrix Rt() const { Eigen::Matrix tmp; tmp.block<3, 3>(0, 0) = quat_to_rotmat(q); tmp.col(3) = t; return tmp; } inline Eigen::Vector3d rotate(const Eigen::Vector3d &p) const { return quat_rotate(q, p); } inline Eigen::Vector3d derotate(const Eigen::Vector3d &p) const { return quat_rotate(quat_conj(q), p); } inline Eigen::Vector3d apply(const Eigen::Vector3d &p) const { return rotate(p) + t; } inline Eigen::Vector3d center() const { return -derotate(t); } }; typedef std::vector CameraPoseVector; struct alignas(32) Image { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Struct simply holds information about camera and its pose CameraPose pose; Camera camera; // Constructors (Defaults to identity camera and pose) Image() : pose(CameraPose()), camera(Camera()) {} Image(CameraPose pose, Camera camera) : pose(std::move(pose)), camera(std::move(camera)) {} }; typedef std::vector ImageVector; struct alignas(32) ImagePair { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Struct simply holds information about two cameras and their relative pose CameraPose pose; Camera camera1; Camera camera2; // Constructors (Defaults to identity camera and poses) ImagePair() : pose(CameraPose()), camera1(Camera()), camera2(Camera()) {} ImagePair(CameraPose pose, Camera camera1, Camera camera2) : pose(std::move(pose)), camera1(std::move(camera1)), camera2(std::move(camera2)) {} }; typedef std::vector ImagePairVector; } // namespace poselib #endif PoseLib-2.0.5/PoseLib/misc/000077500000000000000000000000001504452766400153605ustar00rootroot00000000000000PoseLib-2.0.5/PoseLib/misc/colmap_models.cc000066400000000000000000001005011504452766400205020ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "colmap_models.h" #include #include #include namespace poselib { static const double UNDIST_TOL = 1e-10; static const size_t UNDIST_MAX_ITER = 25; /////////////////////////////////////////////////////////////////// // Camera - base class storing ID Camera::Camera() : model_id(-1), width(-1), height(-1), params() {} Camera::Camera(const std::string &model_name, const std::vector &p, int w, int h) { model_id = id_from_string(model_name); params = p; width = w; height = h; } Camera::Camera(int id, const std::vector &p, int w, int h) { model_id = id; params = p; width = w; height = h; } int Camera::id_from_string(const std::string &model_name) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ if (model_name == Model::to_string()) { \ return Model::model_id; \ } SWITCH_CAMERA_MODELS #undef SWITCH_CAMERA_MODEL_CASE return -1; } std::string Camera::name_from_id(int model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ return Model::to_string(); switch (model_id) { SWITCH_CAMERA_MODELS default: return "INVALID_MODEL"; } #undef SWITCH_CAMERA_MODEL_CASE } // Projection and distortion void Camera::project(const Eigen::Vector2d &x, Eigen::Vector2d *xp) const { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ Model::project(params, x, xp); \ break; switch (model_id) { SWITCH_CAMERA_MODELS default: throw std::runtime_error("PoseLib: CAMERA MODEL NYI"); } #undef SWITCH_CAMERA_MODEL_CASE } void Camera::project_with_jac(const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) const { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ Model::project_with_jac(params, x, xp, jac); \ break; switch (model_id) { SWITCH_CAMERA_MODELS default: throw std::runtime_error("PoseLib: CAMERA MODEL NYI"); } #undef SWITCH_CAMERA_MODEL_CASE } void Camera::unproject(const Eigen::Vector2d &xp, Eigen::Vector2d *x) const { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ Model::unproject(params, xp, x); \ break; switch (model_id) { SWITCH_CAMERA_MODELS default: throw std::runtime_error("PoseLib: CAMERA MODEL NYI"); } #undef SWITCH_CAMERA_MODEL_CASE } void Camera::project(const std::vector &x, std::vector *xp) const { xp->resize(x.size()); #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ for (size_t i = 0; i < x.size(); ++i) { \ Model::project(params, x[i], &((*xp)[i])); \ } \ break; switch (model_id) { SWITCH_CAMERA_MODELS default: throw std::runtime_error("PoseLib: CAMERA MODEL NYI"); } #undef SWITCH_CAMERA_MODEL_CASE } void Camera::project_with_jac(const std::vector &x, std::vector *xp, std::vector> *jac) const { xp->resize(x.size()); jac->resize(x.size()); #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ for (size_t i = 0; i < x.size(); ++i) { \ Model::project_with_jac(params, x[i], &((*xp)[i]), &((*jac)[i])); \ } \ break; switch (model_id) { SWITCH_CAMERA_MODELS default: throw std::runtime_error("PoseLib: CAMERA MODEL NYI"); } #undef SWITCH_CAMERA_MODEL_CASE } void Camera::unproject(const std::vector &xp, std::vector *x) const { x->resize(xp.size()); #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ for (size_t i = 0; i < xp.size(); ++i) { \ Model::unproject(params, xp[i], &((*x)[i])); \ } \ break; switch (model_id) { SWITCH_CAMERA_MODELS default: throw std::runtime_error("PoseLib: CAMERA MODEL NYI"); } #undef SWITCH_CAMERA_MODEL_CASE } std::string Camera::model_name() const { return name_from_id(model_id); } double Camera::focal() const { if (params.empty()) { return 1.0; // empty camera assumed to be identity } double focal = 0.0; switch (model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ for (size_t idx : Model::focal_idx) \ focal += params.at(idx) / Model::focal_idx.size(); \ break; SWITCH_CAMERA_MODELS } #undef SWITCH_CAMERA_MODEL_CASE return focal; } double Camera::focal_x() const { if (params.empty()) { return 1.0; // empty camera assumed to be identity } switch (model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ return params.at(Model::focal_idx[0]); SWITCH_CAMERA_MODELS } #undef SWITCH_CAMERA_MODEL_CASE return -1.0; } double Camera::focal_y() const { if (params.empty()) { return 1.0; // empty camera assumed to be identity } switch (model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ if (Model::focal_idx.size() > 1) { \ return params.at(Model::focal_idx[1]); \ } else { \ return params.at(Model::focal_idx[0]); \ } SWITCH_CAMERA_MODELS } #undef SWITCH_CAMERA_MODEL_CASE return -1.0; } Eigen::Vector2d Camera::principal_point() const { if (params.empty()) { return Eigen::Vector2d(0.0, 0.0); } switch (model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ return Eigen::Vector2d(params.at(Model::principal_point_idx[0]), params.at(Model::principal_point_idx[1])); SWITCH_CAMERA_MODELS } #undef SWITCH_CAMERA_MODEL_CASE return Eigen::Vector2d(-1.0, -1.0); } // Update the camera parameters such that the projections are rescaled void Camera::rescale(double scale) { if (params.empty()) { return; } #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: \ for (size_t idx : Model::focal_idx) \ params.at(idx) *= scale; \ for (size_t idx : Model::principal_point_idx) \ params.at(idx) *= scale; \ break; switch (model_id) { SWITCH_CAMERA_MODELS } #undef SWITCH_CAMERA_MODEL_CASE } int Camera::initialize_from_txt(const std::string &line) { std::stringstream ss(line); int camera_id; ss >> camera_id; // Read the model std::string model_name; ss >> model_name; model_id = id_from_string(model_name); if (model_id == -1) { return -1; } // Read sizes double d; ss >> d; width = d; ss >> d; height = d; // Read parameters params.clear(); double param; while (ss >> param) { params.push_back(param); } return camera_id; } std::string Camera::to_cameras_txt(int camera_id) const { std::stringstream ss; if (camera_id != -1) { ss << camera_id << " "; } ss << model_name(); ss << " " << width; ss << " " << height; ss << std::setprecision(16); for (double d : params) { ss << " " << d; } return ss.str(); } // xp = f * d(r) * x // J = f * d'(r) * Jr + f * d(r) // r = |x|, Jr = x / |x| // Solves // rd = (1+k1 * r*r) * r double undistort_poly1(double k1, double rd) { // f = k1 * r^3 + r + 1 - rd = 0 // fp = 3 * k1 * r^2 + 1 double r = rd; for (size_t iter = 0; iter < UNDIST_MAX_ITER; ++iter) { double r2 = r * r; double f = k1 * r2 * r + r - rd; if (std::abs(f) < UNDIST_TOL) { break; } double fp = 3.0 * k1 * r2 + 1.0; r = r - f / fp; } return r; } // Solves // rd = (1+ k1 * r^2 + k2 * r^4) * r double undistort_poly2(double k1, double k2, double rd) { // f = k2 * r^5 + k1 * r^3 + r + 1 - rd = 0 // fp = 5 * k2 * r^4 + 3 * k1 * r^2 + 1 double r = rd; for (size_t iter = 0; iter < UNDIST_MAX_ITER; ++iter) { double r2 = r * r; double f = k2 * r2 * r2 * r + k1 * r2 * r + r - rd; if (std::abs(f) < UNDIST_TOL) { break; } double fp = 5.0 * k2 * r2 * r2 + 3.0 * k1 * r2 + 1.0; r = r - f / fp; } return r; } /////////////////////////////////////////////////////////////////// // Pinhole camera // params = fx, fy, cx, cy void PinholeCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { (*xp)(0) = params[0] * x(0) + params[2]; (*xp)(1) = params[1] * x(1) + params[3]; } void PinholeCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { (*xp)(0) = params[0] * x(0) + params[2]; (*xp)(1) = params[1] * x(1) + params[3]; (*jac)(0, 0) = params[0]; (*jac)(0, 1) = 0.0; (*jac)(1, 0) = 0.0; (*jac)(1, 1) = params[1]; } void PinholeCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { (*x)(0) = (xp(0) - params[2]) / params[0]; (*x)(1) = (xp(1) - params[3]) / params[1]; } const std::vector PinholeCameraModel::focal_idx = {0, 1}; const std::vector PinholeCameraModel::principal_point_idx = {2, 3}; /////////////////////////////////////////////////////////////////// // Simple Pinhole camera // params = f, cx, cy void SimplePinholeCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { (*xp)(0) = params[0] * x(0) + params[1]; (*xp)(1) = params[0] * x(1) + params[2]; } void SimplePinholeCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { (*xp)(0) = params[0] * x(0) + params[1]; (*xp)(1) = params[0] * x(1) + params[2]; (*jac)(0, 0) = params[0]; (*jac)(0, 1) = 0.0; (*jac)(1, 0) = 0.0; (*jac)(1, 1) = params[0]; } void SimplePinholeCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { (*x)(0) = (xp(0) - params[1]) / params[0]; (*x)(1) = (xp(1) - params[2]) / params[0]; } const std::vector SimplePinholeCameraModel::focal_idx = {0}; const std::vector SimplePinholeCameraModel::principal_point_idx = {1, 2}; /////////////////////////////////////////////////////////////////// // Radial camera // params = f, cx, cy, k1, k2 void RadialCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { const double r2 = x.squaredNorm(); const double alpha = (1.0 + params[3] * r2 + params[4] * r2 * r2); (*xp)(0) = params[0] * alpha * x(0) + params[1]; (*xp)(1) = params[0] * alpha * x(1) + params[2]; } void RadialCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { const double r2 = x.squaredNorm(); const double alpha = (1.0 + params[3] * r2 + params[4] * r2 * r2); const double alphap = (2.0 * params[3] + 4.0 * params[4] * r2); *jac = alphap * (x * x.transpose()); (*jac)(0, 0) += alpha; (*jac)(1, 1) += alpha; (*jac)(0, 0) *= params[0]; (*jac)(0, 1) *= params[0]; (*jac)(1, 0) *= params[0]; (*jac)(1, 1) *= params[0]; (*xp)(0) = params[0] * alpha * x(0) + params[1]; (*xp)(1) = params[0] * alpha * x(1) + params[2]; } void RadialCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { (*x)(0) = (xp(0) - params[1]) / params[0]; (*x)(1) = (xp(1) - params[2]) / params[0]; double r0 = x->norm(); double r = undistort_poly2(params[3], params[4], r0); (*x) *= r / r0; } const std::vector RadialCameraModel::focal_idx = {0}; const std::vector RadialCameraModel::principal_point_idx = {1, 2}; /////////////////////////////////////////////////////////////////// // Simple Radial camera // params = f, cx, cy, k1 void SimpleRadialCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { const double r2 = x.squaredNorm(); const double alpha = (1.0 + params[3] * r2); (*xp)(0) = params[0] * alpha * x(0) + params[1]; (*xp)(1) = params[0] * alpha * x(1) + params[2]; } void SimpleRadialCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { const double r2 = x.squaredNorm(); const double alpha = (1.0 + params[3] * r2); *jac = 2.0 * params[3] * (x * x.transpose()); (*jac)(0, 0) += alpha; (*jac)(1, 1) += alpha; *jac *= params[0]; (*xp)(0) = params[0] * alpha * x(0) + params[1]; (*xp)(1) = params[0] * alpha * x(1) + params[2]; } void SimpleRadialCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { (*x)(0) = (xp(0) - params[1]) / params[0]; (*x)(1) = (xp(1) - params[2]) / params[0]; double r0 = x->norm(); double r = undistort_poly1(params[3], r0); (*x) *= r / r0; } const std::vector SimpleRadialCameraModel::focal_idx = {0}; const std::vector SimpleRadialCameraModel::principal_point_idx = {1, 2}; /////////////////////////////////////////////////////////////////// // OpenCV camera // params = fx, fy, cx, cy, k1, k2, p1, p2 void compute_opencv_distortion(double k1, double k2, double p1, double p2, const Eigen::Vector2d &x, Eigen::Vector2d &xp) { const double u = x(0); const double v = x(1); const double u2 = u * u; const double uv = u * v; const double v2 = v * v; const double r2 = u * u + v * v; const double alpha = 1.0 + k1 * r2 + k2 * r2 * r2; xp(0) = alpha * u + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2); xp(1) = alpha * v + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2); } void compute_opencv_distortion_jac(double k1, double k2, double p1, double p2, const Eigen::Vector2d &x, Eigen::Vector2d &xp, Eigen::Matrix2d &jac) { const double u = x(0); const double v = x(1); const double u2 = u * u; const double uv = u * v; const double v2 = v * v; const double r2 = u * u + v * v; jac(0, 0) = k2 * r2 * r2 + 6 * p2 * u + 2 * p1 * v + u * (2 * k1 * u + 4 * k2 * u * r2) + k1 * r2 + 1.0; jac(0, 1) = 2 * p1 * u + 2 * p2 * v + v * (2 * k1 * u + 4 * k2 * u * r2); jac(1, 0) = 2 * p1 * u + 2 * p2 * v + u * (2 * k1 * v + 4 * k2 * v * r2); jac(1, 1) = k2 * r2 * r2 + 2 * p2 * u + 6 * p1 * v + v * (2 * k1 * v + 4 * k2 * v * r2) + k1 * r2 + 1.0; const double alpha = 1.0 + k1 * r2 + k2 * r2 * r2; xp(0) = alpha * u + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2); xp(1) = alpha * v + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2); } void OpenCVCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { compute_opencv_distortion(params[4], params[5], params[6], params[7], x, *xp); (*xp)(0) = params[0] * (*xp)(0) + params[2]; (*xp)(1) = params[1] * (*xp)(1) + params[3]; } Eigen::Vector2d undistort_opencv(double k1, double k2, double p1, double p2, const Eigen::Vector2d &xp) { Eigen::Vector2d x = xp; Eigen::Vector2d xd; Eigen::Matrix2d jac; static const double lambda = 1e-8; for (size_t iter = 0; iter < UNDIST_MAX_ITER; ++iter) { compute_opencv_distortion_jac(k1, k2, p1, p2, x, xd, jac); jac(0, 0) += lambda; jac(1, 1) += lambda; Eigen::Vector2d res = xd - xp; if (res.norm() < UNDIST_TOL) { break; } x = x - jac.inverse() * res; } return x; } void OpenCVCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { compute_opencv_distortion_jac(params[4], params[5], params[6], params[7], x, *xp, *jac); jac->row(0) *= params[0]; jac->row(1) *= params[1]; (*xp)(0) = params[0] * (*xp)(0) + params[2]; (*xp)(1) = params[1] * (*xp)(1) + params[3]; } void OpenCVCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { (*x)(0) = (xp(0) - params[2]) / params[0]; (*x)(1) = (xp(1) - params[3]) / params[1]; *x = undistort_opencv(params[4], params[5], params[6], params[7], *x); } const std::vector OpenCVCameraModel::focal_idx = {0, 1}; const std::vector OpenCVCameraModel::principal_point_idx = {2, 3}; /////////////////////////////////////////////////////////////////// // Full OpenCV camera // params = fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6 void compute_full_opencv_distortion(double k1, double k2, double p1, double p2, double k3, double k4, double k5, double k6, const Eigen::Vector2d &x, Eigen::Vector2d &xp) { const double u = x(0); const double v = x(1); const double u2 = u * u; const double uv = u * v; const double v2 = v * v; const double r2 = u * u + v * v; const double r4 = r2 * r2; const double r6 = r2 * r4; const double alpha = (1.0 + k1 * r2 + k2 * r4 + k3 * r6) / (1.0 + k4 * r2 + k5 * r4 + k6 * r6); xp(0) = alpha * u + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2); xp(1) = alpha * v + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2); } void compute_full_opencv_distortion_jac(double k1, double k2, double p1, double p2, double k3, double k4, double k5, double k6, const Eigen::Vector2d &x, Eigen::Vector2d &xp, Eigen::Matrix2d &jac) { const double u = x(0); const double v = x(1); const double u2 = u * u; const double uv = u * v; const double v2 = v * v; const double r2 = u * u + v * v; const double r4 = r2 * r2; const double r6 = r2 * r4; const double nn = 1.0 + k1 * r2 + k2 * r4 + k3 * r6; const double dd = 1.0 + k4 * r2 + k5 * r4 + k6 * r6; const double nn_r = 2.0 * k1 + 4.0 * k2 * r2 + 6.0 * k3 * r4; const double dd_r = 2.0 * k4 + 4.0 * k5 * r2 + 6.0 * k6 * r4; const double dd2 = dd * dd; jac(0, 0) = 6 * p2 * u + 2 * p1 * v + nn / dd + (u2 * nn_r) / dd - (nn * u2 * dd_r) / dd2; jac(0, 1) = 2 * p1 * u + 2 * p2 * v + (uv * nn_r) / dd - (nn * uv * dd_r) / dd2; jac(1, 0) = jac(0, 1); // jac(1,0) = 2*p1*u + 2*p2*v + (uv*nn_r)/dd - (nn*uv*dd_r)/dd^2; jac(1, 1) = 2 * p2 * u + 6 * p1 * v + nn / dd + (v2 * nn_r) / dd - (nn * v2 * dd_r) / dd2; const double alpha = nn / dd; xp(0) = alpha * u + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2); xp(1) = alpha * v + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2); } void FullOpenCVCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { compute_full_opencv_distortion(params[4], params[5], params[6], params[7], params[8], params[9], params[10], params[11], x, *xp); (*xp)(0) = params[0] * (*xp)(0) + params[2]; (*xp)(1) = params[1] * (*xp)(1) + params[3]; } Eigen::Vector2d undistort_full_opencv(double k1, double k2, double p1, double p2, double k3, double k4, double k5, double k6, const Eigen::Vector2d &xp) { Eigen::Vector2d x = xp; Eigen::Vector2d xd; Eigen::Matrix2d jac; static const double lambda = 1e-8; for (size_t iter = 0; iter < UNDIST_MAX_ITER; ++iter) { compute_full_opencv_distortion_jac(k1, k2, p1, p2, k3, k4, k5, k6, x, xd, jac); jac(0, 0) += lambda; jac(1, 1) += lambda; Eigen::Vector2d res = xd - xp; if (res.norm() < UNDIST_TOL) { break; } x = x - jac.inverse() * res; } return x; } void FullOpenCVCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { compute_full_opencv_distortion_jac(params[4], params[5], params[6], params[7], params[8], params[9], params[10], params[11], x, *xp, *jac); if (jac) { jac->row(0) *= params[0]; jac->row(1) *= params[1]; } (*xp)(0) = params[0] * (*xp)(0) + params[2]; (*xp)(1) = params[1] * (*xp)(1) + params[3]; } void FullOpenCVCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { Eigen::Vector2d xp0; xp0 << (xp(0) - params[2]) / params[0], (xp(1) - params[3]) / params[1]; Eigen::Vector2d x0; *x = undistort_full_opencv(params[4], params[5], params[6], params[7], params[8], params[9], params[10], params[11], xp0); } const std::vector FullOpenCVCameraModel::focal_idx = {0, 1}; const std::vector FullOpenCVCameraModel::principal_point_idx = {2, 3}; /////////////////////////////////////////////////////////////////// // OpenCV Fisheye camera // params = fx, fy, cx, cy, k1, k2, k3, k4 void OpenCVFisheyeCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) { double rho = x.norm(); if (rho > 1e-8) { double theta = std::atan2(rho, 1.0); double theta2 = theta * theta; double theta4 = theta2 * theta2; double theta6 = theta2 * theta4; double theta8 = theta2 * theta6; double rd = theta * (1.0 + theta2 * params[4] + theta4 * params[5] + theta6 * params[6] + theta8 * params[7]); const double inv_r = 1.0 / rho; (*xp)(0) = params[0] * x(0) * inv_r * rd + params[2]; (*xp)(1) = params[1] * x(1) * inv_r * rd + params[3]; } else { // Very close to the principal axis - ignore distortion (*xp)(0) = params[0] * x(0) + params[2]; (*xp)(1) = params[1] * x(1) + params[3]; } } void OpenCVFisheyeCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) { double rho = x.norm(); if (rho > 1e-8) { double theta = std::atan2(rho, 1.0); double theta2 = theta * theta; double theta4 = theta2 * theta2; double theta6 = theta2 * theta4; double theta8 = theta2 * theta6; double rd = theta * (1.0 + theta2 * params[4] + theta4 * params[5] + theta6 * params[6] + theta8 * params[7]); const double inv_r = 1.0 / rho; double drho_dx = x(0) / rho; double drho_dy = x(1) / rho; double rho_z2 = rho * rho + 1.0; double dtheta_drho = 1.0 / rho_z2; double drd_dtheta = (1.0 + 3.0 * theta2 * params[4] + 5.0 * theta4 * params[5] + 7.0 * theta6 * params[6] + 9.0 * theta8 * params[7]); double drd_dx = drd_dtheta * dtheta_drho * drho_dx; double drd_dy = drd_dtheta * dtheta_drho * drho_dy; double dinv_r_drho = -1.0 / (rho * rho); double dinv_r_dx = dinv_r_drho * drho_dx; double dinv_r_dy = dinv_r_drho * drho_dy; (*xp)(0) = params[0] * x(0) * inv_r * rd + params[2]; (*jac)(0, 0) = params[0] * (inv_r * rd + x(0) * dinv_r_dx * rd + x(0) * inv_r * drd_dx); (*jac)(0, 1) = params[0] * x(0) * (dinv_r_dy * rd + inv_r * drd_dy); (*xp)(1) = params[1] * x(1) * inv_r * rd + params[3]; (*jac)(1, 0) = params[1] * x(1) * (dinv_r_dx * rd + inv_r * drd_dx); (*jac)(1, 1) = params[1] * (inv_r * rd + x(1) * dinv_r_dy * rd + x(1) * inv_r * drd_dy); } else { // Very close to the principal axis - ignore distortion (*xp)(0) = params[0] * x(0) + params[2]; (*xp)(1) = params[1] * x(1) + params[3]; (*jac)(0, 0) = params[0]; (*jac)(0, 1) = 0.0; (*jac)(1, 0) = 0.0; (*jac)(1, 1) = params[1]; } } double opencv_fisheye_newton(const std::vector ¶ms, double rd, double &theta) { double f; for (size_t iter = 0; iter < UNDIST_MAX_ITER; iter++) { const double theta2 = theta * theta; const double theta4 = theta2 * theta2; const double theta6 = theta2 * theta4; const double theta8 = theta2 * theta6; f = theta * (1.0 + theta2 * params[4] + theta4 * params[5] + theta6 * params[6] + theta8 * params[7]) - rd; if (std::abs(f) < UNDIST_TOL) { return std::abs(f); } double fp = (1.0 + 3.0 * theta2 * params[4] + 5.0 * theta4 * params[5] + 7.0 * theta6 * params[6] + 9.0 * theta8 * params[7]); fp += std::copysign(1e-10, fp); theta = theta - f / fp; } return std::abs(f); } void OpenCVFisheyeCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) { const double px = (xp(0) - params[2]) / params[0]; const double py = (xp(1) - params[3]) / params[1]; const double rd = std::sqrt(px * px + py * py); double theta = 0; if (rd > 1e-8) { // try zero-init first double res = opencv_fisheye_newton(params, rd, theta); if (res > UNDIST_TOL || theta < 0) { // If this fails try to initialize with rho (first order approx.) theta = rd; res = opencv_fisheye_newton(params, rd, theta); if (res > UNDIST_TOL || theta < 0) { // try once more theta = 0.5 * rd; res = opencv_fisheye_newton(params, rd, theta); if (res > UNDIST_TOL || theta < 0) { // try once more theta = 1.5 * rd; res = opencv_fisheye_newton(params, rd, theta); // if this does not work, just fail silently... yay } } } const double inv_z = std::tan(theta); (*x)(0) = px / rd * inv_z; (*x)(1) = py / rd * inv_z; } else { (*x)(0) = px; (*x)(1) = py; } } const std::vector OpenCVFisheyeCameraModel::focal_idx = {0, 1}; const std::vector OpenCVFisheyeCameraModel::principal_point_idx = {2, 3}; /////////////////////////////////////////////////////////////////// // Null camera - this is used as a dummy value in various places // params = {} void NullCameraModel::project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp) {} void NullCameraModel::project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) {} void NullCameraModel::unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x) {} const std::vector NullCameraModel::focal_idx = {}; const std::vector NullCameraModel::principal_point_idx = {}; } // namespace poselibPoseLib-2.0.5/PoseLib/misc/colmap_models.h000066400000000000000000000145371504452766400203610ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_COLMAP_MODELS_H_ #define POSELIB_COLMAP_MODELS_H_ #include #include namespace poselib { struct Camera { int model_id; int width; int height; std::vector params; Camera(); Camera(const std::string &model_name, const std::vector ¶ms, int width, int height); Camera(int model_id, const std::vector ¶ms, int width, int height); // Projection and distortion void project(const Eigen::Vector2d &x, Eigen::Vector2d *xp) const; void project_with_jac(const Eigen::Vector2d &x, Eigen::Vector2d *xp, Eigen::Matrix2d *jac) const; void unproject(const Eigen::Vector2d &xp, Eigen::Vector2d *x) const; // vector wrappers for the project/unprojection void project(const std::vector &x, std::vector *xp) const; void project_with_jac(const std::vector &x, std::vector *xp, std::vector> *jac) const; void unproject(const std::vector &xp, std::vector *x) const; // Update the camera parameters such that the projections are rescaled void rescale(double scale); // Return camera model as string std::string model_name() const; // Returns focal length (average in case of non-unit aspect ratio) double focal() const; double focal_x() const; double focal_y() const; Eigen::Vector2d principal_point() const; // Parses a camera from a line from cameras.txt, returns the camera_id int initialize_from_txt(const std::string &line); // Creates line for cameras.txt (inverse of initialize_from_txt) // If camera_id == -1 it is ommited std::string to_cameras_txt(int camera_id = -1) const; // helpers for camera model ids static int id_from_string(const std::string &model_name); static std::string name_from_id(int id); }; #define SETUP_CAMERA_SHARED_DEFS(ClassName, ModelName, ModelId) \ class ClassName { \ public: \ static void project(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp); \ static void project_with_jac(const std::vector ¶ms, const Eigen::Vector2d &x, Eigen::Vector2d *xp, \ Eigen::Matrix2d *jac); \ static void unproject(const std::vector ¶ms, const Eigen::Vector2d &xp, Eigen::Vector2d *x); \ static const std::vector focal_idx; \ static const std::vector principal_point_idx; \ static const int model_id = ModelId; \ static const std::string to_string() { return ModelName; } \ }; SETUP_CAMERA_SHARED_DEFS(NullCameraModel, "NULL", -1); SETUP_CAMERA_SHARED_DEFS(SimplePinholeCameraModel, "SIMPLE_PINHOLE", 0); SETUP_CAMERA_SHARED_DEFS(PinholeCameraModel, "PINHOLE", 1); SETUP_CAMERA_SHARED_DEFS(SimpleRadialCameraModel, "SIMPLE_RADIAL", 2); SETUP_CAMERA_SHARED_DEFS(RadialCameraModel, "RADIAL", 3); SETUP_CAMERA_SHARED_DEFS(OpenCVCameraModel, "OPENCV", 4); SETUP_CAMERA_SHARED_DEFS(OpenCVFisheyeCameraModel, "OPENCV_FISHEYE", 5); SETUP_CAMERA_SHARED_DEFS(FullOpenCVCameraModel, "FULL_OPENCV", 6); #define SWITCH_CAMERA_MODELS \ SWITCH_CAMERA_MODEL_CASE(NullCameraModel) \ SWITCH_CAMERA_MODEL_CASE(SimplePinholeCameraModel) \ SWITCH_CAMERA_MODEL_CASE(PinholeCameraModel) \ SWITCH_CAMERA_MODEL_CASE(SimpleRadialCameraModel) \ SWITCH_CAMERA_MODEL_CASE(RadialCameraModel) \ SWITCH_CAMERA_MODEL_CASE(OpenCVCameraModel) \ SWITCH_CAMERA_MODEL_CASE(OpenCVFisheyeCameraModel) \ SWITCH_CAMERA_MODEL_CASE(FullOpenCVCameraModel) #undef SETUP_CAMERA_SHARED_DEFS } // namespace poselib #endifPoseLib-2.0.5/PoseLib/misc/decompositions.cc000066400000000000000000002311631504452766400207340ustar00rootroot00000000000000#include "PoseLib/misc/decompositions.h" #include "PoseLib/misc/colmap_models.h" namespace poselib { std::pair focals_from_fundamental(const Eigen::Matrix3d &F, const Point2D &pp1, const Point2D &pp2) { Eigen::Vector3d p1 = pp1.homogeneous(); Eigen::Vector3d p2 = pp2.homogeneous(); Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Vector3d e1 = svd.matrixV().col(2); Eigen::Vector3d e2 = svd.matrixU().col(2); Eigen::DiagonalMatrix II(1.0, 1.0, 0.0); Eigen::Matrix3d s_e1, s_e2; s_e1 << 0, -e1(2), e1(1), e1(2), 0, -e1(0), -e1(1), e1(0), 0; s_e2 << 0, -e2(2), e2(1), e2(2), 0, -e2(0), -e2(1), e2(0), 0; Eigen::MatrixXd f1 = (-p2.transpose() * s_e2 * II * F * (p1 * p1.transpose()) * F.transpose() * p2) / (p2.transpose() * s_e2 * II * F * II * F.transpose() * p2); Eigen::MatrixXd f2 = (-p1.transpose() * s_e1 * II * F.transpose() * (p2 * p2.transpose()) * F * p1) / (p1.transpose() * s_e1 * II * F.transpose() * II * F * p1); Camera camera1 = Camera("SIMPLE_PINHOLE", std::vector{std::sqrt(f1(0, 0)), pp1(0), pp1(1)}, -1, -1); Camera camera2 = Camera("SIMPLE_PINHOLE", std::vector{std::sqrt(f2(0, 0)), pp2(0), pp2(1)}, -1, -1); return std::pair(camera1, camera2); } void fast_eigenvector_solver_autocal(double *eigv, int neig, Eigen::Matrix &AM, Eigen::Matrix &sols) { static const int ind[] = {3, 5, 9, 15}; // Truncated action matrix containing non-trivial rows Eigen::Matrix AMs; double zi[7]; for (int i = 0; i < 4; i++) { AMs.row(i) = AM.row(ind[i]); } for (int i = 0; i < neig; i++) { zi[0] = eigv[i]; for (int j = 1; j < 7; j++) { zi[j] = zi[j - 1] * eigv[i]; } Eigen::Matrix AA; AA.col(0) = AMs.col(3); AA.col(1) = AMs.col(2) + zi[0] * AMs.col(4) + zi[1] * AMs.col(5); AA.col(2) = AMs.col(1) + zi[0] * AMs.col(6) + zi[1] * AMs.col(7) + zi[2] * AMs.col(8) + zi[3] * AMs.col(9); AA.col(3) = AMs.col(0) + zi[0] * AMs.col(10) + zi[1] * AMs.col(11) + zi[2] * AMs.col(12) + zi[3] * AMs.col(13) + zi[4] * AMs.col(14) + zi[5] * AMs.col(15); AA(0, 0) = AA(0, 0) - zi[0]; AA(1, 1) = AA(1, 1) - zi[2]; AA(2, 2) = AA(2, 2) - zi[4]; AA(3, 3) = AA(3, 3) - zi[6]; Eigen::Matrix s = AA.leftCols(3).householderQr().solve(-AA.col(3)); sols(0, i) = s(2); sols(1, i) = zi[0]; } } Eigen::MatrixXd solver_robust_autocal(const Eigen::VectorXd &data, int *num_sols) { // Compute coefficients const double *d = data.data(); Eigen::VectorXd coeffs(30); coeffs[0] = d[15]; coeffs[1] = d[16]; coeffs[2] = d[18]; coeffs[3] = d[21]; coeffs[4] = d[25]; coeffs[5] = d[17]; coeffs[6] = d[19]; coeffs[7] = d[22]; coeffs[8] = d[26]; coeffs[9] = d[20]; coeffs[10] = d[23]; coeffs[11] = d[27]; coeffs[12] = d[24]; coeffs[13] = d[28]; coeffs[14] = d[29]; coeffs[15] = d[0]; coeffs[16] = d[1]; coeffs[17] = d[3]; coeffs[18] = d[6]; coeffs[19] = d[10]; coeffs[20] = d[2]; coeffs[21] = d[4]; coeffs[22] = d[7]; coeffs[23] = d[11]; coeffs[24] = d[5]; coeffs[25] = d[8]; coeffs[26] = d[12]; coeffs[27] = d[9]; coeffs[28] = d[13]; coeffs[29] = d[14]; // Setup elimination template static const int coeffs0_ind[] = { 0, 15, 1, 0, 15, 16, 2, 1, 0, 15, 16, 17, 3, 2, 1, 0, 15, 16, 17, 18, 4, 3, 2, 1, 16, 17, 18, 19, 4, 3, 2, 17, 18, 19, 4, 3, 18, 19, 5, 0, 15, 20, 6, 5, 20, 1, 15, 0, 16, 21, 7, 6, 5, 20, 21, 2, 16, 15, 1, 17, 0, 22, 8, 7, 6, 5, 20, 21, 22, 3, 17, 16, 2, 18, 1, 23, 8, 7, 6, 21, 22, 23, 4, 18, 17, 3, 19, 2, 9, 5, 20, 0, 15, 24, 10, 9, 24, 6, 20, 5, 21, 1, 16, 0, 15, 25, 11, 10, 9, 24, 25, 7, 21, 20, 6, 22, 2, 17, 1, 16, 5, 26, 12, 9, 24, 5, 20, 0, 15, 27, 13, 12, 27, 10, 24, 9, 25, 6, 21, 5, 1, 16, 20, 28, 11, 10, 9, 24, 25, 26, 8, 22, 21, 7, 23, 3, 18, 2, 17, 6, 8, 7, 22, 23, 19, 18, 4, 3, 4, 19}; static const int coeffs1_ind[] = { 14, 29, 14, 29, 12, 27, 14, 29, 12, 27, 9, 24, 14, 12, 27, 9, 24, 5, 20, 29, 14, 29, 13, 27, 12, 28, 10, 25, 9, 6, 21, 24, 13, 12, 27, 28, 11, 25, 24, 10, 26, 7, 22, 6, 2, 17, 21, 9, 29, 14, 13, 28, 12, 10, 25, 27, 14, 29, 28, 27, 13, 11, 26, 10, 7, 22, 25, 12, 13, 12, 27, 28, 26, 25, 11, 8, 23, 7, 3, 18, 22, 10, 11, 10, 25, 26, 23, 22, 8, 4, 19, 3, 18, 7, 14, 13, 28, 29, 29, 13, 11, 26, 28, 14, 14, 29, 28, 11, 8, 23, 26, 13, 13, 28, 26, 8, 4, 19, 23, 11, 11, 26, 23, 4, 19, 8, 8, 23, 19, 4}; static const int C0_ind[] = { 0, 19, 20, 21, 26, 39, 40, 41, 42, 45, 46, 59, 60, 61, 62, 63, 64, 65, 66, 79, 80, 81, 82, 83, 84, 85, 86, 99, 101, 102, 103, 104, 105, 106, 122, 123, 124, 125, 140, 147, 151, 159, 160, 161, 166, 167, 168, 170, 171, 179, 180, 181, 182, 185, 186, 187, 188, 189, 190, 191, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 218, 219, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 238, 240, 247, 251, 252, 253, 259, 260, 261, 266, 267, 268, 270, 271, 272, 273, 274, 277, 279, 280, 281, 282, 285, 286, 287, 288, 289, 290, 291, 292, 293, 294, 297, 298, 299, 300, 307, 311, 312, 313, 315, 316, 319, 320, 321, 326, 327, 328, 330, 331, 332, 333, 334, 335, 336, 337, 339, 341, 342, 343, 344, 345, 346, 347, 348, 349, 350, 351, 352, 353, 354, 357, 358, 362, 363, 364, 365, 368, 369, 370, 378, 383, 384}; static const int C1_ind[] = { 15, 16, 32, 33, 35, 36, 47, 51, 52, 53, 55, 56, 60, 67, 71, 72, 73, 75, 76, 79, 81, 86, 87, 88, 90, 91, 92, 93, 94, 95, 96, 97, 101, 102, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 128, 130, 132, 133, 134, 135, 136, 137, 142, 145, 148, 149, 150, 152, 153, 154, 155, 156, 157, 158, 162, 163, 164, 165, 168, 169, 170, 172, 173, 174, 175, 176, 177, 178, 182, 183, 184, 185, 188, 189, 190, 192, 193, 194, 197, 198, 214, 215, 216, 217, 229, 234, 235, 236, 237, 238, 243, 244, 249, 254, 255, 256, 257, 258, 263, 264, 269, 274, 275, 276, 277, 278, 283, 284, 289, 294, 297, 298, 303, 304, 309, 318}; Eigen::Matrix C0; C0.setZero(); Eigen::Matrix C1; C1.setZero(); for (int i = 0; i < 170; i++) { C0(C0_ind[i]) = coeffs(coeffs0_ind[i]); } for (int i = 0; i < 130; i++) { C1(C1_ind[i]) = coeffs(coeffs1_ind[i]); } // Matrix C12 = C0.partialPivLu().solve(C1); Eigen::Matrix C12 = C0.fullPivLu().solve(C1); // Matrix C12 = C0.bdcSvd().solve(C1); // std::cout << "C0" << std::endl; // std::cout << C0 << std::endl; // // // std::cout << "C1" << std::endl; // std::cout << C1 << std::endl; // Setup action matrix Eigen::Matrix RR; RR << -C12.bottomRows(4), Eigen::Matrix::Identity(16, 16); static const int AM_ind[] = {14, 10, 8, 0, 9, 1, 11, 12, 13, 2, 15, 16, 17, 18, 19, 3}; Eigen::Matrix AM; for (int i = 0; i < 16; i++) { AM.row(i) = RR.row(AM_ind[i]); } Eigen::Matrix sols; sols.setZero(); // Solve eigenvalue problem Eigen::EigenSolver es(AM, false); Eigen::ArrayXcd D = es.eigenvalues(); int nroots = 0; double eigv[16]; for (int i = 0; i < 16; i++) { if (std::abs(D(i).imag()) < 1e-6) eigv[nroots++] = D(i).real(); } fast_eigenvector_solver_autocal(eigv, nroots, AM, sols); *num_sols = nroots; return sols; } std::tuple focals_from_fundamental_iterative(const Eigen::Matrix3d &F, const Camera &camera1_prior, const Camera &camera2_prior, const int &max_iters, const Eigen::Vector4d &weights) { Eigen::Vector2d pp1_prior = camera1_prior.principal_point(); Eigen::Vector2d pp2_prior = camera2_prior.principal_point(); double f1_prior = camera1_prior.focal(), f2_prior = camera2_prior.focal(); double w1 = weights[0], w2 = weights[1], w3 = weights[2], w4 = weights[3]; Eigen::Matrix3d T1, T2T; T1.setIdentity(); T1(0, 2) = pp1_prior(0); T1(1, 2) = pp1_prior(1); T2T.setIdentity(); T2T(2, 0) = pp2_prior(0); T2T(2, 1) = pp2_prior(1); Eigen::Matrix3d G = T2T * F * T1; Eigen::JacobiSVD svd(G, Eigen::ComputeFullU | Eigen::ComputeFullV); svd.computeV(); svd.computeU(); Eigen::Vector3d singularValues = svd.singularValues(); // Extract the first two singular values double s1 = singularValues(0); double s2 = singularValues(1); Eigen::Matrix3d V = svd.matrixV(); Eigen::Matrix3d U = svd.matrixU(); double U11 = U(0, 0), U12 = U(0, 1); double U21 = U(1, 0), U22 = U(1, 1); double U31 = U(2, 0), U32 = U(2, 1); double V11 = V(0, 0), V12 = V(0, 1); double V21 = V(1, 0), V22 = V(1, 1); double V31 = V(2, 0), V32 = V(2, 1); double U11_sq = U11 * U11, U12_sq = U12 * U12; double U21_sq = U21 * U21, U22_sq = U22 * U22; double U31_sq = U31 * U31, U32_sq = U32 * U32; double V11_sq = V11 * V11, V12_sq = V12 * V12; double V21_sq = V21 * V21, V22_sq = V22 * V22; double V31_sq = V31 * V31, V32_sq = V32 * V32; double f1prior_sq = std::pow(f1_prior, 2); double f2prior_sq = std::pow(f2_prior, 2); double cf1p1k, cf1p2k, cu1p1k, cu1p2k, cv1p1k, cv1p2k, cf2p1k, cf2p2k, cu2p1k, cu2p2k, cv2p1k, cv2p2k; double cf1p1k_sq, cf1p2k_sq, cu1p1k_sq, cu1p2k_sq, cv1p1k_sq, cv1p2k_sq, cf2p1k_sq, cf2p2k_sq, cu2p1k_sq, cu2p2k_sq, cv2p1k_sq, cv2p2k_sq; double c11, c12, c13, c14, c15, c16, c17, c18, c19, c110, c111, c112, c113, c114, c115; double c21, c22, c23, c24, c25, c26, c27, c28, c29, c210, c211, c212, c213, c214, c215; double f1n = f1_prior, u1n = 0.0, v1n = 0.0; double f2n = f2_prior, u2n = 0.0, v2n = 0.0; double f1n_sq, u1n_sq, v1n_sq, f2n_sq, u2n_sq, v2n_sq; double df1, du1, dv1, df2, du2, dv2; Eigen::VectorXd err(max_iters + 1); err.setZero(); int k; for (k = 0; k < max_iters + 1; k++) { f1n_sq = std::pow(f1n, 2); u1n_sq = std::pow(u1n, 2); v1n_sq = std::pow(v1n, 2); f2n_sq = std::pow(f2n, 2); u2n_sq = std::pow(u2n, 2); v2n_sq = std::pow(v2n, 2); cf1p1k = 1 / w1 * (s2 * (2 * V11 * V12 * f1n + 2 * V21 * V22 * f1n) * (U12 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U22 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U32 * (U32 + U12 * u2n + U22 * v2n)) + s1 * (2 * f1n * V11 * V11 + 2 * f1n * V21 * V21) * (U11 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U21 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U31 * (U32 + U12 * u2n + U22 * v2n))); cf1p2k = 1 / w1 * (s1 * (2 * V11 * V12 * f1n + 2 * V21 * V22 * f1n) * (U11 * (U31 * u2n + U11 * (f2n_sq + u2n_sq) + U21 * u2n * v2n) + U21 * (U31 * v2n + U21 * (f2n_sq + v2n_sq) + U11 * u2n * v2n) + U31 * (U31 + U11 * u2n + U21 * v2n)) + s2 * (2 * f1n * V12 * V12 + 2 * f1n * V22 * V22) * (U11 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U21 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U31 * (U32 + U12 * u2n + U22 * v2n))); cu1p1k = 1 / w2 * (s1 * (U11 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U21 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U31 * (U32 + U12 * u2n + U22 * v2n)) * (V11 * V31 + V11 * (V31 + 2 * V11 * u1n + V21 * v1n) + V11 * V21 * v1n) + s2 * (U12 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U22 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U32 * (U32 + U12 * u2n + U22 * v2n)) * (V12 * V31 + V11 * (V32 + 2 * V12 * u1n + V22 * v1n) + V12 * V21 * v1n)); cu1p2k = 1 / w2 * (s1 * (U11 * (U31 * u2n + U11 * (f2n_sq + u2n_sq) + U21 * u2n * v2n) + U21 * (U31 * v2n + U21 * (f2n_sq + v2n_sq) + U11 * u2n * v2n) + U31 * (U31 + U11 * u2n + U21 * v2n)) * (V12 * V31 + V11 * (V32 + 2 * V12 * u1n + V22 * v1n) + V12 * V21 * v1n) + s2 * (U11 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U21 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U31 * (U32 + U12 * u2n + U22 * v2n)) * (V12 * V32 + V12 * (V32 + 2 * V12 * u1n + V22 * v1n) + V12 * V22 * v1n)); cv1p1k = 1 / w2 * (s1 * (U11 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U21 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U31 * (U32 + U12 * u2n + U22 * v2n)) * (V21 * V31 + V21 * (V31 + V11 * u1n + 2 * V21 * v1n) + V11 * V21 * u1n) + s2 * (U12 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U22 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U32 * (U32 + U12 * u2n + U22 * v2n)) * (V22 * V31 + V21 * (V32 + V12 * u1n + 2 * V22 * v1n) + V11 * V22 * u1n)); cv1p2k = 1 / w2 * (s1 * (U11 * (U31 * u2n + U11 * (f2n_sq + u2n_sq) + U21 * u2n * v2n) + U21 * (U31 * v2n + U21 * (f2n_sq + v2n_sq) + U11 * u2n * v2n) + U31 * (U31 + U11 * u2n + U21 * v2n)) * (V22 * V31 + V21 * (V32 + V12 * u1n + 2 * V22 * v1n) + V11 * V22 * u1n) + s2 * (U11 * (U32 * u2n + U12 * (f2n_sq + u2n_sq) + U22 * u2n * v2n) + U21 * (U32 * v2n + U22 * (f2n_sq + v2n_sq) + U12 * u2n * v2n) + U31 * (U32 + U12 * u2n + U22 * v2n)) * (V22 * V32 + V22 * (V32 + V12 * u1n + 2 * V22 * v1n) + V12 * V22 * u1n)); cf2p1k = 1 / w3 * (s1 * (2 * U11 * U12 * f2n + 2 * U21 * U22 * f2n) * (V11 * (V31 * u1n + V11 * (f1n_sq + u1n_sq) + V21 * u1n * v1n) + V21 * (V31 * v1n + V21 * (f1n_sq + v1n_sq) + V11 * u1n * v1n) + V31 * (V31 + V11 * u1n + V21 * v1n)) + s2 * (2 * f2n * U12 * U12 + 2 * f2n * U22 * U22) * (V11 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V21 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V31 * (V32 + V12 * u1n + V22 * v1n))); cf2p2k = 1 / w3 * (s2 * (2 * U11 * U12 * f2n + 2 * U21 * U22 * f2n) * (V12 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V22 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V32 * (V32 + V12 * u1n + V22 * v1n)) + s1 * (2 * f2n * U11 * U11 + 2 * f2n * U21 * U21) * (V11 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V21 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V31 * (V32 + V12 * u1n + V22 * v1n))); cu2p1k = 1 / w4 * (s1 * (V11 * (V31 * u1n + V11 * (f1n_sq + u1n_sq) + V21 * u1n * v1n) + V21 * (V31 * v1n + V21 * (f1n_sq + v1n_sq) + V11 * u1n * v1n) + V31 * (V31 + V11 * u1n + V21 * v1n)) * (U12 * U31 + U11 * (U32 + 2 * U12 * u2n + U22 * v2n) + U12 * U21 * v2n) + s2 * (V11 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V21 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V31 * (V32 + V12 * u1n + V22 * v1n)) * (U12 * U32 + U12 * (U32 + 2 * U12 * u2n + U22 * v2n) + U12 * U22 * v2n)); cu2p2k = 1 / w4 * (s1 * (V11 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V21 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V31 * (V32 + V12 * u1n + V22 * v1n)) * (U11 * U31 + U11 * (U31 + 2 * U11 * u2n + U21 * v2n) + U11 * U21 * v2n) + s2 * (V12 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V22 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V32 * (V32 + V12 * u1n + V22 * v1n)) * (U12 * U31 + U11 * (U32 + 2 * U12 * u2n + U22 * v2n) + U12 * U21 * v2n)); cv2p1k = 1 / w4 * (s1 * (V11 * (V31 * u1n + V11 * (f1n_sq + u1n_sq) + V21 * u1n * v1n) + V21 * (V31 * v1n + V21 * (f1n_sq + v1n_sq) + V11 * u1n * v1n) + V31 * (V31 + V11 * u1n + V21 * v1n)) * (U22 * U31 + U21 * (U32 + U12 * u2n + 2 * U22 * v2n) + U11 * U22 * u2n) + s2 * (V11 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V21 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V31 * (V32 + V12 * u1n + V22 * v1n)) * (U22 * U32 + U22 * (U32 + U12 * u2n + 2 * U22 * v2n) + U12 * U22 * u2n)); cv2p2k = 1 / w4 * (s1 * (V11 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V21 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V31 * (V32 + V12 * u1n + V22 * v1n)) * (U21 * U31 + U21 * (U31 + U11 * u2n + 2 * U21 * v2n) + U11 * U21 * u2n) + s2 * (V12 * (V32 * u1n + V12 * (f1n_sq + u1n_sq) + V22 * u1n * v1n) + V22 * (V32 * v1n + V22 * (f1n_sq + v1n_sq) + V12 * u1n * v1n) + V32 * (V32 + V12 * u1n + V22 * v1n)) * (U22 * U31 + U21 * (U32 + U12 * u2n + 2 * U22 * v2n) + U11 * U22 * u2n)); cf1p1k_sq = std::pow(cf1p1k, 2); cf1p2k_sq = std::pow(cf1p2k, 2); cu1p1k_sq = std::pow(cu1p1k, 2); cu1p2k_sq = std::pow(cu1p2k, 2); cv1p1k_sq = std::pow(cv1p1k, 2); cv1p2k_sq = std::pow(cv1p2k, 2); cf2p1k_sq = std::pow(cf2p1k, 2); cf2p2k_sq = std::pow(cf2p2k, 2); cu2p1k_sq = std::pow(cu2p1k, 2); cu2p2k_sq = std::pow(cu2p2k, 2); cv2p1k_sq = std::pow(cv2p1k, 2); cv2p2k_sq = std::pow(cv2p2k, 2); c11 = s1 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V11 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V21 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s2 * (U12 * (U12 * (cf2p1k_sq + cu2p1k_sq) + U22 * cu2p1k * cv2p1k) + U22 * (U22 * (cf2p1k_sq + cv2p1k_sq) + U12 * cu2p1k * cv2p1k)) * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)); c12 = s1 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V11 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V21 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s2 * (U12 * (U12 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U22 * cu2p1k * cv2p2k + U22 * cu2p2k * cv2p1k) + U22 * (U22 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U12 * cu2p1k * cv2p2k + U12 * cu2p2k * cv2p1k)) * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s1 * (V11 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V21 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) + s2 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U12 * (U12 * (cf2p1k_sq + cu2p1k_sq) + U22 * cu2p1k * cv2p1k) + U22 * (U22 * (cf2p1k_sq + cv2p1k_sq) + U12 * cu2p1k * cv2p1k)); c13 = s1 * (V11 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V21 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U32 * (U12 * cu2p1k + U22 * cv2p1k) + U12 * (U32 * cu2p1k + 2 * U12 * cf2p1k * f2_prior) + U22 * (U32 * cv2p1k + 2 * U22 * cf2p1k * f2_prior)) + s1 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V31 * (V11 * cu1p1k + V21 * cv1p1k) + V11 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V21 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U12 * (U12 * (cf2p1k_sq + cu2p1k_sq) + U22 * cu2p1k * cv2p1k) + U22 * (U22 * (cf2p1k_sq + cv2p1k_sq) + U12 * cu2p1k * cv2p1k)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)); c14 = s1 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V11 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V21 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s1 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V11 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V21 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s2 * (U12 * (U12 * (cf2p1k_sq + cu2p1k_sq) + U22 * cu2p1k * cv2p1k) + U22 * (U22 * (cf2p1k_sq + cv2p1k_sq) + U12 * cu2p1k * cv2p1k)) * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s2 * (U12 * (U12 * (cf2p2k_sq + cu2p2k_sq) + U22 * cu2p2k * cv2p2k) + U22 * (U22 * (cf2p2k_sq + cv2p2k_sq) + U12 * cu2p2k * cv2p2k)) * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s1 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V11 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V21 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) + s2 * (U12 * (U12 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U22 * cu2p1k * cv2p2k + U22 * cu2p2k * cv2p1k) + U22 * (U22 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U12 * cu2p1k * cv2p2k + U12 * cu2p2k * cv2p1k)) * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)); c15 = s1 * (V11 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V21 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U32 * (U12 * cu2p1k + U22 * cv2p1k) + U12 * (U32 * cu2p1k + 2 * U12 * cf2p1k * f2_prior) + U22 * (U32 * cv2p1k + 2 * U22 * cf2p1k * f2_prior)) + s1 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V31 * (V11 * cu1p1k + V21 * cv1p1k) + V11 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V21 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U12 * (U12 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U22 * cu2p1k * cv2p2k + U22 * cu2p2k * cv2p1k) + U22 * (U22 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U12 * cu2p1k * cv2p2k + U12 * cu2p2k * cv2p1k)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s1 * (V11 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V21 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U32 * (U12 * cu2p2k + U22 * cv2p2k) + U12 * (U32 * cu2p2k + 2 * U12 * cf2p2k * f2_prior) + U22 * (U32 * cv2p2k + 2 * U22 * cf2p2k * f2_prior)) + s1 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V31 * (V11 * cu1p2k + V21 * cv1p2k) + V11 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V21 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U12 * (U12 * (cf2p1k_sq + cu2p1k_sq) + U22 * cu2p1k * cv2p1k) + U22 * (U22 * (cf2p1k_sq + cv2p1k_sq) + U12 * cu2p1k * cv2p1k)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)); c16 = s1 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V31_sq + V11_sq * f1prior_sq + V21_sq * f1prior_sq) + s2 * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U32_sq + U12_sq * f2prior_sq + U22_sq * f2prior_sq) + s1 * (V11 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V21 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) + s2 * (U12 * (U12 * (cf2p1k_sq + cu2p1k_sq) + U22 * cu2p1k * cv2p1k) + U22 * (U22 * (cf2p1k_sq + cv2p1k_sq) + U12 * cu2p1k * cv2p1k)) * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) + s1 * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) * (V31 * (V11 * cu1p1k + V21 * cv1p1k) + V11 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V21 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U32 * (U12 * cu2p1k + U22 * cv2p1k) + U12 * (U32 * cu2p1k + 2 * U12 * cf2p1k * f2_prior) + U22 * (U32 * cv2p1k + 2 * U22 * cf2p1k * f2_prior)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)); c17 = s1 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V11 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V21 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s2 * (U12 * (U12 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U22 * cu2p1k * cv2p2k + U22 * cu2p2k * cv2p1k) + U22 * (U22 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U12 * cu2p1k * cv2p2k + U12 * cu2p2k * cv2p1k)) * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s1 * (V11 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V21 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) + s2 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U12 * (U12 * (cf2p2k_sq + cu2p2k_sq) + U22 * cu2p2k * cv2p2k) + U22 * (U22 * (cf2p2k_sq + cv2p2k_sq) + U12 * cu2p2k * cv2p2k)); c18 = s1 * (V11 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V21 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U32 * (U12 * cu2p2k + U22 * cv2p2k) + U12 * (U32 * cu2p2k + 2 * U12 * cf2p2k * f2_prior) + U22 * (U32 * cv2p2k + 2 * U22 * cf2p2k * f2_prior)) + s1 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V31 * (V11 * cu1p2k + V21 * cv1p2k) + V11 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V21 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U12 * (U12 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U22 * cu2p1k * cv2p2k + U22 * cu2p2k * cv2p1k) + U22 * (U22 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U12 * cu2p1k * cv2p2k + U12 * cu2p2k * cv2p1k)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s1 * (V11 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V21 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U32 * (U12 * cu2p1k + U22 * cv2p1k) + U12 * (U32 * cu2p1k + 2 * U12 * cf2p1k * f2_prior) + U22 * (U32 * cv2p1k + 2 * U22 * cf2p1k * f2_prior)) + s1 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V31 * (V11 * cu1p1k + V21 * cv1p1k) + V11 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V21 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U12 * (U12 * (cf2p2k_sq + cu2p2k_sq) + U22 * cu2p2k * cv2p2k) + U22 * (U22 * (cf2p2k_sq + cv2p2k_sq) + U12 * cu2p2k * cv2p2k)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)); c19 = s2 * (U12 * (U12 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U22 * cu2p1k * cv2p2k + U22 * cu2p2k * cv2p1k) + U22 * (U22 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U12 * cu2p1k * cv2p2k + U12 * cu2p2k * cv2p1k)) * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) + s1 * (V11 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V21 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) + s1 * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) * (V31 * (V11 * cu1p2k + V21 * cv1p2k) + V11 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V21 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s1 * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) * (V31 * (V11 * cu1p1k + V21 * cv1p1k) + V11 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V21 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U32 * (U12 * cu2p1k + U22 * cv2p1k) + U12 * (U32 * cu2p1k + 2 * U12 * cf2p1k * f2_prior) + U22 * (U32 * cv2p1k + 2 * U22 * cf2p1k * f2_prior)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U32 * (U12 * cu2p2k + U22 * cv2p2k) + U12 * (U32 * cu2p2k + 2 * U12 * cf2p2k * f2_prior) + U22 * (U32 * cv2p2k + 2 * U22 * cf2p2k * f2_prior)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s1 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V31_sq + V11_sq * f1prior_sq + V21_sq * f1prior_sq) + s2 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U32_sq + U12_sq * f2prior_sq + U22_sq * f2prior_sq); c110 = s1 * (V31_sq + V11_sq * f1prior_sq + V21_sq * f1prior_sq) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (U32_sq + U12_sq * f2prior_sq + U22_sq * f2prior_sq) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) * (U32 * (U12 * cu2p1k + U22 * cv2p1k) + U12 * (U32 * cu2p1k + 2 * U12 * cf2p1k * f2_prior) + U22 * (U32 * cv2p1k + 2 * U22 * cf2p1k * f2_prior)) + s1 * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) * (V31 * (V11 * cu1p1k + V21 * cv1p1k) + V11 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V21 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)); c111 = s1 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V11 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V21 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s2 * (U12 * (U12 * (cf2p2k_sq + cu2p2k_sq) + U22 * cu2p2k * cv2p2k) + U22 * (U22 * (cf2p2k_sq + cv2p2k_sq) + U12 * cu2p2k * cv2p2k)) * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)); c112 = s1 * (V11 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V21 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U32 * (U12 * cu2p2k + U22 * cv2p2k) + U12 * (U32 * cu2p2k + 2 * U12 * cf2p2k * f2_prior) + U22 * (U32 * cv2p2k + 2 * U22 * cf2p2k * f2_prior)) + s1 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V31 * (V11 * cu1p2k + V21 * cv1p2k) + V11 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V21 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U12 * (U12 * (cf2p2k_sq + cu2p2k_sq) + U22 * cu2p2k * cv2p2k) + U22 * (U22 * (cf2p2k_sq + cv2p2k_sq) + U12 * cu2p2k * cv2p2k)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)); c113 = s1 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V31_sq + V11_sq * f1prior_sq + V21_sq * f1prior_sq) + s2 * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U32_sq + U12_sq * f2prior_sq + U22_sq * f2prior_sq) + s1 * (V11 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V21 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) + s2 * (U12 * (U12 * (cf2p2k_sq + cu2p2k_sq) + U22 * cu2p2k * cv2p2k) + U22 * (U22 * (cf2p2k_sq + cv2p2k_sq) + U12 * cu2p2k * cv2p2k)) * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) + s1 * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) * (V31 * (V11 * cu1p2k + V21 * cv1p2k) + V11 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V21 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U32 * (U12 * cu2p2k + U22 * cv2p2k) + U12 * (U32 * cu2p2k + 2 * U12 * cf2p2k * f2_prior) + U22 * (U32 * cv2p2k + 2 * U22 * cf2p2k * f2_prior)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)); c114 = s1 * (V31_sq + V11_sq * f1prior_sq + V21_sq * f1prior_sq) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (U32_sq + U12_sq * f2prior_sq + U22_sq * f2prior_sq) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) * (U32 * (U12 * cu2p2k + U22 * cv2p2k) + U12 * (U32 * cu2p2k + 2 * U12 * cf2p2k * f2_prior) + U22 * (U32 * cv2p2k + 2 * U22 * cf2p2k * f2_prior)) + s1 * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) * (V31 * (V11 * cu1p2k + V21 * cv1p2k) + V11 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V21 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)); c115 = s1 * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) * (V31_sq + V11_sq * f1prior_sq + V21_sq * f1prior_sq) + s2 * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) * (U32_sq + U12_sq * f2prior_sq + U22_sq * f2prior_sq); c21 = s1 * (U11 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U21 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s2 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V12 * (V12 * (cf1p1k_sq + cu1p1k_sq) + V22 * cu1p1k * cv1p1k) + V22 * (V22 * (cf1p1k_sq + cv1p1k_sq) + V12 * cu1p1k * cv1p1k)); c22 = s1 * (U11 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U21 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s2 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V12 * (V12 * (cf1p1k_sq + cu1p1k_sq) + V22 * cu1p1k * cv1p1k) + V22 * (V22 * (cf1p1k_sq + cv1p1k_sq) + V12 * cu1p1k * cv1p1k)) + s1 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U11 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U21 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) + s2 * (V12 * (V12 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V22 * cu1p1k * cv1p2k + V22 * cu1p2k * cv1p1k) + V22 * (V22 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V12 * cu1p1k * cv1p2k + V12 * cu1p2k * cv1p1k)) * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)); c23 = s1 * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U31 * (U11 * cu2p1k + U21 * cv2p1k) + U11 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U21 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (V12 * (V12 * (cf1p1k_sq + cu1p1k_sq) + V22 * cu1p1k * cv1p1k) + V22 * (V22 * (cf1p1k_sq + cv1p1k_sq) + V12 * cu1p1k * cv1p1k)) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s1 * (U11 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U21 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V32 * (V12 * cu1p1k + V22 * cv1p1k) + V12 * (V32 * cu1p1k + 2 * V12 * cf1p1k * f1_prior) + V22 * (V32 * cv1p1k + 2 * V22 * cf1p1k * f1_prior)); c24 = s1 * (U11 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U21 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s1 * (U11 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U21 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) + s2 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V12 * (V12 * (cf1p2k_sq + cu1p2k_sq) + V22 * cu1p2k * cv1p2k) + V22 * (V22 * (cf1p2k_sq + cv1p2k_sq) + V12 * cu1p2k * cv1p2k)) + s2 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V12 * (V12 * (cf1p1k_sq + cu1p1k_sq) + V22 * cu1p1k * cv1p1k) + V22 * (V22 * (cf1p1k_sq + cv1p1k_sq) + V12 * cu1p1k * cv1p1k)) + s1 * (U11 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U21 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) + s2 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V12 * (V12 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V22 * cu1p1k * cv1p2k + V22 * cu1p2k * cv1p1k) + V22 * (V22 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V12 * cu1p1k * cv1p2k + V12 * cu1p2k * cv1p1k)); c25 = s1 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U31 * (U11 * cu2p1k + U21 * cv2p1k) + U11 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U21 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (V12 * (V12 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V22 * cu1p1k * cv1p2k + V22 * cu1p2k * cv1p1k) + V22 * (V22 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V12 * cu1p1k * cv1p2k + V12 * cu1p2k * cv1p1k)) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s1 * (U11 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U21 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V32 * (V12 * cu1p1k + V22 * cv1p1k) + V12 * (V32 * cu1p1k + 2 * V12 * cf1p1k * f1_prior) + V22 * (V32 * cv1p1k + 2 * V22 * cf1p1k * f1_prior)) + s1 * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U31 * (U11 * cu2p2k + U21 * cv2p2k) + U11 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U21 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (V12 * (V12 * (cf1p1k_sq + cu1p1k_sq) + V22 * cu1p1k * cv1p1k) + V22 * (V22 * (cf1p1k_sq + cv1p1k_sq) + V12 * cu1p1k * cv1p1k)) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s1 * (U11 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U21 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V32 * (V12 * cu1p2k + V22 * cv1p2k) + V12 * (V32 * cu1p2k + 2 * V12 * cf1p2k * f1_prior) + V22 * (V32 * cv1p2k + 2 * V22 * cf1p2k * f1_prior)); c26 = s2 * (U12 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U22 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V32_sq + V12_sq * f1prior_sq + V22_sq * f1prior_sq) + s1 * (V12 * (V11 * (cf1p1k_sq + cu1p1k_sq) + V21 * cu1p1k * cv1p1k) + V22 * (V21 * (cf1p1k_sq + cv1p1k_sq) + V11 * cu1p1k * cv1p1k)) * (U31_sq + U11_sq * f2prior_sq + U21_sq * f2prior_sq) + s2 * (V12 * (V12 * (cf1p1k_sq + cu1p1k_sq) + V22 * cu1p1k * cv1p1k) + V22 * (V22 * (cf1p1k_sq + cv1p1k_sq) + V12 * cu1p1k * cv1p1k)) * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) + s1 * (U11 * (U11 * (cf2p1k_sq + cu2p1k_sq) + U21 * cu2p1k * cv2p1k) + U21 * (U21 * (cf2p1k_sq + cv2p1k_sq) + U11 * cu2p1k * cv2p1k)) * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) + s1 * (U31 * (U11 * cu2p1k + U21 * cv2p1k) + U11 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U21 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) * (V32 * (V12 * cu1p1k + V22 * cv1p1k) + V12 * (V32 * cu1p1k + 2 * V12 * cf1p1k * f1_prior) + V22 * (V32 * cv1p1k + 2 * V22 * cf1p1k * f1_prior)); c27 = s1 * (U11 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U21 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s2 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V12 * (V12 * (cf1p2k_sq + cu1p2k_sq) + V22 * cu1p2k * cv1p2k) + V22 * (V22 * (cf1p2k_sq + cv1p2k_sq) + V12 * cu1p2k * cv1p2k)) + s1 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U11 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U21 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) + s2 * (V12 * (V12 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V22 * cu1p1k * cv1p2k + V22 * cu1p2k * cv1p1k) + V22 * (V22 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V12 * cu1p1k * cv1p2k + V12 * cu1p2k * cv1p1k)) * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)); c28 = s1 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U31 * (U11 * cu2p2k + U21 * cv2p2k) + U11 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U21 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (V12 * (V12 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V22 * cu1p1k * cv1p2k + V22 * cu1p2k * cv1p1k) + V22 * (V22 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V12 * cu1p1k * cv1p2k + V12 * cu1p2k * cv1p1k)) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s1 * (U11 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U21 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V32 * (V12 * cu1p2k + V22 * cv1p2k) + V12 * (V32 * cu1p2k + 2 * V12 * cf1p2k * f1_prior) + V22 * (V32 * cv1p2k + 2 * V22 * cf1p2k * f1_prior)) + s1 * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U31 * (U11 * cu2p1k + U21 * cv2p1k) + U11 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U21 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (V12 * (V12 * (cf1p2k_sq + cu1p2k_sq) + V22 * cu1p2k * cv1p2k) + V22 * (V22 * (cf1p2k_sq + cv1p2k_sq) + V12 * cu1p2k * cv1p2k)) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s1 * (U11 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U21 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V32 * (V12 * cu1p1k + V22 * cv1p1k) + V12 * (V32 * cu1p1k + 2 * V12 * cf1p1k * f1_prior) + V22 * (V32 * cv1p1k + 2 * V22 * cf1p1k * f1_prior)); c29 = s1 * (U11 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U21 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) + s2 * (V12 * (V12 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V22 * cu1p1k * cv1p2k + V22 * cu1p2k * cv1p1k) + V22 * (V22 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V12 * cu1p1k * cv1p2k + V12 * cu1p2k * cv1p1k)) * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) + s1 * (U31 * (U11 * cu2p1k + U21 * cv2p1k) + U11 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U21 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s1 * (U31 * (U11 * cu2p2k + U21 * cv2p2k) + U11 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U21 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s2 * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) * (V32 * (V12 * cu1p2k + V22 * cv1p2k) + V12 * (V32 * cu1p2k + 2 * V12 * cf1p2k * f1_prior) + V22 * (V32 * cv1p2k + 2 * V22 * cf1p2k * f1_prior)) + s2 * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) * (V32 * (V12 * cu1p1k + V22 * cv1p1k) + V12 * (V32 * cu1p1k + 2 * V12 * cf1p1k * f1_prior) + V22 * (V32 * cv1p1k + 2 * V22 * cf1p1k * f1_prior)) + s2 * (U12 * (U11 * (2 * cf2p1k * cf2p2k + 2 * cu2p1k * cu2p2k) + U21 * cu2p1k * cv2p2k + U21 * cu2p2k * cv2p1k) + U22 * (U21 * (2 * cf2p1k * cf2p2k + 2 * cv2p1k * cv2p2k) + U11 * cu2p1k * cv2p2k + U11 * cu2p2k * cv2p1k)) * (V32_sq + V12_sq * f1prior_sq + V22_sq * f1prior_sq) + s1 * (V12 * (V11 * (2 * cf1p1k * cf1p2k + 2 * cu1p1k * cu1p2k) + V21 * cu1p1k * cv1p2k + V21 * cu1p2k * cv1p1k) + V22 * (V21 * (2 * cf1p1k * cf1p2k + 2 * cv1p1k * cv1p2k) + V11 * cu1p1k * cv1p2k + V11 * cu1p2k * cv1p1k)) * (U31_sq + U11_sq * f2prior_sq + U21_sq * f2prior_sq); c210 = s2 * (V32_sq + V12_sq * f1prior_sq + V22_sq * f1prior_sq) * (U32 * (U11 * cu2p1k + U21 * cv2p1k) + U12 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U22 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s1 * (U31_sq + U11_sq * f2prior_sq + U21_sq * f2prior_sq) * (V32 * (V11 * cu1p1k + V21 * cv1p1k) + V12 * (V31 * cu1p1k + 2 * V11 * cf1p1k * f1_prior) + V22 * (V31 * cv1p1k + 2 * V21 * cf1p1k * f1_prior)) + s1 * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) * (U31 * (U11 * cu2p1k + U21 * cv2p1k) + U11 * (U31 * cu2p1k + 2 * U11 * cf2p1k * f2_prior) + U21 * (U31 * cv2p1k + 2 * U21 * cf2p1k * f2_prior)) + s2 * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) * (V32 * (V12 * cu1p1k + V22 * cv1p1k) + V12 * (V32 * cu1p1k + 2 * V12 * cf1p1k * f1_prior) + V22 * (V32 * cv1p1k + 2 * V22 * cf1p1k * f1_prior)); c211 = s1 * (U11 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U21 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) + s2 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V12 * (V12 * (cf1p2k_sq + cu1p2k_sq) + V22 * cu1p2k * cv1p2k) + V22 * (V22 * (cf1p2k_sq + cv1p2k_sq) + V12 * cu1p2k * cv1p2k)); c212 = s1 * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U31 * (U11 * cu2p2k + U21 * cv2p2k) + U11 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U21 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (V12 * (V12 * (cf1p2k_sq + cu1p2k_sq) + V22 * cu1p2k * cv1p2k) + V22 * (V22 * (cf1p2k_sq + cv1p2k_sq) + V12 * cu1p2k * cv1p2k)) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s1 * (U11 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U21 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V32 * (V12 * cu1p2k + V22 * cv1p2k) + V12 * (V32 * cu1p2k + 2 * V12 * cf1p2k * f1_prior) + V22 * (V32 * cv1p2k + 2 * V22 * cf1p2k * f1_prior)); c213 = s2 * (U12 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U22 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V32_sq + V12_sq * f1prior_sq + V22_sq * f1prior_sq) + s1 * (V12 * (V11 * (cf1p2k_sq + cu1p2k_sq) + V21 * cu1p2k * cv1p2k) + V22 * (V21 * (cf1p2k_sq + cv1p2k_sq) + V11 * cu1p2k * cv1p2k)) * (U31_sq + U11_sq * f2prior_sq + U21_sq * f2prior_sq) + s2 * (V12 * (V12 * (cf1p2k_sq + cu1p2k_sq) + V22 * cu1p2k * cv1p2k) + V22 * (V22 * (cf1p2k_sq + cv1p2k_sq) + V12 * cu1p2k * cv1p2k)) * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) + s1 * (U11 * (U11 * (cf2p2k_sq + cu2p2k_sq) + U21 * cu2p2k * cv2p2k) + U21 * (U21 * (cf2p2k_sq + cv2p2k_sq) + U11 * cu2p2k * cv2p2k)) * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) + s1 * (U31 * (U11 * cu2p2k + U21 * cv2p2k) + U11 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U21 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s2 * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) * (V32 * (V12 * cu1p2k + V22 * cv1p2k) + V12 * (V32 * cu1p2k + 2 * V12 * cf1p2k * f1_prior) + V22 * (V32 * cv1p2k + 2 * V22 * cf1p2k * f1_prior)); c214 = s2 * (V32_sq + V12_sq * f1prior_sq + V22_sq * f1prior_sq) * (U32 * (U11 * cu2p2k + U21 * cv2p2k) + U12 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U22 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s1 * (U31_sq + U11_sq * f2prior_sq + U21_sq * f2prior_sq) * (V32 * (V11 * cu1p2k + V21 * cv1p2k) + V12 * (V31 * cu1p2k + 2 * V11 * cf1p2k * f1_prior) + V22 * (V31 * cv1p2k + 2 * V21 * cf1p2k * f1_prior)) + s1 * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) * (U31 * (U11 * cu2p2k + U21 * cv2p2k) + U11 * (U31 * cu2p2k + 2 * U11 * cf2p2k * f2_prior) + U21 * (U31 * cv2p2k + 2 * U21 * cf2p2k * f2_prior)) + s2 * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) * (V32 * (V12 * cu1p2k + V22 * cv1p2k) + V12 * (V32 * cu1p2k + 2 * V12 * cf1p2k * f1_prior) + V22 * (V32 * cv1p2k + 2 * V22 * cf1p2k * f1_prior)); c215 = s2 * (U31 * U32 + U11 * U12 * f2prior_sq + U21 * U22 * f2prior_sq) * (V32_sq + V12_sq * f1prior_sq + V22_sq * f1prior_sq) + s1 * (V31 * V32 + V11 * V12 * f1prior_sq + V21 * V22 * f1prior_sq) * (U31_sq + U11_sq * f2prior_sq + U21_sq * f2prior_sq); Eigen::VectorXd ec(30); ec << c11, c12, c13, c14, c15, c16, c17, c18, c19, c110, c111, c112, c113, c114, c115, c21, c22, c23, c24, c25, c26, c27, c28, c29, c210, c211, c212, c213, c214, c215; ec /= ec.norm(); // ec.head(15) /= ec(14); // ec.tail(15) /= ec(29); int nroots = 0; Eigen::Matrix sols = solver_robust_autocal(ec, &nroots); double best_res = 20000000; double l1 = 10000, l2 = 10000, ll1, ll2; for (int i = 0; i < nroots; i++) { ll2 = sols(1, i); ll1 = sols(0, i); double res = std::abs(ll1) + std::abs(ll2); // Eigen::VectorXd monomials(15); // monomials << std::pow(ll1, 4), // std::pow(ll1, 3)* ll2, // std::pow(ll1, 3), // std::pow(ll1, 2)* std::pow(ll2, 2), // std::pow(ll1, 2)* ll2, // std::pow(ll1, 2), // ll1* std::pow(ll2, 3), // ll1* std::pow(ll2, 2), // ll1* ll2, // ll1, // std::pow(ll2, 4), std::pow(ll2, 3), std::pow(ll2, 2), ll2, 1; // // double res = std::abs(ec.head(15).dot(monomials)) + std::abs(ec.tail(15).dot(monomials)); if (res < best_res) { l1 = ll1; l2 = ll2; best_res = res; } } df1 = l1 * cf1p1k + l2 * cf1p2k; du1 = l1 * cu1p1k + l2 * cu1p2k; dv1 = l1 * cv1p1k + l2 * cv1p2k; df2 = l1 * cf2p1k + l2 * cf2p2k; du2 = l1 * cu2p1k + l2 * cu2p2k; dv2 = l1 * cv2p1k + l2 * cv2p2k; err(k) = df1 * df1 + du1 * du1 + dv1 * dv1 + df2 * df2 + du2 * du2 + dv2 * dv2; f1n = df1 + f1_prior; u1n = du1; v1n = dv1; f2n = df2 + f2_prior; u2n = du2; v2n = dv2; // next iter if (k > 0) { if ((std::abs(err(k - 1) - err(k)) / std::abs(err(k - 1)) < 1e-4) || (std::abs(err(k)) < 1e-8)) break; } } Camera camera1 = Camera("SIMPLE_PINHOLE", std::vector{f1n, u1n + pp1_prior(0), v1n + pp1_prior(1)}, -1, -1); Camera camera2 = Camera("SIMPLE_PINHOLE", std::vector{f2n, u2n + pp2_prior(0), v2n + pp2_prior(1)}, -1, -1); return std::tuple(camera1, camera2, k); } void motion_from_homography(Eigen::Matrix3d HH, std::vector *poses, std::vector *normals) { poses->reserve(4); normals->reserve(4); if (HH.determinant() < 0.0) { HH *= -1.0; } Eigen::JacobiSVD svd(HH, Eigen::ComputeFullV); Eigen::Matrix3d H2 = HH / svd.singularValues()[1]; Eigen::Vector3d S2 = svd.singularValues(); Eigen::Matrix3d V2 = svd.matrixV(); if (abs(S2(0) - S2(2)) < 1.0e-6 * S2(0)) { poses->emplace_back(H2, Eigen::Vector3d(0.0, 0.0, 0.0)); normals->emplace_back(0.0, 0.0, 0.0); return; } if (V2.determinant() < 0) { V2 *= -1.0; } double s1 = S2(0) * S2(0) / (S2(1) * S2(1)); double s3 = S2(2) * S2(2) / (S2(1) * S2(1)); Eigen::Vector3d v1 = V2.col(0); Eigen::Vector3d v2 = V2.col(1); Eigen::Vector3d v3 = V2.col(2); Eigen::Vector3d u1 = (std::sqrt(1.0 - s3) * v1 + std::sqrt(s1 - 1.0) * v3) / std::sqrt(s1 - s3); Eigen::Vector3d u2 = (std::sqrt(1.0 - s3) * v1 - std::sqrt(s1 - 1.0) * v3) / std::sqrt(s1 - s3); Eigen::Matrix3d U1; Eigen::Matrix3d W1; Eigen::Matrix3d U2; Eigen::Matrix3d W2; U1.col(0) = v2; U1.col(1) = u1; U1.col(2) = v2.cross(u1); W1.col(0) = H2 * v2; W1.col(1) = H2 * u1; W1.col(2) = (H2 * v2).cross(H2 * u1); U2.col(0) = v2; U2.col(1) = u2; U2.col(2) = v2.cross(u2); W2.col(0) = H2 * v2; W2.col(1) = H2 * u2; W2.col(2) = (H2 * v2).cross(H2 * u2); // # compute the rotation matrices Eigen::Matrix3d R1 = W1 * U1.transpose(); Eigen::Matrix3d R2 = W2 * U2.transpose(); Eigen::Vector3d n1 = v2.cross(u1); if (n1(2) < 0) { n1 *= -1.0; } Eigen::Vector3d t1 = (H2 - R1) * n1; Eigen::Vector3d n2 = v2.cross(u2); if (n2(2) < 0) { n2 *= -1.0; } Eigen::Vector3d t2 = (H2 - R2) * n2; poses->emplace_back(R1, t1); poses->emplace_back(R1, -t1); poses->emplace_back(R2, t2); poses->emplace_back(R2, -t2); normals->emplace_back(n1); normals->emplace_back(-n1); normals->emplace_back(n2); normals->emplace_back(-n2); } } // namespace poselib PoseLib-2.0.5/PoseLib/misc/decompositions.h000066400000000000000000000027611504452766400205760ustar00rootroot00000000000000#ifndef POSELIB_DECOMPOSITIONS_H #define POSELIB_DECOMPOSITIONS_H #include "PoseLib/camera_pose.h" #include "PoseLib/misc/colmap_models.h" #include "PoseLib/types.h" #include namespace poselib { // Estimate two different focal lengths from F using the formula from: // Bougnoux,"From Projective to Euclidean space under any practical situation, a criticism of self-calibration"(ICCV 98) std::pair focals_from_fundamental(const Eigen::Matrix3d &F, const Point2D &pp1, const Point2D &pp2); // Estimate two different focal lengths using the method from // Kocur, Kyselica, Kukelova, "Robust Self-calibration of Focal Lengths from the Fundamental Matrix" (CVPR 2024) std::tuple focals_from_fundamental_iterative(const Eigen::Matrix3d &F, const Camera &camera1_prior, const Camera &camera2_prior, const int &max_iters = 50, const Eigen::Vector4d &weights = Eigen::Vector4d(5.0e-4, 1.0, 5.0e-4, 1.0)); // Estimate the camera motion from homography. // If you use H obtained using correspondences in image coordinates from two cameras you need to input K2^-1 * H * K1. // Uses an adapted version of the SVD algorithm from "An invitation to 3-d vision" textbook by Ma et al. // with a trick by @yaqding to only use SVD once. void motion_from_homography(Eigen::Matrix3d HH, std::vector *poses, std::vector *normals); } // namespace poselib #endif // POSELIB_DECOMPOSITIONS_H PoseLib-2.0.5/PoseLib/misc/essential.cc000066400000000000000000000222351504452766400176620ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "essential.h" #include namespace poselib { void essential_from_motion(const CameraPose &pose, Eigen::Matrix3d *E) { *E << 0.0, -pose.t(2), pose.t(1), pose.t(2), 0.0, -pose.t(0), -pose.t(1), pose.t(0), 0.0; *E = (*E) * pose.R(); } bool check_cheirality(const CameraPose &pose, const Eigen::Vector3d &x1, const Eigen::Vector3d &x2, double min_depth) { // This code assumes that x1 and x2 are unit vectors const Eigen::Vector3d Rx1 = pose.rotate(x1); // [1 a; a 1] * [lambda1; lambda2] = [b1; b2] // [lambda1; lambda2] = [1 -a; -a 1] * [b1; b2] / (1 - a*a) const double a = -Rx1.dot(x2); const double b1 = -Rx1.dot(pose.t); const double b2 = x2.dot(pose.t); // Note that we drop the factor 1.0/(1-a*a) since it is always positive. const double lambda1 = b1 - a * b2; const double lambda2 = -a * b1 + b2; min_depth = min_depth * (1 - a * a); return lambda1 > min_depth && lambda2 > min_depth; } bool check_cheirality(const CameraPose &pose, const Eigen::Vector3d &p1, const Eigen::Vector3d &x1, const Eigen::Vector3d &p2, const Eigen::Vector3d &x2, double min_depth) { // This code assumes that x1 and x2 are unit vectors const Eigen::Vector3d Rx1 = pose.rotate(x1); // [1 a; a 1] * [lambda1; lambda2] = [b1; b2] // [lambda1; lambda2] = [1 -a; -a 1] * [b1; b2] / (1 - a*a) const Eigen::Vector3d rhs = pose.t + pose.rotate(p1) - p2; const double a = -Rx1.dot(x2); const double b1 = -Rx1.dot(rhs); const double b2 = x2.dot(rhs); // Note that we drop the factor 1.0/(1-a*a) since it is always positive. const double lambda1 = b1 - a * b2; const double lambda2 = -a * b1 + b2; min_depth = min_depth * (1 - a * a); return lambda1 > min_depth && lambda2 > min_depth; } // wrappers for vectors bool check_cheirality(const CameraPose &pose, const std::vector &x1, const std::vector &x2, double min_depth) { for (size_t i = 0; i < x1.size(); ++i) { if (!check_cheirality(pose, x1[i], x2[i], min_depth)) { return false; } } return true; } // Corresponding generalized version bool check_cheirality(const CameraPose &pose, const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, double min_depth) { for (size_t i = 0; i < x1.size(); ++i) { if (!check_cheirality(pose, p1[i], x1[i], p2[i], x2[i], min_depth)) { return false; } } return true; } void motion_from_essential(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, CameraPoseVector *relative_poses) { // Compute the necessary cross products Eigen::Vector3d u12 = E.col(0).cross(E.col(1)); Eigen::Vector3d u13 = E.col(0).cross(E.col(2)); Eigen::Vector3d u23 = E.col(1).cross(E.col(2)); const double n12 = u12.squaredNorm(); const double n13 = u13.squaredNorm(); const double n23 = u23.squaredNorm(); Eigen::Matrix3d UW; Eigen::Matrix3d Vt; // Compute the U*W factor if (n12 > n13) { if (n12 > n23) { UW.col(1) = E.col(0).normalized(); UW.col(2) = u12 / std::sqrt(n12); } else { UW.col(1) = E.col(1).normalized(); UW.col(2) = u23 / std::sqrt(n23); } } else { if (n13 > n23) { UW.col(1) = E.col(0).normalized(); UW.col(2) = u13 / std::sqrt(n13); } else { UW.col(1) = E.col(1).normalized(); UW.col(2) = u23 / std::sqrt(n23); } } UW.col(0) = -UW.col(2).cross(UW.col(1)); // Compute the V factor Vt.row(0) = UW.col(1).transpose() * E; Vt.row(1) = -UW.col(0).transpose() * E; Vt.row(0).normalize(); // Here v1 and v2 should be orthogonal. However, if E is not exactly an essential matrix they might not be // To ensure we end up with a rotation matrix we orthogonalize them again here, this should be a nop for good data Vt.row(1) -= Vt.row(0).dot(Vt.row(1)) * Vt.row(0); Vt.row(1).normalize(); Vt.row(2) = Vt.row(0).cross(Vt.row(1)); poselib::CameraPose pose; pose.q = rotmat_to_quat(UW * Vt); pose.t = UW.col(2); if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } pose.t = -pose.t; if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } // U * W.transpose() UW.block<3, 2>(0, 0) = -UW.block<3, 2>(0, 0); pose.q = rotmat_to_quat(UW * Vt); if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } pose.t = -pose.t; if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } } void motion_from_essential_planar(double e01, double e21, double e10, double e12, const std::vector &x1, const std::vector &x2, poselib::CameraPoseVector *relative_poses) { Eigen::Vector2d z; z << -e01 * e10 - e21 * e12, -e21 * e10 + e01 * e12; z.normalize(); CameraPose pose; Eigen::Matrix3d R; R << z(0), 0.0, -z(1), 0.0, 1.0, 0.0, z(1), 0.0, z(0); pose.q = rotmat_to_quat(R); pose.t << e21, 0.0, -e01; pose.t.normalize(); if (check_cheirality(pose, x1, x2)) { relative_poses->push_back(pose); } pose.t = -pose.t; if (check_cheirality(pose, x1, x2)) { relative_poses->push_back(pose); } // There are two more flipped solutions where // R = [a 0 b; 0 -1 0; b 0 -a] // These are probably not interesting in the planar case /* z << e01 * e10 - e21 * e12, e21* e10 + e01 * e12; z.normalize(); pose.R << z(0), 0.0, z(1), 0.0, -1.0, 0.0, z(1), 0.0, -z(0); relative_poses->push_back(pose); pose.t = -pose.t; relative_poses->push_back(pose); */ } void motion_from_essential_svd(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, poselib::CameraPoseVector *relative_poses) { Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d U = USV.matrixU(); Eigen::Matrix3d Vt = USV.matrixV().transpose(); // Last column of U is undetermined since d = (a a 0). if (U.determinant() < 0) { U.col(2) *= -1; } // Last row of Vt is undetermined since d = (a a 0). if (Vt.determinant() < 0) { Vt.row(2) *= -1; } Eigen::Matrix3d W; W << 0, -1, 0, 1, 0, 0, 0, 0, 1; const Eigen::Matrix3d U_W_Vt = U * W * Vt; const Eigen::Matrix3d U_Wt_Vt = U * W.transpose() * Vt; const std::array R{{U_W_Vt, U_Wt_Vt}}; const std::array t{{U.col(2), -U.col(2)}}; if (relative_poses) { poselib::CameraPose pose; pose.q = rotmat_to_quat(R[0]); pose.t = t[0]; if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } pose.t = t[1]; if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } pose.q = rotmat_to_quat(R[1]); pose.t = t[0]; if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } pose.t = t[1]; if (check_cheirality(pose, x1, x2)) { relative_poses->emplace_back(pose); } } } } // namespace poselibPoseLib-2.0.5/PoseLib/misc/essential.h000066400000000000000000000114031504452766400175170ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_MISC_ESSENTIAL_H_ #define POSELIB_MISC_ESSENTIAL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Computes the essential matrix from the camera motion void essential_from_motion(const CameraPose &pose, Eigen::Matrix3d *E); // Checks the cheirality of the point correspondences, i.e. that // lambda_2 * x2 = R * ( lambda_1 * x1 ) + t // with lambda_1 and lambda_2 positive bool check_cheirality(const CameraPose &pose, const Eigen::Vector3d &x1, const Eigen::Vector3d &x2, double min_depth = 0.0); // Corresponding generalized version bool check_cheirality(const CameraPose &pose, const Eigen::Vector3d &p1, const Eigen::Vector3d &x1, const Eigen::Vector3d &p2, const Eigen::Vector3d &x2, double min_depth = 0.0); // wrappers for vectors bool check_cheirality(const CameraPose &pose, const std::vector &x1, const std::vector &x2, double min_depth = 0.0); // Corresponding generalized version bool check_cheirality(const CameraPose &pose, const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, double min_depth = 0.0); /** * @brief Given an essential matrix computes the 2 rotations and the 2 translations. The method also takes one point * correspondence that is used to filter for cheirality. that can generate four possible motions. * @param E Essential matrix * @param[out] relative_poses The 4 possible relative poses * @ref Multiple View Geometry - Richard Hartley, Andrew Zisserman - second edition * @see HZ 9.7 page 259 (Result 9.19) */ void motion_from_essential_svd(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, CameraPoseVector *relative_poses); /* Computes the factorization using the closed-form SVD suggested in Nister, An Efficient Solution to the Five-Point Relative Pose Problem, PAMI 2004 The method also takes one point correspondence that is used to filter for cheirality. */ void motion_from_essential(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, CameraPoseVector *relative_poses); /* Factorizes the essential matrix into the relative poses. Assumes that the essential matrix corresponds to planar motion, i.e. that we have E = [0 e01 0; e10 0 e12; 0 e21 0] Only returns the solution where the rotation is on the form R = [a 0 -b; 0 1 0; b 0 a]; Note that there is another solution where the rotation is on the form R = [a 0 b; 0 -1 0; b 0 -a]; which is not returned! The method also takes one point correspondence that is used to filter for cheirality. */ void motion_from_essential_planar(double e01, double e21, double e10, double e12, const std::vector &x1, const std::vector &x2, CameraPoseVector *relative_poses); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/misc/qep.cc000066400000000000000000001065511504452766400164640ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "qep.h" #include "PoseLib/misc/sturm.h" #include "PoseLib/misc/univariate.h" namespace poselib { namespace qep { int qep_linearize(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[8], Eigen::Matrix *eig_vecs) { Eigen::Matrix M; M.block<4, 4>(0, 0) = B; M.block<4, 4>(0, 4) = C; M.block<4, 4>(4, 0).setIdentity(); M.block<4, 4>(4, 4).setZero(); M.block<4, 8>(0, 0) = -A.inverse() * M.block<4, 8>(0, 0); Eigen::EigenSolver> es(M, true); Eigen::Matrix, 8, 1> D = es.eigenvalues(); Eigen::Matrix, 8, 8> V = es.eigenvectors(); int n_roots = 0; for (int i = 0; i < 8; ++i) { if (std::abs(D(i).imag()) > 1e-8) continue; eig_vecs->col(n_roots) = V.block<3, 1>(4, i).real() / V(7, i).real(); eig_vals[n_roots++] = D(i).real(); } return n_roots; } // Computes polynomial p(x) = det(x^2*I + x * A + B) void detpoly4(const Eigen::Matrix &A, const Eigen::Matrix &B, double coeffs[9]) { coeffs[0] = B(0, 0) * B(1, 1) * B(2, 2) * B(3, 3) - B(0, 0) * B(1, 1) * B(2, 3) * B(3, 2) - B(0, 0) * B(1, 2) * B(2, 1) * B(3, 3) + B(0, 0) * B(1, 2) * B(2, 3) * B(3, 1) + B(0, 0) * B(1, 3) * B(2, 1) * B(3, 2) - B(0, 0) * B(1, 3) * B(2, 2) * B(3, 1) - B(0, 1) * B(1, 0) * B(2, 2) * B(3, 3) + B(0, 1) * B(1, 0) * B(2, 3) * B(3, 2) + B(0, 1) * B(1, 2) * B(2, 0) * B(3, 3) - B(0, 1) * B(1, 2) * B(2, 3) * B(3, 0) - B(0, 1) * B(1, 3) * B(2, 0) * B(3, 2) + B(0, 1) * B(1, 3) * B(2, 2) * B(3, 0) + B(0, 2) * B(1, 0) * B(2, 1) * B(3, 3) - B(0, 2) * B(1, 0) * B(2, 3) * B(3, 1) - B(0, 2) * B(1, 1) * B(2, 0) * B(3, 3) + B(0, 2) * B(1, 1) * B(2, 3) * B(3, 0) + B(0, 2) * B(1, 3) * B(2, 0) * B(3, 1) - B(0, 2) * B(1, 3) * B(2, 1) * B(3, 0) - B(0, 3) * B(1, 0) * B(2, 1) * B(3, 2) + B(0, 3) * B(1, 0) * B(2, 2) * B(3, 1) + B(0, 3) * B(1, 1) * B(2, 0) * B(3, 2) - B(0, 3) * B(1, 1) * B(2, 2) * B(3, 0) - B(0, 3) * B(1, 2) * B(2, 0) * B(3, 1) + B(0, 3) * B(1, 2) * B(2, 1) * B(3, 0); coeffs[1] = A(0, 0) * B(1, 1) * B(2, 2) * B(3, 3) - A(0, 0) * B(1, 1) * B(2, 3) * B(3, 2) - A(0, 0) * B(1, 2) * B(2, 1) * B(3, 3) + A(0, 0) * B(1, 2) * B(2, 3) * B(3, 1) + A(0, 0) * B(1, 3) * B(2, 1) * B(3, 2) - A(0, 0) * B(1, 3) * B(2, 2) * B(3, 1) - A(0, 1) * B(1, 0) * B(2, 2) * B(3, 3) + A(0, 1) * B(1, 0) * B(2, 3) * B(3, 2) + A(0, 1) * B(1, 2) * B(2, 0) * B(3, 3) - A(0, 1) * B(1, 2) * B(2, 3) * B(3, 0) - A(0, 1) * B(1, 3) * B(2, 0) * B(3, 2) + A(0, 1) * B(1, 3) * B(2, 2) * B(3, 0) + A(0, 2) * B(1, 0) * B(2, 1) * B(3, 3) - A(0, 2) * B(1, 0) * B(2, 3) * B(3, 1) - A(0, 2) * B(1, 1) * B(2, 0) * B(3, 3) + A(0, 2) * B(1, 1) * B(2, 3) * B(3, 0) + A(0, 2) * B(1, 3) * B(2, 0) * B(3, 1) - A(0, 2) * B(1, 3) * B(2, 1) * B(3, 0) - A(0, 3) * B(1, 0) * B(2, 1) * B(3, 2) + A(0, 3) * B(1, 0) * B(2, 2) * B(3, 1) + A(0, 3) * B(1, 1) * B(2, 0) * B(3, 2) - A(0, 3) * B(1, 1) * B(2, 2) * B(3, 0) - A(0, 3) * B(1, 2) * B(2, 0) * B(3, 1) + A(0, 3) * B(1, 2) * B(2, 1) * B(3, 0) - A(1, 0) * B(0, 1) * B(2, 2) * B(3, 3) + A(1, 0) * B(0, 1) * B(2, 3) * B(3, 2) + A(1, 0) * B(0, 2) * B(2, 1) * B(3, 3) - A(1, 0) * B(0, 2) * B(2, 3) * B(3, 1) - A(1, 0) * B(0, 3) * B(2, 1) * B(3, 2) + A(1, 0) * B(0, 3) * B(2, 2) * B(3, 1) + A(1, 1) * B(0, 0) * B(2, 2) * B(3, 3) - A(1, 1) * B(0, 0) * B(2, 3) * B(3, 2) - A(1, 1) * B(0, 2) * B(2, 0) * B(3, 3) + A(1, 1) * B(0, 2) * B(2, 3) * B(3, 0) + A(1, 1) * B(0, 3) * B(2, 0) * B(3, 2) - A(1, 1) * B(0, 3) * B(2, 2) * B(3, 0) - A(1, 2) * B(0, 0) * B(2, 1) * B(3, 3) + A(1, 2) * B(0, 0) * B(2, 3) * B(3, 1) + A(1, 2) * B(0, 1) * B(2, 0) * B(3, 3) - A(1, 2) * B(0, 1) * B(2, 3) * B(3, 0) - A(1, 2) * B(0, 3) * B(2, 0) * B(3, 1) + A(1, 2) * B(0, 3) * B(2, 1) * B(3, 0) + A(1, 3) * B(0, 0) * B(2, 1) * B(3, 2) - A(1, 3) * B(0, 0) * B(2, 2) * B(3, 1) - A(1, 3) * B(0, 1) * B(2, 0) * B(3, 2) + A(1, 3) * B(0, 1) * B(2, 2) * B(3, 0) + A(1, 3) * B(0, 2) * B(2, 0) * B(3, 1) - A(1, 3) * B(0, 2) * B(2, 1) * B(3, 0) + A(2, 0) * B(0, 1) * B(1, 2) * B(3, 3) - A(2, 0) * B(0, 1) * B(1, 3) * B(3, 2) - A(2, 0) * B(0, 2) * B(1, 1) * B(3, 3) + A(2, 0) * B(0, 2) * B(1, 3) * B(3, 1) + A(2, 0) * B(0, 3) * B(1, 1) * B(3, 2) - A(2, 0) * B(0, 3) * B(1, 2) * B(3, 1) - A(2, 1) * B(0, 0) * B(1, 2) * B(3, 3) + A(2, 1) * B(0, 0) * B(1, 3) * B(3, 2) + A(2, 1) * B(0, 2) * B(1, 0) * B(3, 3) - A(2, 1) * B(0, 2) * B(1, 3) * B(3, 0) - A(2, 1) * B(0, 3) * B(1, 0) * B(3, 2) + A(2, 1) * B(0, 3) * B(1, 2) * B(3, 0) + A(2, 2) * B(0, 0) * B(1, 1) * B(3, 3) - A(2, 2) * B(0, 0) * B(1, 3) * B(3, 1) - A(2, 2) * B(0, 1) * B(1, 0) * B(3, 3) + A(2, 2) * B(0, 1) * B(1, 3) * B(3, 0) + A(2, 2) * B(0, 3) * B(1, 0) * B(3, 1) - A(2, 2) * B(0, 3) * B(1, 1) * B(3, 0) - A(2, 3) * B(0, 0) * B(1, 1) * B(3, 2) + A(2, 3) * B(0, 0) * B(1, 2) * B(3, 1) + A(2, 3) * B(0, 1) * B(1, 0) * B(3, 2) - A(2, 3) * B(0, 1) * B(1, 2) * B(3, 0) - A(2, 3) * B(0, 2) * B(1, 0) * B(3, 1) + A(2, 3) * B(0, 2) * B(1, 1) * B(3, 0) - A(3, 0) * B(0, 1) * B(1, 2) * B(2, 3) + A(3, 0) * B(0, 1) * B(1, 3) * B(2, 2) + A(3, 0) * B(0, 2) * B(1, 1) * B(2, 3) - A(3, 0) * B(0, 2) * B(1, 3) * B(2, 1) - A(3, 0) * B(0, 3) * B(1, 1) * B(2, 2) + A(3, 0) * B(0, 3) * B(1, 2) * B(2, 1) + A(3, 1) * B(0, 0) * B(1, 2) * B(2, 3) - A(3, 1) * B(0, 0) * B(1, 3) * B(2, 2) - A(3, 1) * B(0, 2) * B(1, 0) * B(2, 3) + A(3, 1) * B(0, 2) * B(1, 3) * B(2, 0) + A(3, 1) * B(0, 3) * B(1, 0) * B(2, 2) - A(3, 1) * B(0, 3) * B(1, 2) * B(2, 0) - A(3, 2) * B(0, 0) * B(1, 1) * B(2, 3) + A(3, 2) * B(0, 0) * B(1, 3) * B(2, 1) + A(3, 2) * B(0, 1) * B(1, 0) * B(2, 3) - A(3, 2) * B(0, 1) * B(1, 3) * B(2, 0) - A(3, 2) * B(0, 3) * B(1, 0) * B(2, 1) + A(3, 2) * B(0, 3) * B(1, 1) * B(2, 0) + A(3, 3) * B(0, 0) * B(1, 1) * B(2, 2) - A(3, 3) * B(0, 0) * B(1, 2) * B(2, 1) - A(3, 3) * B(0, 1) * B(1, 0) * B(2, 2) + A(3, 3) * B(0, 1) * B(1, 2) * B(2, 0) + A(3, 3) * B(0, 2) * B(1, 0) * B(2, 1) - A(3, 3) * B(0, 2) * B(1, 1) * B(2, 0); coeffs[2] = B(0, 0) * B(1, 1) * B(2, 2) - B(0, 0) * B(1, 2) * B(2, 1) - B(0, 1) * B(1, 0) * B(2, 2) + B(0, 1) * B(1, 2) * B(2, 0) + B(0, 2) * B(1, 0) * B(2, 1) - B(0, 2) * B(1, 1) * B(2, 0) + B(0, 0) * B(1, 1) * B(3, 3) - B(0, 0) * B(1, 3) * B(3, 1) - B(0, 1) * B(1, 0) * B(3, 3) + B(0, 1) * B(1, 3) * B(3, 0) + B(0, 3) * B(1, 0) * B(3, 1) - B(0, 3) * B(1, 1) * B(3, 0) + B(0, 0) * B(2, 2) * B(3, 3) - B(0, 0) * B(2, 3) * B(3, 2) - B(0, 2) * B(2, 0) * B(3, 3) + B(0, 2) * B(2, 3) * B(3, 0) + B(0, 3) * B(2, 0) * B(3, 2) - B(0, 3) * B(2, 2) * B(3, 0) + B(1, 1) * B(2, 2) * B(3, 3) - B(1, 1) * B(2, 3) * B(3, 2) - B(1, 2) * B(2, 1) * B(3, 3) + B(1, 2) * B(2, 3) * B(3, 1) + B(1, 3) * B(2, 1) * B(3, 2) - B(1, 3) * B(2, 2) * B(3, 1) + A(0, 0) * A(1, 1) * B(2, 2) * B(3, 3) - A(0, 0) * A(1, 1) * B(2, 3) * B(3, 2) - A(0, 0) * A(1, 2) * B(2, 1) * B(3, 3) + A(0, 0) * A(1, 2) * B(2, 3) * B(3, 1) + A(0, 0) * A(1, 3) * B(2, 1) * B(3, 2) - A(0, 0) * A(1, 3) * B(2, 2) * B(3, 1) - A(0, 0) * A(2, 1) * B(1, 2) * B(3, 3) + A(0, 0) * A(2, 1) * B(1, 3) * B(3, 2) + A(0, 0) * A(2, 2) * B(1, 1) * B(3, 3) - A(0, 0) * A(2, 2) * B(1, 3) * B(3, 1) - A(0, 0) * A(2, 3) * B(1, 1) * B(3, 2) + A(0, 0) * A(2, 3) * B(1, 2) * B(3, 1) + A(0, 0) * A(3, 1) * B(1, 2) * B(2, 3) - A(0, 0) * A(3, 1) * B(1, 3) * B(2, 2) - A(0, 0) * A(3, 2) * B(1, 1) * B(2, 3) + A(0, 0) * A(3, 2) * B(1, 3) * B(2, 1) + A(0, 0) * A(3, 3) * B(1, 1) * B(2, 2) - A(0, 0) * A(3, 3) * B(1, 2) * B(2, 1) - A(0, 1) * A(1, 0) * B(2, 2) * B(3, 3) + A(0, 1) * A(1, 0) * B(2, 3) * B(3, 2) + A(0, 1) * A(1, 2) * B(2, 0) * B(3, 3) - A(0, 1) * A(1, 2) * B(2, 3) * B(3, 0) - A(0, 1) * A(1, 3) * B(2, 0) * B(3, 2) + A(0, 1) * A(1, 3) * B(2, 2) * B(3, 0) + A(0, 1) * A(2, 0) * B(1, 2) * B(3, 3) - A(0, 1) * A(2, 0) * B(1, 3) * B(3, 2) - A(0, 1) * A(2, 2) * B(1, 0) * B(3, 3) + A(0, 1) * A(2, 2) * B(1, 3) * B(3, 0) + A(0, 1) * A(2, 3) * B(1, 0) * B(3, 2) - A(0, 1) * A(2, 3) * B(1, 2) * B(3, 0) - A(0, 1) * A(3, 0) * B(1, 2) * B(2, 3) + A(0, 1) * A(3, 0) * B(1, 3) * B(2, 2) + A(0, 1) * A(3, 2) * B(1, 0) * B(2, 3) - A(0, 1) * A(3, 2) * B(1, 3) * B(2, 0) - A(0, 1) * A(3, 3) * B(1, 0) * B(2, 2) + A(0, 1) * A(3, 3) * B(1, 2) * B(2, 0) + A(0, 2) * A(1, 0) * B(2, 1) * B(3, 3) - A(0, 2) * A(1, 0) * B(2, 3) * B(3, 1) - A(0, 2) * A(1, 1) * B(2, 0) * B(3, 3) + A(0, 2) * A(1, 1) * B(2, 3) * B(3, 0) + A(0, 2) * A(1, 3) * B(2, 0) * B(3, 1) - A(0, 2) * A(1, 3) * B(2, 1) * B(3, 0) - A(0, 2) * A(2, 0) * B(1, 1) * B(3, 3) + A(0, 2) * A(2, 0) * B(1, 3) * B(3, 1) + A(0, 2) * A(2, 1) * B(1, 0) * B(3, 3) - A(0, 2) * A(2, 1) * B(1, 3) * B(3, 0) - A(0, 2) * A(2, 3) * B(1, 0) * B(3, 1) + A(0, 2) * A(2, 3) * B(1, 1) * B(3, 0) + A(0, 2) * A(3, 0) * B(1, 1) * B(2, 3) - A(0, 2) * A(3, 0) * B(1, 3) * B(2, 1) - A(0, 2) * A(3, 1) * B(1, 0) * B(2, 3) + A(0, 2) * A(3, 1) * B(1, 3) * B(2, 0) + A(0, 2) * A(3, 3) * B(1, 0) * B(2, 1) - A(0, 2) * A(3, 3) * B(1, 1) * B(2, 0) - A(0, 3) * A(1, 0) * B(2, 1) * B(3, 2) + A(0, 3) * A(1, 0) * B(2, 2) * B(3, 1) + A(0, 3) * A(1, 1) * B(2, 0) * B(3, 2) - A(0, 3) * A(1, 1) * B(2, 2) * B(3, 0) - A(0, 3) * A(1, 2) * B(2, 0) * B(3, 1) + A(0, 3) * A(1, 2) * B(2, 1) * B(3, 0) + A(0, 3) * A(2, 0) * B(1, 1) * B(3, 2) - A(0, 3) * A(2, 0) * B(1, 2) * B(3, 1) - A(0, 3) * A(2, 1) * B(1, 0) * B(3, 2) + A(0, 3) * A(2, 1) * B(1, 2) * B(3, 0) + A(0, 3) * A(2, 2) * B(1, 0) * B(3, 1) - A(0, 3) * A(2, 2) * B(1, 1) * B(3, 0) - A(0, 3) * A(3, 0) * B(1, 1) * B(2, 2) + A(0, 3) * A(3, 0) * B(1, 2) * B(2, 1) + A(0, 3) * A(3, 1) * B(1, 0) * B(2, 2) - A(0, 3) * A(3, 1) * B(1, 2) * B(2, 0) - A(0, 3) * A(3, 2) * B(1, 0) * B(2, 1) + A(0, 3) * A(3, 2) * B(1, 1) * B(2, 0) + A(1, 0) * A(2, 1) * B(0, 2) * B(3, 3) - A(1, 0) * A(2, 1) * B(0, 3) * B(3, 2) - A(1, 0) * A(2, 2) * B(0, 1) * B(3, 3) + A(1, 0) * A(2, 2) * B(0, 3) * B(3, 1) + A(1, 0) * A(2, 3) * B(0, 1) * B(3, 2) - A(1, 0) * A(2, 3) * B(0, 2) * B(3, 1) - A(1, 0) * A(3, 1) * B(0, 2) * B(2, 3) + A(1, 0) * A(3, 1) * B(0, 3) * B(2, 2) + A(1, 0) * A(3, 2) * B(0, 1) * B(2, 3) - A(1, 0) * A(3, 2) * B(0, 3) * B(2, 1) - A(1, 0) * A(3, 3) * B(0, 1) * B(2, 2) + A(1, 0) * A(3, 3) * B(0, 2) * B(2, 1) - A(1, 1) * A(2, 0) * B(0, 2) * B(3, 3) + A(1, 1) * A(2, 0) * B(0, 3) * B(3, 2) + A(1, 1) * A(2, 2) * B(0, 0) * B(3, 3) - A(1, 1) * A(2, 2) * B(0, 3) * B(3, 0) - A(1, 1) * A(2, 3) * B(0, 0) * B(3, 2) + A(1, 1) * A(2, 3) * B(0, 2) * B(3, 0) + A(1, 1) * A(3, 0) * B(0, 2) * B(2, 3) - A(1, 1) * A(3, 0) * B(0, 3) * B(2, 2) - A(1, 1) * A(3, 2) * B(0, 0) * B(2, 3) + A(1, 1) * A(3, 2) * B(0, 3) * B(2, 0) + A(1, 1) * A(3, 3) * B(0, 0) * B(2, 2) - A(1, 1) * A(3, 3) * B(0, 2) * B(2, 0) + A(1, 2) * A(2, 0) * B(0, 1) * B(3, 3) - A(1, 2) * A(2, 0) * B(0, 3) * B(3, 1) - A(1, 2) * A(2, 1) * B(0, 0) * B(3, 3) + A(1, 2) * A(2, 1) * B(0, 3) * B(3, 0) + A(1, 2) * A(2, 3) * B(0, 0) * B(3, 1) - A(1, 2) * A(2, 3) * B(0, 1) * B(3, 0) - A(1, 2) * A(3, 0) * B(0, 1) * B(2, 3) + A(1, 2) * A(3, 0) * B(0, 3) * B(2, 1) + A(1, 2) * A(3, 1) * B(0, 0) * B(2, 3) - A(1, 2) * A(3, 1) * B(0, 3) * B(2, 0) - A(1, 2) * A(3, 3) * B(0, 0) * B(2, 1) + A(1, 2) * A(3, 3) * B(0, 1) * B(2, 0) - A(1, 3) * A(2, 0) * B(0, 1) * B(3, 2) + A(1, 3) * A(2, 0) * B(0, 2) * B(3, 1) + A(1, 3) * A(2, 1) * B(0, 0) * B(3, 2) - A(1, 3) * A(2, 1) * B(0, 2) * B(3, 0) - A(1, 3) * A(2, 2) * B(0, 0) * B(3, 1) + A(1, 3) * A(2, 2) * B(0, 1) * B(3, 0) + A(1, 3) * A(3, 0) * B(0, 1) * B(2, 2) - A(1, 3) * A(3, 0) * B(0, 2) * B(2, 1) - A(1, 3) * A(3, 1) * B(0, 0) * B(2, 2) + A(1, 3) * A(3, 1) * B(0, 2) * B(2, 0) + A(1, 3) * A(3, 2) * B(0, 0) * B(2, 1) - A(1, 3) * A(3, 2) * B(0, 1) * B(2, 0) + A(2, 0) * A(3, 1) * B(0, 2) * B(1, 3) - A(2, 0) * A(3, 1) * B(0, 3) * B(1, 2) - A(2, 0) * A(3, 2) * B(0, 1) * B(1, 3) + A(2, 0) * A(3, 2) * B(0, 3) * B(1, 1) + A(2, 0) * A(3, 3) * B(0, 1) * B(1, 2) - A(2, 0) * A(3, 3) * B(0, 2) * B(1, 1) - A(2, 1) * A(3, 0) * B(0, 2) * B(1, 3) + A(2, 1) * A(3, 0) * B(0, 3) * B(1, 2) + A(2, 1) * A(3, 2) * B(0, 0) * B(1, 3) - A(2, 1) * A(3, 2) * B(0, 3) * B(1, 0) - A(2, 1) * A(3, 3) * B(0, 0) * B(1, 2) + A(2, 1) * A(3, 3) * B(0, 2) * B(1, 0) + A(2, 2) * A(3, 0) * B(0, 1) * B(1, 3) - A(2, 2) * A(3, 0) * B(0, 3) * B(1, 1) - A(2, 2) * A(3, 1) * B(0, 0) * B(1, 3) + A(2, 2) * A(3, 1) * B(0, 3) * B(1, 0) + A(2, 2) * A(3, 3) * B(0, 0) * B(1, 1) - A(2, 2) * A(3, 3) * B(0, 1) * B(1, 0) - A(2, 3) * A(3, 0) * B(0, 1) * B(1, 2) + A(2, 3) * A(3, 0) * B(0, 2) * B(1, 1) + A(2, 3) * A(3, 1) * B(0, 0) * B(1, 2) - A(2, 3) * A(3, 1) * B(0, 2) * B(1, 0) - A(2, 3) * A(3, 2) * B(0, 0) * B(1, 1) + A(2, 3) * A(3, 2) * B(0, 1) * B(1, 0); coeffs[3] = A(0, 0) * B(1, 1) * B(2, 2) - A(0, 0) * B(1, 2) * B(2, 1) - A(0, 1) * B(1, 0) * B(2, 2) + A(0, 1) * B(1, 2) * B(2, 0) + A(0, 2) * B(1, 0) * B(2, 1) - A(0, 2) * B(1, 1) * B(2, 0) - A(1, 0) * B(0, 1) * B(2, 2) + A(1, 0) * B(0, 2) * B(2, 1) + A(1, 1) * B(0, 0) * B(2, 2) - A(1, 1) * B(0, 2) * B(2, 0) - A(1, 2) * B(0, 0) * B(2, 1) + A(1, 2) * B(0, 1) * B(2, 0) + A(2, 0) * B(0, 1) * B(1, 2) - A(2, 0) * B(0, 2) * B(1, 1) - A(2, 1) * B(0, 0) * B(1, 2) + A(2, 1) * B(0, 2) * B(1, 0) + A(2, 2) * B(0, 0) * B(1, 1) - A(2, 2) * B(0, 1) * B(1, 0) + A(0, 0) * B(1, 1) * B(3, 3) - A(0, 0) * B(1, 3) * B(3, 1) - A(0, 1) * B(1, 0) * B(3, 3) + A(0, 1) * B(1, 3) * B(3, 0) + A(0, 3) * B(1, 0) * B(3, 1) - A(0, 3) * B(1, 1) * B(3, 0) - A(1, 0) * B(0, 1) * B(3, 3) + A(1, 0) * B(0, 3) * B(3, 1) + A(1, 1) * B(0, 0) * B(3, 3) - A(1, 1) * B(0, 3) * B(3, 0) - A(1, 3) * B(0, 0) * B(3, 1) + A(1, 3) * B(0, 1) * B(3, 0) + A(3, 0) * B(0, 1) * B(1, 3) - A(3, 0) * B(0, 3) * B(1, 1) - A(3, 1) * B(0, 0) * B(1, 3) + A(3, 1) * B(0, 3) * B(1, 0) + A(3, 3) * B(0, 0) * B(1, 1) - A(3, 3) * B(0, 1) * B(1, 0) + A(0, 0) * B(2, 2) * B(3, 3) - A(0, 0) * B(2, 3) * B(3, 2) - A(0, 2) * B(2, 0) * B(3, 3) + A(0, 2) * B(2, 3) * B(3, 0) + A(0, 3) * B(2, 0) * B(3, 2) - A(0, 3) * B(2, 2) * B(3, 0) - A(2, 0) * B(0, 2) * B(3, 3) + A(2, 0) * B(0, 3) * B(3, 2) + A(2, 2) * B(0, 0) * B(3, 3) - A(2, 2) * B(0, 3) * B(3, 0) - A(2, 3) * B(0, 0) * B(3, 2) + A(2, 3) * B(0, 2) * B(3, 0) + A(3, 0) * B(0, 2) * B(2, 3) - A(3, 0) * B(0, 3) * B(2, 2) - A(3, 2) * B(0, 0) * B(2, 3) + A(3, 2) * B(0, 3) * B(2, 0) + A(3, 3) * B(0, 0) * B(2, 2) - A(3, 3) * B(0, 2) * B(2, 0) + A(1, 1) * B(2, 2) * B(3, 3) - A(1, 1) * B(2, 3) * B(3, 2) - A(1, 2) * B(2, 1) * B(3, 3) + A(1, 2) * B(2, 3) * B(3, 1) + A(1, 3) * B(2, 1) * B(3, 2) - A(1, 3) * B(2, 2) * B(3, 1) - A(2, 1) * B(1, 2) * B(3, 3) + A(2, 1) * B(1, 3) * B(3, 2) + A(2, 2) * B(1, 1) * B(3, 3) - A(2, 2) * B(1, 3) * B(3, 1) - A(2, 3) * B(1, 1) * B(3, 2) + A(2, 3) * B(1, 2) * B(3, 1) + A(3, 1) * B(1, 2) * B(2, 3) - A(3, 1) * B(1, 3) * B(2, 2) - A(3, 2) * B(1, 1) * B(2, 3) + A(3, 2) * B(1, 3) * B(2, 1) + A(3, 3) * B(1, 1) * B(2, 2) - A(3, 3) * B(1, 2) * B(2, 1) + A(0, 0) * A(1, 1) * A(2, 2) * B(3, 3) - A(0, 0) * A(1, 1) * A(2, 3) * B(3, 2) - A(0, 0) * A(1, 1) * A(3, 2) * B(2, 3) + A(0, 0) * A(1, 1) * A(3, 3) * B(2, 2) - A(0, 0) * A(1, 2) * A(2, 1) * B(3, 3) + A(0, 0) * A(1, 2) * A(2, 3) * B(3, 1) + A(0, 0) * A(1, 2) * A(3, 1) * B(2, 3) - A(0, 0) * A(1, 2) * A(3, 3) * B(2, 1) + A(0, 0) * A(1, 3) * A(2, 1) * B(3, 2) - A(0, 0) * A(1, 3) * A(2, 2) * B(3, 1) - A(0, 0) * A(1, 3) * A(3, 1) * B(2, 2) + A(0, 0) * A(1, 3) * A(3, 2) * B(2, 1) + A(0, 0) * A(2, 1) * A(3, 2) * B(1, 3) - A(0, 0) * A(2, 1) * A(3, 3) * B(1, 2) - A(0, 0) * A(2, 2) * A(3, 1) * B(1, 3) + A(0, 0) * A(2, 2) * A(3, 3) * B(1, 1) + A(0, 0) * A(2, 3) * A(3, 1) * B(1, 2) - A(0, 0) * A(2, 3) * A(3, 2) * B(1, 1) - A(0, 1) * A(1, 0) * A(2, 2) * B(3, 3) + A(0, 1) * A(1, 0) * A(2, 3) * B(3, 2) + A(0, 1) * A(1, 0) * A(3, 2) * B(2, 3) - A(0, 1) * A(1, 0) * A(3, 3) * B(2, 2) + A(0, 1) * A(1, 2) * A(2, 0) * B(3, 3) - A(0, 1) * A(1, 2) * A(2, 3) * B(3, 0) - A(0, 1) * A(1, 2) * A(3, 0) * B(2, 3) + A(0, 1) * A(1, 2) * A(3, 3) * B(2, 0) - A(0, 1) * A(1, 3) * A(2, 0) * B(3, 2) + A(0, 1) * A(1, 3) * A(2, 2) * B(3, 0) + A(0, 1) * A(1, 3) * A(3, 0) * B(2, 2) - A(0, 1) * A(1, 3) * A(3, 2) * B(2, 0) - A(0, 1) * A(2, 0) * A(3, 2) * B(1, 3) + A(0, 1) * A(2, 0) * A(3, 3) * B(1, 2) + A(0, 1) * A(2, 2) * A(3, 0) * B(1, 3) - A(0, 1) * A(2, 2) * A(3, 3) * B(1, 0) - A(0, 1) * A(2, 3) * A(3, 0) * B(1, 2) + A(0, 1) * A(2, 3) * A(3, 2) * B(1, 0) + A(0, 2) * A(1, 0) * A(2, 1) * B(3, 3) - A(0, 2) * A(1, 0) * A(2, 3) * B(3, 1) - A(0, 2) * A(1, 0) * A(3, 1) * B(2, 3) + A(0, 2) * A(1, 0) * A(3, 3) * B(2, 1) - A(0, 2) * A(1, 1) * A(2, 0) * B(3, 3) + A(0, 2) * A(1, 1) * A(2, 3) * B(3, 0) + A(0, 2) * A(1, 1) * A(3, 0) * B(2, 3) - A(0, 2) * A(1, 1) * A(3, 3) * B(2, 0) + A(0, 2) * A(1, 3) * A(2, 0) * B(3, 1) - A(0, 2) * A(1, 3) * A(2, 1) * B(3, 0) - A(0, 2) * A(1, 3) * A(3, 0) * B(2, 1) + A(0, 2) * A(1, 3) * A(3, 1) * B(2, 0) + A(0, 2) * A(2, 0) * A(3, 1) * B(1, 3) - A(0, 2) * A(2, 0) * A(3, 3) * B(1, 1) - A(0, 2) * A(2, 1) * A(3, 0) * B(1, 3) + A(0, 2) * A(2, 1) * A(3, 3) * B(1, 0) + A(0, 2) * A(2, 3) * A(3, 0) * B(1, 1) - A(0, 2) * A(2, 3) * A(3, 1) * B(1, 0) - A(0, 3) * A(1, 0) * A(2, 1) * B(3, 2) + A(0, 3) * A(1, 0) * A(2, 2) * B(3, 1) + A(0, 3) * A(1, 0) * A(3, 1) * B(2, 2) - A(0, 3) * A(1, 0) * A(3, 2) * B(2, 1) + A(0, 3) * A(1, 1) * A(2, 0) * B(3, 2) - A(0, 3) * A(1, 1) * A(2, 2) * B(3, 0) - A(0, 3) * A(1, 1) * A(3, 0) * B(2, 2) + A(0, 3) * A(1, 1) * A(3, 2) * B(2, 0) - A(0, 3) * A(1, 2) * A(2, 0) * B(3, 1) + A(0, 3) * A(1, 2) * A(2, 1) * B(3, 0) + A(0, 3) * A(1, 2) * A(3, 0) * B(2, 1) - A(0, 3) * A(1, 2) * A(3, 1) * B(2, 0) - A(0, 3) * A(2, 0) * A(3, 1) * B(1, 2) + A(0, 3) * A(2, 0) * A(3, 2) * B(1, 1) + A(0, 3) * A(2, 1) * A(3, 0) * B(1, 2) - A(0, 3) * A(2, 1) * A(3, 2) * B(1, 0) - A(0, 3) * A(2, 2) * A(3, 0) * B(1, 1) + A(0, 3) * A(2, 2) * A(3, 1) * B(1, 0) - A(1, 0) * A(2, 1) * A(3, 2) * B(0, 3) + A(1, 0) * A(2, 1) * A(3, 3) * B(0, 2) + A(1, 0) * A(2, 2) * A(3, 1) * B(0, 3) - A(1, 0) * A(2, 2) * A(3, 3) * B(0, 1) - A(1, 0) * A(2, 3) * A(3, 1) * B(0, 2) + A(1, 0) * A(2, 3) * A(3, 2) * B(0, 1) + A(1, 1) * A(2, 0) * A(3, 2) * B(0, 3) - A(1, 1) * A(2, 0) * A(3, 3) * B(0, 2) - A(1, 1) * A(2, 2) * A(3, 0) * B(0, 3) + A(1, 1) * A(2, 2) * A(3, 3) * B(0, 0) + A(1, 1) * A(2, 3) * A(3, 0) * B(0, 2) - A(1, 1) * A(2, 3) * A(3, 2) * B(0, 0) - A(1, 2) * A(2, 0) * A(3, 1) * B(0, 3) + A(1, 2) * A(2, 0) * A(3, 3) * B(0, 1) + A(1, 2) * A(2, 1) * A(3, 0) * B(0, 3) - A(1, 2) * A(2, 1) * A(3, 3) * B(0, 0) - A(1, 2) * A(2, 3) * A(3, 0) * B(0, 1) + A(1, 2) * A(2, 3) * A(3, 1) * B(0, 0) + A(1, 3) * A(2, 0) * A(3, 1) * B(0, 2) - A(1, 3) * A(2, 0) * A(3, 2) * B(0, 1) - A(1, 3) * A(2, 1) * A(3, 0) * B(0, 2) + A(1, 3) * A(2, 1) * A(3, 2) * B(0, 0) + A(1, 3) * A(2, 2) * A(3, 0) * B(0, 1) - A(1, 3) * A(2, 2) * A(3, 1) * B(0, 0); coeffs[4] = B(0, 0) * B(1, 1) - B(0, 1) * B(1, 0) + B(0, 0) * B(2, 2) - B(0, 2) * B(2, 0) + B(0, 0) * B(3, 3) - B(0, 3) * B(3, 0) + B(1, 1) * B(2, 2) - B(1, 2) * B(2, 1) + B(1, 1) * B(3, 3) - B(1, 3) * B(3, 1) + B(2, 2) * B(3, 3) - B(2, 3) * B(3, 2) + A(0, 0) * A(1, 1) * B(2, 2) - A(0, 0) * A(1, 2) * B(2, 1) - A(0, 0) * A(2, 1) * B(1, 2) + A(0, 0) * A(2, 2) * B(1, 1) - A(0, 1) * A(1, 0) * B(2, 2) + A(0, 1) * A(1, 2) * B(2, 0) + A(0, 1) * A(2, 0) * B(1, 2) - A(0, 1) * A(2, 2) * B(1, 0) + A(0, 2) * A(1, 0) * B(2, 1) - A(0, 2) * A(1, 1) * B(2, 0) - A(0, 2) * A(2, 0) * B(1, 1) + A(0, 2) * A(2, 1) * B(1, 0) + A(1, 0) * A(2, 1) * B(0, 2) - A(1, 0) * A(2, 2) * B(0, 1) - A(1, 1) * A(2, 0) * B(0, 2) + A(1, 1) * A(2, 2) * B(0, 0) + A(1, 2) * A(2, 0) * B(0, 1) - A(1, 2) * A(2, 1) * B(0, 0) + A(0, 0) * A(1, 1) * B(3, 3) - A(0, 0) * A(1, 3) * B(3, 1) - A(0, 0) * A(3, 1) * B(1, 3) + A(0, 0) * A(3, 3) * B(1, 1) - A(0, 1) * A(1, 0) * B(3, 3) + A(0, 1) * A(1, 3) * B(3, 0) + A(0, 1) * A(3, 0) * B(1, 3) - A(0, 1) * A(3, 3) * B(1, 0) + A(0, 3) * A(1, 0) * B(3, 1) - A(0, 3) * A(1, 1) * B(3, 0) - A(0, 3) * A(3, 0) * B(1, 1) + A(0, 3) * A(3, 1) * B(1, 0) + A(1, 0) * A(3, 1) * B(0, 3) - A(1, 0) * A(3, 3) * B(0, 1) - A(1, 1) * A(3, 0) * B(0, 3) + A(1, 1) * A(3, 3) * B(0, 0) + A(1, 3) * A(3, 0) * B(0, 1) - A(1, 3) * A(3, 1) * B(0, 0) + A(0, 0) * A(2, 2) * B(3, 3) - A(0, 0) * A(2, 3) * B(3, 2) - A(0, 0) * A(3, 2) * B(2, 3) + A(0, 0) * A(3, 3) * B(2, 2) - A(0, 2) * A(2, 0) * B(3, 3) + A(0, 2) * A(2, 3) * B(3, 0) + A(0, 2) * A(3, 0) * B(2, 3) - A(0, 2) * A(3, 3) * B(2, 0) + A(0, 3) * A(2, 0) * B(3, 2) - A(0, 3) * A(2, 2) * B(3, 0) - A(0, 3) * A(3, 0) * B(2, 2) + A(0, 3) * A(3, 2) * B(2, 0) + A(2, 0) * A(3, 2) * B(0, 3) - A(2, 0) * A(3, 3) * B(0, 2) - A(2, 2) * A(3, 0) * B(0, 3) + A(2, 2) * A(3, 3) * B(0, 0) + A(2, 3) * A(3, 0) * B(0, 2) - A(2, 3) * A(3, 2) * B(0, 0) + A(1, 1) * A(2, 2) * B(3, 3) - A(1, 1) * A(2, 3) * B(3, 2) - A(1, 1) * A(3, 2) * B(2, 3) + A(1, 1) * A(3, 3) * B(2, 2) - A(1, 2) * A(2, 1) * B(3, 3) + A(1, 2) * A(2, 3) * B(3, 1) + A(1, 2) * A(3, 1) * B(2, 3) - A(1, 2) * A(3, 3) * B(2, 1) + A(1, 3) * A(2, 1) * B(3, 2) - A(1, 3) * A(2, 2) * B(3, 1) - A(1, 3) * A(3, 1) * B(2, 2) + A(1, 3) * A(3, 2) * B(2, 1) + A(2, 1) * A(3, 2) * B(1, 3) - A(2, 1) * A(3, 3) * B(1, 2) - A(2, 2) * A(3, 1) * B(1, 3) + A(2, 2) * A(3, 3) * B(1, 1) + A(2, 3) * A(3, 1) * B(1, 2) - A(2, 3) * A(3, 2) * B(1, 1) + A(0, 0) * A(1, 1) * A(2, 2) * A(3, 3) - A(0, 0) * A(1, 1) * A(2, 3) * A(3, 2) - A(0, 0) * A(1, 2) * A(2, 1) * A(3, 3) + A(0, 0) * A(1, 2) * A(2, 3) * A(3, 1) + A(0, 0) * A(1, 3) * A(2, 1) * A(3, 2) - A(0, 0) * A(1, 3) * A(2, 2) * A(3, 1) - A(0, 1) * A(1, 0) * A(2, 2) * A(3, 3) + A(0, 1) * A(1, 0) * A(2, 3) * A(3, 2) + A(0, 1) * A(1, 2) * A(2, 0) * A(3, 3) - A(0, 1) * A(1, 2) * A(2, 3) * A(3, 0) - A(0, 1) * A(1, 3) * A(2, 0) * A(3, 2) + A(0, 1) * A(1, 3) * A(2, 2) * A(3, 0) + A(0, 2) * A(1, 0) * A(2, 1) * A(3, 3) - A(0, 2) * A(1, 0) * A(2, 3) * A(3, 1) - A(0, 2) * A(1, 1) * A(2, 0) * A(3, 3) + A(0, 2) * A(1, 1) * A(2, 3) * A(3, 0) + A(0, 2) * A(1, 3) * A(2, 0) * A(3, 1) - A(0, 2) * A(1, 3) * A(2, 1) * A(3, 0) - A(0, 3) * A(1, 0) * A(2, 1) * A(3, 2) + A(0, 3) * A(1, 0) * A(2, 2) * A(3, 1) + A(0, 3) * A(1, 1) * A(2, 0) * A(3, 2) - A(0, 3) * A(1, 1) * A(2, 2) * A(3, 0) - A(0, 3) * A(1, 2) * A(2, 0) * A(3, 1) + A(0, 3) * A(1, 2) * A(2, 1) * A(3, 0); coeffs[5] = A(0, 0) * B(1, 1) - A(0, 1) * B(1, 0) - A(1, 0) * B(0, 1) + A(1, 1) * B(0, 0) + A(0, 0) * B(2, 2) - A(0, 2) * B(2, 0) - A(2, 0) * B(0, 2) + A(2, 2) * B(0, 0) + A(0, 0) * B(3, 3) - A(0, 3) * B(3, 0) + A(1, 1) * B(2, 2) - A(1, 2) * B(2, 1) - A(2, 1) * B(1, 2) + A(2, 2) * B(1, 1) - A(3, 0) * B(0, 3) + A(3, 3) * B(0, 0) + A(1, 1) * B(3, 3) - A(1, 3) * B(3, 1) - A(3, 1) * B(1, 3) + A(3, 3) * B(1, 1) + A(2, 2) * B(3, 3) - A(2, 3) * B(3, 2) - A(3, 2) * B(2, 3) + A(3, 3) * B(2, 2) + A(0, 0) * A(1, 1) * A(2, 2) - A(0, 0) * A(1, 2) * A(2, 1) - A(0, 1) * A(1, 0) * A(2, 2) + A(0, 1) * A(1, 2) * A(2, 0) + A(0, 2) * A(1, 0) * A(2, 1) - A(0, 2) * A(1, 1) * A(2, 0) + A(0, 0) * A(1, 1) * A(3, 3) - A(0, 0) * A(1, 3) * A(3, 1) - A(0, 1) * A(1, 0) * A(3, 3) + A(0, 1) * A(1, 3) * A(3, 0) + A(0, 3) * A(1, 0) * A(3, 1) - A(0, 3) * A(1, 1) * A(3, 0) + A(0, 0) * A(2, 2) * A(3, 3) - A(0, 0) * A(2, 3) * A(3, 2) - A(0, 2) * A(2, 0) * A(3, 3) + A(0, 2) * A(2, 3) * A(3, 0) + A(0, 3) * A(2, 0) * A(3, 2) - A(0, 3) * A(2, 2) * A(3, 0) + A(1, 1) * A(2, 2) * A(3, 3) - A(1, 1) * A(2, 3) * A(3, 2) - A(1, 2) * A(2, 1) * A(3, 3) + A(1, 2) * A(2, 3) * A(3, 1) + A(1, 3) * A(2, 1) * A(3, 2) - A(1, 3) * A(2, 2) * A(3, 1); coeffs[6] = B(0, 0) + B(1, 1) + B(2, 2) + B(3, 3) + A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0) + A(0, 0) * A(2, 2) - A(0, 2) * A(2, 0) + A(0, 0) * A(3, 3) - A(0, 3) * A(3, 0) + A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1) + A(1, 1) * A(3, 3) - A(1, 3) * A(3, 1) + A(2, 2) * A(3, 3) - A(2, 3) * A(3, 2); coeffs[7] = A(0, 0) + A(1, 1) + A(2, 2) + A(3, 3); coeffs[8] = 1; } // Computes polynomial p(x) = det(x^2*I + x * A + B) void detpoly3(const Eigen::Matrix &A, const Eigen::Matrix &B, double coeffs[7]) { coeffs[0] = B(0, 0) * B(1, 1) * B(2, 2) - B(0, 0) * B(1, 2) * B(2, 1) - B(0, 1) * B(1, 0) * B(2, 2) + B(0, 1) * B(1, 2) * B(2, 0) + B(0, 2) * B(1, 0) * B(2, 1) - B(0, 2) * B(1, 1) * B(2, 0); coeffs[1] = A(0, 0) * B(1, 1) * B(2, 2) - A(0, 0) * B(1, 2) * B(2, 1) - A(0, 1) * B(1, 0) * B(2, 2) + A(0, 1) * B(1, 2) * B(2, 0) + A(0, 2) * B(1, 0) * B(2, 1) - A(0, 2) * B(1, 1) * B(2, 0) - A(1, 0) * B(0, 1) * B(2, 2) + A(1, 0) * B(0, 2) * B(2, 1) + A(1, 1) * B(0, 0) * B(2, 2) - A(1, 1) * B(0, 2) * B(2, 0) - A(1, 2) * B(0, 0) * B(2, 1) + A(1, 2) * B(0, 1) * B(2, 0) + A(2, 0) * B(0, 1) * B(1, 2) - A(2, 0) * B(0, 2) * B(1, 1) - A(2, 1) * B(0, 0) * B(1, 2) + A(2, 1) * B(0, 2) * B(1, 0) + A(2, 2) * B(0, 0) * B(1, 1) - A(2, 2) * B(0, 1) * B(1, 0); coeffs[2] = B(0, 0) * B(1, 1) - B(0, 1) * B(1, 0) + B(0, 0) * B(2, 2) - B(0, 2) * B(2, 0) + B(1, 1) * B(2, 2) - B(1, 2) * B(2, 1) + A(0, 0) * A(1, 1) * B(2, 2) - A(0, 0) * A(1, 2) * B(2, 1) - A(0, 0) * A(2, 1) * B(1, 2) + A(0, 0) * A(2, 2) * B(1, 1) - A(0, 1) * A(1, 0) * B(2, 2) + A(0, 1) * A(1, 2) * B(2, 0) + A(0, 1) * A(2, 0) * B(1, 2) - A(0, 1) * A(2, 2) * B(1, 0) + A(0, 2) * A(1, 0) * B(2, 1) - A(0, 2) * A(1, 1) * B(2, 0) - A(0, 2) * A(2, 0) * B(1, 1) + A(0, 2) * A(2, 1) * B(1, 0) + A(1, 0) * A(2, 1) * B(0, 2) - A(1, 0) * A(2, 2) * B(0, 1) - A(1, 1) * A(2, 0) * B(0, 2) + A(1, 1) * A(2, 2) * B(0, 0) + A(1, 2) * A(2, 0) * B(0, 1) - A(1, 2) * A(2, 1) * B(0, 0); coeffs[3] = A(0, 0) * B(1, 1) - A(0, 1) * B(1, 0) - A(1, 0) * B(0, 1) + A(1, 1) * B(0, 0) + A(0, 0) * B(2, 2) - A(0, 2) * B(2, 0) - A(2, 0) * B(0, 2) + A(2, 2) * B(0, 0) + A(1, 1) * B(2, 2) - A(1, 2) * B(2, 1) - A(2, 1) * B(1, 2) + A(2, 2) * B(1, 1) + A(0, 0) * A(1, 1) * A(2, 2) - A(0, 0) * A(1, 2) * A(2, 1) - A(0, 1) * A(1, 0) * A(2, 2) + A(0, 1) * A(1, 2) * A(2, 0) + A(0, 2) * A(1, 0) * A(2, 1) - A(0, 2) * A(1, 1) * A(2, 0); coeffs[4] = B(0, 0) + B(1, 1) + B(2, 2) + A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0) + A(0, 0) * A(2, 2) - A(0, 2) * A(2, 0) + A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1); coeffs[5] = A(0, 0) + A(1, 1) + A(2, 2); coeffs[6] = 1.0; } int qep_sturm(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[8], Eigen::Matrix *eig_vecs) { double coeffs[9]; Eigen::Matrix Ainv = A.inverse(); detpoly4(Ainv * B, Ainv * C, coeffs); int n_roots = sturm::bisect_sturm<8>(coeffs, eig_vals); // For computing the eigenvectors we try to use the top 3x3 block only. If this fails we revert to QR on the 4x3 // system Eigen::Matrix M; Eigen::Matrix Minv; bool invertible; for (int i = 0; i < n_roots; ++i) { M = (eig_vals[i] * eig_vals[i]) * A + eig_vals[i] * B + C; M.block<3, 3>(0, 0).computeInverseWithCheck(Minv, invertible, 1e-8); if (invertible) { eig_vecs->col(i) = -Minv * M.block<3, 1>(0, 3); } else { eig_vecs->col(i) = -M.block<4, 3>(0, 0).colPivHouseholderQr().solve(M.block<4, 1>(0, 3)); } } return n_roots; } int qep_sturm_div_1_q2(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[6], Eigen::Matrix *eig_vecs) { double coeffs[9]; Eigen::Matrix Ainv = A.inverse(); detpoly4(Ainv * B, Ainv * C, coeffs); // We know that (1+q*q) is a factor. Dividing by this gives us a deg 6 poly. coeffs[2] = coeffs[2] - coeffs[0]; coeffs[3] = coeffs[3] - coeffs[1]; coeffs[4] = coeffs[4] - coeffs[2]; coeffs[5] = coeffs[7]; coeffs[6] = coeffs[8]; int n_roots = sturm::bisect_sturm<6>(coeffs, eig_vals); // For computing the eigenvectors we try to use the top 3x3 block only. If this fails we revert to QR on the 4x3 // system Eigen::Matrix M; Eigen::Matrix Minv; bool invertible; for (int i = 0; i < n_roots; ++i) { M = (eig_vals[i] * eig_vals[i]) * A + eig_vals[i] * B + C; M.block<3, 3>(0, 0).computeInverseWithCheck(Minv, invertible, 1e-8); if (invertible) { eig_vecs->col(i) = -Minv * M.block<3, 1>(0, 3); } else { eig_vecs->col(i) = -M.block<4, 3>(0, 0).colPivHouseholderQr().solve(M.block<4, 1>(0, 3)); } } return n_roots; } int qep_sturm(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[6], Eigen::Matrix *eig_vecs) { double coeffs[7]; Eigen::Matrix Ainv = A.inverse(); detpoly3(Ainv * B, Ainv * C, coeffs); int n_roots = sturm::bisect_sturm<6>(coeffs, eig_vals); // For computing the eigenvectors we first try to use the top 2x3 block only. Eigen::Matrix M; for (int i = 0; i < n_roots; ++i) { M = (eig_vals[i] * eig_vals[i]) * A + eig_vals[i] * B + C; Eigen::Vector3d t = M.row(0).cross(M.row(1)).normalized(); if (std::abs(M.row(2) * t) > 1e-8) { t = M.row(0).cross(M.row(2)).normalized(); if (std::abs(M.row(1) * t) > 1e-8) { t = M.row(1).cross(M.row(2)).normalized(); } } eig_vecs->col(i) = t; } return n_roots; } int qep_div_1_q2(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[4], Eigen::Matrix *eig_vecs) { double coeffs[7]; Eigen::Matrix Ainv = A.inverse(); detpoly3(Ainv * B, Ainv * C, coeffs); int n_roots = univariate::solve_quartic_real(coeffs[5], coeffs[2] - coeffs[0], coeffs[1], coeffs[0], eig_vals); // For computing the eigenvectors we first try to use the top 2x3 block only. Eigen::Matrix M; for (int i = 0; i < n_roots; ++i) { M = (eig_vals[i] * eig_vals[i]) * A + eig_vals[i] * B + C; Eigen::Vector3d t = M.row(0).cross(M.row(1)).normalized(); if (std::abs(M.row(2) * t) > 1e-8) { t = M.row(0).cross(M.row(2)).normalized(); if (std::abs(M.row(1) * t) > 1e-8) { t = M.row(1).cross(M.row(2)).normalized(); } } eig_vecs->col(i) = t; } return n_roots; } } // namespace qep } // namespace poselibPoseLib-2.0.5/PoseLib/misc/qep.h000066400000000000000000000072211504452766400163200ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_MISC_QEP_H_ #define POSELIB_MISC_QEP_H_ #include namespace poselib { namespace qep { // Helper functions for solving quadratic eigenvalue problems // (Currently only 4x4 problems are implemented) // Note: The impl. assumes that fourth element of eigenvector is non-zero. // The return eigenvectors are only the first three elements (fourth is normalized to 1) // Solves the QEP by reduction to normal eigenvalue problem int qep_linearize(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[8], Eigen::Matrix *eig_vecs); // Solves the QEP by sturm bracketing on det(lambda^2*A + lambda*B + C) int qep_sturm(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[8], Eigen::Matrix *eig_vecs); // Solves the QEP by solving det(lambda^2*A + lambda*B + C) where we know that (1+lambda^2) is a factor. // This is the case in the upright solvers from Sweeney et al. // The roots are found using sturm bracketing. int qep_sturm_div_1_q2(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[6], Eigen::Matrix *eig_vecs); // Solves the QEP by sturm bracketing on det(lambda^2*A + lambda*B + C) int qep_sturm(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[6], Eigen::Matrix *eig_vecs); // Solves the QEP by solving det(lambda^2*A + lambda*B + C) where we know that (1+lambda^2) is a factor. // This is the case in the upright solvers from Sweeney et al. // The roots are found using the closed form solver for the quartic. int qep_div_1_q2(const Eigen::Matrix &A, const Eigen::Matrix &B, const Eigen::Matrix &C, double eig_vals[4], Eigen::Matrix *eig_vecs); } // namespace qep } // namespace poselib #endifPoseLib-2.0.5/PoseLib/misc/quaternion.h000066400000000000000000000114041504452766400177160ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_MISC_QUATERNION_H_ #define POSELIB_MISC_QUATERNION_H_ #include // We dont use Eigen::Quaterniond here since we want qw,qx,qy,qz ordering namespace poselib { inline Eigen::Matrix3d quat_to_rotmat(const Eigen::Vector4d &q) { return Eigen::Quaterniond(q(0), q(1), q(2), q(3)).toRotationMatrix(); } inline Eigen::Matrix quat_to_rotmatvec(const Eigen::Vector4d &q) { Eigen::Matrix3d R = quat_to_rotmat(q); Eigen::Matrix r = Eigen::Map>(R.data()); return r; } inline Eigen::Vector4d rotmat_to_quat(const Eigen::Matrix3d &R) { Eigen::Quaterniond q_flip(R); Eigen::Vector4d q; q << q_flip.w(), q_flip.x(), q_flip.y(), q_flip.z(); q.normalize(); return q; } inline Eigen::Vector4d quat_multiply(const Eigen::Vector4d &qa, const Eigen::Vector4d &qb) { const double qa1 = qa(0), qa2 = qa(1), qa3 = qa(2), qa4 = qa(3); const double qb1 = qb(0), qb2 = qb(1), qb3 = qb(2), qb4 = qb(3); return Eigen::Vector4d(qa1 * qb1 - qa2 * qb2 - qa3 * qb3 - qa4 * qb4, qa1 * qb2 + qa2 * qb1 + qa3 * qb4 - qa4 * qb3, qa1 * qb3 + qa3 * qb1 - qa2 * qb4 + qa4 * qb2, qa1 * qb4 + qa2 * qb3 - qa3 * qb2 + qa4 * qb1); } inline Eigen::Vector3d quat_rotate(const Eigen::Vector4d &q, const Eigen::Vector3d &p) { const double q1 = q(0), q2 = q(1), q3 = q(2), q4 = q(3); const double p1 = p(0), p2 = p(1), p3 = p(2); const double px1 = -p1 * q2 - p2 * q3 - p3 * q4; const double px2 = p1 * q1 - p2 * q4 + p3 * q3; const double px3 = p2 * q1 + p1 * q4 - p3 * q2; const double px4 = p2 * q2 - p1 * q3 + p3 * q1; return Eigen::Vector3d(px2 * q1 - px1 * q2 - px3 * q4 + px4 * q3, px3 * q1 - px1 * q3 + px2 * q4 - px4 * q2, px3 * q2 - px2 * q3 - px1 * q4 + px4 * q1); } inline Eigen::Vector4d quat_conj(const Eigen::Vector4d &q) { return Eigen::Vector4d(q(0), -q(1), -q(2), -q(3)); } inline Eigen::Vector4d quat_exp(const Eigen::Vector3d &w) { const double theta2 = w.squaredNorm(); const double theta = std::sqrt(theta2); const double theta_half = 0.5 * theta; double re, im; if (theta > 1e-6) { re = std::cos(theta_half); im = std::sin(theta_half) / theta; } else { // we are close to zero, use taylor expansion to avoid problems // with zero divisors in sin(theta/2)/theta const double theta4 = theta2 * theta2; re = 1.0 - (1.0 / 8.0) * theta2 + (1.0 / 384.0) * theta4; im = 0.5 - (1.0 / 48.0) * theta2 + (1.0 / 3840.0) * theta4; // for the linearized part we re-normalize to ensure unit length // here s should be roughly 1.0 anyways, so no problem with zero div const double s = std::sqrt(re * re + im * im * theta2); re /= s; im /= s; } return Eigen::Vector4d(re, im * w(0), im * w(1), im * w(2)); } inline Eigen::Vector4d quat_step_pre(const Eigen::Vector4d &q, const Eigen::Vector3d &w_delta) { return quat_multiply(quat_exp(w_delta), q); } inline Eigen::Vector4d quat_step_post(const Eigen::Vector4d &q, const Eigen::Vector3d &w_delta) { return quat_multiply(q, quat_exp(w_delta)); } } // namespace poselib #endifPoseLib-2.0.5/PoseLib/misc/re3q3.cc000066400000000000000000000662561504452766400166430ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "re3q3.h" #include "PoseLib/misc/quaternion.h" #include "PoseLib/misc/sturm.h" #include namespace poselib { namespace re3q3 { /* Homogeneous linear constraints on rotation matrix Rcoeffs*R(:) = 0 converted into 3q3 problem. */ void rotation_to_3q3(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *coeffs) { for (int k = 0; k < 3; k++) { (*coeffs)(k, 0) = Rcoeffs(k, 0) - Rcoeffs(k, 4) - Rcoeffs(k, 8); (*coeffs)(k, 1) = 2 * Rcoeffs(k, 1) + 2 * Rcoeffs(k, 3); (*coeffs)(k, 2) = 2 * Rcoeffs(k, 2) + 2 * Rcoeffs(k, 6); (*coeffs)(k, 3) = Rcoeffs(k, 4) - Rcoeffs(k, 0) - Rcoeffs(k, 8); (*coeffs)(k, 4) = 2 * Rcoeffs(k, 5) + 2 * Rcoeffs(k, 7); (*coeffs)(k, 5) = Rcoeffs(k, 8) - Rcoeffs(k, 4) - Rcoeffs(k, 0); (*coeffs)(k, 6) = 2 * Rcoeffs(k, 5) - 2 * Rcoeffs(k, 7); (*coeffs)(k, 7) = 2 * Rcoeffs(k, 6) - 2 * Rcoeffs(k, 2); (*coeffs)(k, 8) = 2 * Rcoeffs(k, 1) - 2 * Rcoeffs(k, 3); (*coeffs)(k, 9) = Rcoeffs(k, 0) + Rcoeffs(k, 4) + Rcoeffs(k, 8); } } /* Inhomogeneous linear constraints on rotation matrix Rcoeffs*[R(:);1] = 0 converted into 3q3 problem. */ void rotation_to_3q3(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *coeffs) { for (int k = 0; k < 3; k++) { (*coeffs)(k, 0) = Rcoeffs(k, 0) - Rcoeffs(k, 4) - Rcoeffs(k, 8) + Rcoeffs(k, 9); (*coeffs)(k, 1) = 2 * Rcoeffs(k, 1) + 2 * Rcoeffs(k, 3); (*coeffs)(k, 2) = 2 * Rcoeffs(k, 2) + 2 * Rcoeffs(k, 6); (*coeffs)(k, 3) = Rcoeffs(k, 4) - Rcoeffs(k, 0) - Rcoeffs(k, 8) + Rcoeffs(k, 9); (*coeffs)(k, 4) = 2 * Rcoeffs(k, 5) + 2 * Rcoeffs(k, 7); (*coeffs)(k, 5) = Rcoeffs(k, 8) - Rcoeffs(k, 4) - Rcoeffs(k, 0) + Rcoeffs(k, 9); (*coeffs)(k, 6) = 2 * Rcoeffs(k, 5) - 2 * Rcoeffs(k, 7); (*coeffs)(k, 7) = 2 * Rcoeffs(k, 6) - 2 * Rcoeffs(k, 2); (*coeffs)(k, 8) = 2 * Rcoeffs(k, 1) - 2 * Rcoeffs(k, 3); (*coeffs)(k, 9) = Rcoeffs(k, 0) + Rcoeffs(k, 4) + Rcoeffs(k, 8) + Rcoeffs(k, 9); } } void cayley_param(const Eigen::Matrix &c, Eigen::Matrix *R) { *R << c(0) * c(0) - c(1) * c(1) - c(2) * c(2) + 1, 2 * c(0) * c(1) - 2 * c(2), 2 * c(1) + 2 * c(0) * c(2), 2 * c(2) + 2 * c(0) * c(1), c(1) * c(1) - c(0) * c(0) - c(2) * c(2) + 1, 2 * c(1) * c(2) - 2 * c(0), 2 * c(0) * c(2) - 2 * c(1), 2 * c(0) + 2 * c(1) * c(2), c(2) * c(2) - c(1) * c(1) - c(0) * c(0) + 1; *R /= 1 + c(0) * c(0) + c(1) * c(1) + c(2) * c(2); } inline void refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) { Eigen::Matrix3d J; Eigen::Vector3d r; Eigen::Vector3d dx; double x, y, z; for (int i = 0; i < n_sols; ++i) { x = (*solutions)(0, i); y = (*solutions)(1, i); z = (*solutions)(2, i); // [x^2, x*y, x*z, y^2, y*z, z^2, x, y, z, 1.0] for (int iter = 0; iter < 5; ++iter) { r = coeffs.col(0) * x * x + coeffs.col(1) * x * y + coeffs.col(2) * x * z + coeffs.col(3) * y * y + coeffs.col(4) * y * z + coeffs.col(5) * z * z + coeffs.col(6) * x + coeffs.col(7) * y + coeffs.col(8) * z + coeffs.col(9); if (r.cwiseAbs().maxCoeff() < 1e-8) break; J.col(0) = 2.0 * coeffs.col(0) * x + coeffs.col(1) * y + coeffs.col(2) * z + coeffs.col(6); J.col(1) = coeffs.col(1) * x + 2.0 * coeffs.col(3) * y + coeffs.col(4) * z + coeffs.col(7); J.col(2) = coeffs.col(2) * x + coeffs.col(4) * y + 2.0 * coeffs.col(5) * z + coeffs.col(8); dx = J.inverse() * r; x -= dx(0); y -= dx(1); z -= dx(2); } (*solutions)(0, i) = x; (*solutions)(1, i) = y; (*solutions)(2, i) = z; } } /* * Order of coefficients is: x^2, xy, xz, y^2, yz, z^2, x, y, z, 1.0; * */ int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) { Eigen::Matrix Ax, Ay, Az; Ax << coeffs.col(3), coeffs.col(5), coeffs.col(4); // y^2, z^2, yz Ay << coeffs.col(0), coeffs.col(5), coeffs.col(2); // x^2, z^2, xz Az << coeffs.col(3), coeffs.col(0), coeffs.col(1); // y^2, x^2, yx // We check det(A) as a cheaper proxy for condition number int elim_var = 0; double detx = std::abs(Ax.determinant()); double dety = std::abs(Ay.determinant()); double detz = std::abs(Az.determinant()); double det = detx; if (det < dety) { det = dety; elim_var = 1; } if (det < detz) { det = detz; elim_var = 2; } if (try_random_var_change && det < 1e-10) { Eigen::Matrix A; A.block<3, 3>(0, 0) = Eigen::Quaternion::UnitRandom().toRotationMatrix(); A.block<3, 1>(0, 3).setRandom().normalize(); Eigen::Matrix B; B << A(0, 0) * A(0, 0), 2 * A(0, 0) * A(0, 1), 2 * A(0, 0) * A(0, 2), A(0, 1) * A(0, 1), 2 * A(0, 1) * A(0, 2), A(0, 2) * A(0, 2), 2 * A(0, 0) * A(0, 3), 2 * A(0, 1) * A(0, 3), 2 * A(0, 2) * A(0, 3), A(0, 3) * A(0, 3), A(0, 0) * A(1, 0), A(0, 0) * A(1, 1) + A(0, 1) * A(1, 0), A(0, 0) * A(1, 2) + A(0, 2) * A(1, 0), A(0, 1) * A(1, 1), A(0, 1) * A(1, 2) + A(0, 2) * A(1, 1), A(0, 2) * A(1, 2), A(0, 0) * A(1, 3) + A(0, 3) * A(1, 0), A(0, 1) * A(1, 3) + A(0, 3) * A(1, 1), A(0, 2) * A(1, 3) + A(0, 3) * A(1, 2), A(0, 3) * A(1, 3), A(0, 0) * A(2, 0), A(0, 0) * A(2, 1) + A(0, 1) * A(2, 0), A(0, 0) * A(2, 2) + A(0, 2) * A(2, 0), A(0, 1) * A(2, 1), A(0, 1) * A(2, 2) + A(0, 2) * A(2, 1), A(0, 2) * A(2, 2), A(0, 0) * A(2, 3) + A(0, 3) * A(2, 0), A(0, 1) * A(2, 3) + A(0, 3) * A(2, 1), A(0, 2) * A(2, 3) + A(0, 3) * A(2, 2), A(0, 3) * A(2, 3), A(1, 0) * A(1, 0), 2 * A(1, 0) * A(1, 1), 2 * A(1, 0) * A(1, 2), A(1, 1) * A(1, 1), 2 * A(1, 1) * A(1, 2), A(1, 2) * A(1, 2), 2 * A(1, 0) * A(1, 3), 2 * A(1, 1) * A(1, 3), 2 * A(1, 2) * A(1, 3), A(1, 3) * A(1, 3), A(1, 0) * A(2, 0), A(1, 0) * A(2, 1) + A(1, 1) * A(2, 0), A(1, 0) * A(2, 2) + A(1, 2) * A(2, 0), A(1, 1) * A(2, 1), A(1, 1) * A(2, 2) + A(1, 2) * A(2, 1), A(1, 2) * A(2, 2), A(1, 0) * A(2, 3) + A(1, 3) * A(2, 0), A(1, 1) * A(2, 3) + A(1, 3) * A(2, 1), A(1, 2) * A(2, 3) + A(1, 3) * A(2, 2), A(1, 3) * A(2, 3), A(2, 0) * A(2, 0), 2 * A(2, 0) * A(2, 1), 2 * A(2, 0) * A(2, 2), A(2, 1) * A(2, 1), 2 * A(2, 1) * A(2, 2), A(2, 2) * A(2, 2), 2 * A(2, 0) * A(2, 3), 2 * A(2, 1) * A(2, 3), 2 * A(2, 2) * A(2, 3), A(2, 3) * A(2, 3), 0, 0, 0, 0, 0, 0, A(0, 0), A(0, 1), A(0, 2), A(0, 3), 0, 0, 0, 0, 0, 0, A(1, 0), A(1, 1), A(1, 2), A(1, 3), 0, 0, 0, 0, 0, 0, A(2, 0), A(2, 1), A(2, 2), A(2, 3), 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; Eigen::Matrix coeffsB = coeffs * B; int n_sols = re3q3(coeffsB, solutions, false); // Revert change of variables for (int k = 0; k < n_sols; k++) { solutions->col(k) = A.block<3, 3>(0, 0) * solutions->col(k) + A.col(3); } // In some cases the numerics are quite poor after the change of variables, so we do some newton steps with the // original coefficients. refine_3q3(coeffs, solutions, n_sols); return n_sols; } Eigen::Matrix P; if (elim_var == 0) { // re-order columns to eliminate x (target: y^2 z^2 yz x^2 xy xz x y z 1) P << coeffs.col(0), coeffs.col(1), coeffs.col(2), coeffs.col(6), coeffs.col(7), coeffs.col(8), coeffs.col(9); P = -Ax.inverse() * P; } else if (elim_var == 1) { // re-order columns to eliminate y (target: x^2 z^2 xz y^2 xy yz y x z 1) P << coeffs.col(3), coeffs.col(1), coeffs.col(4), coeffs.col(7), coeffs.col(6), coeffs.col(8), coeffs.col(9); P = -Ay.inverse() * P; } else if (elim_var == 2) { // re-order columns to eliminate z (target: y^2 x^2 yx z^2 zy z y x 1) P << coeffs.col(5), coeffs.col(4), coeffs.col(2), coeffs.col(8), coeffs.col(7), coeffs.col(6), coeffs.col(9); P = -Az.inverse() * P; } double a11 = P(0, 1) * P(2, 1) + P(0, 2) * P(1, 1) - P(2, 1) * P(0, 1) - P(2, 2) * P(2, 1) - P(2, 0); double a12 = P(0, 1) * P(2, 4) + P(0, 4) * P(2, 1) + P(0, 2) * P(1, 4) + P(0, 5) * P(1, 1) - P(2, 1) * P(0, 4) - P(2, 4) * P(0, 1) - P(2, 2) * P(2, 4) - P(2, 5) * P(2, 1) - P(2, 3); double a13 = P(0, 4) * P(2, 4) + P(0, 5) * P(1, 4) - P(2, 4) * P(0, 4) - P(2, 5) * P(2, 4) - P(2, 6); double a14 = P(0, 1) * P(2, 2) + P(0, 2) * P(1, 2) - P(2, 1) * P(0, 2) - P(2, 2) * P(2, 2) + P(0, 0); double a15 = P(0, 1) * P(2, 5) + P(0, 4) * P(2, 2) + P(0, 2) * P(1, 5) + P(0, 5) * P(1, 2) - P(2, 1) * P(0, 5) - P(2, 4) * P(0, 2) - P(2, 2) * P(2, 5) - P(2, 5) * P(2, 2) + P(0, 3); double a16 = P(0, 4) * P(2, 5) + P(0, 5) * P(1, 5) - P(2, 4) * P(0, 5) - P(2, 5) * P(2, 5) + P(0, 6); double a17 = P(0, 1) * P(2, 0) + P(0, 2) * P(1, 0) - P(2, 1) * P(0, 0) - P(2, 2) * P(2, 0); double a18 = P(0, 1) * P(2, 3) + P(0, 4) * P(2, 0) + P(0, 2) * P(1, 3) + P(0, 5) * P(1, 0) - P(2, 1) * P(0, 3) - P(2, 4) * P(0, 0) - P(2, 2) * P(2, 3) - P(2, 5) * P(2, 0); double a19 = P(0, 1) * P(2, 6) + P(0, 4) * P(2, 3) + P(0, 2) * P(1, 6) + P(0, 5) * P(1, 3) - P(2, 1) * P(0, 6) - P(2, 4) * P(0, 3) - P(2, 2) * P(2, 6) - P(2, 5) * P(2, 3); double a110 = P(0, 4) * P(2, 6) + P(0, 5) * P(1, 6) - P(2, 4) * P(0, 6) - P(2, 5) * P(2, 6); double a21 = P(2, 1) * P(2, 1) + P(2, 2) * P(1, 1) - P(1, 1) * P(0, 1) - P(1, 2) * P(2, 1) - P(1, 0); double a22 = P(2, 1) * P(2, 4) + P(2, 4) * P(2, 1) + P(2, 2) * P(1, 4) + P(2, 5) * P(1, 1) - P(1, 1) * P(0, 4) - P(1, 4) * P(0, 1) - P(1, 2) * P(2, 4) - P(1, 5) * P(2, 1) - P(1, 3); double a23 = P(2, 4) * P(2, 4) + P(2, 5) * P(1, 4) - P(1, 4) * P(0, 4) - P(1, 5) * P(2, 4) - P(1, 6); double a24 = P(2, 1) * P(2, 2) + P(2, 2) * P(1, 2) - P(1, 1) * P(0, 2) - P(1, 2) * P(2, 2) + P(2, 0); double a25 = P(2, 1) * P(2, 5) + P(2, 4) * P(2, 2) + P(2, 2) * P(1, 5) + P(2, 5) * P(1, 2) - P(1, 1) * P(0, 5) - P(1, 4) * P(0, 2) - P(1, 2) * P(2, 5) - P(1, 5) * P(2, 2) + P(2, 3); double a26 = P(2, 4) * P(2, 5) + P(2, 5) * P(1, 5) - P(1, 4) * P(0, 5) - P(1, 5) * P(2, 5) + P(2, 6); double a27 = P(2, 1) * P(2, 0) + P(2, 2) * P(1, 0) - P(1, 1) * P(0, 0) - P(1, 2) * P(2, 0); double a28 = P(2, 1) * P(2, 3) + P(2, 4) * P(2, 0) + P(2, 2) * P(1, 3) + P(2, 5) * P(1, 0) - P(1, 1) * P(0, 3) - P(1, 4) * P(0, 0) - P(1, 2) * P(2, 3) - P(1, 5) * P(2, 0); double a29 = P(2, 1) * P(2, 6) + P(2, 4) * P(2, 3) + P(2, 2) * P(1, 6) + P(2, 5) * P(1, 3) - P(1, 1) * P(0, 6) - P(1, 4) * P(0, 3) - P(1, 2) * P(2, 6) - P(1, 5) * P(2, 3); double a210 = P(2, 4) * P(2, 6) + P(2, 5) * P(1, 6) - P(1, 4) * P(0, 6) - P(1, 5) * P(2, 6); double t2 = P(2, 1) * P(2, 1); double t3 = P(2, 2) * P(2, 2); double t4 = P(0, 1) * P(1, 4); double t5 = P(0, 4) * P(1, 1); double t6 = t4 + t5; double t7 = P(0, 2) * P(1, 5); double t8 = P(0, 5) * P(1, 2); double t9 = t7 + t8; double t10 = P(0, 1) * P(1, 5); double t11 = P(0, 4) * P(1, 2); double t12 = t10 + t11; double t13 = P(0, 2) * P(1, 4); double t14 = P(0, 5) * P(1, 1); double t15 = t13 + t14; double t16 = P(2, 1) * P(2, 5); double t17 = P(2, 2) * P(2, 4); double t18 = t16 + t17; double t19 = P(2, 4) * P(2, 4); double t20 = P(2, 5) * P(2, 5); double a31 = P(0, 0) * P(1, 1) + P(0, 1) * P(1, 0) - P(2, 0) * P(2, 1) * 2.0 - P(0, 1) * t2 - P(1, 1) * t3 - P(2, 2) * t2 * 2.0 + (P(0, 1) * P(0, 1)) * P(1, 1) + P(0, 2) * P(1, 1) * P(1, 2) + P(0, 1) * P(1, 2) * P(2, 1) + P(0, 2) * P(1, 1) * P(2, 1); double a32 = P(0, 0) * P(1, 4) + P(0, 1) * P(1, 3) + P(0, 3) * P(1, 1) + P(0, 4) * P(1, 0) - P(2, 0) * P(2, 4) * 2.0 - P(2, 1) * P(2, 3) * 2.0 - P(0, 4) * t2 + P(0, 1) * t6 - P(1, 4) * t3 + P(1, 1) * t9 + P(2, 1) * t12 + P(2, 1) * t15 - P(2, 1) * t18 * 2.0 + P(0, 1) * P(0, 4) * P(1, 1) + P(0, 2) * P(1, 2) * P(1, 4) + P(0, 1) * P(1, 2) * P(2, 4) + P(0, 2) * P(1, 1) * P(2, 4) - P(0, 1) * P(2, 1) * P(2, 4) * 2.0 - P(1, 1) * P(2, 2) * P(2, 5) * 2.0 - P(2, 1) * P(2, 2) * P(2, 4) * 2.0; double a33 = P(0, 1) * P(1, 6) + P(0, 3) * P(1, 4) + P(0, 4) * P(1, 3) + P(0, 6) * P(1, 1) - P(2, 1) * P(2, 6) * 2.0 - P(2, 3) * P(2, 4) * 2.0 + P(0, 4) * t6 - P(0, 1) * t19 + P(1, 4) * t9 - P(1, 1) * t20 + P(2, 4) * t12 + P(2, 4) * t15 - P(2, 4) * t18 * 2.0 + P(0, 1) * P(0, 4) * P(1, 4) + P(0, 5) * P(1, 1) * P(1, 5) + P(0, 4) * P(1, 5) * P(2, 1) + P(0, 5) * P(1, 4) * P(2, 1) - P(0, 4) * P(2, 1) * P(2, 4) * 2.0 - P(1, 4) * P(2, 2) * P(2, 5) * 2.0 - P(2, 1) * P(2, 4) * P(2, 5) * 2.0; double a34 = P(0, 4) * P(1, 6) + P(0, 6) * P(1, 4) - P(2, 4) * P(2, 6) * 2.0 - P(0, 4) * t19 - P(1, 4) * t20 - P(2, 5) * t19 * 2.0 + (P(0, 4) * P(0, 4)) * P(1, 4) + P(0, 5) * P(1, 4) * P(1, 5) + P(0, 4) * P(1, 5) * P(2, 4) + P(0, 5) * P(1, 4) * P(2, 4); double a35 = P(0, 0) * P(1, 2) + P(0, 2) * P(1, 0) - P(2, 0) * P(2, 2) * 2.0 - P(0, 2) * t2 - P(1, 2) * t3 - P(2, 1) * t3 * 2.0 + P(0, 2) * (P(1, 2) * P(1, 2)) + P(0, 1) * P(0, 2) * P(1, 1) + P(0, 1) * P(1, 2) * P(2, 2) + P(0, 2) * P(1, 1) * P(2, 2); double a36 = P(0, 0) * P(1, 5) + P(0, 2) * P(1, 3) + P(0, 3) * P(1, 2) + P(0, 5) * P(1, 0) - P(2, 0) * P(2, 5) * 2.0 - P(2, 2) * P(2, 3) * 2.0 - P(0, 5) * t2 + P(0, 2) * t6 - P(1, 5) * t3 + P(1, 2) * t9 + P(2, 2) * t12 + P(2, 2) * t15 - P(2, 2) * t18 * 2.0 + P(0, 1) * P(0, 5) * P(1, 1) + P(0, 2) * P(1, 2) * P(1, 5) + P(0, 1) * P(1, 2) * P(2, 5) + P(0, 2) * P(1, 1) * P(2, 5) - P(0, 2) * P(2, 1) * P(2, 4) * 2.0 - P(1, 2) * P(2, 2) * P(2, 5) * 2.0 - P(2, 1) * P(2, 2) * P(2, 5) * 2.0; double a37 = P(0, 2) * P(1, 6) + P(0, 3) * P(1, 5) + P(0, 5) * P(1, 3) + P(0, 6) * P(1, 2) - P(2, 2) * P(2, 6) * 2.0 - P(2, 3) * P(2, 5) * 2.0 + P(0, 5) * t6 - P(0, 2) * t19 + P(1, 5) * t9 - P(1, 2) * t20 + P(2, 5) * t12 + P(2, 5) * t15 - P(2, 5) * t18 * 2.0 + P(0, 2) * P(0, 4) * P(1, 4) + P(0, 5) * P(1, 2) * P(1, 5) + P(0, 4) * P(1, 5) * P(2, 2) + P(0, 5) * P(1, 4) * P(2, 2) - P(0, 5) * P(2, 1) * P(2, 4) * 2.0 - P(1, 5) * P(2, 2) * P(2, 5) * 2.0 - P(2, 2) * P(2, 4) * P(2, 5) * 2.0; double a38 = P(0, 5) * P(1, 6) + P(0, 6) * P(1, 5) - P(2, 5) * P(2, 6) * 2.0 - P(0, 5) * t19 - P(1, 5) * t20 - P(2, 4) * t20 * 2.0 + P(0, 5) * (P(1, 5) * P(1, 5)) + P(0, 4) * P(0, 5) * P(1, 4) + P(0, 4) * P(1, 5) * P(2, 5) + P(0, 5) * P(1, 4) * P(2, 5); double a39 = P(0, 0) * P(1, 0) - P(0, 0) * t2 - P(1, 0) * t3 - P(2, 0) * P(2, 0) + P(0, 0) * P(0, 1) * P(1, 1) + P(0, 2) * P(1, 0) * P(1, 2) + P(0, 1) * P(1, 2) * P(2, 0) + P(0, 2) * P(1, 1) * P(2, 0) - P(2, 0) * P(2, 1) * P(2, 2) * 2.0; double a310 = P(0, 0) * P(1, 3) + P(0, 3) * P(1, 0) - P(2, 0) * P(2, 3) * 2.0 - P(0, 3) * t2 + P(0, 0) * t6 - P(1, 3) * t3 + P(1, 0) * t9 + P(2, 0) * t12 + P(2, 0) * t15 - P(2, 0) * t18 * 2.0 + P(0, 1) * P(0, 3) * P(1, 1) + P(0, 2) * P(1, 2) * P(1, 3) + P(0, 1) * P(1, 2) * P(2, 3) + P(0, 2) * P(1, 1) * P(2, 3) - P(0, 0) * P(2, 1) * P(2, 4) * 2.0 - P(1, 0) * P(2, 2) * P(2, 5) * 2.0 - P(2, 1) * P(2, 2) * P(2, 3) * 2.0; double a311 = P(0, 0) * P(1, 6) + P(0, 3) * P(1, 3) + P(0, 6) * P(1, 0) - P(2, 0) * P(2, 6) * 2.0 - P(0, 6) * t2 + P(0, 3) * t6 - P(0, 0) * t19 - P(1, 6) * t3 + P(1, 3) * t9 - P(1, 0) * t20 + P(2, 3) * t12 + P(2, 3) * t15 - P(2, 3) * t18 * 2.0 - P(2, 3) * P(2, 3) + P(0, 0) * P(0, 4) * P(1, 4) + P(0, 1) * P(0, 6) * P(1, 1) + P(0, 2) * P(1, 2) * P(1, 6) + P(0, 5) * P(1, 0) * P(1, 5) + P(0, 1) * P(1, 2) * P(2, 6) + P(0, 2) * P(1, 1) * P(2, 6) + P(0, 4) * P(1, 5) * P(2, 0) + P(0, 5) * P(1, 4) * P(2, 0) - P(0, 3) * P(2, 1) * P(2, 4) * 2.0 - P(1, 3) * P(2, 2) * P(2, 5) * 2.0 - P(2, 0) * P(2, 4) * P(2, 5) * 2.0 - P(2, 1) * P(2, 2) * P(2, 6) * 2.0; double a312 = P(0, 3) * P(1, 6) + P(0, 6) * P(1, 3) - P(2, 3) * P(2, 6) * 2.0 + P(0, 6) * t6 - P(0, 3) * t19 + P(1, 6) * t9 - P(1, 3) * t20 + P(2, 6) * t12 + P(2, 6) * t15 - P(2, 6) * t18 * 2.0 + P(0, 3) * P(0, 4) * P(1, 4) + P(0, 5) * P(1, 3) * P(1, 5) + P(0, 4) * P(1, 5) * P(2, 3) + P(0, 5) * P(1, 4) * P(2, 3) - P(0, 6) * P(2, 1) * P(2, 4) * 2.0 - P(1, 6) * P(2, 2) * P(2, 5) * 2.0 - P(2, 3) * P(2, 4) * P(2, 5) * 2.0; double a313 = P(0, 6) * P(1, 6) - P(0, 6) * t19 - P(1, 6) * t20 - P(2, 6) * P(2, 6) + P(0, 4) * P(0, 6) * P(1, 4) + P(0, 5) * P(1, 5) * P(1, 6) + P(0, 4) * P(1, 5) * P(2, 6) + P(0, 5) * P(1, 4) * P(2, 6) - P(2, 4) * P(2, 5) * P(2, 6) * 2.0; // det(M(x)) double c[9]; c[8] = a14 * a27 * a31 - a17 * a24 * a31 - a11 * a27 * a35 + a17 * a21 * a35 + a11 * a24 * a39 - a14 * a21 * a39; c[7] = a14 * a27 * a32 + a14 * a28 * a31 + a15 * a27 * a31 - a17 * a24 * a32 - a17 * a25 * a31 - a18 * a24 * a31 - a11 * a27 * a36 - a11 * a28 * a35 - a12 * a27 * a35 + a17 * a21 * a36 + a17 * a22 * a35 + a18 * a21 * a35 + a11 * a25 * a39 + a12 * a24 * a39 - a14 * a22 * a39 - a15 * a21 * a39 + a11 * a24 * a310 - a14 * a21 * a310; c[6] = a14 * a27 * a33 + a14 * a28 * a32 + a14 * a29 * a31 + a15 * a27 * a32 + a15 * a28 * a31 + a16 * a27 * a31 - a17 * a24 * a33 - a17 * a25 * a32 - a17 * a26 * a31 - a18 * a24 * a32 - a18 * a25 * a31 - a19 * a24 * a31 - a11 * a27 * a37 - a11 * a28 * a36 - a11 * a29 * a35 - a12 * a27 * a36 - a12 * a28 * a35 - a13 * a27 * a35 + a17 * a21 * a37 + a17 * a22 * a36 + a17 * a23 * a35 + a18 * a21 * a36 + a18 * a22 * a35 + a19 * a21 * a35 + a11 * a26 * a39 + a12 * a25 * a39 + a13 * a24 * a39 - a14 * a23 * a39 - a15 * a22 * a39 - a16 * a21 * a39 + a11 * a24 * a311 + a11 * a25 * a310 + a12 * a24 * a310 - a14 * a21 * a311 - a14 * a22 * a310 - a15 * a21 * a310; c[5] = a14 * a27 * a34 + a14 * a28 * a33 + a14 * a29 * a32 + a15 * a27 * a33 + a15 * a28 * a32 + a15 * a29 * a31 + a16 * a27 * a32 + a16 * a28 * a31 - a17 * a24 * a34 - a17 * a25 * a33 - a17 * a26 * a32 - a18 * a24 * a33 - a18 * a25 * a32 - a18 * a26 * a31 - a19 * a24 * a32 - a19 * a25 * a31 - a11 * a27 * a38 - a11 * a28 * a37 - a11 * a29 * a36 - a12 * a27 * a37 - a12 * a28 * a36 - a12 * a29 * a35 - a13 * a27 * a36 - a13 * a28 * a35 + a17 * a21 * a38 + a17 * a22 * a37 + a17 * a23 * a36 + a18 * a21 * a37 + a18 * a22 * a36 + a18 * a23 * a35 + a19 * a21 * a36 + a19 * a22 * a35 + a12 * a26 * a39 + a13 * a25 * a39 - a15 * a23 * a39 - a16 * a22 * a39 - a24 * a31 * a110 + a21 * a35 * a110 + a14 * a31 * a210 - a11 * a35 * a210 + a11 * a24 * a312 + a11 * a25 * a311 + a11 * a26 * a310 + a12 * a24 * a311 + a12 * a25 * a310 + a13 * a24 * a310 - a14 * a21 * a312 - a14 * a22 * a311 - a14 * a23 * a310 - a15 * a21 * a311 - a15 * a22 * a310 - a16 * a21 * a310; c[4] = a14 * a28 * a34 + a14 * a29 * a33 + a15 * a27 * a34 + a15 * a28 * a33 + a15 * a29 * a32 + a16 * a27 * a33 + a16 * a28 * a32 + a16 * a29 * a31 - a17 * a25 * a34 - a17 * a26 * a33 - a18 * a24 * a34 - a18 * a25 * a33 - a18 * a26 * a32 - a19 * a24 * a33 - a19 * a25 * a32 - a19 * a26 * a31 - a11 * a28 * a38 - a11 * a29 * a37 - a12 * a27 * a38 - a12 * a28 * a37 - a12 * a29 * a36 - a13 * a27 * a37 - a13 * a28 * a36 - a13 * a29 * a35 + a17 * a22 * a38 + a17 * a23 * a37 + a18 * a21 * a38 + a18 * a22 * a37 + a18 * a23 * a36 + a19 * a21 * a37 + a19 * a22 * a36 + a19 * a23 * a35 + a13 * a26 * a39 - a16 * a23 * a39 - a24 * a32 * a110 - a25 * a31 * a110 + a21 * a36 * a110 + a22 * a35 * a110 + a14 * a32 * a210 + a15 * a31 * a210 - a11 * a36 * a210 - a12 * a35 * a210 + a11 * a24 * a313 + a11 * a25 * a312 + a11 * a26 * a311 + a12 * a24 * a312 + a12 * a25 * a311 + a12 * a26 * a310 + a13 * a24 * a311 + a13 * a25 * a310 - a14 * a21 * a313 - a14 * a22 * a312 - a14 * a23 * a311 - a15 * a21 * a312 - a15 * a22 * a311 - a15 * a23 * a310 - a16 * a21 * a311 - a16 * a22 * a310; c[3] = a14 * a29 * a34 + a15 * a28 * a34 + a15 * a29 * a33 + a16 * a27 * a34 + a16 * a28 * a33 + a16 * a29 * a32 - a17 * a26 * a34 - a18 * a25 * a34 - a18 * a26 * a33 - a19 * a24 * a34 - a19 * a25 * a33 - a19 * a26 * a32 - a11 * a29 * a38 - a12 * a28 * a38 - a12 * a29 * a37 - a13 * a27 * a38 - a13 * a28 * a37 - a13 * a29 * a36 + a17 * a23 * a38 + a18 * a22 * a38 + a18 * a23 * a37 + a19 * a21 * a38 + a19 * a22 * a37 + a19 * a23 * a36 - a24 * a33 * a110 - a25 * a32 * a110 - a26 * a31 * a110 + a21 * a37 * a110 + a22 * a36 * a110 + a23 * a35 * a110 + a14 * a33 * a210 + a15 * a32 * a210 + a16 * a31 * a210 - a11 * a37 * a210 - a12 * a36 * a210 - a13 * a35 * a210 + a11 * a25 * a313 + a11 * a26 * a312 + a12 * a24 * a313 + a12 * a25 * a312 + a12 * a26 * a311 + a13 * a24 * a312 + a13 * a25 * a311 + a13 * a26 * a310 - a14 * a22 * a313 - a14 * a23 * a312 - a15 * a21 * a313 - a15 * a22 * a312 - a15 * a23 * a311 - a16 * a21 * a312 - a16 * a22 * a311 - a16 * a23 * a310; c[2] = a15 * a29 * a34 + a16 * a28 * a34 + a16 * a29 * a33 - a18 * a26 * a34 - a19 * a25 * a34 - a19 * a26 * a33 - a12 * a29 * a38 - a13 * a28 * a38 - a13 * a29 * a37 + a18 * a23 * a38 + a19 * a22 * a38 + a19 * a23 * a37 - a24 * a34 * a110 - a25 * a33 * a110 - a26 * a32 * a110 + a21 * a38 * a110 + a22 * a37 * a110 + a23 * a36 * a110 + a14 * a34 * a210 + a15 * a33 * a210 + a16 * a32 * a210 - a11 * a38 * a210 - a12 * a37 * a210 - a13 * a36 * a210 + a11 * a26 * a313 + a12 * a25 * a313 + a12 * a26 * a312 + a13 * a24 * a313 + a13 * a25 * a312 + a13 * a26 * a311 - a14 * a23 * a313 - a15 * a22 * a313 - a15 * a23 * a312 - a16 * a21 * a313 - a16 * a22 * a312 - a16 * a23 * a311; c[1] = a16 * a29 * a34 - a19 * a26 * a34 - a13 * a29 * a38 + a19 * a23 * a38 - a25 * a34 * a110 - a26 * a33 * a110 + a22 * a38 * a110 + a23 * a37 * a110 + a15 * a34 * a210 + a16 * a33 * a210 - a12 * a38 * a210 - a13 * a37 * a210 + a12 * a26 * a313 + a13 * a25 * a313 + a13 * a26 * a312 - a15 * a23 * a313 - a16 * a22 * a313 - a16 * a23 * a312; c[0] = -a26 * a34 * a110 + a23 * a38 * a110 + a16 * a34 * a210 - a13 * a38 * a210 + a13 * a26 * a313 - a16 * a23 * a313; double roots[8]; int n_roots = sturm::bisect_sturm<8>(c, roots); Eigen::Matrix A; for (int i = 0; i < n_roots; ++i) { double xs1 = roots[i]; double xs2 = xs1 * xs1; double xs3 = xs1 * xs2; double xs4 = xs1 * xs3; A << a11 * xs2 + a12 * xs1 + a13, a14 * xs2 + a15 * xs1 + a16, a17 * xs3 + a18 * xs2 + a19 * xs1 + a110, a21 * xs2 + a22 * xs1 + a23, a24 * xs2 + a25 * xs1 + a26, a27 * xs3 + a28 * xs2 + a29 * xs1 + a210, a31 * xs3 + a32 * xs2 + a33 * xs1 + a34, a35 * xs3 + a36 * xs2 + a37 * xs1 + a38, a39 * xs4 + a310 * xs3 + a311 * xs2 + a312 * xs1 + a313; (*solutions)(0, i) = xs1; (*solutions)(1, i) = (A(1, 2) * A(0, 1) - A(0, 2) * A(1, 1)) / (A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1)); (*solutions)(2, i) = (A(1, 2) * A(0, 0) - A(0, 2) * A(1, 0)) / (A(0, 1) * A(1, 0) - A(1, 1) * A(0, 0)); } if (elim_var == 1) { solutions->row(0).swap(solutions->row(1)); } else if (elim_var == 2) { solutions->row(0).swap(solutions->row(2)); } refine_3q3(coeffs, solutions, n_roots); return n_roots; } inline int re3q3_rotation_impl(Eigen::Matrix &Rcoeffs, Eigen::Matrix *solutions, bool try_random_var_change) { Eigen::Vector4d q0 = Eigen::Quaterniond::UnitRandom().coeffs(); Eigen::Matrix3d R0 = quat_to_rotmat(q0); Rcoeffs.block<3, 3>(0, 0) = Rcoeffs.block<3, 3>(0, 0) * R0; Rcoeffs.block<3, 3>(0, 3) = Rcoeffs.block<3, 3>(0, 3) * R0; Rcoeffs.block<3, 3>(0, 6) = Rcoeffs.block<3, 3>(0, 6) * R0; Eigen::Matrix coeffs; rotation_to_3q3(Rcoeffs, &coeffs); Eigen::Matrix solutions_cayley; int n_sols = re3q3(coeffs, &solutions_cayley, try_random_var_change); for (int i = 0; i < n_sols; ++i) { Eigen::Vector4d q{1.0, solutions_cayley(0, i), solutions_cayley(1, i), solutions_cayley(2, i)}; q.normalize(); solutions->col(i) = quat_multiply(q0, q); } return n_sols; } int re3q3_rotation(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *solutions, bool try_random_var_change) { Eigen::Matrix Rcoeffs_copy; Rcoeffs_copy.block<3, 9>(0, 0) = Rcoeffs; Rcoeffs_copy.block<3, 1>(0, 9).setZero(); return re3q3_rotation_impl(Rcoeffs_copy, solutions, try_random_var_change); } int re3q3_rotation(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *solutions, bool try_random_var_change) { Eigen::Matrix Rcoeffs_copy = Rcoeffs; return re3q3_rotation_impl(Rcoeffs_copy, solutions, try_random_var_change); } } // namespace re3q3 } // namespace poselib PoseLib-2.0.5/PoseLib/misc/re3q3.h000066400000000000000000000067351504452766400165010ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_MISC_RE3Q3_H_ #define POSELIB_MISC_RE3Q3_H_ #include namespace poselib { namespace re3q3 { /* * Re-implementation of E3Q3. Adapted from Jan Heller's original implementation. * Added tricks for improving stability based on choosing the elimination variable. * see Zhou et al., A Stable Algebraic Camera Pose Estimation for Minimal Configurations of 2D/3D Point and Line * Correspondences, ACCV 2018 Additionally we do a random affine change of variables to handle further degeneracies. * * Order of coefficients is: x^2, xy, xz, y^2, yz, z^2, x, y, z, 1.0; * */ int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true); // Helper functions for setting up 3Q3 problems /* Homogeneous linear constraints on rotation matrix Rcoeffs*R(:) = 0 converted into 3q3 problem. */ void rotation_to_3q3(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *coeffs); /* Inhomogeneous linear constraints on rotation matrix Rcoeffs*[R(:);1] = 0 converted into 3q3 problem. */ void rotation_to_3q3(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *coeffs); void cayley_param(const Eigen::Matrix &c, Eigen::Matrix *R); /* Helper functions which performs a random rotation to avoid the degeneracy with cayley transform. The solutions matrix is 4x8 and contains quaternions. To get back rotation matrices you can use Eigen::Quaterniond(solutions.col(i)).toRotationMatrix(); */ int re3q3_rotation(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *solutions, bool try_random_var_change = true); int re3q3_rotation(const Eigen::Matrix &Rcoeffs, Eigen::Matrix *solutions, bool try_random_var_change = true); } // namespace re3q3 } // namespace poselib #endifPoseLib-2.0.5/PoseLib/misc/sturm.h000066400000000000000000000233721504452766400167120ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_MISC_STURM_H_ #define POSELIB_MISC_STURM_H_ #include #include #include #include #ifdef _MSC_VER #include #define __builtin_popcount __popcnt #endif namespace poselib { namespace sturm { // Constructs the quotients needed for evaluating the sturm sequence. template void build_sturm_seq(const double *fvec, double *svec) { double f[3 * N]; double *f1 = f; double *f2 = f1 + N + 1; double *f3 = f2 + N; std::copy(fvec, fvec + (2 * N + 1), f); for (int i = 0; i < N - 1; ++i) { const double q1 = f1[N - i] * f2[N - 1 - i]; const double q0 = f1[N - 1 - i] * f2[N - 1 - i] - f1[N - i] * f2[N - 2 - i]; f3[0] = f1[0] - q0 * f2[0]; for (int j = 1; j < N - 1 - i; ++j) { f3[j] = f1[j] - q1 * f2[j - 1] - q0 * f2[j]; } const double c = -std::abs(f3[N - 2 - i]); const double ci = 1.0 / c; for (int j = 0; j < N - 1 - i; ++j) { f3[j] = f3[j] * ci; } // juggle pointers (f1,f2,f3) -> (f2,f3,f1) double *tmp = f1; f1 = f2; f2 = f3; f3 = tmp; svec[3 * i] = q0; svec[3 * i + 1] = q1; svec[3 * i + 2] = c; } svec[3 * N - 3] = f1[0]; svec[3 * N - 2] = f1[1]; svec[3 * N - 1] = f2[0]; } // Evaluates polynomial using Horner's method. // Assumes that f[N] = 1.0 template inline double polyval(const double *f, double x) { double fx = x + f[N - 1]; for (int i = N - 2; i >= 0; --i) { fx = x * fx + f[i]; } return fx; } // Daniel Thul is responsible for this template-trickery :) template inline unsigned int flag_negative(const double *const f) { return ((f[D] < 0) << D) | flag_negative(f); } template <> inline unsigned int flag_negative<0>(const double *const f) { return f[0] < 0; } // Evaluates the sturm sequence and counts the number of sign changes template ::type * = nullptr> inline int signchanges(const double *svec, double x) { double f[N + 1]; f[N] = svec[3 * N - 1]; f[N - 1] = svec[3 * N - 3] + x * svec[3 * N - 2]; for (int i = N - 2; i >= 0; --i) { f[i] = (svec[3 * i] + x * svec[3 * i + 1]) * f[i + 1] + svec[3 * i + 2] * f[i + 2]; } // In testing this turned out to be slightly faster compared to a naive loop unsigned int S = flag_negative(f); return __builtin_popcount((S ^ (S >> 1)) & ~(0xFFFFFFFF << N)); } template = 32), void>::type * = nullptr> inline int signchanges(const double *svec, double x) { double f[N + 1]; f[N] = svec[3 * N - 1]; f[N - 1] = svec[3 * N - 3] + x * svec[3 * N - 2]; for (int i = N - 2; i >= 0; --i) { f[i] = (svec[3 * i] + x * svec[3 * i + 1]) * f[i + 1] + svec[3 * i + 2] * f[i + 2]; } int count = 0; bool neg1 = f[0] < 0; for (int i = 0; i < N; ++i) { bool neg2 = f[i + 1] < 0; if (neg1 ^ neg2) { ++count; } neg1 = neg2; } return count; } // Computes the Cauchy bound on the real roots. // Experiments with more complicated (expensive) bounds did not seem to have a good trade-off. template inline double get_bounds(const double *fvec) { double max = 0; for (int i = 0; i < N; ++i) { max = std::max(max, std::abs(fvec[i])); } return 1.0 + max; } // Applies Ridder's bracketing method until we get close to root, followed by newton iterations template void ridders_method_newton(const double *fvec, double a, double b, double *roots, int &n_roots, double tol) { double fa = polyval(fvec, a); double fb = polyval(fvec, b); if (!((fa < 0) ^ (fb < 0))) return; const double tol_newton = 1e-3; for (int iter = 0; iter < 30; ++iter) { if (std::abs(a - b) < tol_newton) { break; } const double c = (a + b) * 0.5; const double fc = polyval(fvec, c); const double s = std::sqrt(fc * fc - fa * fb); if (!s) break; const double d = (fa < fb) ? c + (a - c) * fc / s : c + (c - a) * fc / s; const double fd = polyval(fvec, d); if (fd >= 0 ? (fc < 0) : (fc > 0)) { a = c; fa = fc; b = d; fb = fd; } else if (fd >= 0 ? (fa < 0) : (fa > 0)) { b = d; fb = fd; } else { a = d; fa = fd; } } // We switch to Newton's method once we are close to the root double x = (a + b) * 0.5; double fx, fpx, dx; const double *fpvec = fvec + N + 1; for (int iter = 0; iter < 10; ++iter) { fx = polyval(fvec, x); if (std::abs(fx) < tol) { break; } fpx = static_cast(N) * polyval(fpvec, x); dx = fx / fpx; x = x - dx; if (std::abs(dx) < tol) { break; } } roots[n_roots++] = x; } template void isolate_roots(const double *fvec, const double *svec, double a, double b, int sa, int sb, double *roots, int &n_roots, double tol, int depth) { if (depth > 300) return; int n_rts = sa - sb; if (n_rts > 1) { double c = (a + b) * 0.5; int sc = signchanges(svec, c); isolate_roots(fvec, svec, a, c, sa, sc, roots, n_roots, tol, depth + 1); isolate_roots(fvec, svec, c, b, sc, sb, roots, n_roots, tol, depth + 1); } else if (n_rts == 1) { ridders_method_newton(fvec, a, b, roots, n_roots, tol); } } template inline int bisect_sturm(const double *coeffs, double *roots, double tol = 1e-10) { if (coeffs[N] == 0.0) return 0; // return bisect_sturm(coeffs,roots,tol); // This explodes compile times... double fvec[2 * N + 1]; double svec[3 * N]; // fvec is the polynomial and its first derivative. std::copy(coeffs, coeffs + N + 1, fvec); // Normalize w.r.t. leading coeff double c_inv = 1.0 / fvec[N]; for (int i = 0; i < N; ++i) fvec[i] *= c_inv; fvec[N] = 1.0; // Compute the derivative with normalized coefficients for (int i = 0; i < N - 1; ++i) { fvec[N + 1 + i] = fvec[i + 1] * ((i + 1) / static_cast(N)); } fvec[2 * N] = 1.0; // Compute sturm sequences build_sturm_seq(fvec, svec); // All real roots are in the interval [-r0, r0] double r0 = get_bounds(fvec); double a = -r0; double b = r0; int sa = signchanges(svec, a); int sb = signchanges(svec, b); int n_roots = sa - sb; if (n_roots == 0) return 0; n_roots = 0; isolate_roots(fvec, svec, a, b, sa, sb, roots, n_roots, tol, 0); return n_roots; } template <> inline int bisect_sturm<1>(const double *coeffs, double *roots, double tol) { if (coeffs[1] == 0.0) { return 0; } else { roots[0] = -coeffs[0] / coeffs[1]; return 1; } } template <> inline int bisect_sturm<0>(const double *coeffs, double *roots, double tol) { return 0; } template void charpoly_danilevsky_piv(Eigen::MatrixBase &A, double *p) { int n = A.rows(); for (int i = n - 1; i > 0; i--) { int piv_ind = i - 1; double piv = std::abs(A(i, i - 1)); // Find largest pivot for (int j = 0; j < i - 1; j++) { if (std::abs(A(i, j)) > piv) { piv = std::abs(A(i, j)); piv_ind = j; } } if (piv_ind != i - 1) { // Perform permutation A.row(i - 1).swap(A.row(piv_ind)); A.col(i - 1).swap(A.col(piv_ind)); } piv = A(i, i - 1); Eigen::VectorXd v = A.row(i); A.row(i - 1) = v.transpose() * A; Eigen::VectorXd vinv = (-1.0) * v; vinv(i - 1) = 1; vinv /= piv; vinv(i - 1) -= 1; Eigen::VectorXd Acol = A.col(i - 1); for (int j = 0; j <= i; j++) A.row(j) = A.row(j) + Acol(j) * vinv.transpose(); A.row(i).setZero(); A(i, i - 1) = 1; } p[n] = 1; for (int i = 0; i < n; i++) p[i] = -A(0, n - i - 1); } } // namespace sturm } // namespace poselib #endifPoseLib-2.0.5/PoseLib/misc/univariate.cc000066400000000000000000000200271504452766400200370ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "univariate.h" #include #include namespace poselib { namespace univariate { /* Solves the quadratic equation a*x^2 + b*x + c = 0 */ void solve_quadratic(double a, double b, double c, std::complex roots[2]) { std::complex b2m4ac = b * b - 4 * a * c; std::complex sq = std::sqrt(b2m4ac); // Choose sign to avoid cancellations roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq); roots[1] = c / (a * roots[0]); } /* Solves the quadratic equation a*x^2 + b*x + c = 0 */ int solve_quadratic_real(double a, double b, double c, double roots[2]) { double b2m4ac = b * b - 4 * a * c; if (b2m4ac < 0) return 0; double sq = std::sqrt(b2m4ac); // Choose sign to avoid cancellations roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq); roots[1] = c / (a * roots[0]); return 2; } /* Sign of component with largest magnitude */ inline double sign2(const std::complex z) { if (std::abs(z.real()) > std::abs(z.imag())) return z.real() < 0 ? -1.0 : 1.0; else return z.imag() < 0 ? -1.0 : 1.0; } /* Sign of component with largest magnitude */ inline double sign(const double z) { return z < 0 ? -1.0 : 1.0; } bool solve_cubic_single_real(double c2, double c1, double c0, double &root) { double a = c1 - c2 * c2 / 3.0; double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0; double c = b * b / 4.0 + a * a * a / 27.0; if (c != 0) { if (c > 0) { c = std::sqrt(c); b *= -0.5; root = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0; return true; } else { c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a); root = 2.0 * std::sqrt(-a / 3.0) * std::cos(std::acos(c) / 3.0) - c2 / 3.0; } } else { root = -c2 / 3.0 + (a != 0 ? (3.0 * b / a) : 0); } return false; } int solve_cubic_real(double c2, double c1, double c0, double roots[3]) { double a = c1 - c2 * c2 / 3.0; double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0; double c = b * b / 4.0 + a * a * a / 27.0; int n_roots; if (c > 0) { c = std::sqrt(c); b *= -0.5; roots[0] = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0; n_roots = 1; } else { c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a); double d = 2.0 * std::sqrt(-a / 3.0); roots[0] = d * std::cos(std::acos(c) / 3.0) - c2 / 3.0; roots[1] = d * std::cos(std::acos(c) / 3.0 - 2.09439510239319526263557236234192) - c2 / 3.0; // 2*pi/3 roots[2] = d * std::cos(std::acos(c) / 3.0 - 4.18879020478639052527114472468384) - c2 / 3.0; // 4*pi/3 n_roots = 3; } // single newton iteration for (int i = 0; i < n_roots; ++i) { double x = roots[i]; double x2 = x * x; double x3 = x * x2; double dx = -(x3 + c2 * x2 + c1 * x + c0) / (3 * x2 + 2 * c2 * x + c1); roots[i] += dx; } return n_roots; } /* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */ void solve_quartic(double b, double c, double d, double e, std::complex roots[4]) { // Find depressed quartic std::complex p = c - 3.0 * b * b / 8.0; std::complex q = b * b * b / 8.0 - 0.5 * b * c + d; std::complex r = (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) / 256.0; // Resolvent cubic is now // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2 std::complex bb = 2.0 * p; std::complex cc = p * p - 4.0 * r; std::complex dd = -q * q; // Solve resolvent cubic std::complex d0 = bb * bb - 3.0 * cc; std::complex d1 = 2.0 * bb * bb * bb - 9.0 * bb * cc + 27.0 * dd; std::complex C3 = (d1.real() < 0) ? (d1 - sqrt(d1 * d1 - 4.0 * d0 * d0 * d0)) / 2.0 : (d1 + sqrt(d1 * d1 - 4.0 * d0 * d0 * d0)) / 2.0; std::complex C; if (C3.real() < 0) C = -std::pow(-C3, 1.0 / 3); else C = std::pow(C3, 1.0 / 3); std::complex u2 = (bb + C + d0 / C) / -3.0; // std::complex db = u2 * u2 * u2 + bb * u2 * u2 + cc * u2 + dd; std::complex u = sqrt(u2); std::complex s = -u; std::complex t = (p + u * u + q / u) / 2.0; std::complex v = (p + u * u - q / u) / 2.0; roots[0] = (-u - sign2(u) * sqrt(u * u - 4.0 * v)) / 2.0; roots[1] = v / roots[0]; roots[2] = (-s - sign2(s) * sqrt(s * s - 4.0 * t)) / 2.0; roots[3] = t / roots[2]; for (int i = 0; i < 4; i++) { roots[i] = roots[i] - b / 4.0; // do one step of newton refinement std::complex x = roots[i]; std::complex x2 = x * x; std::complex x3 = x * x2; std::complex dx = -(x2 * x2 + b * x3 + c * x2 + d * x + e) / (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d); roots[i] = x + dx; } } /* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */ int solve_quartic_real(double b, double c, double d, double e, double roots[4]) { // Find depressed quartic double p = c - 3.0 * b * b / 8.0; double q = b * b * b / 8.0 - 0.5 * b * c + d; double r = (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) / 256.0; // Resolvent cubic is now // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2 double bb = 2.0 * p; double cc = p * p - 4.0 * r; double dd = -q * q; // Solve resolvent cubic double u2; solve_cubic_single_real(bb, cc, dd, u2); if (u2 < 0) return 0; double u = sqrt(u2); double s = -u; double t = (p + u * u + q / u) / 2.0; double v = (p + u * u - q / u) / 2.0; int sols = 0; double disc = u * u - 4.0 * v; if (disc > 0) { roots[0] = (-u - sign(u) * std::sqrt(disc)) / 2.0; roots[1] = v / roots[0]; sols += 2; } disc = s * s - 4.0 * t; if (disc > 0) { roots[sols] = (-s - sign(s) * std::sqrt(disc)) / 2.0; roots[sols + 1] = t / roots[sols]; sols += 2; } for (int i = 0; i < sols; i++) { roots[i] = roots[i] - b / 4.0; // do one step of newton refinement double x = roots[i]; double x2 = x * x; double x3 = x * x2; double dx = -(x2 * x2 + b * x3 + c * x2 + d * x + e) / (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d); roots[i] = x + dx; } return sols; } }; // namespace univariate }; // namespace poselib PoseLib-2.0.5/PoseLib/misc/univariate.h000066400000000000000000000053351504452766400177060ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_MISC_UNIVARIATE_H_ #define POSELIB_MISC_UNIVARIATE_H_ #include #include namespace poselib { namespace univariate { /* Solves the quadratic equation a*x^2 + b*x + c = 0 */ void solve_quadratic(double a, double b, double c, std::complex roots[2]); /* Solves the quadratic equation a*x^2 + b*x + c = 0. Only returns real roots */ int solve_quadratic_real(double a, double b, double c, double roots[2]); /* Sign of component with largest magnitude */ double sign2(const std::complex z); /* Finds a single real root of x^3 + b*x^2 + c*x + d = 0 */ bool solve_cubic_single_real(double b, double c, double d, double &root); /* Finds the real roots of x^3 + b*x^2 + c*x + d = 0 */ int solve_cubic_real(double b, double c, double d, double roots[3]); /* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */ void solve_quartic(double b, double c, double d, double e, std::complex roots[4]); /* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0. Only returns real roots */ int solve_quartic_real(double b, double c, double d, double e, double roots[4]); }; // namespace univariate }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/poselib.h.in000066400000000000000000000011621504452766400166400ustar00rootroot00000000000000#ifndef @PROJECT_NAME_UPPERCASE@_H #define @PROJECT_NAME_UPPERCASE@_H #include #include // // Autogenerated lisf of #include <...> // based on the .h header files in PoseLib/ folder // @LIB_INCLUDES_STRING@ // Print PoseLib version and buidling type inline std::string poselib_info(void) { #if (POSELIB_DEBUG) const std::string build_type = "DEBUG"; #else const std::string build_type = "RELEASE"; #endif return "PoseLib library version " + std::string(POSELIB_VERSION) + " (" + build_type + ")"; } #endif // @PROJECT_NAME_UPPERCASE@_H PoseLib-2.0.5/PoseLib/robust.cc000066400000000000000000000574131504452766400162640ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "robust.h" #include "PoseLib/robust/utils.h" namespace poselib { RansacStats estimate_absolute_pose(const std::vector &points2D, const std::vector &points3D, const Camera &camera, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers) { std::vector points2D_calib(points2D.size()); for (size_t k = 0; k < points2D.size(); ++k) { camera.unproject(points2D[k], &points2D_calib[k]); } RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_reproj_error /= camera.focal(); RansacStats stats = ransac_pnp(points2D_calib, points3D, ransac_opt_scaled, pose, inliers); if (stats.num_inliers > 3) { // Collect inlier for additional bundle adjustment std::vector points2D_inliers; std::vector points3D_inliers; points2D_inliers.reserve(points2D.size()); points3D_inliers.reserve(points3D.size()); // We re-scale with focal length to improve numerics in the opt. const double scale = 1.0 / camera.focal(); Camera norm_camera = camera; norm_camera.rescale(scale); BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale *= scale; for (size_t k = 0; k < points2D.size(); ++k) { if (!(*inliers)[k]) continue; points2D_inliers.push_back(points2D[k] * scale); points3D_inliers.push_back(points3D[k]); } bundle_adjust(points2D_inliers, points3D_inliers, norm_camera, pose, bundle_opt_scaled); } return stats; } RansacStats estimate_generalized_absolute_pose(const std::vector> &points2D, const std::vector> &points3D, const std::vector &camera_ext, const std::vector &cameras, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector> *inliers) { const size_t num_cams = cameras.size(); // Normalize image points for the RANSAC std::vector> points2D_calib; points2D_calib.resize(num_cams); double scaled_threshold = 0; size_t total_num_pts = 0; for (size_t cam_k = 0; cam_k < num_cams; ++cam_k) { const size_t pts = points2D[cam_k].size(); points2D_calib[cam_k].resize(pts); for (size_t pt_k = 0; pt_k < pts; ++pt_k) { cameras[cam_k].unproject(points2D[cam_k][pt_k], &points2D_calib[cam_k][pt_k]); } total_num_pts += pts; scaled_threshold += (ransac_opt.max_reproj_error * pts) / cameras[cam_k].focal(); } scaled_threshold /= static_cast(total_num_pts); // TODO allow per-camera thresholds RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_reproj_error = scaled_threshold; RansacStats stats = ransac_gen_pnp(points2D_calib, points3D, camera_ext, ransac_opt_scaled, pose, inliers); if (stats.num_inliers > 3) { // Collect inlier for additional bundle adjustment std::vector> points2D_inliers; std::vector> points3D_inliers; points2D_inliers.resize(num_cams); points3D_inliers.resize(num_cams); for (size_t cam_k = 0; cam_k < num_cams; ++cam_k) { const size_t pts = points2D[cam_k].size(); points2D_inliers[cam_k].reserve(pts); points3D_inliers[cam_k].reserve(pts); for (size_t pt_k = 0; pt_k < pts; ++pt_k) { if (!(*inliers)[cam_k][pt_k]) continue; points2D_inliers[cam_k].push_back(points2D[cam_k][pt_k]); points3D_inliers[cam_k].push_back(points3D[cam_k][pt_k]); } } generalized_bundle_adjust(points2D_inliers, points3D_inliers, camera_ext, cameras, pose, bundle_opt); } return stats; } RansacStats estimate_absolute_pose_pnpl(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, const Camera &camera, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers_points, std::vector *inliers_lines) { std::vector points2D_calib(points2D.size()); for (size_t k = 0; k < points2D.size(); ++k) { camera.unproject(points2D[k], &points2D_calib[k]); } std::vector lines2D_calib(lines2D.size()); for (size_t k = 0; k < lines2D.size(); ++k) { camera.unproject(lines2D[k].x1, &lines2D_calib[k].x1); camera.unproject(lines2D[k].x2, &lines2D_calib[k].x2); } RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_reproj_error /= camera.focal(); ransac_opt_scaled.max_epipolar_error /= camera.focal(); RansacStats stats = ransac_pnpl(points2D_calib, points3D, lines2D_calib, lines3D, ransac_opt_scaled, pose, inliers_points, inliers_lines); if (stats.num_inliers > 3) { // Collect inlier for additional bundle adjustment std::vector points2D_inliers; std::vector points3D_inliers; points2D_inliers.reserve(points2D.size()); points3D_inliers.reserve(points3D.size()); for (size_t k = 0; k < inliers_points->size(); ++k) { if (!(*inliers_points)[k]) continue; points2D_inliers.push_back(points2D_calib[k]); points3D_inliers.push_back(points3D[k]); } std::vector lines2D_inliers; std::vector lines3D_inliers; lines2D_inliers.reserve(lines2D.size()); lines3D_inliers.reserve(lines3D.size()); for (size_t k = 0; k < inliers_lines->size(); ++k) { if (!(*inliers_lines)[k]) continue; lines2D_inliers.push_back(lines2D_calib[k]); lines3D_inliers.push_back(lines3D[k]); } BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale /= camera.focal(); bundle_adjust(points2D_inliers, points3D_inliers, lines2D_inliers, lines3D_inliers, pose, bundle_opt_scaled, bundle_opt_scaled); } return stats; } RansacStats estimate_relative_pose(const std::vector &points2D_1, const std::vector &points2D_2, const Camera &camera1, const Camera &camera2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers) { const size_t num_pts = points2D_1.size(); std::vector x1_calib(num_pts); std::vector x2_calib(num_pts); for (size_t k = 0; k < num_pts; ++k) { camera1.unproject(points2D_1[k], &x1_calib[k]); camera2.unproject(points2D_2[k], &x2_calib[k]); } RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_epipolar_error = ransac_opt.max_epipolar_error * 0.5 * (1.0 / camera1.focal() + 1.0 / camera2.focal()); RansacStats stats = ransac_relpose(x1_calib, x2_calib, ransac_opt_scaled, pose, inliers); if (stats.num_inliers > 5) { // Collect inlier for additional bundle adjustment // TODO: use camera models for this refinement! std::vector x1_inliers; std::vector x2_inliers; x1_inliers.reserve(stats.num_inliers); x2_inliers.reserve(stats.num_inliers); for (size_t k = 0; k < num_pts; ++k) { if (!(*inliers)[k]) continue; x1_inliers.push_back(x1_calib[k]); x2_inliers.push_back(x2_calib[k]); } BundleOptions scaled_bundle_opt = bundle_opt; scaled_bundle_opt.loss_scale = bundle_opt.loss_scale * 0.5 * (1.0 / camera1.focal() + 1.0 / camera2.focal()); refine_relpose(x1_inliers, x2_inliers, pose, scaled_bundle_opt); } return stats; } RansacStats estimate_shared_focal_relative_pose(const std::vector &points2D_1, const std::vector &points2D_2, const Point2D &pp, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, ImagePair *image_pair, std::vector *inliers) { const size_t num_pts = points2D_1.size(); Eigen::Matrix3d T1, T2; std::vector x1_norm = points2D_1; std::vector x2_norm = points2D_2; for (size_t i = 0; i < x1_norm.size(); i++) { x1_norm[i] -= pp; x2_norm[i] -= pp; } // We normalize points here to improve conditioning. Note that the normalization // only amounts to a uniform rescaling of the image coordinate system // and the cost we minimize is equivalent to the cost in the original image // We do not perform shifting as we require pp to remain at [0, 0] double scale = normalize_points(x1_norm, x2_norm, T1, T2, true, false, true); RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_epipolar_error /= scale; BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale /= scale; if (ransac_opt.score_initial_model) { image_pair->camera1 = Camera("SIMPLE_PINHOLE", std::vector{image_pair->camera1.focal() / scale, 0.0, 0.0}, -1, -1); image_pair->camera2 = Camera("SIMPLE_PINHOLE", std::vector{image_pair->camera2.focal() / scale, 0.0, 0.0}, -1, -1); } RansacStats stats = ransac_shared_focal_relpose(x1_norm, x2_norm, ransac_opt_scaled, image_pair, inliers); if (stats.num_inliers > 6) { std::vector x1_inliers; std::vector x2_inliers; x1_inliers.reserve(stats.num_inliers); x2_inliers.reserve(stats.num_inliers); for (size_t k = 0; k < num_pts; ++k) { if (!(*inliers)[k]) continue; x1_inliers.push_back(x1_norm[k]); x2_inliers.push_back(x2_norm[k]); } refine_shared_focal_relpose(x1_inliers, x2_inliers, image_pair, bundle_opt_scaled); } image_pair->camera1.params[0] *= scale; image_pair->camera1.params[1] = pp(0); image_pair->camera1.params[2] = pp(1); image_pair->camera2 = image_pair->camera1; return stats; } RansacStats estimate_fundamental(const std::vector &x1, const std::vector &x2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, Eigen::Matrix3d *F, std::vector *inliers) { const size_t num_pts = x1.size(); if (num_pts < 7) { return RansacStats(); } // We normalize points here to improve conditioning. Note that the normalization // only ammounts to a uniform rescaling and shift of the image coordinate system // and the cost we minimize is equivalent to the cost in the original image // for RFC we do not perform the shift as the pp needs to remain at [0, 0] Eigen::Matrix3d T1, T2; std::vector x1_norm = x1; std::vector x2_norm = x2; double scale = normalize_points(x1_norm, x2_norm, T1, T2, true, !ransac_opt.real_focal_check, true); RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_epipolar_error /= scale; BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale /= scale; if (ransac_opt.score_initial_model) { *F = T2.transpose().inverse() * (*F) * T1.inverse(); *F /= F->norm(); } RansacStats stats = ransac_fundamental(x1_norm, x2_norm, ransac_opt_scaled, F, inliers); if (stats.num_inliers > 7) { // Collect inlier for additional non-linear refinement std::vector x1_inliers; std::vector x2_inliers; x1_inliers.reserve(stats.num_inliers); x2_inliers.reserve(stats.num_inliers); for (size_t k = 0; k < num_pts; ++k) { if (!(*inliers)[k]) continue; x1_inliers.push_back(x1_norm[k]); x2_inliers.push_back(x2_norm[k]); } refine_fundamental(x1_inliers, x2_inliers, F, bundle_opt_scaled); } *F = T2.transpose() * (*F) * T1; *F /= F->norm(); return stats; } RansacStats estimate_homography(const std::vector &x1, const std::vector &x2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, Eigen::Matrix3d *H, std::vector *inliers) { const size_t num_pts = x1.size(); if (num_pts < 4) { return RansacStats(); } Eigen::Matrix3d T1, T2; std::vector x1_norm = x1; std::vector x2_norm = x2; double scale = normalize_points(x1_norm, x2_norm, T1, T2, true, true, true); RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_reproj_error /= scale; BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale /= scale; if (ransac_opt.score_initial_model) { *H = T2 * (*H) * T1.inverse(); *H /= H->norm(); } RansacStats stats = ransac_homography(x1_norm, x2_norm, ransac_opt_scaled, H, inliers); if (stats.num_inliers > 4) { // Collect inlier for additional non-linear refinement std::vector x1_inliers; std::vector x2_inliers; x1_inliers.reserve(stats.num_inliers); x2_inliers.reserve(stats.num_inliers); for (size_t k = 0; k < num_pts; ++k) { if (!(*inliers)[k]) continue; x1_inliers.push_back(x1_norm[k]); x2_inliers.push_back(x2_norm[k]); } refine_homography(x1_inliers, x2_inliers, H, bundle_opt_scaled); } *H = T2.inverse() * (*H) * T1; *H /= H->norm(); return stats; } RansacStats estimate_generalized_relative_pose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &cameras1, const std::vector &camera2_ext, const std::vector &cameras2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *relative_pose, std::vector> *inliers) { std::vector calib_matches = matches; for (PairwiseMatches &m : calib_matches) { for (size_t k = 0; k < m.x1.size(); ++k) { cameras1[m.cam_id1].unproject(m.x1[k], &m.x1[k]); cameras2[m.cam_id2].unproject(m.x2[k], &m.x2[k]); } } double scaling_factor = 0; for (size_t k = 0; k < cameras1.size(); ++k) { scaling_factor += 1.0 / cameras1[k].focal(); } for (size_t k = 0; k < cameras2.size(); ++k) { scaling_factor += 1.0 / cameras2[k].focal(); } scaling_factor /= cameras1.size() + cameras2.size(); RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_epipolar_error *= scaling_factor; RansacStats stats = ransac_gen_relpose(calib_matches, camera1_ext, camera2_ext, ransac_opt_scaled, relative_pose, inliers); if (stats.num_inliers > 6) { // Collect inlier for additional bundle adjustment // TODO: use camera models for this refinement! // TODO: check that inliers are actually meaningfully distributed std::vector inlier_matches; inlier_matches.resize(calib_matches.size()); for (size_t match_k = 0; match_k < matches.size(); ++match_k) { const PairwiseMatches &m = calib_matches[match_k]; inlier_matches[match_k].cam_id1 = m.cam_id1; inlier_matches[match_k].cam_id2 = m.cam_id2; inlier_matches[match_k].x1.reserve(m.x1.size()); inlier_matches[match_k].x2.reserve(m.x2.size()); for (size_t k = 0; k < m.x1.size(); ++k) { if ((*inliers)[match_k][k]) { inlier_matches[match_k].x1.push_back(m.x1[k]); inlier_matches[match_k].x2.push_back(m.x2[k]); } } } BundleOptions scaled_bundle_opt = bundle_opt; scaled_bundle_opt.loss_scale *= scaling_factor; refine_generalized_relpose(inlier_matches, camera1_ext, camera2_ext, relative_pose, scaled_bundle_opt); } return stats; } RansacStats estimate_hybrid_pose(const std::vector &points2D, const std::vector &points3D, const std::vector &matches2D_2D, const Camera &camera, const std::vector &map_ext, const std::vector &map_cameras, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers_2D_3D, std::vector> *inliers_2D_2D) { if (points2D.size() < 3) { // Not possible to generate minimal sample (until hybrid estimators are added into the ransac as well) return RansacStats(); } // Compute normalized image points std::vector matches_calib = matches2D_2D; for (PairwiseMatches &m : matches_calib) { for (size_t k = 0; k < m.x1.size(); ++k) { map_cameras[m.cam_id1].unproject(m.x1[k], &m.x1[k]); camera.unproject(m.x2[k], &m.x2[k]); } } std::vector points2D_calib = points2D; for (size_t k = 0; k < points2D_calib.size(); ++k) { camera.unproject(points2D_calib[k], &points2D_calib[k]); } // TODO: different thresholds for 2D-2D and 2D-3D constraints double scaling_factor = 1.0 / camera.focal(); for (size_t k = 0; k < map_cameras.size(); ++k) { scaling_factor += 1.0 / map_cameras[k].focal(); } scaling_factor /= 1 + map_cameras.size(); RansacOptions ransac_opt_scaled = ransac_opt; ransac_opt_scaled.max_reproj_error *= 1.0 / camera.focal(); ransac_opt_scaled.max_epipolar_error *= scaling_factor; RansacStats stats = ransac_hybrid_pose(points2D_calib, points3D, matches_calib, map_ext, ransac_opt_scaled, pose, inliers_2D_3D, inliers_2D_2D); if (stats.num_inliers > 3) { // Collect inliers std::vector points2D_inliers; std::vector points3D_inliers; std::vector matches_inliers(matches_calib.size()); points2D_inliers.reserve(points2D.size()); points3D_inliers.reserve(points3D.size()); for (size_t pt_k = 0; pt_k < inliers_2D_3D->size(); ++pt_k) { if ((*inliers_2D_3D)[pt_k]) { points2D_inliers.push_back(points2D_calib[pt_k]); points3D_inliers.push_back(points3D[pt_k]); } } for (size_t match_k = 0; match_k < inliers_2D_2D->size(); ++match_k) { matches_inliers[match_k].cam_id1 = matches_calib[match_k].cam_id1; matches_inliers[match_k].cam_id2 = matches_calib[match_k].cam_id2; matches_inliers[match_k].x1.reserve(matches_calib[match_k].x1.size()); matches_inliers[match_k].x2.reserve(matches_calib[match_k].x1.size()); for (size_t pt_k = 0; pt_k < (*inliers_2D_2D)[match_k].size(); ++pt_k) { if ((*inliers_2D_2D)[match_k][pt_k]) { matches_inliers[match_k].x1.push_back(matches_calib[match_k].x1[pt_k]); matches_inliers[match_k].x2.push_back(matches_calib[match_k].x2[pt_k]); } } } // TODO: a nicer way to scale the robust loss for the epipolar part refine_hybrid_pose(points2D_inliers, points3D_inliers, matches_inliers, map_ext, pose, bundle_opt, bundle_opt.loss_scale * ransac_opt.max_epipolar_error / ransac_opt.max_reproj_error); } return stats; } RansacStats estimate_1D_radial_absolute_pose(const std::vector &points2D, const std::vector &points3D, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers) { if (points2D.size() < 5) { // Not possible to generate minimal sample return RansacStats(); } // scale by the average norm (improves numerics in the bundle) double scale = 0.0; for (size_t k = 0; k < points2D.size(); ++k) { scale += points2D[k].norm(); } scale = points2D.size() / scale; std::vector points2D_scaled = points2D; for (size_t k = 0; k < points2D_scaled.size(); ++k) { points2D_scaled[k] *= scale; } RansacOptions ransac_opt_scaled = ransac_opt; BundleOptions bundle_opt_scaled = bundle_opt; ransac_opt_scaled.max_reproj_error *= scale; bundle_opt_scaled.loss_scale *= scale; RansacStats stats = ransac_1D_radial_pnp(points2D_scaled, points3D, ransac_opt_scaled, pose, inliers); if (stats.num_inliers > 5) { // Collect inlier for additional bundle adjustment std::vector points2D_inliers; std::vector points3D_inliers; points2D_inliers.reserve(points2D.size()); points3D_inliers.reserve(points3D.size()); for (size_t k = 0; k < points2D.size(); ++k) { if (!(*inliers)[k]) continue; points2D_inliers.push_back(points2D_scaled[k]); points3D_inliers.push_back(points3D[k]); } bundle_adjust_1D_radial(points2D_inliers, points3D_inliers, pose, bundle_opt_scaled); } return stats; } } // namespace poselibPoseLib-2.0.5/PoseLib/robust.h000066400000000000000000000233231504452766400161170ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_H_ #define POSELIB_ROBUST_H_ #include "PoseLib/camera_pose.h" #include "PoseLib/misc/colmap_models.h" #include "PoseLib/robust/bundle.h" #include "PoseLib/robust/ransac.h" #include "PoseLib/types.h" #include namespace poselib { // Estimates absolute pose using LO-RANSAC followed by non-linear refinement // Threshold for reprojection error is set by RansacOptions.max_reproj_error RansacStats estimate_absolute_pose(const std::vector &points2D, const std::vector &points3D, const Camera &camera, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers); // Estimates generalized absolute pose using LO-RANSAC followed by non-linear refinement // Threshold for reprojection error is set by RansacOptions.max_reproj_error RansacStats estimate_generalized_absolute_pose(const std::vector> &points2D, const std::vector> &points3D, const std::vector &camera_ext, const std::vector &cameras, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector> *inliers); // Estimates absolute pose using LO-RANSAC followed by non-linear refinement // using both 2D-3D point and line matches // Note that line segments are described by their endpoints // Threshold for point reprojection error is set by RansacOptions.max_reproj_error // and for lines the threshold is set by RansacOptions.max_epipolar_error RansacStats estimate_absolute_pose_pnpl(const std::vector &points2D, const std::vector &points3D, const std::vector &line2D, const std::vector &line3D, const Camera &camera, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers_points, std::vector *inliers_lines); // Estimates relative pose using LO-RANSAC followed by non-linear refinement // Threshold for Sampson error is set by RansacOptions.max_epipolar_error RansacStats estimate_relative_pose(const std::vector &points2D_1, const std::vector &points2D_2, const Camera &camera1, const Camera &camera2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *relative_pose, std::vector *inliers); // Estimates relative pose with shared unknown focal length using LO-RANSAC followed by non-linear refinement // Threshold for Sampson error is set by RansacOptions.max_epipolar_error RansacStats estimate_shared_focal_relative_pose(const std::vector &points2D_1, const std::vector &points2D_2, const Point2D &pp, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, ImagePair *image_pair, std::vector *inliers); // Estimates a fundamental matrix using LO-RANSAC followed by non-linear refinement // NOTE: USE estimate_relative_pose IF YOU KNOW THE INTRINSICS!!! // Threshold for Sampson error is set by RansacOptions.max_epipolar_error RansacStats estimate_fundamental(const std::vector &points2D_1, const std::vector &points2D_2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, Eigen::Matrix3d *F, std::vector *inliers); // Estimates a homography matrix using LO-RANSAC followed by non-linear refinement // Convention is x2 = H*x1 // Threshold for transfer error is set by RansacOptions.max_reproj_error RansacStats estimate_homography(const std::vector &points2D_1, const std::vector &points2D_2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, Eigen::Matrix3d *H, std::vector *inliers); // Estimates generalized relative pose using LO-RANSAC followed by non-linear refinement // Threshold for Sampson error is set by RansacOptions.max_epipolar_error RansacStats estimate_generalized_relative_pose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &cameras1, const std::vector &camera2_ext, const std::vector &cameras2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *relative_pose, std::vector> *inliers); // Estimates camera pose from hybrid correspondences using LO-RANSAC followed by non-linear refinement // camera are the intrinsics for the query camera // (points2D, points3D) are the 2D-3D matches // (matches2D_2D, map_ext, map_cameras) are the 2D-2D matches to the map images with extrinsics/intrinsics // Note for matches2D_2D it is assumed that cam_ind1 indexes into map_cameras and map_ext, and cam_ind2 = 0 // So that PairwiseMatches::x1 are the map image 2D points and PairwiseMatches::x2 are in the query camera // TODO: Not fully implemented (only p3p sampling for now) and very untested! RansacStats estimate_hybrid_pose(const std::vector &points2D, const std::vector &points3D, const std::vector &matches2D_2D, const Camera &camera, const std::vector &map_ext, const std::vector &map_cameras, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers_2D_3D, std::vector> *inliers_2D_2D); // Estimates generalized camera pose from hybrid correspondences using LO-RANSAC followed by non-linear refinement // (points2D_1, points3D_1) are the 2D-3D matches where the 2D point is in the first rig and the 3D points are in the // second (points2D_2, points3D_2) are the 2D-3D matches where the 2D point is in the second rig and the 3D points are // in the first (matches2D_2D) are the 2D-2D matches between the generalized cameras camerasX, cameraX_ext are the // intrinsics/extrinsics for each of the generalized cameras // TODO: Not yet implemented. RansacStats estimate_generalized_hybrid_pose( const std::vector> &points2D_1, const std::vector> &points3D_1, const std::vector> &points2D_2, const std::vector> &points3D_2, const std::vector &matches2D_2D, const std::vector &camera1_ext, const std::vector &cameras1, const std::vector &camera2_ext, const std::vector &cameras2, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector> *inliers_1, std::vector> *inliers_2, std::vector> *inliers_2D_2D); // Estimates the 1D absolute pose using LO-RANSAC followed by non-linear refinement // Assumes that the image points are centered already // Threshold for radial reprojection error is set by RansacOptions.max_reproj_error RansacStats estimate_1D_radial_absolute_pose(const std::vector &points2D, const std::vector &points3D, const RansacOptions &ransac_opt, const BundleOptions &bundle_opt, CameraPose *pose, std::vector *inliers); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/000077500000000000000000000000001504452766400157435ustar00rootroot00000000000000PoseLib-2.0.5/PoseLib/robust/bundle.cc000066400000000000000000000767771504452766400175530ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #if __GNUC__ >= 12 #pragma GCC diagnostic ignored "-Warray-bounds" #endif #include "bundle.h" #include "PoseLib/robust/jacobian_impl.h" #include "PoseLib/robust/lm_impl.h" #include "PoseLib/robust/robust_loss.h" #include namespace poselib { //////////////////////////////////////////////////////////////////////// // Below here we have wrappers for the refinement // These are super messy due to the loss functions being templated // and the hack we use to handle weights // (see UniformWeightVector in jacobian_impl.h) #define SWITCH_LOSS_FUNCTIONS \ case BundleOptions::LossType::TRIVIAL: \ SWITCH_LOSS_FUNCTION_CASE(TrivialLoss); \ break; \ case BundleOptions::LossType::TRUNCATED: \ SWITCH_LOSS_FUNCTION_CASE(TruncatedLoss); \ break; \ case BundleOptions::LossType::HUBER: \ SWITCH_LOSS_FUNCTION_CASE(HuberLoss); \ break; \ case BundleOptions::LossType::CAUCHY: \ SWITCH_LOSS_FUNCTION_CASE(CauchyLoss); \ break; \ case BundleOptions::LossType::TRUNCATED_LE_ZACH: \ SWITCH_LOSS_FUNCTION_CASE(TruncatedLossLeZach); \ break; /////////////////////////////////////////////////////////////////////////////////////////////////////// // Iteration callbacks (called after each LM iteration) // Callback which prints debug info from the iterations void print_iteration(const BundleStats &stats) { if (stats.iterations == 0) { std::cout << "initial_cost=" << stats.initial_cost << "\n"; } std::cout << "iter=" << stats.iterations << ", cost=" << stats.cost << ", step=" << stats.step_norm << ", grad=" << stats.grad_norm << ", lambda=" << stats.lambda << "\n"; } template IterationCallback setup_callback(const BundleOptions &opt, LossFunction &loss_fn) { if (opt.verbose) { return print_iteration; } else { return nullptr; } } // For using the IRLS scheme proposed by Le and Zach 3DV2021, we have a callback // for each iteration which updates the mu parameter template <> IterationCallback setup_callback(const BundleOptions &opt, TruncatedLossLeZach &loss_fn) { if (opt.verbose) { return [&loss_fn](const BundleStats &stats) { print_iteration(stats); loss_fn.mu *= TruncatedLossLeZach::alpha; }; } else { return [&loss_fn](const BundleStats &stats) { loss_fn.mu *= TruncatedLossLeZach::alpha; }; } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Absolute pose with points (PnP) // Interface for calibrated camera BundleStats bundle_adjust(const std::vector &x, const std::vector &X, CameraPose *pose, const BundleOptions &opt, const std::vector &weights) { poselib::Camera camera; camera.model_id = NullCameraModel::model_id; return bundle_adjust(x, X, camera, pose, opt); } template BundleStats bundle_adjust(const std::vector &x, const std::vector &X, const Camera &camera, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); CameraJacobianAccumulator accum(x, X, camera, loss_fn, weights); return lm_impl(accum, pose, opt, callback); } template BundleStats bundle_adjust(const std::vector &x, const std::vector &X, const Camera &camera, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return bundle_adjust(x, X, camera, pose, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } template BundleStats bundle_adjust(const std::vector &x, const std::vector &X, const Camera &camera, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { switch (camera.model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: { \ return bundle_adjust(x, X, camera, pose, opt, weights); \ } SWITCH_CAMERA_MODELS #undef SWITCH_CAMERA_MODEL_CASE default: return BundleStats(); } } // Entry point for PnP refinement BundleStats bundle_adjust(const std::vector &x, const std::vector &X, const Camera &camera, CameraPose *pose, const BundleOptions &opt, const std::vector &weights) { if (weights.size() == x.size()) { return bundle_adjust>(x, X, camera, pose, opt, weights); } else { return bundle_adjust(x, X, camera, pose, opt, UniformWeightVector()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Absolute pose with points and lines (PnPL) // Note that we currently do not support different camera models here // TODO: decide how to handle lines for non-linear camera models... template BundleStats bundle_adjust(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, CameraPose *pose, const BundleOptions &opt, const BundleOptions &opt_line, const PointWeightType &weights_pts, const LineWeightType &weights_lines) { PointLossFunction pt_loss_fn(opt.loss_scale); LineLossFunction line_loss_fn(opt_line.loss_scale); IterationCallback callback = setup_callback(opt, pt_loss_fn); PointLineJacobianAccumulator accum( points2D, points3D, lines2D, lines3D, pt_loss_fn, line_loss_fn, weights_pts, weights_lines); return lm_impl(accum, pose, opt, callback); } template BundleStats bundle_adjust(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, CameraPose *pose, const BundleOptions &opt, const BundleOptions &opt_line, const PointWeightType &weights_pts, const LineWeightType &weights_lines) { switch (opt_line.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return bundle_adjust( \ points2D, points3D, lines2D, lines3D, pose, opt, opt_line, weights_pts, weights_lines); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } template BundleStats bundle_adjust(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, CameraPose *pose, const BundleOptions &opt, const BundleOptions &opt_line, const PointWeightType &weights_pts, const LineWeightType &weights_lines) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return bundle_adjust(points2D, points3D, lines2D, lines3D, pose, \ opt, opt_line, weights_pts, weights_lines); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for PnPL refinement BundleStats bundle_adjust(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, CameraPose *pose, const BundleOptions &opt, const BundleOptions &opt_line, const std::vector &weights_pts, const std::vector &weights_lines) { bool have_pts_weights = weights_pts.size() == points2D.size(); bool have_line_weights = weights_lines.size() == lines2D.size(); if (have_pts_weights && have_line_weights) { return bundle_adjust, std::vector>(points2D, points3D, lines2D, lines3D, pose, opt, opt_line, weights_pts, weights_lines); } else if (have_pts_weights && !have_line_weights) { return bundle_adjust, UniformWeightVector>(points2D, points3D, lines2D, lines3D, pose, opt, opt_line, weights_pts, UniformWeightVector()); } else if (!have_pts_weights && have_line_weights) { return bundle_adjust>(points2D, points3D, lines2D, lines3D, pose, opt, opt_line, UniformWeightVector(), weights_lines); } else { return bundle_adjust( points2D, points3D, lines2D, lines3D, pose, opt, opt_line, UniformWeightVector(), UniformWeightVector()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Generalized absolute pose with points (GPnP) // Interface for calibrated camera BundleStats generalized_bundle_adjust(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, CameraPose *pose, const BundleOptions &opt, const std::vector> &weights) { std::vector dummy_cameras; dummy_cameras.resize(x.size()); for (size_t k = 0; k < x.size(); ++k) { dummy_cameras[k].model_id = -1; } return generalized_bundle_adjust(x, X, camera_ext, dummy_cameras, pose, opt, weights); } template BundleStats generalized_bundle_adjust(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, const std::vector &cameras, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); GeneralizedCameraJacobianAccumulator accum(x, X, camera_ext, cameras, loss_fn, weights); return lm_impl(accum, pose, opt, callback); } template BundleStats generalized_bundle_adjust(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, const std::vector &cameras, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return generalized_bundle_adjust(x, X, camera_ext, cameras, pose, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for GPnP refinement BundleStats generalized_bundle_adjust(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, const std::vector &cameras, CameraPose *pose, const BundleOptions &opt, const std::vector> &weights) { if (weights.size() == x.size()) { return generalized_bundle_adjust>>(x, X, camera_ext, cameras, pose, opt, weights); } else { return generalized_bundle_adjust(x, X, camera_ext, cameras, pose, opt, UniformWeightVectors()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Relative pose (essential matrix) refinement template BundleStats refine_relpose(const std::vector &x1, const std::vector &x2, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); RelativePoseJacobianAccumulator accum(x1, x2, loss_fn, weights); return lm_impl(accum, pose, opt, callback); } template BundleStats refine_relpose(const std::vector &x1, const std::vector &x2, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return refine_relpose(x1, x2, pose, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for essential matrix refinement BundleStats refine_relpose(const std::vector &x1, const std::vector &x2, CameraPose *pose, const BundleOptions &opt, const std::vector &weights) { if (weights.size() == x1.size()) { return refine_relpose>(x1, x2, pose, opt, weights); } else { return refine_relpose(x1, x2, pose, opt, UniformWeightVector()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Relative pose (essential matrix) refinement template BundleStats refine_shared_focal_relpose(const std::vector &x1, const std::vector &x2, ImagePair *image_pair, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); SharedFocalRelativePoseJacobianAccumulator accum(x1, x2, loss_fn, weights); return lm_impl(accum, image_pair, opt, callback); } template BundleStats refine_shared_focal_relpose(const std::vector &x1, const std::vector &x2, ImagePair *image_pair, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return refine_shared_focal_relpose(x1, x2, image_pair, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for essential matrix refinement BundleStats refine_shared_focal_relpose(const std::vector &x1, const std::vector &x2, ImagePair *image_pair, const BundleOptions &opt, const std::vector &weights) { if (weights.size() == x1.size()) { return refine_shared_focal_relpose>(x1, x2, image_pair, opt, weights); } else { return refine_shared_focal_relpose(x1, x2, image_pair, opt, UniformWeightVector()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Uncalibrated relative pose (fundamental matrix) refinement template BundleStats refine_fundamental(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *F, const BundleOptions &opt, const WeightType &weights) { // We optimize over the SVD-based factorization from Bartoli and Sturm FactorizedFundamentalMatrix factorized_fund_mat(*F); LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); FundamentalJacobianAccumulator accum(x1, x2, loss_fn, weights); BundleStats stats = lm_impl(accum, &factorized_fund_mat, opt, callback); *F = factorized_fund_mat.F(); return stats; } template BundleStats refine_fundamental(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *F, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return refine_fundamental(x1, x2, F, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for fundamental matrix refinement BundleStats refine_fundamental(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *F, const BundleOptions &opt, const std::vector &weights) { if (weights.size() == x1.size()) { return refine_fundamental>(x1, x2, F, opt, weights); } else { return refine_fundamental(x1, x2, F, opt, UniformWeightVector()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Homography matrix refinement template BundleStats refine_homography(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *H, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); HomographyJacobianAccumulator accum(x1, x2, loss_fn, weights); return lm_impl(accum, H, opt, callback); } template BundleStats refine_homography(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *H, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return refine_homography(x1, x2, H, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for fundamental matrix refinement BundleStats refine_homography(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *H, const BundleOptions &opt, const std::vector &weights) { if (weights.size() == x1.size()) { return refine_homography>(x1, x2, H, opt, weights); } else { return refine_homography(x1, x2, H, opt, UniformWeightVector()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Generalized relative pose refinement template BundleStats refine_generalized_relpose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &camera2_ext, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); GeneralizedRelativePoseJacobianAccumulator accum(matches, camera1_ext, camera2_ext, loss_fn, weights); return lm_impl(accum, pose, opt, callback); } template BundleStats refine_generalized_relpose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &camera2_ext, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return refine_generalized_relpose(matches, camera1_ext, camera2_ext, pose, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for generalized relpose refinement BundleStats refine_generalized_relpose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &camera2_ext, CameraPose *pose, const BundleOptions &opt, const std::vector> &weights) { if (weights.size() == matches.size()) { return refine_generalized_relpose>>(matches, camera1_ext, camera2_ext, pose, opt, weights); } else { return refine_generalized_relpose(matches, camera1_ext, camera2_ext, pose, opt, UniformWeightVectors()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // Hybrid pose refinement (i.e. both 2D-3D and 2D-2D point correspondences) template BundleStats refine_hybrid_pose(const std::vector &x, const std::vector &X, const std::vector &matches_2D_2D, const std::vector &map_ext, CameraPose *pose, const BundleOptions &opt, double loss_scale_epipolar, const AbsWeightType &weights_abs, const RelWeightType &weights_rel) { LossFunction loss_fn(opt.loss_scale); LossFunction loss_fn_epipolar(loss_scale_epipolar); // TODO: refactor such that the callback can handle multiple loss-functions // currently this only affects TruncatedLossLeZach IterationCallback callback = setup_callback(opt, loss_fn); HybridPoseJacobianAccumulator accum( x, X, matches_2D_2D, map_ext, loss_fn, loss_fn_epipolar, weights_abs, weights_rel); return lm_impl(accum, pose, opt, callback); } template BundleStats refine_hybrid_pose(const std::vector &x, const std::vector &X, const std::vector &matches_2D_2D, const std::vector &map_ext, CameraPose *pose, const BundleOptions &opt, double loss_scale_epipolar, const AbsWeightType &weights_abs, const RelWeightType &weights_rel) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return refine_hybrid_pose( \ x, X, matches_2D_2D, map_ext, pose, opt, loss_scale_epipolar, weights_abs, weights_rel); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for hybrid pose refinement BundleStats refine_hybrid_pose(const std::vector &x, const std::vector &X, const std::vector &matches_2D_2D, const std::vector &map_ext, CameraPose *pose, const BundleOptions &opt, double loss_scale_epipolar, const std::vector &weights_abs, const std::vector> &weights_rel) { bool have_abs_weights = weights_abs.size() == x.size(); bool have_rel_weights = weights_rel.size() == matches_2D_2D.size(); if (have_abs_weights && have_rel_weights) { return refine_hybrid_pose, std::vector>>( x, X, matches_2D_2D, map_ext, pose, opt, loss_scale_epipolar, weights_abs, weights_rel); } else if (have_abs_weights && !have_rel_weights) { return refine_hybrid_pose, UniformWeightVectors>( x, X, matches_2D_2D, map_ext, pose, opt, loss_scale_epipolar, weights_abs, UniformWeightVectors()); } else if (!have_abs_weights && have_rel_weights) { return refine_hybrid_pose>>( x, X, matches_2D_2D, map_ext, pose, opt, loss_scale_epipolar, UniformWeightVector(), weights_rel); } else { return refine_hybrid_pose(x, X, matches_2D_2D, map_ext, pose, opt, loss_scale_epipolar, UniformWeightVector(), UniformWeightVectors()); } } /////////////////////////////////////////////////////////////////////////////////////////////////////// // 1D-radial absolute pose refinement (1D-radial PnP) template BundleStats bundle_adjust_1D_radial(const std::vector &x, const std::vector &X, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { LossFunction loss_fn(opt.loss_scale); IterationCallback callback = setup_callback(opt, loss_fn); Radial1DJacobianAccumulator accum(x, X, loss_fn, weights); return lm_impl(accum, pose, opt, callback); } template BundleStats bundle_adjust_1D_radial(const std::vector &x, const std::vector &X, CameraPose *pose, const BundleOptions &opt, const WeightType &weights) { switch (opt.loss_type) { #define SWITCH_LOSS_FUNCTION_CASE(LossFunction) \ return bundle_adjust_1D_radial(x, X, pose, opt, weights); SWITCH_LOSS_FUNCTIONS default: return BundleStats(); } #undef SWITCH_LOSS_FUNCTION_CASE } // Entry point for 1D radial absolute pose refinement (Assumes that the image points are centered) BundleStats bundle_adjust_1D_radial(const std::vector &x, const std::vector &X, CameraPose *pose, const BundleOptions &opt, const std::vector &weights) { if (weights.size() == x.size()) { return bundle_adjust_1D_radial>(x, X, pose, opt, weights); } else { return bundle_adjust_1D_radial(x, X, pose, opt, UniformWeightVector()); } } #undef SWITCH_LOSS_FUNCTIONS } // namespace poselibPoseLib-2.0.5/PoseLib/robust/bundle.h000066400000000000000000000167731504452766400174030ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_BUNDLE_H_ #define POSELIB_BUNDLE_H_ #include "PoseLib/camera_pose.h" #include "PoseLib/misc/colmap_models.h" #include "PoseLib/types.h" #include namespace poselib { // Minimizes reprojection error. Assumes identity intrinsics (calibrated camera) BundleStats bundle_adjust(const std::vector &x, const std::vector &X, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); // Uses intrinsic calibration from Camera (see colmap_models.h) // Slightly slower than bundle_adjust above BundleStats bundle_adjust(const std::vector &x, const std::vector &X, const Camera &camera, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); // opt_line is used to define the robust loss used for the line correspondences BundleStats bundle_adjust(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const BundleOptions &opt_line = BundleOptions(), const std::vector &weights_pts = std::vector(), const std::vector &weights_line = std::vector()); /* // Camera models for lines are currently not supported... int bundle_adjust(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, const Camera &camera, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); */ // Minimizes reprojection error. Assumes identity intrinsics (calibrated camera) BundleStats generalized_bundle_adjust(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector> &weights = std::vector>()); // Uses intrinsic calibration from Camera (see colmap_models.h) // Slightly slower than bundle_adjust above BundleStats generalized_bundle_adjust(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, const std::vector &cameras, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector> &weights = std::vector>()); // Relative pose refinement. Minimizes Sampson error error. Assumes identity intrinsics (calibrated camera) BundleStats refine_relpose(const std::vector &x1, const std::vector &x2, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); // Relative pose with single unknown focal refinement. Minimizes Sampson error error. Assumes identity intrinsics // (calibrated camera) BundleStats refine_shared_focal_relpose(const std::vector &x1, const std::vector &x2, ImagePair *image_pair, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); // Fundamental matrix refinement. Minimizes Sampson error error. BundleStats refine_fundamental(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *F, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); // Homography matrix refinement. BundleStats refine_homography(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *H, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); // Generalized relative pose refinement. Minimizes Sampson error error. Assumes identity intrinsics (calibrated camera) BundleStats refine_generalized_relpose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &camera2_ext, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector> &weights = std::vector>()); // If you use weights, then both weights_abs and weights_rel needs to be passed! BundleStats refine_hybrid_pose(const std::vector &x, const std::vector &X, const std::vector &matches_2D_2D, const std::vector &map_ext, CameraPose *pose, const BundleOptions &opt = BundleOptions(), double loss_scale_epipolar = 1.0, const std::vector &weights_abs = std::vector(), const std::vector> &weights_rel = std::vector>()); // Minimizes the 1D radial reprojection error. Assumes that the image points are centered BundleStats bundle_adjust_1D_radial(const std::vector &x, const std::vector &X, CameraPose *pose, const BundleOptions &opt = BundleOptions(), const std::vector &weights = std::vector()); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/estimators/000077500000000000000000000000001504452766400201355ustar00rootroot00000000000000PoseLib-2.0.5/PoseLib/robust/estimators/absolute_pose.cc000066400000000000000000000166541504452766400233240ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "absolute_pose.h" #include "PoseLib/robust/bundle.h" #include "PoseLib/solvers/gp3p.h" #include "PoseLib/solvers/p1p2ll.h" #include "PoseLib/solvers/p2p1ll.h" #include "PoseLib/solvers/p3ll.h" #include "PoseLib/solvers/p3p.h" #include "PoseLib/solvers/p5lp_radial.h" namespace poselib { void AbsolutePoseEstimator::generate_models(std::vector *models) { sampler.generate_sample(&sample); for (size_t k = 0; k < sample_sz; ++k) { xs[k] = x[sample[k]].homogeneous().normalized(); Xs[k] = X[sample[k]]; } p3p(xs, Xs, models); } double AbsolutePoseEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { return compute_msac_score(pose, x, X, opt.max_reproj_error * opt.max_reproj_error, inlier_count); } void AbsolutePoseEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_reproj_error; bundle_opt.max_iterations = 25; // TODO: for high outlier scenarios, make a copy of (x,X) and find points close to inlier threshold // TODO: experiment with good thresholds for copy vs iterating full point set bundle_adjust(x, X, pose, bundle_opt); } void GeneralizedAbsolutePoseEstimator::generate_models(std::vector *models) { draw_sample(sample_sz, num_pts_camera, &sample, rng); for (size_t k = 0; k < sample_sz; ++k) { const size_t cam_k = sample[k].first; const size_t pt_k = sample[k].second; ps[k] = camera_centers[cam_k]; xs[k] = rig_poses[cam_k].derotate(x[cam_k][pt_k].homogeneous().normalized()); Xs[k] = X[cam_k][pt_k]; } gp3p(ps, xs, Xs, models); } double GeneralizedAbsolutePoseEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { const double sq_threshold = opt.max_reproj_error * opt.max_reproj_error; double score = 0; *inlier_count = 0; size_t cam_inlier_count; for (size_t k = 0; k < num_cams; ++k) { CameraPose full_pose; full_pose.q = quat_multiply(rig_poses[k].q, pose.q); full_pose.t = rig_poses[k].rotate(pose.t) + rig_poses[k].t; score += compute_msac_score(full_pose, x[k], X[k], sq_threshold, &cam_inlier_count); *inlier_count += cam_inlier_count; } return score; } void GeneralizedAbsolutePoseEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_reproj_error; bundle_opt.max_iterations = 25; generalized_bundle_adjust(x, X, rig_poses, pose, bundle_opt); } void AbsolutePosePointLineEstimator::generate_models(std::vector *models) { draw_sample(sample_sz, num_data, &sample, rng); size_t pt_idx = 0; size_t line_idx = 0; for (size_t k = 0; k < sample_sz; ++k) { size_t idx = sample[k]; if (idx < points2D.size()) { // we sampled a point correspondence xs[pt_idx] = points2D[idx].homogeneous(); xs[pt_idx].normalize(); Xs[pt_idx] = points3D[idx]; pt_idx++; } else { // we sampled a line correspondence idx -= points2D.size(); ls[line_idx] = lines2D[idx].x1.homogeneous().cross(lines2D[idx].x2.homogeneous()); ls[line_idx].normalize(); Cs[line_idx] = lines3D[idx].X1; Vs[line_idx] = lines3D[idx].X2 - lines3D[idx].X1; Vs[line_idx].normalize(); line_idx++; } } if (pt_idx == 3 && line_idx == 0) { p3p(xs, Xs, models); } else if (pt_idx == 2 && line_idx == 1) { p2p1ll(xs, Xs, ls, Cs, Vs, models); } else if (pt_idx == 1 && line_idx == 2) { p1p2ll(xs, Xs, ls, Cs, Vs, models); } else if (pt_idx == 0 && line_idx == 3) { p3ll(ls, Cs, Vs, models); } } double AbsolutePosePointLineEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { size_t point_inliers, line_inliers; double score_pt = compute_msac_score(pose, points2D, points3D, opt.max_reproj_error * opt.max_reproj_error, &point_inliers); double score_l = compute_msac_score(pose, lines2D, lines3D, opt.max_epipolar_error * opt.max_epipolar_error, &line_inliers); *inlier_count = point_inliers + line_inliers; return score_pt + score_l; } void AbsolutePosePointLineEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_reproj_error; bundle_opt.max_iterations = 25; BundleOptions line_bundle_opt; line_bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; line_bundle_opt.loss_scale = opt.max_epipolar_error; bundle_adjust(points2D, points3D, lines2D, lines3D, pose, bundle_opt, line_bundle_opt); } void Radial1DAbsolutePoseEstimator::generate_models(std::vector *models) { sampler.generate_sample(&sample); for (size_t k = 0; k < sample_sz; ++k) { xs[k] = x[sample[k]].normalized(); Xs[k] = X[sample[k]]; } p5lp_radial(xs, Xs, models); } double Radial1DAbsolutePoseEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { return compute_msac_score_1D_radial(pose, x, X, opt.max_reproj_error * opt.max_reproj_error, inlier_count); } void Radial1DAbsolutePoseEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_reproj_error; bundle_opt.max_iterations = 25; // TODO: for high outlier scenarios, make a copy of (x,X) and find points close to inlier threshold // TODO: experiment with good thresholds for copy vs iterating full point set bundle_adjust_1D_radial(x, X, pose, bundle_opt); } } // namespace poselibPoseLib-2.0.5/PoseLib/robust/estimators/absolute_pose.h000066400000000000000000000152121504452766400231530ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_ESTIMATORS_ABSOLUTE_POSE_H #define POSELIB_ROBUST_ESTIMATORS_ABSOLUTE_POSE_H #include "PoseLib/camera_pose.h" #include "PoseLib/robust/sampling.h" #include "PoseLib/robust/utils.h" #include "PoseLib/types.h" namespace poselib { class AbsolutePoseEstimator { public: AbsolutePoseEstimator(const RansacOptions &ransac_opt, const std::vector &points2D, const std::vector &points3D) : num_data(points2D.size()), opt(ransac_opt), x(points2D), X(points3D), sampler(num_data, sample_sz, opt.seed, opt.progressive_sampling, opt.max_prosac_iterations) { xs.resize(sample_sz); Xs.resize(sample_sz); sample.resize(sample_sz); } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 3; const size_t num_data; private: const RansacOptions &opt; const std::vector &x; const std::vector &X; RandomSampler sampler; // pre-allocated vectors for sampling std::vector xs, Xs; std::vector sample; }; class GeneralizedAbsolutePoseEstimator { public: GeneralizedAbsolutePoseEstimator(const RansacOptions &ransac_opt, const std::vector> &points2D, const std::vector> &points3D, const std::vector &camera_ext) : num_cams(points2D.size()), opt(ransac_opt), x(points2D), X(points3D), rig_poses(camera_ext) { rng = opt.seed; ps.resize(sample_sz); xs.resize(sample_sz); Xs.resize(sample_sz); sample.resize(sample_sz); camera_centers.resize(num_cams); for (size_t k = 0; k < num_cams; ++k) { camera_centers[k] = camera_ext[k].center(); } num_data = 0; num_pts_camera.resize(num_cams); for (size_t k = 0; k < num_cams; ++k) { num_pts_camera[k] = points2D[k].size(); num_data += num_pts_camera[k]; } } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 3; size_t num_data; const size_t num_cams; private: const RansacOptions &opt; const std::vector> &x; const std::vector> &X; const std::vector &rig_poses; std::vector camera_centers; std::vector num_pts_camera; // number of points in each camera RNG_t rng; // pre-allocated vectors for sampling std::vector ps, xs, Xs; std::vector> sample; }; class AbsolutePosePointLineEstimator { public: AbsolutePosePointLineEstimator(const RansacOptions &ransac_opt, const std::vector &x, const std::vector &X, const std::vector &l, const std::vector &L) : num_data(x.size() + l.size()), opt(ransac_opt), points2D(x), points3D(X), lines2D(l), lines3D(L) { rng = opt.seed; xs.resize(sample_sz); Xs.resize(sample_sz); ls.resize(sample_sz); Cs.resize(sample_sz); Vs.resize(sample_sz); sample.resize(sample_sz); } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 3; const size_t num_data; private: const RansacOptions &opt; const std::vector &points2D; const std::vector &points3D; const std::vector &lines2D; const std::vector &lines3D; RNG_t rng; // pre-allocated vectors for sampling std::vector xs, Xs, ls, Cs, Vs; std::vector sample; }; class Radial1DAbsolutePoseEstimator { public: Radial1DAbsolutePoseEstimator(const RansacOptions &ransac_opt, const std::vector &points2D, const std::vector &points3D) : num_data(points2D.size()), opt(ransac_opt), x(points2D), X(points3D), sampler(num_data, sample_sz, opt.seed, opt.progressive_sampling, opt.max_prosac_iterations) { xs.resize(sample_sz); Xs.resize(sample_sz); sample.resize(sample_sz); } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 5; const size_t num_data; private: const RansacOptions &opt; const std::vector &x; const std::vector &X; RandomSampler sampler; // pre-allocated vectors for sampling std::vector xs; std::vector Xs; std::vector sample; }; } // namespace poselib #endif PoseLib-2.0.5/PoseLib/robust/estimators/homography.cc000066400000000000000000000051631504452766400226260ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "homography.h" #include "PoseLib/robust/bundle.h" #include "PoseLib/solvers/homography_4pt.h" namespace poselib { void HomographyEstimator::generate_models(std::vector *models) { sampler.generate_sample(&sample); for (size_t k = 0; k < sample_sz; ++k) { x1s[k] = x1[sample[k]].homogeneous().normalized(); x2s[k] = x2[sample[k]].homogeneous().normalized(); } Eigen::Matrix3d H; int sols = homography_4pt(x1s, x2s, &H, true); if (sols > 0) { models->push_back(H); } } double HomographyEstimator::score_model(const Eigen::Matrix3d &H, size_t *inlier_count) const { return compute_homography_msac_score(H, x1, x2, opt.max_reproj_error * opt.max_reproj_error, inlier_count); } void HomographyEstimator::refine_model(Eigen::Matrix3d *H) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_reproj_error; bundle_opt.max_iterations = 25; refine_homography(x1, x2, H, bundle_opt); } } // namespace poselibPoseLib-2.0.5/PoseLib/robust/estimators/homography.h000066400000000000000000000055041504452766400224670ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_ESTIMATORS_HOMOGRAPHY_H #define POSELIB_ROBUST_ESTIMATORS_HOMOGRAPHY_H #include "PoseLib/camera_pose.h" #include "PoseLib/robust/sampling.h" #include "PoseLib/robust/utils.h" #include "PoseLib/types.h" namespace poselib { class HomographyEstimator { public: HomographyEstimator(const RansacOptions &ransac_opt, const std::vector &points2D_1, const std::vector &points2D_2) : num_data(points2D_1.size()), opt(ransac_opt), x1(points2D_1), x2(points2D_2), sampler(num_data, sample_sz, opt.seed, opt.progressive_sampling, opt.max_prosac_iterations) { x1s.resize(sample_sz); x2s.resize(sample_sz); sample.resize(sample_sz); } void generate_models(std::vector *models); double score_model(const Eigen::Matrix3d &H, size_t *inlier_count) const; void refine_model(Eigen::Matrix3d *H) const; const size_t sample_sz = 4; const size_t num_data; private: const RansacOptions &opt; const std::vector &x1; const std::vector &x2; RandomSampler sampler; // pre-allocated vectors for sampling std::vector x1s, x2s; std::vector sample; }; } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/estimators/hybrid_pose.cc000066400000000000000000000067701504452766400227650ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "hybrid_pose.h" #include "PoseLib/robust/bundle.h" #include "PoseLib/solvers/gp3p.h" #include "PoseLib/solvers/p3p.h" namespace poselib { void HybridPoseEstimator::generate_models(std::vector *models) { draw_sample(sample_sz, num_data, &sample, rng); for (size_t k = 0; k < sample_sz; ++k) { xs[k] = x[sample[k]].homogeneous().normalized(); Xs[k] = X[sample[k]]; } p3p(xs, Xs, models); // TODO: actual hybrid sampling (we have p2p2pl and 5+1 gen-relpose already implemented, should be enough) } double HybridPoseEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { double score = compute_msac_score(pose, x, X, opt.max_reproj_error * opt.max_reproj_error, inlier_count); for (const PairwiseMatches &m : matches) { const CameraPose &map_pose = map_poses[m.cam_id1]; // Cameras are // [map.R map.t] // [R t] // Relative pose is [R * rig.R', t - R*rig.R'*rig.t] CameraPose rel_pose = pose; rel_pose.q = quat_multiply(rel_pose.q, quat_conj(map_pose.q)); rel_pose.t -= rel_pose.rotate(map_pose.t); size_t inliers_2d2d = 0; score += compute_sampson_msac_score(rel_pose, m.x1, m.x2, opt.max_epipolar_error * opt.max_epipolar_error, &inliers_2d2d); *inlier_count += inliers_2d2d; } return score; } void HybridPoseEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_reproj_error; bundle_opt.max_iterations = 25; // TODO: for high outlier scenarios, make a copy of (x,X) and find points close to inlier threshold // TODO: experiment with good thresholds for copy vs iterating full point set refine_hybrid_pose(x, X, matches, map_poses, pose, bundle_opt, opt.max_epipolar_error); } } // namespace poselibPoseLib-2.0.5/PoseLib/robust/estimators/hybrid_pose.h000066400000000000000000000060611504452766400226200ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_ESTIMATORS_HYBRID_POSE_H #define POSELIB_ROBUST_ESTIMATORS_HYBRID_POSE_H #include "PoseLib/camera_pose.h" #include "PoseLib/robust/sampling.h" #include "PoseLib/robust/utils.h" #include "PoseLib/types.h" namespace poselib { class HybridPoseEstimator { public: HybridPoseEstimator(const RansacOptions &ransac_opt, const std::vector &points2D, const std::vector &points3D, const std::vector &pairwise_matches, const std::vector &map_ext) : opt(ransac_opt), x(points2D), X(points3D), matches(pairwise_matches), map_poses(map_ext) { rng = opt.seed; xs.resize(sample_sz); Xs.resize(sample_sz); sample.resize(sample_sz); num_data = points2D.size(); for (const PairwiseMatches &m : matches) { num_data += m.x1.size(); } } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 3; size_t num_data; private: const RansacOptions &opt; const std::vector &x; const std::vector &X; const std::vector &matches; const std::vector &map_poses; RNG_t rng; // pre-allocated vectors for sampling std::vector xs, Xs; std::vector sample; }; } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/estimators/relative_pose.cc000066400000000000000000000252751504452766400233200ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "relative_pose.h" #include "PoseLib/misc/essential.h" #include "PoseLib/robust/bundle.h" #include "PoseLib/solvers/gen_relpose_5p1pt.h" #include "PoseLib/solvers/relpose_5pt.h" #include "PoseLib/solvers/relpose_6pt_focal.h" #include "PoseLib/solvers/relpose_7pt.h" #include namespace poselib { void RelativePoseEstimator::generate_models(std::vector *models) { sampler.generate_sample(&sample); for (size_t k = 0; k < sample_sz; ++k) { x1s[k] = x1[sample[k]].homogeneous().normalized(); x2s[k] = x2[sample[k]].homogeneous().normalized(); } relpose_5pt(x1s, x2s, models); } double RelativePoseEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { return compute_sampson_msac_score(pose, x1, x2, opt.max_epipolar_error * opt.max_epipolar_error, inlier_count); } void RelativePoseEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_epipolar_error; bundle_opt.max_iterations = 25; // Find approximate inliers and bundle over these with a truncated loss std::vector inliers; int num_inl = get_inliers(*pose, x1, x2, 5 * (opt.max_epipolar_error * opt.max_epipolar_error), &inliers); std::vector x1_inlier, x2_inlier; x1_inlier.reserve(num_inl); x2_inlier.reserve(num_inl); if (num_inl <= 5) { return; } for (size_t pt_k = 0; pt_k < x1.size(); ++pt_k) { if (inliers[pt_k]) { x1_inlier.push_back(x1[pt_k]); x2_inlier.push_back(x2[pt_k]); } } refine_relpose(x1_inlier, x2_inlier, pose, bundle_opt); } void SharedFocalRelativePoseEstimator::generate_models(ImagePairVector *models) { sampler.generate_sample(&sample); for (size_t k = 0; k < sample_sz; ++k) { x1s[k] = x1[sample[k]].homogeneous().normalized(); x2s[k] = x2[sample[k]].homogeneous().normalized(); } relpose_6pt_shared_focal(x1s, x2s, models); } double SharedFocalRelativePoseEstimator::score_model(const ImagePair &image_pair, size_t *inlier_count) const { Eigen::Matrix3d K_inv; K_inv << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, image_pair.camera1.focal(); // K_inv << 1.0 / calib_pose.camera.focal(), 0.0, 0.0, 0.0, 1.0 / calib_pose.camera.focal(), 0.0, 0.0, 0.0, 1.0; Eigen::Matrix3d E; essential_from_motion(image_pair.pose, &E); Eigen::Matrix3d F = K_inv * (E * K_inv); return compute_sampson_msac_score(F, x1, x2, opt.max_epipolar_error * opt.max_epipolar_error, inlier_count); } void SharedFocalRelativePoseEstimator::refine_model(ImagePair *image_pair) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_epipolar_error; bundle_opt.max_iterations = 25; Eigen::Matrix3d K_inv; // K_inv << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, calib_pose->camera.focal(); K_inv << 1.0 / image_pair->camera1.focal(), 0.0, 0.0, 0.0, 1.0 / image_pair->camera1.focal(), 0.0, 0.0, 0.0, 1.0; Eigen::Matrix3d E; essential_from_motion(image_pair->pose, &E); Eigen::Matrix3d F = K_inv * (E * K_inv); // Find approximate inliers and bundle over these with a truncated loss std::vector inliers; int num_inl = get_inliers(F, x1, x2, 5 * (opt.max_epipolar_error * opt.max_epipolar_error), &inliers); std::vector x1_inlier, x2_inlier; x1_inlier.reserve(num_inl); x2_inlier.reserve(num_inl); if (num_inl <= 6) { return; } for (size_t pt_k = 0; pt_k < x1.size(); ++pt_k) { if (inliers[pt_k]) { x1_inlier.push_back(x1[pt_k]); x2_inlier.push_back(x2[pt_k]); } } refine_shared_focal_relpose(x1_inlier, x2_inlier, image_pair, bundle_opt); } void GeneralizedRelativePoseEstimator::generate_models(std::vector *models) { // TODO replace by general 6pt solver? bool done = false; int pair0 = 0, pair1 = 1; while (!done) { pair0 = random_int(rng) % matches.size(); if (matches[pair0].x1.size() < 5) continue; pair1 = random_int(rng) % matches.size(); if (pair0 == pair1 || matches[pair1].x1.size() == 0) continue; done = true; } // Sample 5 points from the first camera pair CameraPose pose1 = rig1_poses[matches[pair0].cam_id1]; CameraPose pose2 = rig2_poses[matches[pair0].cam_id2]; Eigen::Vector3d p1 = pose1.center(); Eigen::Vector3d p2 = pose2.center(); draw_sample(5, matches[pair0].x1.size(), &sample, rng); for (size_t k = 0; k < 5; ++k) { x1s[k] = pose1.derotate(matches[pair0].x1[sample[k]].homogeneous().normalized()); p1s[k] = p1; x2s[k] = pose2.derotate(matches[pair0].x2[sample[k]].homogeneous().normalized()); p2s[k] = p2; } // Sample one point from the second camera pair pose1 = rig1_poses[matches[pair1].cam_id1]; pose2 = rig2_poses[matches[pair1].cam_id2]; p1 = pose1.center(); p2 = pose2.center(); size_t ind = random_int(rng) % matches[pair1].x1.size(); x1s[5] = pose1.derotate(matches[pair1].x1[ind].homogeneous().normalized()); p1s[5] = p1; x2s[5] = pose2.derotate(matches[pair1].x2[ind].homogeneous().normalized()); p2s[5] = p2; gen_relpose_5p1pt(p1s, x1s, p2s, x2s, models); } double GeneralizedRelativePoseEstimator::score_model(const CameraPose &pose, size_t *inlier_count) const { *inlier_count = 0; double cost = 0; for (size_t match_k = 0; match_k < matches.size(); ++match_k) { const PairwiseMatches &m = matches[match_k]; CameraPose pose1 = rig1_poses[m.cam_id1]; CameraPose pose2 = rig2_poses[m.cam_id2]; // Apply transform (transforming second rig into the first) pose2.t = pose2.t + pose2.rotate(pose.t); pose2.q = quat_multiply(pose2.q, pose.q); // Now the relative poses should be consistent with the pairwise measurements CameraPose relpose; relpose.q = quat_multiply(pose2.q, quat_conj(pose1.q)); relpose.t = pose2.t - relpose.rotate(pose1.t); size_t local_inlier_count = 0; cost += compute_sampson_msac_score(relpose, m.x1, m.x2, opt.max_epipolar_error * opt.max_epipolar_error, &local_inlier_count); *inlier_count += local_inlier_count; } return cost; } void GeneralizedRelativePoseEstimator::refine_model(CameraPose *pose) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_epipolar_error; bundle_opt.max_iterations = 25; std::vector inlier_matches; inlier_matches.resize(matches.size()); for (size_t match_k = 0; match_k < matches.size(); ++match_k) { const PairwiseMatches &m = matches[match_k]; CameraPose pose1 = rig1_poses[m.cam_id1]; CameraPose pose2 = rig2_poses[m.cam_id2]; // Apply transform (transforming second rig into the first) pose2.t = pose2.t + pose2.rotate(pose->t); pose2.q = quat_multiply(pose2.q, pose->q); // Now the relative poses should be consistent with the pairwise measurements CameraPose relpose; relpose.q = quat_multiply(pose2.q, quat_conj(pose1.q)); relpose.t = pose2.t - relpose.rotate(pose1.t); // Compute inliers with a relaxed threshold std::vector inliers; int num_inl = get_inliers(relpose, m.x1, m.x2, 5 * (opt.max_epipolar_error * opt.max_epipolar_error), &inliers); inlier_matches[match_k].cam_id1 = m.cam_id1; inlier_matches[match_k].cam_id2 = m.cam_id2; inlier_matches[match_k].x1.reserve(num_inl); inlier_matches[match_k].x2.reserve(num_inl); for (size_t k = 0; k < m.x1.size(); ++k) { if (inliers[k]) { inlier_matches[match_k].x1.push_back(m.x1[k]); inlier_matches[match_k].x2.push_back(m.x2[k]); } } } refine_generalized_relpose(inlier_matches, rig1_poses, rig2_poses, pose, bundle_opt); } void FundamentalEstimator::generate_models(std::vector *models) { sampler.generate_sample(&sample); for (size_t k = 0; k < sample_sz; ++k) { x1s[k] = x1[sample[k]].homogeneous().normalized(); x2s[k] = x2[sample[k]].homogeneous().normalized(); } relpose_7pt(x1s, x2s, models); if (opt.real_focal_check) { for (int i = models->size() - 1; i >= 0; i--) { if (!calculate_RFC((*models)[i])) models->erase(models->begin() + i); } } } double FundamentalEstimator::score_model(const Eigen::Matrix3d &F, size_t *inlier_count) const { return compute_sampson_msac_score(F, x1, x2, opt.max_epipolar_error * opt.max_epipolar_error, inlier_count); } void FundamentalEstimator::refine_model(Eigen::Matrix3d *F) const { BundleOptions bundle_opt; bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; bundle_opt.loss_scale = opt.max_epipolar_error; bundle_opt.max_iterations = 25; refine_fundamental(x1, x2, F, bundle_opt); } } // namespace poselib PoseLib-2.0.5/PoseLib/robust/estimators/relative_pose.h000066400000000000000000000142271504452766400231550ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_ESTIMATORS_RELATIVE_POSE_H #define POSELIB_ROBUST_ESTIMATORS_RELATIVE_POSE_H #include "PoseLib/camera_pose.h" #include "PoseLib/robust/sampling.h" #include "PoseLib/robust/utils.h" #include "PoseLib/types.h" namespace poselib { class RelativePoseEstimator { public: RelativePoseEstimator(const RansacOptions &ransac_opt, const std::vector &points2D_1, const std::vector &points2D_2) : num_data(points2D_1.size()), opt(ransac_opt), x1(points2D_1), x2(points2D_2), sampler(num_data, sample_sz, opt.seed, opt.progressive_sampling, opt.max_prosac_iterations) { x1s.resize(sample_sz); x2s.resize(sample_sz); sample.resize(sample_sz); } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 5; const size_t num_data; private: const RansacOptions &opt; const std::vector &x1; const std::vector &x2; RandomSampler sampler; // pre-allocated vectors for sampling std::vector x1s, x2s; std::vector sample; }; class SharedFocalRelativePoseEstimator { public: SharedFocalRelativePoseEstimator(const RansacOptions &ransac_opt, const std::vector &points2D_1, const std::vector &points2D_2) : num_data(points2D_1.size()), opt(ransac_opt), x1(points2D_1), x2(points2D_2), sampler(num_data, sample_sz, opt.seed, opt.progressive_sampling, opt.max_prosac_iterations) { x1s.resize(sample_sz); x2s.resize(sample_sz); sample.resize(sample_sz); } void generate_models(ImagePairVector *models); double score_model(const ImagePair &image_pair, size_t *inlier_count) const; void refine_model(ImagePair *image_pair) const; const size_t sample_sz = 6; const size_t num_data; private: const RansacOptions &opt; const std::vector &x1; const std::vector &x2; RandomSampler sampler; // pre-allocated vectors for sampling std::vector x1s, x2s; std::vector sample; }; class GeneralizedRelativePoseEstimator { public: GeneralizedRelativePoseEstimator(const RansacOptions &ransac_opt, const std::vector &pairwise_matches, const std::vector &camera1_ext, const std::vector &camera2_ext) : opt(ransac_opt), matches(pairwise_matches), rig1_poses(camera1_ext), rig2_poses(camera2_ext) { rng = opt.seed; x1s.resize(sample_sz); x2s.resize(sample_sz); p1s.resize(sample_sz); p2s.resize(sample_sz); sample.resize(sample_sz); num_data = 0; for (const PairwiseMatches &m : matches) { num_data += m.x1.size(); } } void generate_models(std::vector *models); double score_model(const CameraPose &pose, size_t *inlier_count) const; void refine_model(CameraPose *pose) const; const size_t sample_sz = 6; size_t num_data; private: const RansacOptions &opt; const std::vector &matches; const std::vector &rig1_poses; const std::vector &rig2_poses; RNG_t rng; // pre-allocated vectors for sampling std::vector x1s, x2s, p1s, p2s; std::vector sample; }; class FundamentalEstimator { public: FundamentalEstimator(const RansacOptions &ransac_opt, const std::vector &points2D_1, const std::vector &points2D_2) : num_data(points2D_1.size()), opt(ransac_opt), x1(points2D_1), x2(points2D_2), sampler(num_data, sample_sz, opt.seed, opt.progressive_sampling, opt.max_prosac_iterations) { x1s.resize(sample_sz); x2s.resize(sample_sz); sample.resize(sample_sz); } void generate_models(std::vector *models); double score_model(const Eigen::Matrix3d &F, size_t *inlier_count) const; void refine_model(Eigen::Matrix3d *F) const; const size_t sample_sz = 7; const size_t num_data; private: const RansacOptions &opt; const std::vector &x1; const std::vector &x2; RandomSampler sampler; // pre-allocated vectors for sampling std::vector x1s, x2s; std::vector sample; }; } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/jacobian_impl.h000066400000000000000000002143511504452766400207110ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_JACOBIAN_IMPL_H_ #define POSELIB_JACOBIAN_IMPL_H_ #include "PoseLib/camera_pose.h" #include "PoseLib/misc/colmap_models.h" #include "PoseLib/misc/essential.h" #include "PoseLib/types.h" namespace poselib { // For the accumulators we support supplying a vector with point-wise weights for the residuals // In case we don't want to have weighted residuals, we can pass UniformWeightVector instead of filling a std::vector // with 1.0 The multiplication is then hopefully is optimized away since it always returns 1.0 class UniformWeightVector { public: UniformWeightVector() {} constexpr double operator[](std::size_t idx) const { return 1.0; } }; class UniformWeightVectors { // this corresponds to std::vector> used for generalized cameras etc public: UniformWeightVectors() {} constexpr const UniformWeightVector &operator[](std::size_t idx) const { return w; } const UniformWeightVector w; typedef UniformWeightVector value_type; }; template class CameraJacobianAccumulator { public: CameraJacobianAccumulator(const std::vector &points2D, const std::vector &points3D, const Camera &cam, const LossFunction &loss, const ResidualWeightVector &w = ResidualWeightVector()) : x(points2D), X(points3D), camera(cam), loss_fn(loss), weights(w) {} double residual(const CameraPose &pose) const { double cost = 0; for (size_t i = 0; i < x.size(); ++i) { const Eigen::Vector3d Z = pose.apply(X[i]); // Note this assumes points that are behind the camera will stay behind the camera // during the optimization if (Z(2) < 0) continue; const double inv_z = 1.0 / Z(2); Eigen::Vector2d p(Z(0) * inv_z, Z(1) * inv_z); CameraModel::project(camera.params, p, &p); const double r0 = p(0) - x[i](0); const double r1 = p(1) - x[i](1); const double r_squared = r0 * r0 + r1 * r1; cost += weights[i] * loss_fn.loss(r_squared); } return cost; } // computes J.transpose() * J and J.transpose() * res // Only computes the lower half of JtJ size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { Eigen::Matrix3d R = pose.R(); Eigen::Matrix2d Jcam; Jcam.setIdentity(); // we initialize to identity here (this is for the calibrated case) size_t num_residuals = 0; for (size_t i = 0; i < x.size(); ++i) { const Eigen::Vector3d Z = R * X[i] + pose.t; const Eigen::Vector2d z = Z.hnormalized(); // Note this assumes points that are behind the camera will stay behind the camera // during the optimization if (Z(2) < 0) continue; // Project with intrinsics Eigen::Vector2d zp = z; CameraModel::project_with_jac(camera.params, z, &zp, &Jcam); // Setup residual Eigen::Vector2d r = zp - x[i]; const double r_squared = r.squaredNorm(); const double weight = weights[i] * loss_fn.weight(r_squared); if (weight == 0.0) { continue; } num_residuals++; // Compute jacobian w.r.t. Z (times R) Eigen::Matrix dZ; dZ.block<2, 2>(0, 0) = Jcam; dZ.col(2) = -Jcam * z; dZ *= 1.0 / Z(2); dZ *= R; const double X0 = X[i](0); const double X1 = X[i](1); const double X2 = X[i](2); const double dZtdZ_0_0 = weight * dZ.col(0).dot(dZ.col(0)); const double dZtdZ_1_0 = weight * dZ.col(1).dot(dZ.col(0)); const double dZtdZ_1_1 = weight * dZ.col(1).dot(dZ.col(1)); const double dZtdZ_2_0 = weight * dZ.col(2).dot(dZ.col(0)); const double dZtdZ_2_1 = weight * dZ.col(2).dot(dZ.col(1)); const double dZtdZ_2_2 = weight * dZ.col(2).dot(dZ.col(2)); JtJ(0, 0) += X2 * (X2 * dZtdZ_1_1 - X1 * dZtdZ_2_1) + X1 * (X1 * dZtdZ_2_2 - X2 * dZtdZ_2_1); JtJ(1, 0) += -X2 * (X2 * dZtdZ_1_0 - X0 * dZtdZ_2_1) - X1 * (X0 * dZtdZ_2_2 - X2 * dZtdZ_2_0); JtJ(2, 0) += X1 * (X0 * dZtdZ_2_1 - X1 * dZtdZ_2_0) - X2 * (X0 * dZtdZ_1_1 - X1 * dZtdZ_1_0); JtJ(3, 0) += X1 * dZtdZ_2_0 - X2 * dZtdZ_1_0; JtJ(4, 0) += X1 * dZtdZ_2_1 - X2 * dZtdZ_1_1; JtJ(5, 0) += X1 * dZtdZ_2_2 - X2 * dZtdZ_2_1; JtJ(1, 1) += X2 * (X2 * dZtdZ_0_0 - X0 * dZtdZ_2_0) + X0 * (X0 * dZtdZ_2_2 - X2 * dZtdZ_2_0); JtJ(2, 1) += -X2 * (X1 * dZtdZ_0_0 - X0 * dZtdZ_1_0) - X0 * (X0 * dZtdZ_2_1 - X1 * dZtdZ_2_0); JtJ(3, 1) += X2 * dZtdZ_0_0 - X0 * dZtdZ_2_0; JtJ(4, 1) += X2 * dZtdZ_1_0 - X0 * dZtdZ_2_1; JtJ(5, 1) += X2 * dZtdZ_2_0 - X0 * dZtdZ_2_2; JtJ(2, 2) += X1 * (X1 * dZtdZ_0_0 - X0 * dZtdZ_1_0) + X0 * (X0 * dZtdZ_1_1 - X1 * dZtdZ_1_0); JtJ(3, 2) += X0 * dZtdZ_1_0 - X1 * dZtdZ_0_0; JtJ(4, 2) += X0 * dZtdZ_1_1 - X1 * dZtdZ_1_0; JtJ(5, 2) += X0 * dZtdZ_2_1 - X1 * dZtdZ_2_0; JtJ(3, 3) += dZtdZ_0_0; JtJ(4, 3) += dZtdZ_1_0; JtJ(5, 3) += dZtdZ_2_0; JtJ(4, 4) += dZtdZ_1_1; JtJ(5, 4) += dZtdZ_2_1; JtJ(5, 5) += dZtdZ_2_2; r *= weight; Jtr(0) += (r(0) * (X1 * dZ(0, 2) - X2 * dZ(0, 1)) + r(1) * (X1 * dZ(1, 2) - X2 * dZ(1, 1))); Jtr(1) += (-r(0) * (X0 * dZ(0, 2) - X2 * dZ(0, 0)) - r(1) * (X0 * dZ(1, 2) - X2 * dZ(1, 0))); Jtr(2) += (r(0) * (X0 * dZ(0, 1) - X1 * dZ(0, 0)) + r(1) * (X0 * dZ(1, 1) - X1 * dZ(1, 0))); Jtr(3) += (dZ(0, 0) * r(0) + dZ(1, 0) * r(1)); Jtr(4) += (dZ(0, 1) * r(0) + dZ(1, 1) * r(1)); Jtr(5) += (dZ(0, 2) * r(0) + dZ(1, 2) * r(1)); } return num_residuals; } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; // The rotation is parameterized via the lie-rep. and post-multiplication // i.e. R(delta) = R * expm([delta]_x) pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); // Translation is parameterized as (negative) shift in position // i.e. t(delta) = t + R*delta pose_new.t = pose.t + pose.rotate(dp.block<3, 1>(3, 0)); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 6; private: const std::vector &x; const std::vector &X; const Camera &camera; const LossFunction &loss_fn; const ResidualWeightVector &weights; }; template class GeneralizedCameraJacobianAccumulator { public: GeneralizedCameraJacobianAccumulator(const std::vector> &points2D, const std::vector> &points3D, const std::vector &camera_ext, const std::vector &camera_int, const LossFunction &l, const ResidualWeightVectors &w = ResidualWeightVectors()) : num_cams(points2D.size()), x(points2D), X(points3D), rig_poses(camera_ext), cameras(camera_int), loss_fn(l), weights(w) {} double residual(const CameraPose &pose) const { double cost = 0.0; for (size_t k = 0; k < num_cams; ++k) { if (x[k].size() == 0) { continue; } const Camera &camera = cameras[k]; CameraPose full_pose; full_pose.q = quat_multiply(rig_poses[k].q, pose.q); full_pose.t = rig_poses[k].rotate(pose.t) + rig_poses[k].t; switch (camera.model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: { \ CameraJacobianAccumulator accum( \ x[k], X[k], cameras[k], loss_fn, weights[k]); \ cost += accum.residual(full_pose); \ break; \ } SWITCH_CAMERA_MODELS #undef SWITCH_CAMERA_MODEL_CASE } } return cost; } size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { size_t num_residuals = 0; for (size_t k = 0; k < num_cams; ++k) { if (x[k].size() == 0) { continue; } const Camera &camera = cameras[k]; CameraPose full_pose; full_pose.q = quat_multiply(rig_poses[k].q, pose.q); full_pose.t = rig_poses[k].rotate(pose.t) + rig_poses[k].t; switch (camera.model_id) { #define SWITCH_CAMERA_MODEL_CASE(Model) \ case Model::model_id: { \ CameraJacobianAccumulator accum( \ x[k], X[k], cameras[k], loss_fn, weights[k]); \ num_residuals += accum.accumulate(full_pose, JtJ, Jtr); \ break; \ } SWITCH_CAMERA_MODELS #undef SWITCH_CAMERA_MODEL_CASE } } return num_residuals; } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); pose_new.t = pose.t + pose.rotate(dp.block<3, 1>(3, 0)); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 6; private: const size_t num_cams; const std::vector> &x; const std::vector> &X; const std::vector &rig_poses; const std::vector &cameras; const LossFunction &loss_fn; const ResidualWeightVectors &weights; }; template class LineJacobianAccumulator { public: LineJacobianAccumulator(const std::vector &lines2D_, const std::vector &lines3D_, const LossFunction &loss, const ResidualWeightVector &w = ResidualWeightVector()) : lines2D(lines2D_), lines3D(lines3D_), loss_fn(loss), weights(w) {} double residual(const CameraPose &pose) const { Eigen::Matrix3d R = pose.R(); double cost = 0; for (size_t i = 0; i < lines2D.size(); ++i) { const Eigen::Vector3d Z1 = R * lines3D[i].X1 + pose.t; const Eigen::Vector3d Z2 = R * lines3D[i].X2 + pose.t; Eigen::Vector3d l = Z1.cross(Z2); l /= l.topRows<2>().norm(); const double r0 = l.dot(lines2D[i].x1.homogeneous()); const double r1 = l.dot(lines2D[i].x2.homogeneous()); const double r_squared = r0 * r0 + r1 * r1; cost += weights[i] * loss_fn.loss(r_squared); } return cost; } // computes J.transpose() * J and J.transpose() * res // Only computes the lower half of JtJ size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { Eigen::Matrix3d E, R; R = pose.R(); E << pose.t.cross(R.col(0)), pose.t.cross(R.col(1)), pose.t.cross(R.col(2)); size_t num_residuals = 0; for (size_t k = 0; k < lines2D.size(); ++k) { const Eigen::Vector3d Z1 = R * lines3D[k].X1 + pose.t; const Eigen::Vector3d Z2 = R * lines3D[k].X2 + pose.t; const Eigen::Vector3d X12 = lines3D[k].X1.cross(lines3D[k].X2); const Eigen::Vector3d dX = lines3D[k].X1 - lines3D[k].X2; // Projected line const Eigen::Vector3d l = Z1.cross(Z2); // Normalized line by first two coordinates Eigen::Vector2d alpha = l.topRows<2>(); double beta = l(2); const double n_alpha = alpha.norm(); alpha /= n_alpha; beta /= n_alpha; // Compute residual Eigen::Vector2d r; r << alpha.dot(lines2D[k].x1) + beta, alpha.dot(lines2D[k].x2) + beta; const double r_squared = r.squaredNorm(); const double weight = weights[k] * loss_fn.weight(r_squared); if (weight == 0.0) { continue; } num_residuals++; Eigen::Matrix dl_drt; // Differentiate line with respect to rotation parameters dl_drt.block<1, 3>(0, 0) = E.row(0).cross(dX) - R.row(0).cross(X12); dl_drt.block<1, 3>(1, 0) = E.row(1).cross(dX) - R.row(1).cross(X12); dl_drt.block<1, 3>(2, 0) = E.row(2).cross(dX) - R.row(2).cross(X12); // and translation params dl_drt.block<1, 3>(0, 3) = R.row(0).cross(dX); dl_drt.block<1, 3>(1, 3) = R.row(1).cross(dX); dl_drt.block<1, 3>(2, 3) = R.row(2).cross(dX); // Differentiate normalized line w.r.t. original line Eigen::Matrix3d dln_dl; dln_dl.block<2, 2>(0, 0) = (Eigen::Matrix2d::Identity() - alpha * alpha.transpose()) / n_alpha; dln_dl.block<1, 2>(2, 0) = -beta * alpha / n_alpha; dln_dl.block<2, 1>(0, 2).setZero(); dln_dl(2, 2) = 1 / n_alpha; // Differentiate residual w.r.t. line Eigen::Matrix dr_dl; dr_dl.row(0) << lines2D[k].x1.transpose(), 1.0; dr_dl.row(1) << lines2D[k].x2.transpose(), 1.0; Eigen::Matrix J = dr_dl * dln_dl * dl_drt; // Accumulate into JtJ and Jtr Jtr += weight * J.transpose() * r; for (size_t i = 0; i < 6; ++i) { for (size_t j = 0; j <= i; ++j) { JtJ(i, j) += weight * (J.col(i).dot(J.col(j))); } } } return num_residuals; } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; // The rotation is parameterized via the lie-rep. and post-multiplication // i.e. R(delta) = R * expm([delta]_x) pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); // Translation is parameterized as (negative) shift in position // i.e. t(delta) = t + R*delta pose_new.t = pose.t + pose.rotate(dp.block<3, 1>(3, 0)); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 6; private: const std::vector &lines2D; const std::vector &lines3D; const LossFunction &loss_fn; const ResidualWeightVector &weights; }; template class PointLineJacobianAccumulator { public: PointLineJacobianAccumulator(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, const PointLossFunction &l_point, const LineLossFunction &l_line, const PointResidualsVector &weights_pts = PointResidualsVector(), const LineResidualsVector &weights_l = LineResidualsVector()) : pts_accum(points2D, points3D, trivial_camera, l_point, weights_pts), line_accum(lines2D, lines3D, l_line, weights_l) { trivial_camera.model_id = NullCameraModel::model_id; } double residual(const CameraPose &pose) const { return pts_accum.residual(pose) + line_accum.residual(pose); } size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { return pts_accum.accumulate(pose, JtJ, Jtr) + line_accum.accumulate(pose, JtJ, Jtr); } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { // Both CameraJacobianAccumulator and LineJacobianAccumulator have the same step! CameraPose pose_new; pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); pose_new.t = pose.t + pose.rotate(dp.block<3, 1>(3, 0)); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 6; private: Camera trivial_camera; CameraJacobianAccumulator pts_accum; LineJacobianAccumulator line_accum; }; template class RelativePoseJacobianAccumulator { public: RelativePoseJacobianAccumulator(const std::vector &points2D_1, const std::vector &points2D_2, const LossFunction &l, const ResidualWeightVector &w = ResidualWeightVector()) : x1(points2D_1), x2(points2D_2), loss_fn(l), weights(w) {} double residual(const CameraPose &pose) const { Eigen::Matrix3d E; essential_from_motion(pose, &E); double cost = 0.0; for (size_t k = 0; k < x1.size(); ++k) { double C = x2[k].homogeneous().dot(E * x1[k].homogeneous()); double nJc_sq = (E.block<2, 3>(0, 0) * x1[k].homogeneous()).squaredNorm() + (E.block<3, 2>(0, 0).transpose() * x2[k].homogeneous()).squaredNorm(); double r2 = (C * C) / nJc_sq; cost += weights[k] * loss_fn.loss(r2); } return cost; } size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) { // We start by setting up a basis for the updates in the translation (orthogonal to t) // We find the minimum element of t and cross product with the corresponding basis vector. // (this ensures that the first cross product is not close to the zero vector) if (std::abs(pose.t.x()) < std::abs(pose.t.y())) { // x < y if (std::abs(pose.t.x()) < std::abs(pose.t.z())) { tangent_basis.col(0) = pose.t.cross(Eigen::Vector3d::UnitX()).normalized(); } else { tangent_basis.col(0) = pose.t.cross(Eigen::Vector3d::UnitZ()).normalized(); } } else { // x > y if (std::abs(pose.t.y()) < std::abs(pose.t.z())) { tangent_basis.col(0) = pose.t.cross(Eigen::Vector3d::UnitY()).normalized(); } else { tangent_basis.col(0) = pose.t.cross(Eigen::Vector3d::UnitZ()).normalized(); } } tangent_basis.col(1) = tangent_basis.col(0).cross(pose.t).normalized(); Eigen::Matrix3d E, R; R = pose.R(); essential_from_motion(pose, &E); // Matrices contain the jacobians of E w.r.t. the rotation and translation parameters Eigen::Matrix dR; Eigen::Matrix dt; // Each column is vec(E*skew(e_k)) where e_k is k:th basis vector dR.block<3, 1>(0, 0).setZero(); dR.block<3, 1>(0, 1) = -E.col(2); dR.block<3, 1>(0, 2) = E.col(1); dR.block<3, 1>(3, 0) = E.col(2); dR.block<3, 1>(3, 1).setZero(); dR.block<3, 1>(3, 2) = -E.col(0); dR.block<3, 1>(6, 0) = -E.col(1); dR.block<3, 1>(6, 1) = E.col(0); dR.block<3, 1>(6, 2).setZero(); // Each column is vec(skew(tangent_basis[k])*R) dt.block<3, 1>(0, 0) = tangent_basis.col(0).cross(R.col(0)); dt.block<3, 1>(0, 1) = tangent_basis.col(1).cross(R.col(0)); dt.block<3, 1>(3, 0) = tangent_basis.col(0).cross(R.col(1)); dt.block<3, 1>(3, 1) = tangent_basis.col(1).cross(R.col(1)); dt.block<3, 1>(6, 0) = tangent_basis.col(0).cross(R.col(2)); dt.block<3, 1>(6, 1) = tangent_basis.col(1).cross(R.col(2)); size_t num_residuals = 0; for (size_t k = 0; k < x1.size(); ++k) { double C = x2[k].homogeneous().dot(E * x1[k].homogeneous()); // J_C is the Jacobian of the epipolar constraint w.r.t. the image points Eigen::Vector4d J_C; J_C << E.block<3, 2>(0, 0).transpose() * x2[k].homogeneous(), E.block<2, 3>(0, 0) * x1[k].homogeneous(); const double nJ_C = J_C.norm(); const double inv_nJ_C = 1.0 / nJ_C; const double r = C * inv_nJ_C; // Compute weight from robust loss function (used in the IRLS) const double weight = weights[k] * loss_fn.weight(r * r); if (weight == 0.0) { continue; } num_residuals++; // Compute Jacobian of Sampson error w.r.t the fundamental/essential matrix (3x3) Eigen::Matrix dF; dF << x1[k](0) * x2[k](0), x1[k](0) * x2[k](1), x1[k](0), x1[k](1) * x2[k](0), x1[k](1) * x2[k](1), x1[k](1), x2[k](0), x2[k](1), 1.0; const double s = C * inv_nJ_C * inv_nJ_C; dF(0) -= s * (J_C(2) * x1[k](0) + J_C(0) * x2[k](0)); dF(1) -= s * (J_C(3) * x1[k](0) + J_C(0) * x2[k](1)); dF(2) -= s * (J_C(0)); dF(3) -= s * (J_C(2) * x1[k](1) + J_C(1) * x2[k](0)); dF(4) -= s * (J_C(3) * x1[k](1) + J_C(1) * x2[k](1)); dF(5) -= s * (J_C(1)); dF(6) -= s * (J_C(2)); dF(7) -= s * (J_C(3)); dF *= inv_nJ_C; // and then w.r.t. the pose parameters (rotation + tangent basis for translation) Eigen::Matrix J; J.block<1, 3>(0, 0) = dF * dR; J.block<1, 2>(0, 3) = dF * dt; // Accumulate into JtJ and Jtr Jtr += weight * C * inv_nJ_C * J.transpose(); JtJ(0, 0) += weight * (J(0) * J(0)); JtJ(1, 0) += weight * (J(1) * J(0)); JtJ(1, 1) += weight * (J(1) * J(1)); JtJ(2, 0) += weight * (J(2) * J(0)); JtJ(2, 1) += weight * (J(2) * J(1)); JtJ(2, 2) += weight * (J(2) * J(2)); JtJ(3, 0) += weight * (J(3) * J(0)); JtJ(3, 1) += weight * (J(3) * J(1)); JtJ(3, 2) += weight * (J(3) * J(2)); JtJ(3, 3) += weight * (J(3) * J(3)); JtJ(4, 0) += weight * (J(4) * J(0)); JtJ(4, 1) += weight * (J(4) * J(1)); JtJ(4, 2) += weight * (J(4) * J(2)); JtJ(4, 3) += weight * (J(4) * J(3)); JtJ(4, 4) += weight * (J(4) * J(4)); } return num_residuals; } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); pose_new.t = pose.t + tangent_basis * dp.block<2, 1>(3, 0); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 5; private: const std::vector &x1; const std::vector &x2; const LossFunction &loss_fn; const ResidualWeightVector &weights; Eigen::Matrix tangent_basis; }; template class SharedFocalRelativePoseJacobianAccumulator { public: SharedFocalRelativePoseJacobianAccumulator(const std::vector &points2D_1, const std::vector &points2D_2, const LossFunction &l, const ResidualWeightVector &w = ResidualWeightVector()) : x1(points2D_1), x2(points2D_2), loss_fn(l), weights(w) {} double residual(const ImagePair &image_pair) const { Eigen::Matrix3d E; essential_from_motion(image_pair.pose, &E); Eigen::Matrix3d K_inv; K_inv << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, image_pair.camera1.focal(); Eigen::Matrix3d F = K_inv * (E * K_inv); double cost = 0.0; for (size_t k = 0; k < x1.size(); ++k) { double C = x2[k].homogeneous().dot(F * x1[k].homogeneous()); double nJc_sq = (F.block<2, 3>(0, 0) * x1[k].homogeneous()).squaredNorm() + (F.block<3, 2>(0, 0).transpose() * x2[k].homogeneous()).squaredNorm(); double r2 = (C * C) / nJc_sq; cost += weights[k] * loss_fn.loss(r2); } return cost; } size_t accumulate(const ImagePair &image_pair, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) { // We start by setting up a basis for the updates in the translation (orthogonal to t) // We find the minimum element of t and cross product with the corresponding basis vector. // (this ensures that the first cross product is not close to the zero vector) if (std::abs(image_pair.pose.t.x()) < std::abs(image_pair.pose.t.y())) { // x < y if (std::abs(image_pair.pose.t.x()) < std::abs(image_pair.pose.t.z())) { tangent_basis.col(0) = image_pair.pose.t.cross(Eigen::Vector3d::UnitX()).normalized(); } else { tangent_basis.col(0) = image_pair.pose.t.cross(Eigen::Vector3d::UnitZ()).normalized(); } } else { // x > y if (std::abs(image_pair.pose.t.y()) < std::abs(image_pair.pose.t.z())) { tangent_basis.col(0) = image_pair.pose.t.cross(Eigen::Vector3d::UnitY()).normalized(); } else { tangent_basis.col(0) = image_pair.pose.t.cross(Eigen::Vector3d::UnitZ()).normalized(); } } tangent_basis.col(1) = tangent_basis.col(0).cross(image_pair.pose.t).normalized(); double focal = image_pair.camera1.focal(); Eigen::Matrix3d K_inv; K_inv << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, focal; Eigen::Matrix3d E, R; R = image_pair.pose.R(); essential_from_motion(image_pair.pose, &E); Eigen::Matrix3d F = K_inv * (E * K_inv); // Matrices contain the jacobians of E w.r.t. the rotation and translation parameters Eigen::Matrix dR; Eigen::Matrix dt; // Each column is vec(E*skew(e_k)) where e_k is k:th basis vector dR.block<3, 1>(0, 0).setZero(); dR.block<3, 1>(0, 1) = -E.col(2); dR.block<3, 1>(0, 2) = E.col(1); dR.block<3, 1>(3, 0) = E.col(2); dR.block<3, 1>(3, 1).setZero(); dR.block<3, 1>(3, 2) = -E.col(0); dR.block<3, 1>(6, 0) = -E.col(1); dR.block<3, 1>(6, 1) = E.col(0); dR.block<3, 1>(6, 2).setZero(); dR.row(2) *= focal; dR.row(5) *= focal; dR.row(6) *= focal; dR.row(7) *= focal; dR.row(8) *= focal * focal; // Each column is vec(skew(tangent_basis[k])*R) dt.block<3, 1>(0, 0) = tangent_basis.col(0).cross(R.col(0)); dt.block<3, 1>(0, 1) = tangent_basis.col(1).cross(R.col(0)); dt.block<3, 1>(3, 0) = tangent_basis.col(0).cross(R.col(1)); dt.block<3, 1>(3, 1) = tangent_basis.col(1).cross(R.col(1)); dt.block<3, 1>(6, 0) = tangent_basis.col(0).cross(R.col(2)); dt.block<3, 1>(6, 1) = tangent_basis.col(1).cross(R.col(2)); dt.row(2) *= focal; dt.row(5) *= focal; dt.row(6) *= focal; dt.row(7) *= focal; dt.row(8) *= focal * focal; Eigen::Matrix df; df << 0.0, 0.0, E(2, 0), 0.0, 0.0, E(2, 1), E(0, 2), E(1, 2), 2 * E(2, 2) * focal; size_t num_residuals = 0; for (size_t k = 0; k < x1.size(); ++k) { double C = x2[k].homogeneous().dot(F * x1[k].homogeneous()); // J_C is the Jacobian of the epipolar constraint w.r.t. the image points Eigen::Vector4d J_C; J_C << F.block<3, 2>(0, 0).transpose() * x2[k].homogeneous(), F.block<2, 3>(0, 0) * x1[k].homogeneous(); const double nJ_C = J_C.norm(); const double inv_nJ_C = 1.0 / nJ_C; const double r = C * inv_nJ_C; // Compute weight from robust loss function (used in the IRLS) const double weight = weights[k] * loss_fn.weight(r * r); if (weight == 0.0) { continue; } num_residuals++; // Compute Jacobian of Sampson error w.r.t the fundamental/essential matrix (3x3) Eigen::Matrix dF; dF << x1[k](0) * x2[k](0), x1[k](0) * x2[k](1), x1[k](0), x1[k](1) * x2[k](0), x1[k](1) * x2[k](1), x1[k](1), x2[k](0), x2[k](1), 1.0; const double s = C * inv_nJ_C * inv_nJ_C; dF(0) -= s * (J_C(2) * x1[k](0) + J_C(0) * x2[k](0)); dF(1) -= s * (J_C(3) * x1[k](0) + J_C(0) * x2[k](1)); dF(2) -= s * (J_C(0)); dF(3) -= s * (J_C(2) * x1[k](1) + J_C(1) * x2[k](0)); dF(4) -= s * (J_C(3) * x1[k](1) + J_C(1) * x2[k](1)); dF(5) -= s * (J_C(1)); dF(6) -= s * (J_C(2)); dF(7) -= s * (J_C(3)); dF *= inv_nJ_C; // and then w.r.t. the pose parameters (rotation + tangent basis for translation) Eigen::Matrix J; J.block<1, 3>(0, 0) = dF * dR; J.block<1, 2>(0, 3) = dF * dt; J(0, 5) = dF * df; // Accumulate into JtJ and Jtr Jtr += weight * C * inv_nJ_C * J.transpose(); JtJ(0, 0) += weight * (J(0) * J(0)); JtJ(1, 0) += weight * (J(1) * J(0)); JtJ(1, 1) += weight * (J(1) * J(1)); JtJ(2, 0) += weight * (J(2) * J(0)); JtJ(2, 1) += weight * (J(2) * J(1)); JtJ(2, 2) += weight * (J(2) * J(2)); JtJ(3, 0) += weight * (J(3) * J(0)); JtJ(3, 1) += weight * (J(3) * J(1)); JtJ(3, 2) += weight * (J(3) * J(2)); JtJ(3, 3) += weight * (J(3) * J(3)); JtJ(4, 0) += weight * (J(4) * J(0)); JtJ(4, 1) += weight * (J(4) * J(1)); JtJ(4, 2) += weight * (J(4) * J(2)); JtJ(4, 3) += weight * (J(4) * J(3)); JtJ(4, 4) += weight * (J(4) * J(4)); JtJ(5, 0) += weight * (J(5) * J(0)); JtJ(5, 1) += weight * (J(5) * J(1)); JtJ(5, 2) += weight * (J(5) * J(2)); JtJ(5, 3) += weight * (J(5) * J(3)); JtJ(5, 4) += weight * (J(5) * J(4)); JtJ(5, 5) += weight * (J(5) * J(5)); } return num_residuals; } ImagePair step(Eigen::Matrix dp, const ImagePair &image_pair) const { CameraPose pose_new; pose_new.q = quat_step_post(image_pair.pose.q, dp.block<3, 1>(0, 0)); pose_new.t = image_pair.pose.t + tangent_basis * dp.block<2, 1>(3, 0); Camera camera_new = Camera("SIMPLE_PINHOLE", std::vector{std::max(image_pair.camera1.focal() + dp(5, 0), 0.0), 0.0, 0.0}, -1, -1); ImagePair calib_pose_new(pose_new, camera_new, camera_new); return calib_pose_new; } typedef ImagePair param_t; static constexpr size_t num_params = 6; private: const std::vector &x1; const std::vector &x2; const LossFunction &loss_fn; const ResidualWeightVector &weights; Eigen::Matrix tangent_basis; }; template class GeneralizedRelativePoseJacobianAccumulator { public: GeneralizedRelativePoseJacobianAccumulator(const std::vector &pairwise_matches, const std::vector &camera1_ext, const std::vector &camera2_ext, const LossFunction &l, const ResidualWeightVectors &w = ResidualWeightVectors()) : matches(pairwise_matches), rig1_poses(camera1_ext), rig2_poses(camera2_ext), loss_fn(l), weights(w) {} double residual(const CameraPose &pose) const { double cost = 0.0; for (size_t match_k = 0; match_k < matches.size(); ++match_k) { const PairwiseMatches &m = matches[match_k]; Eigen::Vector4d q1 = rig1_poses[m.cam_id1].q; Eigen::Vector3d t1 = rig1_poses[m.cam_id1].t; Eigen::Vector4d q2 = rig2_poses[m.cam_id2].q; Eigen::Vector3d t2 = rig2_poses[m.cam_id2].t; CameraPose relpose; relpose.q = quat_multiply(q2, quat_multiply(pose.q, quat_conj(q1))); relpose.t = t2 + quat_rotate(q2, pose.t) - relpose.rotate(t1); RelativePoseJacobianAccumulator accum( m.x1, m.x2, loss_fn, weights[match_k]); cost += accum.residual(relpose); } return cost; } size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { Eigen::Matrix3d R = pose.R(); size_t num_residuals = 0; for (size_t match_k = 0; match_k < matches.size(); ++match_k) { const PairwiseMatches &m = matches[match_k]; // Cameras are // [R1 t1] // [R2 t2] * [R t; 0 1] = [R2*R t2+R2*t] // Relative pose is // [R2*R*R1' t2+R2*t-R2*R*R1'*t1] // Essential matrix is // [t2]_x*R2*R*R1' + [R2*t]_x*R2*R*R1' - R2*R*R1'*[t1]_x Eigen::Vector4d q1 = rig1_poses[m.cam_id1].q; Eigen::Matrix3d R1 = quat_to_rotmat(q1); Eigen::Vector3d t1 = rig1_poses[m.cam_id1].t; Eigen::Vector4d q2 = rig2_poses[m.cam_id2].q; Eigen::Matrix3d R2 = quat_to_rotmat(q2); Eigen::Vector3d t2 = rig2_poses[m.cam_id2].t; CameraPose relpose; relpose.q = quat_multiply(q2, quat_multiply(pose.q, quat_conj(q1))); relpose.t = t2 + R2 * pose.t - relpose.rotate(t1); Eigen::Matrix3d E; essential_from_motion(relpose, &E); Eigen::Matrix3d R2R = R2 * R; Eigen::Vector3d Rt = R.transpose() * pose.t; // The messy expressions below compute // dRdw = [vec(S1) vec(S2) vec(S3)]; // dR = (kron(R1,skew(t2)*R2R+ R2*skew(t)*R) + kron(skew(t1)*R1,R2*R)) * dRdw // dt = [vec(R2*R*S1*R1.') vec(R2*R*S2*R1.') vec(R2*R*S3*R1.')] // TODO: Replace with something nice Eigen::Matrix dR; Eigen::Matrix dt; dR(0, 0) = R2R(0, 1) * (R1(1, 2) * t1(2) - R1(2, 2) * t1(1)) - R2R(0, 2) * (R1(1, 1) * t1(2) - R1(2, 1) * t1(1)) + R1(0, 1) * (R2R(0, 0) * Rt(1) - R2R(0, 1) * Rt(0) - R2R(1, 2) * t2(2) + R2R(2, 2) * t2(1)) + R1(0, 2) * (R2R(0, 0) * Rt(2) - R2R(0, 2) * Rt(0) + R2R(1, 1) * t2(2) - R2R(2, 1) * t2(1)); dR(0, 1) = R2R(0, 2) * (R1(1, 0) * t1(2) - R1(2, 0) * t1(1)) - R2R(0, 0) * (R1(1, 2) * t1(2) - R1(2, 2) * t1(1)) - R1(0, 0) * (R2R(0, 0) * Rt(1) - R2R(0, 1) * Rt(0) - R2R(1, 2) * t2(2) + R2R(2, 2) * t2(1)) + R1(0, 2) * (R2R(0, 1) * Rt(2) - R2R(0, 2) * Rt(1) - R2R(1, 0) * t2(2) + R2R(2, 0) * t2(1)); dR(0, 2) = R2R(0, 0) * (R1(1, 1) * t1(2) - R1(2, 1) * t1(1)) - R2R(0, 1) * (R1(1, 0) * t1(2) - R1(2, 0) * t1(1)) - R1(0, 0) * (R2R(0, 0) * Rt(2) - R2R(0, 2) * Rt(0) + R2R(1, 1) * t2(2) - R2R(2, 1) * t2(1)) - R1(0, 1) * (R2R(0, 1) * Rt(2) - R2R(0, 2) * Rt(1) - R2R(1, 0) * t2(2) + R2R(2, 0) * t2(1)); dR(1, 0) = R2R(1, 1) * (R1(1, 2) * t1(2) - R1(2, 2) * t1(1)) - R2R(1, 2) * (R1(1, 1) * t1(2) - R1(2, 1) * t1(1)) + R1(0, 1) * (R2R(1, 0) * Rt(1) - R2R(1, 1) * Rt(0) + R2R(0, 2) * t2(2) - R2R(2, 2) * t2(0)) + R1(0, 2) * (R2R(1, 0) * Rt(2) - R2R(1, 2) * Rt(0) - R2R(0, 1) * t2(2) + R2R(2, 1) * t2(0)); dR(1, 1) = R2R(1, 2) * (R1(1, 0) * t1(2) - R1(2, 0) * t1(1)) - R2R(1, 0) * (R1(1, 2) * t1(2) - R1(2, 2) * t1(1)) - R1(0, 0) * (R2R(1, 0) * Rt(1) - R2R(1, 1) * Rt(0) + R2R(0, 2) * t2(2) - R2R(2, 2) * t2(0)) + R1(0, 2) * (R2R(1, 1) * Rt(2) - R2R(1, 2) * Rt(1) + R2R(0, 0) * t2(2) - R2R(2, 0) * t2(0)); dR(1, 2) = R2R(1, 0) * (R1(1, 1) * t1(2) - R1(2, 1) * t1(1)) - R2R(1, 1) * (R1(1, 0) * t1(2) - R1(2, 0) * t1(1)) - R1(0, 0) * (R2R(1, 0) * Rt(2) - R2R(1, 2) * Rt(0) - R2R(0, 1) * t2(2) + R2R(2, 1) * t2(0)) - R1(0, 1) * (R2R(1, 1) * Rt(2) - R2R(1, 2) * Rt(1) + R2R(0, 0) * t2(2) - R2R(2, 0) * t2(0)); dR(2, 0) = R2R(2, 1) * (R1(1, 2) * t1(2) - R1(2, 2) * t1(1)) - R2R(2, 2) * (R1(1, 1) * t1(2) - R1(2, 1) * t1(1)) + R1(0, 1) * (R2R(2, 0) * Rt(1) - R2R(2, 1) * Rt(0) - R2R(0, 2) * t2(1) + R2R(1, 2) * t2(0)) + R1(0, 2) * (R2R(2, 0) * Rt(2) - R2R(2, 2) * Rt(0) + R2R(0, 1) * t2(1) - R2R(1, 1) * t2(0)); dR(2, 1) = R2R(2, 2) * (R1(1, 0) * t1(2) - R1(2, 0) * t1(1)) - R2R(2, 0) * (R1(1, 2) * t1(2) - R1(2, 2) * t1(1)) - R1(0, 0) * (R2R(2, 0) * Rt(1) - R2R(2, 1) * Rt(0) - R2R(0, 2) * t2(1) + R2R(1, 2) * t2(0)) + R1(0, 2) * (R2R(2, 1) * Rt(2) - R2R(2, 2) * Rt(1) - R2R(0, 0) * t2(1) + R2R(1, 0) * t2(0)); dR(2, 2) = R2R(2, 0) * (R1(1, 1) * t1(2) - R1(2, 1) * t1(1)) - R2R(2, 1) * (R1(1, 0) * t1(2) - R1(2, 0) * t1(1)) - R1(0, 0) * (R2R(2, 0) * Rt(2) - R2R(2, 2) * Rt(0) + R2R(0, 1) * t2(1) - R2R(1, 1) * t2(0)) - R1(0, 1) * (R2R(2, 1) * Rt(2) - R2R(2, 2) * Rt(1) - R2R(0, 0) * t2(1) + R2R(1, 0) * t2(0)); dR(3, 0) = R2R(0, 2) * (R1(0, 1) * t1(2) - R1(2, 1) * t1(0)) - R2R(0, 1) * (R1(0, 2) * t1(2) - R1(2, 2) * t1(0)) + R1(1, 1) * (R2R(0, 0) * Rt(1) - R2R(0, 1) * Rt(0) - R2R(1, 2) * t2(2) + R2R(2, 2) * t2(1)) + R1(1, 2) * (R2R(0, 0) * Rt(2) - R2R(0, 2) * Rt(0) + R2R(1, 1) * t2(2) - R2R(2, 1) * t2(1)); dR(3, 1) = R2R(0, 0) * (R1(0, 2) * t1(2) - R1(2, 2) * t1(0)) - R2R(0, 2) * (R1(0, 0) * t1(2) - R1(2, 0) * t1(0)) - R1(1, 0) * (R2R(0, 0) * Rt(1) - R2R(0, 1) * Rt(0) - R2R(1, 2) * t2(2) + R2R(2, 2) * t2(1)) + R1(1, 2) * (R2R(0, 1) * Rt(2) - R2R(0, 2) * Rt(1) - R2R(1, 0) * t2(2) + R2R(2, 0) * t2(1)); dR(3, 2) = R2R(0, 1) * (R1(0, 0) * t1(2) - R1(2, 0) * t1(0)) - R2R(0, 0) * (R1(0, 1) * t1(2) - R1(2, 1) * t1(0)) - R1(1, 0) * (R2R(0, 0) * Rt(2) - R2R(0, 2) * Rt(0) + R2R(1, 1) * t2(2) - R2R(2, 1) * t2(1)) - R1(1, 1) * (R2R(0, 1) * Rt(2) - R2R(0, 2) * Rt(1) - R2R(1, 0) * t2(2) + R2R(2, 0) * t2(1)); dR(4, 0) = R2R(1, 2) * (R1(0, 1) * t1(2) - R1(2, 1) * t1(0)) - R2R(1, 1) * (R1(0, 2) * t1(2) - R1(2, 2) * t1(0)) + R1(1, 1) * (R2R(1, 0) * Rt(1) - R2R(1, 1) * Rt(0) + R2R(0, 2) * t2(2) - R2R(2, 2) * t2(0)) + R1(1, 2) * (R2R(1, 0) * Rt(2) - R2R(1, 2) * Rt(0) - R2R(0, 1) * t2(2) + R2R(2, 1) * t2(0)); dR(4, 1) = R2R(1, 0) * (R1(0, 2) * t1(2) - R1(2, 2) * t1(0)) - R2R(1, 2) * (R1(0, 0) * t1(2) - R1(2, 0) * t1(0)) - R1(1, 0) * (R2R(1, 0) * Rt(1) - R2R(1, 1) * Rt(0) + R2R(0, 2) * t2(2) - R2R(2, 2) * t2(0)) + R1(1, 2) * (R2R(1, 1) * Rt(2) - R2R(1, 2) * Rt(1) + R2R(0, 0) * t2(2) - R2R(2, 0) * t2(0)); dR(4, 2) = R2R(1, 1) * (R1(0, 0) * t1(2) - R1(2, 0) * t1(0)) - R2R(1, 0) * (R1(0, 1) * t1(2) - R1(2, 1) * t1(0)) - R1(1, 0) * (R2R(1, 0) * Rt(2) - R2R(1, 2) * Rt(0) - R2R(0, 1) * t2(2) + R2R(2, 1) * t2(0)) - R1(1, 1) * (R2R(1, 1) * Rt(2) - R2R(1, 2) * Rt(1) + R2R(0, 0) * t2(2) - R2R(2, 0) * t2(0)); dR(5, 0) = R2R(2, 2) * (R1(0, 1) * t1(2) - R1(2, 1) * t1(0)) - R2R(2, 1) * (R1(0, 2) * t1(2) - R1(2, 2) * t1(0)) + R1(1, 1) * (R2R(2, 0) * Rt(1) - R2R(2, 1) * Rt(0) - R2R(0, 2) * t2(1) + R2R(1, 2) * t2(0)) + R1(1, 2) * (R2R(2, 0) * Rt(2) - R2R(2, 2) * Rt(0) + R2R(0, 1) * t2(1) - R2R(1, 1) * t2(0)); dR(5, 1) = R2R(2, 0) * (R1(0, 2) * t1(2) - R1(2, 2) * t1(0)) - R2R(2, 2) * (R1(0, 0) * t1(2) - R1(2, 0) * t1(0)) - R1(1, 0) * (R2R(2, 0) * Rt(1) - R2R(2, 1) * Rt(0) - R2R(0, 2) * t2(1) + R2R(1, 2) * t2(0)) + R1(1, 2) * (R2R(2, 1) * Rt(2) - R2R(2, 2) * Rt(1) - R2R(0, 0) * t2(1) + R2R(1, 0) * t2(0)); dR(5, 2) = R2R(2, 1) * (R1(0, 0) * t1(2) - R1(2, 0) * t1(0)) - R2R(2, 0) * (R1(0, 1) * t1(2) - R1(2, 1) * t1(0)) - R1(1, 0) * (R2R(2, 0) * Rt(2) - R2R(2, 2) * Rt(0) + R2R(0, 1) * t2(1) - R2R(1, 1) * t2(0)) - R1(1, 1) * (R2R(2, 1) * Rt(2) - R2R(2, 2) * Rt(1) - R2R(0, 0) * t2(1) + R2R(1, 0) * t2(0)); dR(6, 0) = R2R(0, 1) * (R1(0, 2) * t1(1) - R1(1, 2) * t1(0)) - R2R(0, 2) * (R1(0, 1) * t1(1) - R1(1, 1) * t1(0)) + R1(2, 1) * (R2R(0, 0) * Rt(1) - R2R(0, 1) * Rt(0) - R2R(1, 2) * t2(2) + R2R(2, 2) * t2(1)) + R1(2, 2) * (R2R(0, 0) * Rt(2) - R2R(0, 2) * Rt(0) + R2R(1, 1) * t2(2) - R2R(2, 1) * t2(1)); dR(6, 1) = R2R(0, 2) * (R1(0, 0) * t1(1) - R1(1, 0) * t1(0)) - R2R(0, 0) * (R1(0, 2) * t1(1) - R1(1, 2) * t1(0)) - R1(2, 0) * (R2R(0, 0) * Rt(1) - R2R(0, 1) * Rt(0) - R2R(1, 2) * t2(2) + R2R(2, 2) * t2(1)) + R1(2, 2) * (R2R(0, 1) * Rt(2) - R2R(0, 2) * Rt(1) - R2R(1, 0) * t2(2) + R2R(2, 0) * t2(1)); dR(6, 2) = R2R(0, 0) * (R1(0, 1) * t1(1) - R1(1, 1) * t1(0)) - R2R(0, 1) * (R1(0, 0) * t1(1) - R1(1, 0) * t1(0)) - R1(2, 0) * (R2R(0, 0) * Rt(2) - R2R(0, 2) * Rt(0) + R2R(1, 1) * t2(2) - R2R(2, 1) * t2(1)) - R1(2, 1) * (R2R(0, 1) * Rt(2) - R2R(0, 2) * Rt(1) - R2R(1, 0) * t2(2) + R2R(2, 0) * t2(1)); dR(7, 0) = R2R(1, 1) * (R1(0, 2) * t1(1) - R1(1, 2) * t1(0)) - R2R(1, 2) * (R1(0, 1) * t1(1) - R1(1, 1) * t1(0)) + R1(2, 1) * (R2R(1, 0) * Rt(1) - R2R(1, 1) * Rt(0) + R2R(0, 2) * t2(2) - R2R(2, 2) * t2(0)) + R1(2, 2) * (R2R(1, 0) * Rt(2) - R2R(1, 2) * Rt(0) - R2R(0, 1) * t2(2) + R2R(2, 1) * t2(0)); dR(7, 1) = R2R(1, 2) * (R1(0, 0) * t1(1) - R1(1, 0) * t1(0)) - R2R(1, 0) * (R1(0, 2) * t1(1) - R1(1, 2) * t1(0)) - R1(2, 0) * (R2R(1, 0) * Rt(1) - R2R(1, 1) * Rt(0) + R2R(0, 2) * t2(2) - R2R(2, 2) * t2(0)) + R1(2, 2) * (R2R(1, 1) * Rt(2) - R2R(1, 2) * Rt(1) + R2R(0, 0) * t2(2) - R2R(2, 0) * t2(0)); dR(7, 2) = R2R(1, 0) * (R1(0, 1) * t1(1) - R1(1, 1) * t1(0)) - R2R(1, 1) * (R1(0, 0) * t1(1) - R1(1, 0) * t1(0)) - R1(2, 0) * (R2R(1, 0) * Rt(2) - R2R(1, 2) * Rt(0) - R2R(0, 1) * t2(2) + R2R(2, 1) * t2(0)) - R1(2, 1) * (R2R(1, 1) * Rt(2) - R2R(1, 2) * Rt(1) + R2R(0, 0) * t2(2) - R2R(2, 0) * t2(0)); dR(8, 0) = R2R(2, 1) * (R1(0, 2) * t1(1) - R1(1, 2) * t1(0)) - R2R(2, 2) * (R1(0, 1) * t1(1) - R1(1, 1) * t1(0)) + R1(2, 1) * (R2R(2, 0) * Rt(1) - R2R(2, 1) * Rt(0) - R2R(0, 2) * t2(1) + R2R(1, 2) * t2(0)) + R1(2, 2) * (R2R(2, 0) * Rt(2) - R2R(2, 2) * Rt(0) + R2R(0, 1) * t2(1) - R2R(1, 1) * t2(0)); dR(8, 1) = R2R(2, 2) * (R1(0, 0) * t1(1) - R1(1, 0) * t1(0)) - R2R(2, 0) * (R1(0, 2) * t1(1) - R1(1, 2) * t1(0)) - R1(2, 0) * (R2R(2, 0) * Rt(1) - R2R(2, 1) * Rt(0) - R2R(0, 2) * t2(1) + R2R(1, 2) * t2(0)) + R1(2, 2) * (R2R(2, 1) * Rt(2) - R2R(2, 2) * Rt(1) - R2R(0, 0) * t2(1) + R2R(1, 0) * t2(0)); dR(8, 2) = R2R(2, 0) * (R1(0, 1) * t1(1) - R1(1, 1) * t1(0)) - R2R(2, 1) * (R1(0, 0) * t1(1) - R1(1, 0) * t1(0)) - R1(2, 0) * (R2R(2, 0) * Rt(2) - R2R(2, 2) * Rt(0) + R2R(0, 1) * t2(1) - R2R(1, 1) * t2(0)) - R1(2, 1) * (R2R(2, 1) * Rt(2) - R2R(2, 2) * Rt(1) - R2R(0, 0) * t2(1) + R2R(1, 0) * t2(0)); dt(0, 0) = R2R(0, 2) * R1(0, 1) - R2R(0, 1) * R1(0, 2); dt(0, 1) = R2R(0, 0) * R1(0, 2) - R2R(0, 2) * R1(0, 0); dt(0, 2) = R2R(0, 1) * R1(0, 0) - R2R(0, 0) * R1(0, 1); dt(1, 0) = R2R(1, 2) * R1(0, 1) - R2R(1, 1) * R1(0, 2); dt(1, 1) = R2R(1, 0) * R1(0, 2) - R2R(1, 2) * R1(0, 0); dt(1, 2) = R2R(1, 1) * R1(0, 0) - R2R(1, 0) * R1(0, 1); dt(2, 0) = R2R(2, 2) * R1(0, 1) - R2R(2, 1) * R1(0, 2); dt(2, 1) = R2R(2, 0) * R1(0, 2) - R2R(2, 2) * R1(0, 0); dt(2, 2) = R2R(2, 1) * R1(0, 0) - R2R(2, 0) * R1(0, 1); dt(3, 0) = R2R(0, 2) * R1(1, 1) - R2R(0, 1) * R1(1, 2); dt(3, 1) = R2R(0, 0) * R1(1, 2) - R2R(0, 2) * R1(1, 0); dt(3, 2) = R2R(0, 1) * R1(1, 0) - R2R(0, 0) * R1(1, 1); dt(4, 0) = R2R(1, 2) * R1(1, 1) - R2R(1, 1) * R1(1, 2); dt(4, 1) = R2R(1, 0) * R1(1, 2) - R2R(1, 2) * R1(1, 0); dt(4, 2) = R2R(1, 1) * R1(1, 0) - R2R(1, 0) * R1(1, 1); dt(5, 0) = R2R(2, 2) * R1(1, 1) - R2R(2, 1) * R1(1, 2); dt(5, 1) = R2R(2, 0) * R1(1, 2) - R2R(2, 2) * R1(1, 0); dt(5, 2) = R2R(2, 1) * R1(1, 0) - R2R(2, 0) * R1(1, 1); dt(6, 0) = R2R(0, 2) * R1(2, 1) - R2R(0, 1) * R1(2, 2); dt(6, 1) = R2R(0, 0) * R1(2, 2) - R2R(0, 2) * R1(2, 0); dt(6, 2) = R2R(0, 1) * R1(2, 0) - R2R(0, 0) * R1(2, 1); dt(7, 0) = R2R(1, 2) * R1(2, 1) - R2R(1, 1) * R1(2, 2); dt(7, 1) = R2R(1, 0) * R1(2, 2) - R2R(1, 2) * R1(2, 0); dt(7, 2) = R2R(1, 1) * R1(2, 0) - R2R(1, 0) * R1(2, 1); dt(8, 0) = R2R(2, 2) * R1(2, 1) - R2R(2, 1) * R1(2, 2); dt(8, 1) = R2R(2, 0) * R1(2, 2) - R2R(2, 2) * R1(2, 0); dt(8, 2) = R2R(2, 1) * R1(2, 0) - R2R(2, 0) * R1(2, 1); for (size_t k = 0; k < m.x1.size(); ++k) { double C = m.x2[k].homogeneous().dot(E * m.x1[k].homogeneous()); // J_C is the Jacobian of the epipolar constraint w.r.t. the image points Eigen::Vector4d J_C; J_C << E.block<3, 2>(0, 0).transpose() * m.x2[k].homogeneous(), E.block<2, 3>(0, 0) * m.x1[k].homogeneous(); const double nJ_C = J_C.norm(); const double inv_nJ_C = 1.0 / nJ_C; const double r = C * inv_nJ_C; // Compute weight from robust loss function (used in the IRLS) const double weight = weights[match_k][k] * loss_fn.weight(r * r); if (weight == 0.0) { continue; } num_residuals++; // Compute Jacobian of Sampson error w.r.t the fundamental/essential matrix (3x3) Eigen::Matrix dF; dF << m.x1[k](0) * m.x2[k](0), m.x1[k](0) * m.x2[k](1), m.x1[k](0), m.x1[k](1) * m.x2[k](0), m.x1[k](1) * m.x2[k](1), m.x1[k](1), m.x2[k](0), m.x2[k](1), 1.0; const double s = C * inv_nJ_C * inv_nJ_C; dF(0) -= s * (J_C(2) * m.x1[k](0) + J_C(0) * m.x2[k](0)); dF(1) -= s * (J_C(3) * m.x1[k](0) + J_C(0) * m.x2[k](1)); dF(2) -= s * (J_C(0)); dF(3) -= s * (J_C(2) * m.x1[k](1) + J_C(1) * m.x2[k](0)); dF(4) -= s * (J_C(3) * m.x1[k](1) + J_C(1) * m.x2[k](1)); dF(5) -= s * (J_C(1)); dF(6) -= s * (J_C(2)); dF(7) -= s * (J_C(3)); dF *= inv_nJ_C; // and then w.r.t. the pose parameters Eigen::Matrix J; J.block<1, 3>(0, 0) = dF * dR; J.block<1, 3>(0, 3) = dF * dt; // Accumulate into JtJ and Jtr Jtr += weight * C * inv_nJ_C * J.transpose(); for (size_t i = 0; i < 6; ++i) { for (size_t j = 0; j <= i; ++j) { JtJ(i, j) += weight * (J(i) * J(j)); } } } } return num_residuals; } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); pose_new.t = pose.t + pose.rotate(dp.block<3, 1>(3, 0)); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 6; private: const std::vector &matches; const std::vector &rig1_poses; const std::vector &rig2_poses; const LossFunction &loss_fn; const ResidualWeightVectors &weights; }; template class HybridPoseJacobianAccumulator { public: HybridPoseJacobianAccumulator(const std::vector &points2D, const std::vector &points3D, const std::vector &pairwise_matches, const std::vector &map_ext, const LossFunction &l, const LossFunction &l_epi, const AbsResidualsVector &weights_abs = AbsResidualsVector(), const RelResidualsVectors &weights_rel = RelResidualsVectors()) : abs_pose_accum(points2D, points3D, trivial_camera, l, weights_abs), gen_rel_accum(pairwise_matches, map_ext, trivial_rig, l_epi, weights_rel) { trivial_camera.model_id = NullCameraModel::model_id; trivial_rig.emplace_back(); } double residual(const CameraPose &pose) const { return abs_pose_accum.residual(pose) + gen_rel_accum.residual(pose); } size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { return abs_pose_accum.accumulate(pose, JtJ, Jtr) + gen_rel_accum.accumulate(pose, JtJ, Jtr); } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; pose_new.q = quat_step_post(pose.q, dp.block<3, 1>(0, 0)); pose_new.t = pose.t + pose.rotate(dp.block<3, 1>(3, 0)); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 6; private: Camera trivial_camera; std::vector trivial_rig; CameraJacobianAccumulator abs_pose_accum; GeneralizedRelativePoseJacobianAccumulator gen_rel_accum; }; // This is the SVD factorization proposed by Bartoli and Sturm in // Non-Linear Estimation of the Fundamental Matrix With Minimal Parameters, PAMI 2004 // Though we do different updates (lie vs the euler angles used in the original paper) struct FactorizedFundamentalMatrix { FactorizedFundamentalMatrix() {} FactorizedFundamentalMatrix(const Eigen::Matrix3d &F) { Eigen::JacobiSVD svd(F, Eigen::ComputeFullV | Eigen::ComputeFullU); Eigen::Matrix3d U = svd.matrixU(); Eigen::Matrix3d V = svd.matrixV(); if (U.determinant() < 0) { U = -U; } if (V.determinant() < 0) { V = -V; } qU = rotmat_to_quat(U); qV = rotmat_to_quat(V); Eigen::Vector3d s = svd.singularValues(); sigma = s(1) / s(0); } Eigen::Matrix3d F() const { Eigen::Matrix3d U = quat_to_rotmat(qU); Eigen::Matrix3d V = quat_to_rotmat(qV); return U.col(0) * V.col(0).transpose() + sigma * U.col(1) * V.col(1).transpose(); } Eigen::Vector4d qU, qV; double sigma; }; template class FundamentalJacobianAccumulator { public: FundamentalJacobianAccumulator(const std::vector &points2D_1, const std::vector &points2D_2, const LossFunction &l, const ResidualWeightVector &w = ResidualWeightVector()) : x1(points2D_1), x2(points2D_2), loss_fn(l), weights(w) {} double residual(const FactorizedFundamentalMatrix &FF) const { Eigen::Matrix3d F = FF.F(); double cost = 0.0; for (size_t k = 0; k < x1.size(); ++k) { double C = x2[k].homogeneous().dot(F * x1[k].homogeneous()); double nJc_sq = (F.block<2, 3>(0, 0) * x1[k].homogeneous()).squaredNorm() + (F.block<3, 2>(0, 0).transpose() * x2[k].homogeneous()).squaredNorm(); double r2 = (C * C) / nJc_sq; cost += weights[k] * loss_fn.loss(r2); } return cost; } size_t accumulate(const FactorizedFundamentalMatrix &FF, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { const Eigen::Matrix3d F = FF.F(); // Matrices contain the jacobians of F w.r.t. the factorized fundamental matrix (U,V,sigma) const Eigen::Matrix3d U = quat_to_rotmat(FF.qU); const Eigen::Matrix3d V = quat_to_rotmat(FF.qV); const Eigen::Matrix3d d_sigma = U.col(1) * V.col(1).transpose(); Eigen::Matrix dF_dparams; dF_dparams << 0, F(2, 0), -F(1, 0), 0, F(0, 2), -F(0, 1), d_sigma(0, 0), -F(2, 0), 0, F(0, 0), 0, F(1, 2), -F(1, 1), d_sigma(1, 0), F(1, 0), -F(0, 0), 0, 0, F(2, 2), -F(2, 1), d_sigma(2, 0), 0, F(2, 1), -F(1, 1), -F(0, 2), 0, F(0, 0), d_sigma(0, 1), -F(2, 1), 0, F(0, 1), -F(1, 2), 0, F(1, 0), d_sigma(1, 1), F(1, 1), -F(0, 1), 0, -F(2, 2), 0, F(2, 0), d_sigma(2, 1), 0, F(2, 2), -F(1, 2), F(0, 1), -F(0, 0), 0, d_sigma(0, 2), -F(2, 2), 0, F(0, 2), F(1, 1), -F(1, 0), 0, d_sigma(1, 2), F(1, 2), -F(0, 2), 0, F(2, 1), -F(2, 0), 0, d_sigma(2, 2); size_t num_residuals = 0; for (size_t k = 0; k < x1.size(); ++k) { const double C = x2[k].homogeneous().dot(F * x1[k].homogeneous()); // J_C is the Jacobian of the epipolar constraint w.r.t. the image points Eigen::Vector4d J_C; J_C << F.block<3, 2>(0, 0).transpose() * x2[k].homogeneous(), F.block<2, 3>(0, 0) * x1[k].homogeneous(); const double nJ_C = J_C.norm(); const double inv_nJ_C = 1.0 / nJ_C; const double r = C * inv_nJ_C; // Compute weight from robust loss function (used in the IRLS) const double weight = weights[k] * loss_fn.weight(r * r); if (weight == 0.0) { continue; } num_residuals++; // Compute Jacobian of Sampson error w.r.t the fundamental/essential matrix (3x3) Eigen::Matrix dF; dF << x1[k](0) * x2[k](0), x1[k](0) * x2[k](1), x1[k](0), x1[k](1) * x2[k](0), x1[k](1) * x2[k](1), x1[k](1), x2[k](0), x2[k](1), 1.0; const double s = C * inv_nJ_C * inv_nJ_C; dF(0) -= s * (J_C(2) * x1[k](0) + J_C(0) * x2[k](0)); dF(1) -= s * (J_C(3) * x1[k](0) + J_C(0) * x2[k](1)); dF(2) -= s * (J_C(0)); dF(3) -= s * (J_C(2) * x1[k](1) + J_C(1) * x2[k](0)); dF(4) -= s * (J_C(3) * x1[k](1) + J_C(1) * x2[k](1)); dF(5) -= s * (J_C(1)); dF(6) -= s * (J_C(2)); dF(7) -= s * (J_C(3)); dF *= inv_nJ_C; // and then w.r.t. the pose parameters (rotation + tangent basis for translation) Eigen::Matrix J = dF * dF_dparams; // Accumulate into JtJ and Jtr Jtr += weight * C * inv_nJ_C * J.transpose(); for (size_t i = 0; i < 7; ++i) { for (size_t j = 0; j <= i; ++j) { JtJ(i, j) += weight * (J(i) * J(j)); } } } return num_residuals; } FactorizedFundamentalMatrix step(Eigen::Matrix dp, const FactorizedFundamentalMatrix &F) const { FactorizedFundamentalMatrix F_new; F_new.qU = quat_step_pre(F.qU, dp.block<3, 1>(0, 0)); F_new.qV = quat_step_pre(F.qV, dp.block<3, 1>(3, 0)); F_new.sigma = F.sigma + dp(6); return F_new; } typedef FactorizedFundamentalMatrix param_t; static constexpr size_t num_params = 7; private: const std::vector &x1; const std::vector &x2; const LossFunction &loss_fn; const ResidualWeightVector &weights; }; // Non-linear refinement of symmetric transfer error |x2 - pi(H*x1)|^2 + |x1 - pi(inv(H)*x2)|^2 // Code is Based on the original single-side transfer error by Viktor Larsson. Implementations of other // parameterizations (different affine patches, linear least squares, SVD as in Bartoli/Sturm, etc) can be found at // https://github.com/vlarsson/homopt // Use adjugate of H to formulate inv(H) since the transfer error is independent of the scale. // Consider H(2,2) as a constant (not necessary to be 1), we only update the first 8 elements of H. // Author: Yaqing Ding template class HomographyJacobianAccumulator { public: HomographyJacobianAccumulator(const std::vector &points2D_1, const std::vector &points2D_2, const LossFunction &l, const ResidualWeightVector &w = ResidualWeightVector()) : x1(points2D_1), x2(points2D_2), loss_fn(l), weights(w) {} double residual(const Eigen::Matrix3d &H) const { double cost = 0.0; const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); const double H2_0 = H(2, 0), H2_1 = H(2, 1), H2_2 = H(2, 2); const Eigen::Matrix3d G = adjugate(H); const double G0_0 = G(0, 0), G0_1 = G(0, 1), G0_2 = G(0, 2); const double G1_0 = G(1, 0), G1_1 = G(1, 1), G1_2 = G(1, 2); const double G2_0 = G(2, 0), G2_1 = G(2, 1), G2_2 = G(2, 2); for (size_t k = 0; k < x1.size(); ++k) { // Forward error: |x2 - pi(H*x1)|^2 const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Hx1_0 = H0_0 * x1_0 + H0_1 * x1_1 + H0_2; const double Hx1_1 = H1_0 * x1_0 + H1_1 * x1_1 + H1_2; const double inv_Hx1_2 = 1.0 / (H2_0 * x1_0 + H2_1 * x1_1 + H2_2); const double r0 = Hx1_0 * inv_Hx1_2 - x2_0; const double r1 = Hx1_1 * inv_Hx1_2 - x2_1; const double r2 = r0 * r0 + r1 * r1; // Backward error: |x1 - pi(G*x2)|^2 const double Gx2_0 = G0_0 * x2_0 + G0_1 * x2_1 + G0_2; const double Gx2_1 = G1_0 * x2_0 + G1_1 * x2_1 + G1_2; const double inv_Gx2_2 = 1.0 / (G2_0 * x2_0 + G2_1 * x2_1 + G2_2); const double s0 = Gx2_0 * inv_Gx2_2 - x1_0; const double s1 = Gx2_1 * inv_Gx2_2 - x1_1; const double s2 = s0 * s0 + s1 * s1; cost += weights[k] * (loss_fn.loss(r2) + loss_fn.loss(s2)); } return cost; } size_t accumulate(const Eigen::Matrix3d &H, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) { Eigen::Matrix dH; const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); const double H2_0 = H(2, 0), H2_1 = H(2, 1), H2_2 = H(2, 2); const Eigen::Matrix3d G = adjugate(H); const double G0_0 = G(0, 0), G0_1 = G(0, 1), G0_2 = G(0, 2); const double G1_0 = G(1, 0), G1_1 = G(1, 1), G1_2 = G(1, 2); const double G2_0 = G(2, 0), G2_1 = G(2, 1), G2_2 = G(2, 2); size_t num_residuals = 0; for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); // Forward error const double Hx1_0 = H0_0 * x1_0 + H0_1 * x1_1 + H0_2; const double Hx1_1 = H1_0 * x1_0 + H1_1 * x1_1 + H1_2; const double inv_Hx1_2 = 1.0 / (H2_0 * x1_0 + H2_1 * x1_1 + H2_2); const double z0 = Hx1_0 * inv_Hx1_2; const double z1 = Hx1_1 * inv_Hx1_2; const double r0 = z0 - x2_0; const double r1 = z1 - x2_1; const double r2 = r0 * r0 + r1 * r1; // Compute weight from robust loss function (used in the IRLS) const double weight = weights[k] * loss_fn.weight(r2); if (weight != 0.0) { dH << x1_0, 0.0, -x1_0 * z0, x1_1, 0.0, -x1_1 * z0, 1.0, 0.0, // -z0, 0.0, x1_0, -x1_0 * z1, 0.0, x1_1, -x1_1 * z1, 0.0, 1.0; // -z1, dH = dH * inv_Hx1_2; // accumulate into JtJ and Jtr Jtr += dH.transpose() * (weight * Eigen::Vector2d(r0, r1)); for (size_t i = 0; i < 8; ++i) { for (size_t j = 0; j <= i; ++j) { JtJ(i, j) += weight * dH.col(i).dot(dH.col(j)); } } num_residuals++; } const double Gx2_0 = G0_0 * x2_0 + G0_1 * x2_1 + G0_2; const double Gx2_1 = G1_0 * x2_0 + G1_1 * x2_1 + G1_2; const double inv_Gx2_2 = 1.0 / (G2_0 * x2_0 + G2_1 * x2_1 + G2_2); const double y0 = Gx2_0 * inv_Gx2_2; const double y1 = Gx2_1 * inv_Gx2_2; const double s0 = y0 - x1_0; const double s1 = y1 - x1_1; const double s2 = s0 * s0 + s1 * s1; const double y0x2_1 = y0 * x2_1; const double y0x2_0 = y0 * x2_0; const double y1x2_1 = y1 * x2_1; const double y1x2_0 = y1 * x2_0; // Compute weight from robust loss function (used in the IRLS) const double weightg = weights[k] * loss_fn.weight(s2); if (weightg != 0.0) { Eigen::Matrix dH_backward; dH_backward << H2_1 * y0x2_1 - H1_1 * y0, H0_1 * y0 - H2_1 * y0x2_0, H1_1 * y0x2_0 - H0_1 * y0x2_1, H1_2 - H2_2 * x2_1 + H1_0 * y0 - H2_0 * y0x2_1, H2_2 * x2_0 - H0_2 - H0_0 * y0 + H2_0 * y0x2_0, H0_2 * x2_1 - H1_2 * x2_0 + H0_0 * y0x2_1 - H1_0 * y0x2_0, H2_1 * x2_1 - H1_1, H0_1 - H2_1 * x2_0, H2_2 * x2_1 - H1_2 - H1_1 * y1 + H2_1 * y1x2_1, H0_2 - H2_2 * x2_0 + H0_1 * y1 - H2_1 * y1x2_0, H1_2 * x2_0 - H0_2 * x2_1 - H0_1 * y1x2_1 + H1_1 * y1x2_0, H1_0 * y1 - H2_0 * y1x2_1, H2_0 * y1x2_0 - H0_0 * y1, H0_0 * y1x2_1 - H1_0 * y1x2_0, H1_0 - H2_0 * x2_1, H2_0 * x2_0 - H0_0; dH_backward = dH_backward * inv_Gx2_2; // Accumulate backward error Jtr += dH_backward.transpose() * (weightg * Eigen::Vector2d(s0, s1)); for (size_t i = 0; i < 8; ++i) { for (size_t j = 0; j <= i; ++j) { JtJ(i, j) += weightg * dH_backward.col(i).dot(dH_backward.col(j)); } } num_residuals++; } } return num_residuals; } Eigen::Matrix3d step(Eigen::Matrix dp, const Eigen::Matrix3d &H) const { Eigen::Matrix3d H_new = H; Eigen::Map>(H_new.data()) += dp; return H_new; } typedef Eigen::Matrix3d param_t; static constexpr size_t num_params = 8; private: Eigen::Matrix3d adjugate(const Eigen::Matrix3d &H) const { Eigen::Matrix3d adj; adj(0, 0) = H(1, 1) * H(2, 2) - H(1, 2) * H(2, 1); adj(0, 1) = H(0, 2) * H(2, 1) - H(0, 1) * H(2, 2); adj(0, 2) = H(0, 1) * H(1, 2) - H(0, 2) * H(1, 1); adj(1, 0) = H(1, 2) * H(2, 0) - H(1, 0) * H(2, 2); adj(1, 1) = H(0, 0) * H(2, 2) - H(0, 2) * H(2, 0); adj(1, 2) = H(0, 2) * H(1, 0) - H(0, 0) * H(1, 2); adj(2, 0) = H(1, 0) * H(2, 1) - H(1, 1) * H(2, 0); adj(2, 1) = H(0, 1) * H(2, 0) - H(0, 0) * H(2, 1); adj(2, 2) = H(0, 0) * H(1, 1) - H(0, 1) * H(1, 0); return adj; } const std::vector &x1, &x2; const LossFunction &loss_fn; const ResidualWeightVector &weights; }; template class Radial1DJacobianAccumulator { public: Radial1DJacobianAccumulator(const std::vector &points2D, const std::vector &points3D, const LossFunction &l, const ResidualWeightVector &w = ResidualWeightVector()) : x(points2D), X(points3D), loss_fn(l), weights(w) {} double residual(const CameraPose &pose) const { double cost = 0.0; Eigen::Matrix3d R = pose.R(); for (size_t k = 0; k < x.size(); ++k) { Eigen::Vector2d z = (R * X[k] + pose.t).template topRows<2>().normalized(); double alpha = z.dot(x[k]); // This assumes points will not cross the half-space during optimization if (alpha < 0) continue; double r2 = (alpha * z - x[k]).squaredNorm(); cost += weights[k] * loss_fn.loss(r2); } return cost; } size_t accumulate(const CameraPose &pose, Eigen::Matrix &JtJ, Eigen::Matrix &Jtr) const { Eigen::Matrix3d R = pose.R(); size_t num_residuals = 0; for (size_t k = 0; k < x.size(); ++k) { Eigen::Vector3d RX = R * X[k]; const Eigen::Vector2d z = (RX + pose.t).topRows<2>(); const double n_z = z.norm(); const Eigen::Vector2d zh = z / n_z; const double alpha = zh.dot(x[k]); // This assumes points will not cross the half-space during optimization if (alpha < 0) continue; // Setup residual Eigen::Vector2d r = alpha * zh - x[k]; const double r_squared = r.squaredNorm(); const double weight = weights[k] * loss_fn.weight(r_squared); if (weight == 0.0) { continue; } num_residuals++; // differentiate residual with respect to z Eigen::Matrix2d dr_dz = (zh * x[k].transpose() + alpha * Eigen::Matrix2d::Identity()) * (Eigen::Matrix2d::Identity() - zh * zh.transpose()) / n_z; Eigen::Matrix dz; dz << 0.0, RX(2), -RX(1), 1.0, 0.0, -RX(2), 0.0, RX(0), 0.0, 1.0; Eigen::Matrix J = dr_dz * dz; // Accumulate into JtJ and Jtr Jtr += weight * J.transpose() * r; for (size_t i = 0; i < 5; ++i) { for (size_t j = 0; j <= i; ++j) { JtJ(i, j) += weight * (J.col(i).dot(J.col(j))); } } } return num_residuals; } CameraPose step(Eigen::Matrix dp, const CameraPose &pose) const { CameraPose pose_new; pose_new.q = quat_step_pre(pose.q, dp.block<3, 1>(0, 0)); pose_new.t(0) = pose.t(0) + dp(3); pose_new.t(1) = pose.t(1) + dp(4); return pose_new; } typedef CameraPose param_t; static constexpr size_t num_params = 5; private: const std::vector &x; const std::vector &X; const LossFunction &loss_fn; const ResidualWeightVector &weights; }; } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/lm_impl.h000066400000000000000000000105741504452766400175540ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_LM_IMPL_ #define POSELIB_ROBUST_LM_IMPL_ #include "PoseLib/types.h" namespace poselib { /* Templated implementation of Levenberg-Marquadt. The Problem class must provide Problem::num_params - number of parameters to optimize over Problem::params_t - type for the parameters which optimize over Problem::accumulate(param, JtJ, Jtr) - compute jacobians Problem::residual(param) - compute the current residuals Problem::step(delta_params, param) - take a step in parameter space Check jacobian_impl.h for examples */ typedef std::function IterationCallback; template BundleStats lm_impl(Problem &problem, Param *parameters, const BundleOptions &opt, IterationCallback callback = nullptr) { constexpr int n_params = Problem::num_params; Eigen::Matrix JtJ; Eigen::Matrix Jtr; // Initialize BundleStats stats; stats.cost = problem.residual(*parameters); stats.initial_cost = stats.cost; stats.grad_norm = -1; stats.step_norm = -1; stats.invalid_steps = 0; stats.lambda = opt.initial_lambda; bool recompute_jac = true; for (stats.iterations = 0; stats.iterations < opt.max_iterations; ++stats.iterations) { // We only recompute jacobian and residual vector if last step was successful if (recompute_jac) { JtJ.setZero(); Jtr.setZero(); problem.accumulate(*parameters, JtJ, Jtr); stats.grad_norm = Jtr.norm(); if (stats.grad_norm < opt.gradient_tol) { break; } } // Add dampening for (size_t k = 0; k < n_params; ++k) { JtJ(k, k) += stats.lambda; } Eigen::Matrix sol = -JtJ.template selfadjointView().llt().solve(Jtr); stats.step_norm = sol.norm(); if (stats.step_norm < opt.step_tol) { break; } Param parameters_new = problem.step(sol, *parameters); double cost_new = problem.residual(parameters_new); if (cost_new < stats.cost) { *parameters = parameters_new; stats.lambda = std::max(opt.min_lambda, stats.lambda / 10); stats.cost = cost_new; recompute_jac = true; } else { stats.invalid_steps++; // Remove dampening for (size_t k = 0; k < n_params; ++k) { JtJ(k, k) -= stats.lambda; } stats.lambda = std::min(opt.max_lambda, stats.lambda * 10); recompute_jac = false; } if (callback != nullptr) { callback(stats); } } return stats; } } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/ransac.cc000066400000000000000000000256061504452766400175320ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ransac.h" #include "PoseLib/misc/essential.h" #include "PoseLib/robust/bundle.h" #include "PoseLib/robust/estimators/absolute_pose.h" #include "PoseLib/robust/estimators/homography.h" #include "PoseLib/robust/estimators/hybrid_pose.h" #include "PoseLib/robust/estimators/relative_pose.h" #include "PoseLib/solvers/gen_relpose_5p1pt.h" #include "PoseLib/solvers/p3p.h" #include "PoseLib/solvers/relpose_5pt.h" #include "ransac_impl.h" namespace poselib { RansacStats ransac_pnp(const std::vector &x, const std::vector &X, const RansacOptions &opt, CameraPose *best_model, std::vector *best_inliers) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } AbsolutePoseEstimator estimator(opt, x, X); RansacStats stats = ransac(estimator, opt, best_model); get_inliers(*best_model, x, X, opt.max_reproj_error * opt.max_reproj_error, best_inliers); return stats; } RansacStats ransac_gen_pnp(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, const RansacOptions &opt, CameraPose *best_model, std::vector> *best_inliers) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } GeneralizedAbsolutePoseEstimator estimator(opt, x, X, camera_ext); RansacStats stats = ransac(estimator, opt, best_model); best_inliers->resize(camera_ext.size()); for (size_t k = 0; k < camera_ext.size(); ++k) { CameraPose full_pose; full_pose.q = quat_multiply(camera_ext[k].q, best_model->q); full_pose.t = camera_ext[k].rotate(best_model->t) + camera_ext[k].t; get_inliers(full_pose, x[k], X[k], opt.max_reproj_error * opt.max_reproj_error, &(*best_inliers)[k]); } return stats; } RansacStats ransac_pnpl(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, const RansacOptions &opt, CameraPose *best_model, std::vector *inliers_points, std::vector *inliers_lines) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } AbsolutePosePointLineEstimator estimator(opt, points2D, points3D, lines2D, lines3D); RansacStats stats = ransac(estimator, opt, best_model); get_inliers(*best_model, points2D, points3D, opt.max_reproj_error * opt.max_reproj_error, inliers_points); get_inliers(*best_model, lines2D, lines3D, opt.max_epipolar_error * opt.max_epipolar_error, inliers_lines); return stats; } RansacStats ransac_relpose(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, CameraPose *best_model, std::vector *best_inliers) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } RelativePoseEstimator estimator(opt, x1, x2); RansacStats stats = ransac(estimator, opt, best_model); get_inliers(*best_model, x1, x2, opt.max_epipolar_error * opt.max_epipolar_error, best_inliers); return stats; } RansacStats ransac_shared_focal_relpose(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, ImagePair *best_model, std::vector *best_inliers) { if (!opt.score_initial_model) { best_model->pose.q << 1.0, 0.0, 0.0, 0.0; best_model->pose.t.setZero(); best_model->camera1 = Camera("SIMPLE_PINHOLE", std::vector{1.0, 0.0, 0.0}, -1, -1); best_model->camera2 = best_model->camera1; } SharedFocalRelativePoseEstimator estimator(opt, x1, x2); RansacStats stats = ransac(estimator, opt, best_model); Eigen::Matrix3d K_inv; K_inv << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, best_model->camera1.focal(); Eigen::Matrix3d E; essential_from_motion(best_model->pose, &E); Eigen::Matrix3d F = K_inv * (E * K_inv); get_inliers(F, x1, x2, opt.max_epipolar_error * opt.max_epipolar_error, best_inliers); return stats; } RansacStats ransac_fundamental(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, Eigen::Matrix3d *best_model, std::vector *best_inliers) { if (!opt.score_initial_model) { best_model->setIdentity(); } RansacStats stats; FundamentalEstimator estimator(opt, x1, x2); stats = ransac(estimator, opt, best_model); get_inliers(*best_model, x1, x2, opt.max_epipolar_error * opt.max_epipolar_error, best_inliers); return stats; } RansacStats ransac_homography(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, Eigen::Matrix3d *best_model, std::vector *best_inliers) { if (!opt.score_initial_model) { best_model->setIdentity(); } HomographyEstimator estimator(opt, x1, x2); RansacStats stats = ransac(estimator, opt, best_model); get_homography_inliers(*best_model, x1, x2, opt.max_reproj_error * opt.max_reproj_error, best_inliers); return stats; } RansacStats ransac_gen_relpose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &camera2_ext, const RansacOptions &opt, CameraPose *best_model, std::vector> *best_inliers) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } GeneralizedRelativePoseEstimator estimator(opt, matches, camera1_ext, camera2_ext); RansacStats stats = ransac(estimator, opt, best_model); best_inliers->resize(matches.size()); for (size_t match_k = 0; match_k < matches.size(); ++match_k) { const PairwiseMatches &m = matches[match_k]; CameraPose pose1 = camera1_ext[m.cam_id1]; CameraPose pose2 = camera2_ext[m.cam_id2]; // Apply transform (transforming second rig into the first) pose2.t = pose2.t + pose2.rotate(best_model->t); pose2.q = quat_multiply(pose2.q, best_model->q); // Now the relative poses should be consistent with the pairwise measurements CameraPose relpose; relpose.q = quat_multiply(pose2.q, quat_conj(pose1.q)); relpose.t = pose2.t - relpose.rotate(pose1.t); // Compute inliers std::vector &inliers = (*best_inliers)[match_k]; get_inliers(relpose, m.x1, m.x2, (opt.max_epipolar_error * opt.max_epipolar_error), &inliers); } return stats; } RansacStats ransac_hybrid_pose(const std::vector &points2D, const std::vector &points3D, const std::vector &matches2D_2D, const std::vector &map_ext, const RansacOptions &opt, CameraPose *best_model, std::vector *inliers_2D_3D, std::vector> *inliers_2D_2D) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } HybridPoseEstimator estimator(opt, points2D, points3D, matches2D_2D, map_ext); RansacStats stats = ransac(estimator, opt, best_model); get_inliers(*best_model, points2D, points3D, opt.max_reproj_error * opt.max_reproj_error, inliers_2D_3D); inliers_2D_2D->resize(matches2D_2D.size()); for (size_t match_k = 0; match_k < matches2D_2D.size(); ++match_k) { const PairwiseMatches &m = matches2D_2D[match_k]; const CameraPose &map_pose = map_ext[m.cam_id1]; // Cameras are // [rig.R rig.t] // [R t] // Relative pose is [R * rig.R' t - R*rig.R' * rig.t] CameraPose rel_pose = *best_model; // rel_pose.R = rel_pose.R * map_pose.R.transpose(); // rel_pose.t -= rel_pose.R * map_pose.t; rel_pose.q = quat_multiply(rel_pose.q, quat_conj(map_pose.q)); rel_pose.t -= rel_pose.rotate(map_pose.t); std::vector &inliers = (*inliers_2D_2D)[match_k]; get_inliers(rel_pose, m.x1, m.x2, (opt.max_epipolar_error * opt.max_epipolar_error), &inliers); } return stats; } RansacStats ransac_1D_radial_pnp(const std::vector &x, const std::vector &X, const RansacOptions &opt, CameraPose *best_model, std::vector *best_inliers) { if (!opt.score_initial_model) { best_model->q << 1.0, 0.0, 0.0, 0.0; best_model->t.setZero(); } Radial1DAbsolutePoseEstimator estimator(opt, x, X); RansacStats stats = ransac(estimator, opt, best_model); get_inliers_1D_radial(*best_model, x, X, opt.max_reproj_error * opt.max_reproj_error, best_inliers); return stats; } } // namespace poselib PoseLib-2.0.5/PoseLib/robust/ransac.h000066400000000000000000000111241504452766400173620ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RANSAC_H_ #define POSELIB_RANSAC_H_ #include "PoseLib/camera_pose.h" #include "PoseLib/types.h" #include namespace poselib { // Absolute pose estimation RansacStats ransac_pnp(const std::vector &x, const std::vector &X, const RansacOptions &opt, CameraPose *best_model, std::vector *best_inliers); RansacStats ransac_gen_pnp(const std::vector> &x, const std::vector> &X, const std::vector &camera_ext, const RansacOptions &opt, CameraPose *best_model, std::vector> *best_inliers); RansacStats ransac_pnpl(const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D, const std::vector &lines3D, const RansacOptions &opt, CameraPose *best_model, std::vector *inliers_points, std::vector *inliers_lines); // Relative pose estimation RansacStats ransac_relpose(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, CameraPose *best_model, std::vector *best_inliers); RansacStats ransac_shared_focal_relpose(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, ImagePair *best_model, std::vector *best_inliers); RansacStats ransac_fundamental(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, Eigen::Matrix3d *best_model, std::vector *best_inliers); RansacStats ransac_homography(const std::vector &x1, const std::vector &x2, const RansacOptions &opt, Eigen::Matrix3d *best_model, std::vector *best_inliers); RansacStats ransac_gen_relpose(const std::vector &matches, const std::vector &camera1_ext, const std::vector &camera2_ext, const RansacOptions &opt, CameraPose *best_model, std::vector> *best_inliers); // Hybrid pose estimation (both 2D-2D and 2D-3D correspondences) RansacStats ransac_hybrid_pose(const std::vector &points2D, const std::vector &points3D, const std::vector &matches2D_2D, const std::vector &map_ext, const RansacOptions &opt, CameraPose *best_model, std::vector *inliers_2D_3D, std::vector> *inliers_2D_2D); // Absolute pose estimation with the 1D radial camera model RansacStats ransac_1D_radial_pnp(const std::vector &x, const std::vector &X, const RansacOptions &opt, CameraPose *best_model, std::vector *best_inliers); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/ransac_impl.h000066400000000000000000000135321504452766400204100ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RANSAC_IMPL_H_ #define POSELIB_RANSAC_IMPL_H_ #include "PoseLib/types.h" #include namespace poselib { struct RansacState { size_t best_minimal_inlier_count = 0; double best_minimal_msac_score = std::numeric_limits::max(); size_t dynamic_max_iter = 100000; double log_prob_missing_model = std::log(1.0 - 0.9999); }; template void score_models(const Solver &estimator, const std::vector &models, const RansacOptions &opt, RansacState &state, RansacStats &stats, Model *best_model) { // Find best model among candidates int best_model_ind = -1; size_t inlier_count = 0; for (size_t i = 0; i < models.size(); ++i) { double score_msac = estimator.score_model(models[i], &inlier_count); bool more_inliers = inlier_count > state.best_minimal_inlier_count; bool better_score = score_msac < state.best_minimal_msac_score; if (more_inliers || better_score) { if (more_inliers) { state.best_minimal_inlier_count = inlier_count; } if (better_score) { state.best_minimal_msac_score = score_msac; } best_model_ind = i; // check if we should update best model already if (score_msac < stats.model_score) { stats.model_score = score_msac; *best_model = models[i]; stats.num_inliers = inlier_count; } } } if (best_model_ind == -1) return; // Refinement Model refined_model = models[best_model_ind]; estimator.refine_model(&refined_model); stats.refinements++; double refined_msac_score = estimator.score_model(refined_model, &inlier_count); if (refined_msac_score < stats.model_score) { stats.model_score = refined_msac_score; stats.num_inliers = inlier_count; *best_model = refined_model; } // update number of iterations stats.inlier_ratio = static_cast(stats.num_inliers) / static_cast(estimator.num_data); if (stats.inlier_ratio >= 0.9999) { // this is to avoid log(prob_outlier) = -inf below state.dynamic_max_iter = opt.min_iterations; } else if (stats.inlier_ratio <= 0.0001) { // this is to avoid log(prob_outlier) = 0 below state.dynamic_max_iter = opt.max_iterations; } else { const double prob_outlier = 1.0 - std::pow(stats.inlier_ratio, estimator.sample_sz); state.dynamic_max_iter = std::ceil(state.log_prob_missing_model / std::log(prob_outlier) * opt.dyn_num_trials_mult); } } // Templated LO-RANSAC implementation (inspired by RansacLib from Torsten Sattler) template RansacStats ransac(Solver &estimator, const RansacOptions &opt, Model *best_model) { RansacStats stats; if (estimator.num_data < estimator.sample_sz) { return stats; } // Score/Inliers for best model found so far stats.num_inliers = 0; stats.model_score = std::numeric_limits::max(); // best inl/score for minimal model, used to decide when to LO RansacState state; state.dynamic_max_iter = opt.max_iterations; state.log_prob_missing_model = std::log(1.0 - opt.success_prob); // Score initial model if it was supplied if (opt.score_initial_model) { score_models(estimator, {*best_model}, opt, state, stats, best_model); } size_t inlier_count = 0; std::vector models; for (stats.iterations = 0; stats.iterations < opt.max_iterations; stats.iterations++) { if (stats.iterations > opt.min_iterations && stats.iterations > state.dynamic_max_iter) { break; } models.clear(); estimator.generate_models(&models); score_models(estimator, models, opt, state, stats, best_model); } // Final refinement Model refined_model = *best_model; estimator.refine_model(&refined_model); stats.refinements++; double refined_msac_score = estimator.score_model(refined_model, &inlier_count); if (refined_msac_score < stats.model_score) { *best_model = refined_model; stats.num_inliers = inlier_count; } return stats; } } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/robust_loss.h000066400000000000000000000104551504452766400204770ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_LOSS_H_ #define POSELIB_ROBUST_LOSS_H_ #include #include #include namespace poselib { // Robust loss functions class TrivialLoss { public: TrivialLoss(double) {} // dummy to ensure we have consistent calling interface TrivialLoss() {} double loss(double r2) const { return r2; } double weight(double r2) const { return 1.0; } }; class TruncatedLoss { public: TruncatedLoss(double threshold) : squared_thr(threshold * threshold) {} double loss(double r2) const { return std::min(r2, squared_thr); } double weight(double r2) const { return (r2 < squared_thr) ? 1.0 : 0.0; } private: const double squared_thr; }; // The method from // Le and Zach, Robust Fitting with Truncated Least Squares: A Bilevel Optimization Approach, 3DV 2021 // for truncated least squares optimization with IRLS. class TruncatedLossLeZach { public: TruncatedLossLeZach(double threshold) : squared_thr(threshold * threshold), mu(0.5) {} double loss(double r2) const { return std::min(r2, squared_thr); } double weight(double r2) const { double r2_hat = r2 / squared_thr; double zstar = std::min(r2_hat, 1.0); if (r2_hat < 1.0) { return 0.5; } else { // assumes mu > 0.5 double r2m1 = r2_hat - 1.0; double rho = (2.0 * r2m1 + std::sqrt(4.0 * r2m1 * r2m1 * mu * mu + 2 * mu * r2m1)) / mu; double a = (r2_hat + mu * rho * zstar - 0.5 * rho) / (1 + mu * rho); double zbar = std::max(0.0, std::min(a, 1.0)); return (zstar - zbar) / rho; } } private: const double squared_thr; public: // hyper-parameter for penalty strength double mu; // schedule for increasing mu in each iteration static constexpr double alpha = 1.5; }; class HuberLoss { public: HuberLoss(double threshold) : thr(threshold) {} double loss(double r2) const { const double r = std::sqrt(r2); if (r <= thr) { return r2; } else { return thr * (2.0 * r - thr); } } double weight(double r2) const { const double r = std::sqrt(r2); if (r <= thr) { return 1.0; } else { return thr / r; } } private: const double thr; }; class CauchyLoss { public: CauchyLoss(double threshold) : sq_thr(threshold * threshold), inv_sq_thr(1.0 / sq_thr) {} double loss(double r2) const { return sq_thr * std::log1p(r2 * inv_sq_thr); } double weight(double r2) const { return std::max(std::numeric_limits::min(), 1.0 / (1.0 + r2 * inv_sq_thr)); } private: const double sq_thr; const double inv_sq_thr; }; } // namespace poselib #endif PoseLib-2.0.5/PoseLib/robust/sampling.cc000066400000000000000000000112221504452766400200620ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "sampling.h" #include namespace poselib { // Splitmix64 PRNG typedef uint64_t RNG_t; int random_int(RNG_t &state) { state += 0x9e3779b97f4a7c15; uint64_t z = state; z = (z ^ (z >> 30)) * 0xbf58476d1ce4e5b9; z = (z ^ (z >> 27)) * 0x94d049bb133111eb; return z ^ (z >> 31); } // Draws a random sample void draw_sample(size_t sample_sz, size_t N, std::vector *sample, RNG_t &rng) { for (size_t i = 0; i < sample_sz; ++i) { bool done = false; while (!done) { (*sample)[i] = random_int(rng) % N; done = true; for (size_t j = 0; j < i; ++j) { if ((*sample)[i] == (*sample)[j]) { done = false; break; } } } } } // Sampling for multi-camera systems void draw_sample(size_t sample_sz, const std::vector &N, std::vector> *sample, RNG_t &rng) { for (size_t i = 0; i < sample_sz; ++i) { bool done = false; while (!done) { (*sample)[i].first = random_int(rng) % N.size(); if (N[(*sample)[i].first] == 0) { continue; } (*sample)[i].second = random_int(rng) % N[(*sample)[i].first]; done = true; for (size_t j = 0; j < i; ++j) { if ((*sample)[i] == (*sample)[j]) { done = false; break; } } } } } void RandomSampler::generate_sample(std::vector *sample) { if (use_prosac && sample_k < max_prosac_iterations) { draw_sample(sample_sz - 1, subset_sz - 1, sample, state); (*sample)[sample_sz - 1] = subset_sz - 1; // update prosac state sample_k++; if (sample_k < max_prosac_iterations) { if (sample_k > growth[subset_sz - 1]) { if (++subset_sz > num_data) { subset_sz = num_data; } } } } else { // uniform ransac sampling draw_sample(sample_sz, num_data, sample, state); } } void RandomSampler::initialize_prosac() { growth.resize(std::max(num_data, sample_sz), 0); // In the paper, T_N = max_prosac_iterations // Initialize T_n for n = sample_sz double T_n = max_prosac_iterations; for (size_t i = 0; i < sample_sz; ++i) T_n *= static_cast(sample_sz - i) / (num_data - i); // Note that that growth[] stores T_n prime // The growth function is then defined as // g(t) = smallest n such that T_n prime > t for (size_t n = 0; n < sample_sz; ++n) { growth[n] = 1; } size_t T_np = 1; for (size_t n = sample_sz; n < num_data; ++n) { // Recursive relation from eq. 3 double T_n_next = T_n * (n + 1.0) / (n + 1.0 - sample_sz); // Eq. 4 T_np += std::ceil(T_n_next - T_n); growth[n] = T_np; T_n = T_n_next; } // counter keeping track of which sample we are at sample_k = 1; subset_sz = sample_sz; } } // namespace poselibPoseLib-2.0.5/PoseLib/robust/sampling.h000066400000000000000000000056721504452766400177400ustar00rootroot00000000000000#ifndef POSELIB_ROBUST_SAMPLING_H_ #define POSELIB_ROBUST_SAMPLING_H_ // Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "PoseLib/types.h" #include #include namespace poselib { typedef uint64_t RNG_t; int random_int(RNG_t &state); // Draws a random sample (NOTE: This assumes sample_sz >= N but does not check it!) void draw_sample(size_t sample_sz, size_t N, std::vector *sample, RNG_t &rng); // Sampling for multi-camera systems void draw_sample(size_t sample_sz, const std::vector &N, std::vector> *sample, RNG_t &rng); class RandomSampler { public: RandomSampler(size_t N, size_t K, RNG_t seed = 0, bool use_prosac_sampling = false, int prosac_iters = 100000) : num_data(N), sample_sz(K), state(seed), use_prosac(use_prosac_sampling), max_prosac_iterations(prosac_iters) { if (use_prosac_sampling) { initialize_prosac(); } } void generate_sample(std::vector *sample); private: void initialize_prosac(); public: size_t num_data; size_t sample_sz; RNG_t state; // state for PROSAC sampling bool use_prosac; size_t max_prosac_iterations; // number of iterations before reverting to standard RANSAC size_t sample_k; size_t subset_sz; // pre-computed growth function used in PROSAC sampling std::vector growth; }; } // namespace poselib #endifPoseLib-2.0.5/PoseLib/robust/utils.cc000066400000000000000000000436461504452766400174270ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "utils.h" #include "PoseLib/misc/essential.h" namespace poselib { // Returns MSAC score double compute_msac_score(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, size_t *inlier_count) { *inlier_count = 0; double score = 0.0; const Eigen::Matrix3d R = pose.R(); const double P0_0 = R(0, 0), P0_1 = R(0, 1), P0_2 = R(0, 2), P0_3 = pose.t(0); const double P1_0 = R(1, 0), P1_1 = R(1, 1), P1_2 = R(1, 2), P1_3 = pose.t(1); const double P2_0 = R(2, 0), P2_1 = R(2, 1), P2_2 = R(2, 2), P2_3 = pose.t(2); for (size_t k = 0; k < x.size(); ++k) { const double X0 = X[k](0), X1 = X[k](1), X2 = X[k](2); const double x0 = x[k](0), x1 = x[k](1); const double z0 = P0_0 * X0 + P0_1 * X1 + P0_2 * X2 + P0_3; const double z1 = P1_0 * X0 + P1_1 * X1 + P1_2 * X2 + P1_3; const double z2 = P2_0 * X0 + P2_1 * X1 + P2_2 * X2 + P2_3; const double inv_z2 = 1.0 / z2; const double r_0 = z0 * inv_z2 - x0; const double r_1 = z1 * inv_z2 - x1; const double r_sq = r_0 * r_0 + r_1 * r_1; if (r_sq < sq_threshold && z2 > 0.0) { (*inlier_count)++; score += r_sq; } } score += (x.size() - *inlier_count) * sq_threshold; return score; } double compute_msac_score(const CameraPose &pose, const std::vector &lines2D, const std::vector &lines3D, double sq_threshold, size_t *inlier_count) { *inlier_count = 0; double score = 0.0; const Eigen::Matrix3d R = pose.R(); for (size_t k = 0; k < lines2D.size(); ++k) { Eigen::Vector3d Z1 = (R * lines3D[k].X1 + pose.t); Eigen::Vector3d Z2 = (R * lines3D[k].X2 + pose.t); Eigen::Vector3d proj_line = Z1.cross(Z2); proj_line /= proj_line.topRows<2>().norm(); const double r = std::abs(proj_line.dot(lines2D[k].x1.homogeneous())) + std::abs(proj_line.dot(lines2D[k].x2.homogeneous())); const double r2 = r * r; if (r2 < sq_threshold) { // TODO Cheirality check? (*inlier_count)++; score += r2; } else { score += sq_threshold; } } return score; } // Returns MSAC score of the Sampson error (checks cheirality of points as well) double compute_sampson_msac_score(const CameraPose &pose, const std::vector &x1, const std::vector &x2, double sq_threshold, size_t *inlier_count) { *inlier_count = 0; Eigen::Matrix3d E; essential_from_motion(pose, &E); // For some reason this is a lot faster than just using nice Eigen expressions... const double E0_0 = E(0, 0), E0_1 = E(0, 1), E0_2 = E(0, 2); const double E1_0 = E(1, 0), E1_1 = E(1, 1), E1_2 = E(1, 2); const double E2_0 = E(2, 0), E2_1 = E(2, 1), E2_2 = E(2, 2); double score = 0.0; for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Ex1_0 = E0_0 * x1_0 + E0_1 * x1_1 + E0_2; const double Ex1_1 = E1_0 * x1_0 + E1_1 * x1_1 + E1_2; const double Ex1_2 = E2_0 * x1_0 + E2_1 * x1_1 + E2_2; const double Ex2_0 = E0_0 * x2_0 + E1_0 * x2_1 + E2_0; const double Ex2_1 = E0_1 * x2_0 + E1_1 * x2_1 + E2_1; // const double Ex2_2 = E0_2 * x2_0 + E1_2 * x2_1 + E2_2; const double C = x2_0 * Ex1_0 + x2_1 * Ex1_1 + Ex1_2; const double Cx = Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1; const double Cy = Ex2_0 * Ex2_0 + Ex2_1 * Ex2_1; const double r2 = C * C / (Cx + Cy); if (r2 < sq_threshold) { bool cheirality = check_cheirality(pose, x1[k].homogeneous().normalized(), x2[k].homogeneous().normalized(), 0.01); if (cheirality) { (*inlier_count)++; score += r2; } else { score += sq_threshold; } } else { score += sq_threshold; } } return score; } // Returns MSAC score of the Sampson error (no cheirality check) double compute_sampson_msac_score(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, double sq_threshold, size_t *inlier_count) { *inlier_count = 0; // For some reason this is a lot faster than just using nice Eigen expressions... const double E0_0 = E(0, 0), E0_1 = E(0, 1), E0_2 = E(0, 2); const double E1_0 = E(1, 0), E1_1 = E(1, 1), E1_2 = E(1, 2); const double E2_0 = E(2, 0), E2_1 = E(2, 1), E2_2 = E(2, 2); double score = 0.0; for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Ex1_0 = E0_0 * x1_0 + E0_1 * x1_1 + E0_2; const double Ex1_1 = E1_0 * x1_0 + E1_1 * x1_1 + E1_2; const double Ex1_2 = E2_0 * x1_0 + E2_1 * x1_1 + E2_2; const double Ex2_0 = E0_0 * x2_0 + E1_0 * x2_1 + E2_0; const double Ex2_1 = E0_1 * x2_0 + E1_1 * x2_1 + E2_1; // const double Ex2_2 = E0_2 * x2_0 + E1_2 * x2_1 + E2_2; const double C = x2_0 * Ex1_0 + x2_1 * Ex1_1 + Ex1_2; const double Cx = Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1; const double Cy = Ex2_0 * Ex2_0 + Ex2_1 * Ex2_1; const double r2 = C * C / (Cx + Cy); if (r2 < sq_threshold) { (*inlier_count)++; score += r2; } else { score += sq_threshold; } } return score; } double compute_homography_msac_score(const Eigen::Matrix3d &H, const std::vector &x1, const std::vector &x2, double sq_threshold, size_t *inlier_count) { *inlier_count = 0; double score = 0; const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); const double H2_0 = H(2, 0), H2_1 = H(2, 1), H2_2 = H(2, 2); for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Hx1_0 = H0_0 * x1_0 + H0_1 * x1_1 + H0_2; const double Hx1_1 = H1_0 * x1_0 + H1_1 * x1_1 + H1_2; const double inv_Hx1_2 = 1.0 / (H2_0 * x1_0 + H2_1 * x1_1 + H2_2); const double r0 = Hx1_0 * inv_Hx1_2 - x2_0; const double r1 = Hx1_1 * inv_Hx1_2 - x2_1; const double r2 = r0 * r0 + r1 * r1; if (r2 < sq_threshold) { (*inlier_count)++; score += r2; } else { score += sq_threshold; } } return score; } void get_homography_inliers(const Eigen::Matrix3d &H, const std::vector &x1, const std::vector &x2, double sq_threshold, std::vector *inliers) { const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); const double H2_0 = H(2, 0), H2_1 = H(2, 1), H2_2 = H(2, 2); inliers->resize(x1.size()); for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Hx1_0 = H0_0 * x1_0 + H0_1 * x1_1 + H0_2; const double Hx1_1 = H1_0 * x1_0 + H1_1 * x1_1 + H1_2; const double inv_Hx1_2 = 1.0 / (H2_0 * x1_0 + H2_1 * x1_1 + H2_2); const double r0 = Hx1_0 * inv_Hx1_2 - x2_0; const double r1 = Hx1_1 * inv_Hx1_2 - x2_1; const double r2 = r0 * r0 + r1 * r1; (*inliers)[k] = (r2 < sq_threshold); } } // Returns MSAC score for the 1D radial camera model double compute_msac_score_1D_radial(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, size_t *inlier_count) { *inlier_count = 0; const Eigen::Matrix3d R = pose.R(); double score = 0.0; for (size_t k = 0; k < x.size(); ++k) { Eigen::Vector2d z = (R * X[k] + pose.t).topRows<2>().normalized(); const double alpha = z.dot(x[k]); const double r2 = (x[k] - alpha * z).squaredNorm(); if (r2 < sq_threshold && alpha > 0.0) { (*inlier_count)++; score += r2; } else { score += sq_threshold; } } return score; } // Compute inliers for absolute pose estimation (using reprojection error and cheirality check) void get_inliers(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, std::vector *inliers) { inliers->resize(x.size()); const Eigen::Matrix3d R = pose.R(); for (size_t k = 0; k < x.size(); ++k) { Eigen::Vector3d Z = (R * X[k] + pose.t); double r2 = (Z.hnormalized() - x[k]).squaredNorm(); (*inliers)[k] = (r2 < sq_threshold && Z(2) > 0.0); } } void get_inliers(const CameraPose &pose, const std::vector &lines2D, const std::vector &lines3D, double sq_threshold, std::vector *inliers) { inliers->resize(lines2D.size()); const Eigen::Matrix3d R = pose.R(); for (size_t k = 0; k < lines2D.size(); ++k) { Eigen::Vector3d Z1 = (R * lines3D[k].X1 + pose.t); Eigen::Vector3d Z2 = (R * lines3D[k].X2 + pose.t); Eigen::Vector3d proj_line = Z1.cross(Z2); proj_line /= proj_line.topRows<2>().norm(); const double r = std::abs(proj_line.dot(lines2D[k].x1.homogeneous())) + std::abs(proj_line.dot(lines2D[k].x2.homogeneous())); const double r2 = r * r; (*inliers)[k] = (r2 < sq_threshold); } } // Compute inliers for relative pose estimation (using Sampson error) int get_inliers(const CameraPose &pose, const std::vector &x1, const std::vector &x2, double sq_threshold, std::vector *inliers) { inliers->resize(x1.size()); Eigen::Matrix3d E; essential_from_motion(pose, &E); const double E0_0 = E(0, 0), E0_1 = E(0, 1), E0_2 = E(0, 2); const double E1_0 = E(1, 0), E1_1 = E(1, 1), E1_2 = E(1, 2); const double E2_0 = E(2, 0), E2_1 = E(2, 1), E2_2 = E(2, 2); size_t inlier_count = 0.0; for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Ex1_0 = E0_0 * x1_0 + E0_1 * x1_1 + E0_2; const double Ex1_1 = E1_0 * x1_0 + E1_1 * x1_1 + E1_2; const double Ex1_2 = E2_0 * x1_0 + E2_1 * x1_1 + E2_2; const double Ex2_0 = E0_0 * x2_0 + E1_0 * x2_1 + E2_0; const double Ex2_1 = E0_1 * x2_0 + E1_1 * x2_1 + E2_1; // const double Ex2_2 = E0_2 * x2_0 + E1_2 * x2_1 + E2_2; const double C = x2_0 * Ex1_0 + x2_1 * Ex1_1 + Ex1_2; const double Cx = Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1; const double Cy = Ex2_0 * Ex2_0 + Ex2_1 * Ex2_1; const double r2 = C * C / (Cx + Cy); bool inlier = (r2 < sq_threshold); if (inlier) { bool cheirality = check_cheirality(pose, x1[k].homogeneous().normalized(), x2[k].homogeneous().normalized(), 0.01); if (cheirality) { inlier_count++; } else { inlier = false; } } (*inliers)[k] = inlier; } return inlier_count; } // Compute inliers for relative pose estimation (using Sampson error) int get_inliers(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, double sq_threshold, std::vector *inliers) { inliers->resize(x1.size()); const double E0_0 = E(0, 0), E0_1 = E(0, 1), E0_2 = E(0, 2); const double E1_0 = E(1, 0), E1_1 = E(1, 1), E1_2 = E(1, 2); const double E2_0 = E(2, 0), E2_1 = E(2, 1), E2_2 = E(2, 2); size_t inlier_count = 0.0; for (size_t k = 0; k < x1.size(); ++k) { const double x1_0 = x1[k](0), x1_1 = x1[k](1); const double x2_0 = x2[k](0), x2_1 = x2[k](1); const double Ex1_0 = E0_0 * x1_0 + E0_1 * x1_1 + E0_2; const double Ex1_1 = E1_0 * x1_0 + E1_1 * x1_1 + E1_2; const double Ex1_2 = E2_0 * x1_0 + E2_1 * x1_1 + E2_2; const double Ex2_0 = E0_0 * x2_0 + E1_0 * x2_1 + E2_0; const double Ex2_1 = E0_1 * x2_0 + E1_1 * x2_1 + E2_1; // const double Ex2_2 = E0_2 * x2_0 + E1_2 * x2_1 + E2_2; const double C = x2_0 * Ex1_0 + x2_1 * Ex1_1 + Ex1_2; const double Cx = Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1; const double Cy = Ex2_0 * Ex2_0 + Ex2_1 * Ex2_1; const double r2 = C * C / (Cx + Cy); bool inlier = (r2 < sq_threshold); if (inlier) { inlier_count++; } (*inliers)[k] = inlier; } return inlier_count; } // Compute inliers for absolute pose estimation (using reprojection error and cheirality check) void get_inliers_1D_radial(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, std::vector *inliers) { inliers->resize(x.size()); const Eigen::Matrix3d R = pose.R(); for (size_t k = 0; k < x.size(); ++k) { Eigen::Vector2d z = (R * X[k] + pose.t).topRows<2>().normalized(); const double alpha = z.dot(x[k]); const double r2 = (x[k] - alpha * z).squaredNorm(); (*inliers)[k] = (r2 < sq_threshold && alpha > 0.0); } } double normalize_points(std::vector &x1, std::vector &x2, Eigen::Matrix3d &T1, Eigen::Matrix3d &T2, bool normalize_scale, bool normalize_centroid, bool shared_scale) { T1.setIdentity(); T2.setIdentity(); if (normalize_centroid) { Eigen::Vector2d c1(0, 0), c2(0, 0); for (size_t k = 0; k < x1.size(); ++k) { c1 += x1[k]; c2 += x2[k]; } c1 /= x1.size(); c2 /= x2.size(); T1.block<2, 1>(0, 2) = -c1; T2.block<2, 1>(0, 2) = -c2; for (size_t k = 0; k < x1.size(); ++k) { x1[k] -= c1; x2[k] -= c2; } } if (normalize_scale && shared_scale) { double scale = 0.0; for (size_t k = 0; k < x1.size(); ++k) { scale += x1[k].norm(); scale += x2[k].norm(); } scale /= std::sqrt(2) * x1.size(); for (size_t k = 0; k < x1.size(); ++k) { x1[k] /= scale; x2[k] /= scale; } T1.block<2, 3>(0, 0) *= 1.0 / scale; T2.block<2, 3>(0, 0) *= 1.0 / scale; return scale; } else if (normalize_scale && !shared_scale) { double scale1 = 0.0, scale2 = 0.0; for (size_t k = 0; k < x1.size(); ++k) { scale1 += x1[k].norm(); scale2 += x2[k].norm(); } scale1 /= x1.size() / std::sqrt(2); scale2 /= x2.size() / std::sqrt(2); for (size_t k = 0; k < x1.size(); ++k) { x1[k] /= scale1; x2[k] /= scale2; } T1.block<2, 3>(0, 0) *= 1.0 / scale1; T2.block<2, 3>(0, 0) *= 1.0 / scale2; return std::sqrt(scale1 * scale2); } return 1.0; } bool calculate_RFC(const Eigen::Matrix3d &F) { float den, num; den = F(0, 0) * F(0, 1) * F(2, 0) * F(2, 2) - F(0, 0) * F(0, 2) * F(2, 0) * F(2, 1) + F(0, 1) * F(0, 1) * F(2, 1) * F(2, 2) - F(0, 1) * F(0, 2) * F(2, 1) * F(2, 1) + F(1, 0) * F(1, 1) * F(2, 0) * F(2, 2) - F(1, 0) * F(1, 2) * F(2, 0) * F(2, 1) + F(1, 1) * F(1, 1) * F(2, 1) * F(2, 2) - F(1, 1) * F(1, 2) * F(2, 1) * F(2, 1); num = -F(2, 2) * (F(0, 1) * F(0, 2) * F(2, 2) - F(0, 2) * F(0, 2) * F(2, 1) + F(1, 1) * F(1, 2) * F(2, 2) - F(1, 2) * F(1, 2) * F(2, 1)); if (num * den < 0) return false; den = F(0, 0) * F(1, 0) * F(0, 2) * F(2, 2) - F(0, 0) * F(2, 0) * F(0, 2) * F(1, 2) + F(1, 0) * F(1, 0) * F(1, 2) * F(2, 2) - F(1, 0) * F(2, 0) * F(1, 2) * F(1, 2) + F(0, 1) * F(1, 1) * F(0, 2) * F(2, 2) - F(0, 1) * F(2, 1) * F(0, 2) * F(1, 2) + F(1, 1) * F(1, 1) * F(1, 2) * F(2, 2) - F(1, 1) * F(2, 1) * F(1, 2) * F(1, 2); num = -F(2, 2) * (F(1, 0) * F(2, 0) * F(2, 2) - F(2, 0) * F(2, 0) * F(1, 2) + F(1, 1) * F(2, 1) * F(2, 2) - F(2, 1) * F(2, 1) * F(1, 2)); if (num * den < 0) return false; return true; } } // namespace poselibPoseLib-2.0.5/PoseLib/robust/utils.h000066400000000000000000000114101504452766400172510ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_UTILS_H #define POSELIB_ROBUST_UTILS_H #include "PoseLib/camera_pose.h" #include "PoseLib/types.h" #include #include namespace poselib { // Returns MSAC score of the reprojection error double compute_msac_score(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, size_t *inlier_count); double compute_msac_score(const CameraPose &pose, const std::vector &lines2D, const std::vector &lines3D, double sq_threshold, size_t *inlier_count); // Returns MSAC score of the Sampson error double compute_sampson_msac_score(const CameraPose &pose, const std::vector &x1, const std::vector &x2, double sq_threshold, size_t *inlier_count); double compute_sampson_msac_score(const Eigen::Matrix3d &F, const std::vector &x1, const std::vector &x2, double sq_threshold, size_t *inlier_count); // Returns MSAC score of transfer error for homography double compute_homography_msac_score(const Eigen::Matrix3d &H, const std::vector &x1, const std::vector &x2, double sq_threshold, size_t *inlier_count); // Compute inliers for absolute pose estimation (using reprojection error and cheirality check) void get_inliers(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, std::vector *inliers); void get_inliers(const CameraPose &pose, const std::vector &lines2D, const std::vector &lines3D, double sq_threshold, std::vector *inliers); // Compute inliers for relative pose estimation (using Sampson error) int get_inliers(const CameraPose &pose, const std::vector &x1, const std::vector &x2, double sq_threshold, std::vector *inliers); int get_inliers(const Eigen::Matrix3d &E, const std::vector &x1, const std::vector &x2, double sq_threshold, std::vector *inliers); // inliers for homography void get_homography_inliers(const Eigen::Matrix3d &H, const std::vector &x1, const std::vector &x2, double sq_threshold, std::vector *inliers); // Helpers for the 1D radial camera model double compute_msac_score_1D_radial(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, size_t *inlier_count); void get_inliers_1D_radial(const CameraPose &pose, const std::vector &x, const std::vector &X, double sq_threshold, std::vector *inliers); // Normalize points by shifting/scaling coordinate systems. double normalize_points(std::vector &x1, std::vector &x2, Eigen::Matrix3d &T1, Eigen::Matrix3d &T2, bool normalize_scale, bool normalize_centroid, bool shared_scale); // Calculate whether F would yield real focals, assumes both pp at [0, 0] bool calculate_RFC(const Eigen::Matrix3d &F); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/000077500000000000000000000000001504452766400161225ustar00rootroot00000000000000PoseLib-2.0.5/PoseLib/solvers/gen_relpose_5p1pt.cc000066400000000000000000000024451504452766400217710ustar00rootroot00000000000000#include "gen_relpose_5p1pt.h" #include "PoseLib/misc/essential.h" #include "PoseLib/solvers/relpose_5pt.h" #include namespace poselib { int gen_relpose_5p1pt(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, std::vector *output) { output->clear(); relpose_5pt(x1, x2, output); for (size_t k = 0; k < output->size(); ++k) { CameraPose &pose = (*output)[k]; // the translation is given by // t = p2 - R_5pt*p1 + gamma * t_5pt = a + gamma * b // we need to solve for gamma using our extra correspondence // R * (p1 + lambda_1 * x1) + a + gamma * b = lambda_2 * x2 + p2 Eigen::Matrix3d R = pose.R(); Eigen::Vector3d a = p2[0] - R * p1[0]; Eigen::Vector3d b = pose.t; // vector used to eliminate lambda1 and lambda2 Eigen::Vector3d w = x2[5].cross(R * x1[5]); const double c0 = w.dot(p2[5] - R * p1[5] - a); const double c1 = w.dot(b); const double gamma = c0 / c1; pose.t = a + gamma * b; // TODO: Cheirality check for the last point } return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/gen_relpose_5p1pt.h000066400000000000000000000042751504452766400216360ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_GEN_RELPOSE_5P1PT_H #define POSELIB_GEN_RELPOSE_5P1PT_H #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for generalized relative pose from 6 correspondences assuming the first 5 are from the same camera // NOTE ASSUMES THAT p1[0] == p1[1] = ... = p1[4] and p2[0] == p2[1] = ... = p2[4] int gen_relpose_5p1pt(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, std::vector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/gen_relpose_6pt.cc000066400000000000000000004031171504452766400215320ustar00rootroot00000000000000#include "gen_relpose_6pt.h" #include "PoseLib/misc/essential.h" #include #define USE_FAST_EIGENVECTOR_SOLVER namespace poselib { static const int coeffs0_ind[] = { 0, 84, 168, 252, 336, 420, 504, 588, 1, 85, 169, 253, 337, 421, 505, 589, 2, 86, 170, 254, 338, 422, 506, 590, 3, 87, 171, 255, 339, 423, 507, 591, 4, 88, 172, 256, 340, 424, 508, 592, 5, 89, 173, 257, 341, 425, 509, 593, 6, 90, 174, 258, 342, 426, 510, 594, 0, 84, 168, 252, 336, 420, 504, 1, 84, 85, 168, 169, 252, 253, 336, 0, 337, 420, 421, 504, 505, 588, 672, 756, 840, 924, 1008, 7, 91, 175, 259, 343, 427, 511, 2, 85, 86, 169, 170, 253, 254, 337, 1, 338, 421, 422, 505, 506, 589, 595, 673, 757, 841, 925, 1009, 8, 92, 176, 260, 344, 428, 512, 3, 86, 87, 170, 171, 254, 255, 338, 2, 339, 422, 423, 506, 507, 590, 596, 674, 758, 842, 926, 1010, 9, 93, 177, 261, 345, 429, 513, 4, 87, 88, 171, 172, 255, 256, 339, 3, 340, 423, 424, 507, 508, 591, 597, 675, 759, 843, 927, 1011, 10, 94, 178, 262, 346, 430, 514, 5, 88, 89, 172, 173, 256, 257, 340, 4, 341, 424, 425, 508, 509, 592, 598, 676, 760, 844, 928, 1012, 11, 95, 179, 263, 347, 431, 515, 6, 89, 90, 173, 174, 257, 258, 341, 5, 342, 425, 426, 509, 510, 593, 599, 677, 761, 845, 929, 1013, 12, 96, 180, 264, 348, 432, 516, 90, 174, 258, 342, 6, 426, 510, 594, 600, 678, 762, 846, 930, 1014, 7, 91, 175, 259, 168, 84, 0, 252, 336, 343, 420, 427, 504, 511, 588, 672, 756, 840, 924, 1008, 1092, 1176, 8, 91, 92, 175, 176, 259, 260, 343, 169, 85, 1, 253, 337, 7, 344, 421, 427, 428, 505, 511, 512, 589, 595, 673, 679, 757, 763, 841, 847, 925, 931, 1009, 1015, 1093, 1177, 13, 97, 181, 265, 349, 433, 517, 9, 92, 93, 176, 177, 260, 261, 344, 170, 86, 2, 254, 338, 8, 345, 422, 428, 429, 506, 512, 513, 590, 596, 601, 674, 680, 758, 764, 842, 848, 926, 932, 1010, 1016, 1094, 1178, 14, 98, 182, 266, 350, 434, 518, 10, 93, 94, 177, 178, 261, 262, 345, 171, 87, 3, 255, 339, 9, 346, 423, 429, 430, 507, 513, 514, 591, 597, 602, 675, 681, 759, 765, 843, 849, 927, 933, 1011, 1017, 1095, 1179, 15, 99, 183, 267, 351, 435, 519, 11, 94, 95, 178, 179, 262, 263, 346, 172, 88, 4, 256, 340, 10, 347, 424, 430, 431, 508, 514, 515, 592, 598, 603, 676, 682, 760, 766, 844, 850, 928, 934, 1012, 1018, 1096, 1180, 16, 100, 184, 268, 352, 436, 520, 12, 95, 96, 179, 180, 263, 264, 347, 173, 89, 5, 257, 341, 11, 348, 425, 431, 432, 509, 515, 516, 593, 599, 604, 677, 683, 761, 767, 845, 851, 929, 935, 1013, 1019, 1097, 1181, 17, 101, 185, 269, 353, 437, 521, 96, 180, 264, 348, 174, 90, 6, 258, 342, 12, 426, 432, 510, 516, 594, 600, 605, 678, 684, 762, 768, 846, 852, 930, 936, 1014, 1020, 1098, 1182, 13, 97, 181, 265, 175, 91, 7, 259, 343, 349, 427, 433, 511, 517, 595, 679, 763, 847, 931, 1015, 1099, 1183, 14, 97, 98, 181, 182, 265, 266, 349, 176, 92, 8, 260, 344, 13, 350, 428, 433, 434, 512, 517, 518, 596, 601, 680, 685, 764, 769, 848, 853, 932, 937, 1016, 1021, 1100, 1184, 18, 102, 186, 270, 354, 438, 522, 15, 98, 99, 182, 183, 266, 267, 350, 177, 93, 9, 261, 345, 14, 351, 429, 434, 435, 513, 518, 519, 597, 602, 606, 681, 686, 765, 770, 849, 854, 933, 938, 1017, 1022, 1101, 1185, 19, 103, 187, 271, 355, 439, 523, 16, 99, 100, 183, 184, 267, 268, 351, 178, 94, 10, 262, 346, 15, 352, 430, 435, 436, 514, 519, 520, 598, 603, 607, 682, 687, 766, 771, 850, 855, 934, 939, 1018, 1023, 1102, 1186, 20, 104, 188, 272, 356, 440, 524, 17, 100, 101, 184, 185, 268, 269, 352, 179, 95, 11, 263, 347, 16, 353, 431, 436, 437, 515, 520, 521, 599, 604, 608, 683, 688, 767, 772, 851, 856, 935, 940, 1019, 1024, 1103, 1187, 21, 105, 189, 273, 357, 441, 525, 101, 185, 269, 353, 180, 96, 12, 264, 348, 17, 432, 437, 516, 521, 600, 605, 609, 684, 689, 768, 773, 852, 857, 936, 941, 1020, 1025, 1104, 1188, 18, 102, 186, 270, 181, 97, 13, 265, 349, 354, 433, 438, 517, 522, 601, 685, 769, 853, 937, 1021, 1105, 1189, 19, 102, 103, 186, 187, 270, 271, 354, 182, 98, 14, 266, 350, 18, 355, 434, 438, 439, 518, 522, 523, 602, 606, 686, 690, 770, 774, 854, 858, 938, 942, 1022, 1026, 1106, 1190, 22, 106, 190, 274, 358, 442, 526, 20, 103, 104, 187, 188, 271, 272, 355, 183, 99, 15, 267, 351, 19, 356, 435, 439, 440, 519, 523, 524, 603, 607, 610, 687, 691, 771, 775, 855, 859, 939, 943, 1023, 1027, 1107, 1191, 23, 107, 191, 275, 359, 443, 527, 21, 104, 105, 188, 189, 272, 273, 356, 184, 100, 16, 268, 352, 20, 357, 436, 440, 441, 520, 524, 525, 604, 608, 611, 688, 692, 772, 776, 856, 860, 940, 944, 1024, 1028, 1108, 1192, 24, 108, 192, 276, 360, 444, 528, 105, 189, 273, 357, 185, 101, 17, 269, 353, 21, 437, 441, 521, 525, 605, 609, 612, 689, 693, 773, 777, 857, 861, 941, 945, 1025, 1029, 1109, 1193, 22, 106, 190, 274, 186, 102, 18, 270, 354, 358, 438, 442, 522, 526, 606, 690, 774, 858, 942, 1026, 1110, 1194, 23, 106, 107, 190, 191, 274, 275, 358, 187, 103, 19, 271, 355, 22, 359, 439, 442, 443, 523, 526, 527, 607, 610, 691, 694, 775, 778, 859, 862, 943, 946, 1027, 1030, 1111, 1195, 25, 109, 193, 277, 361, 445, 529, 24, 107, 108, 191, 192, 275, 276, 359, 188, 104, 20, 272, 356, 23, 360, 440, 443, 444, 524, 527, 528, 608, 611, 613, 692, 695, 776, 779, 860, 863, 944, 947, 1028, 1031, 1112, 1196, 26, 110, 194, 278, 362, 446, 530, 108, 192, 276, 360, 189, 105, 21, 273, 357, 24, 441, 444, 525, 528, 609, 612, 614, 693, 696, 777, 780, 861, 864, 945, 948, 1029, 1032, 1113, 1197, 25, 109, 193, 277, 190, 106, 22, 274, 358, 361, 442, 445, 526, 529, 610, 694, 778, 862, 946, 1030, 1114, 1198, 26, 109, 110, 193, 194, 277, 278, 361, 191, 107, 23, 275, 359, 25, 362, 443, 445, 446, 527, 529, 530, 611, 613, 695, 697, 779, 781, 863, 865, 947, 949, 1031, 1033, 1115, 1199, 27, 111, 195, 279, 363, 447, 531, 110, 194, 278, 362, 192, 108, 24, 276, 360, 26, 444, 446, 528, 530, 612, 614, 615, 696, 698, 780, 782, 864, 866, 948, 950, 1032, 1034, 1116, 1200, 27, 111, 195, 279, 193, 109, 25, 277, 361, 363, 445, 447, 529, 531, 613, 697, 781, 865, 949, 1033, 1117, 1201, 111, 195, 279, 363, 194, 110, 26, 278, 362, 27, 446, 447, 530, 531, 614, 615, 698, 699, 782, 783, 866, 867, 950, 951, 1034, 1035, 1118, 1202, 168, 420, 0, 504, 252, 588, 672, 756, 84, 840, 336, 924, 169, 420, 84, 421, 504, 1, 505, 252, 588, 0, 253, 589, 672, 673, 756, 168, 757, 336, 840, 85, 841, 924, 337, 925, 1008, 1092, 28, 112, 196, 280, 364, 448, 532, 616, 170, 421, 85, 422, 505, 2, 506, 253, 589, 1, 254, 590, 673, 674, 757, 169, 758, 337, 841, 86, 842, 925, 338, 926, 1009, 1093, 29, 113, 197, 281, 365, 449, 533, 617, 171, 422, 86, 423, 506, 3, 507, 254, 590, 2, 255, 591, 674, 675, 758, 170, 759, 338, 842, 87, 843, 926, 339, 927, 1010, 1094, 30, 114, 198, 282, 366, 450, 534, 618, 172, 423, 87, 424, 507, 4, 508, 255, 591, 3, 256, 592, 675, 676, 759, 171, 760, 339, 843, 88, 844, 927, 340, 928, 1011, 1095, 31, 115, 199, 283, 367, 451, 535, 619, 173, 424, 88, 425, 508, 5, 509, 256, 592, 4, 257, 593, 676, 677, 760, 172, 761, 340, 844, 89, 845, 928, 341, 929, 1012, 1096, 32, 116, 200, 284, 368, 452, 536, 620, 174, 425, 89, 426, 509, 6, 510, 257, 593, 5, 258, 594, 677, 678, 761, 173, 762, 341, 845, 90, 846, 929, 342, 930, 1013, 1097, 33, 117, 201, 285, 369, 453, 537, 621, 426, 90, 510, 258, 594, 6, 678, 762, 174, 342, 846, 930, 1014, 1098, 28, 112, 196, 280, 364, 448, 532, 175, 427, 7, 511, 252, 84, 504, 588, 259, 595, 672, 0, 679, 756, 336, 763, 840, 91, 847, 924, 343, 931, 1008, 168, 1092, 420, 1176, 29, 112, 113, 196, 197, 280, 281, 364, 28, 365, 448, 449, 532, 533, 616, 700, 784, 868, 952, 1036, 176, 427, 91, 428, 511, 8, 512, 259, 253, 85, 505, 589, 595, 7, 260, 596, 673, 1, 679, 680, 757, 337, 763, 175, 764, 841, 343, 847, 92, 848, 925, 931, 344, 932, 1009, 169, 1015, 1093, 421, 1099, 1177, 34, 118, 202, 286, 370, 454, 538, 30, 113, 114, 197, 198, 281, 282, 365, 29, 366, 449, 450, 533, 534, 617, 622, 701, 785, 869, 953, 1037, 177, 428, 92, 429, 512, 9, 513, 260, 254, 86, 506, 590, 596, 8, 261, 597, 674, 2, 680, 681, 758, 338, 764, 176, 765, 842, 344, 848, 93, 849, 926, 932, 345, 933, 1010, 170, 1016, 1094, 422, 1100, 1178, 35, 119, 203, 287, 371, 455, 539, 31, 114, 115, 198, 199, 282, 283, 366, 30, 367, 450, 451, 534, 535, 618, 623, 702, 786, 870, 954, 1038, 178, 429, 93, 430, 513, 10, 514, 261, 255, 87, 507, 591, 597, 9, 262, 598, 675, 3, 681, 682, 759, 339, 765, 177, 766, 843, 345, 849, 94, 850, 927, 933, 346, 934, 1011, 171, 1017, 1095, 423, 1101, 1179, 36, 120, 204, 288, 372, 456, 540, 32, 115, 116, 199, 200, 283, 284, 367, 31, 368, 451, 452, 535, 536, 619, 624, 703, 787, 871, 955, 1039, 179, 430, 94, 431, 514, 11, 515, 262, 256, 88, 508, 592, 598, 10, 263, 599, 676, 4, 682, 683, 760, 340, 766, 178, 767, 844, 346, 850, 95, 851, 928, 934, 347, 935, 1012, 172, 1018, 1096, 424, 1102, 1180, 37, 121, 205, 289, 373, 457, 541, 33, 116, 117, 200, 201, 284, 285, 368, 32, 369, 452, 453, 536, 537, 620, 625, 704, 788, 872, 956, 1040, 180, 431, 95, 432, 515, 12, 516, 263, 257, 89, 509, 593, 599, 11, 264, 600, 677, 5, 683, 684, 761, 341, 767, 179, 768, 845, 347, 851, 96, 852, 929, 935, 348, 936, 1013, 173, 1019, 1097, 425, 1103, 1181, 38, 122, 206, 290, 374, 458, 542, 117, 201, 285, 369, 33, 453, 537, 621, 626, 705, 789, 873, 957, 1041, 432, 96, 516, 264, 258, 90, 510, 594, 600, 12, 678, 6, 684, 762, 342, 768, 180, 846, 348, 852, 930, 936, 1014, 174, 1020, 1098, 426, 1104, 1182, 34, 118, 202, 286, 196, 112, 28, 280, 364, 370, 448, 454, 532, 538, 616, 700, 784, 868, 952, 1036, 1120, 181, 433, 13, 517, 259, 91, 511, 595, 265, 601, 679, 7, 685, 763, 343, 769, 847, 97, 853, 931, 349, 937, 1015, 175, 1099, 427, 1183, 1204, 35, 118, 119, 202, 203, 286, 287, 370, 197, 113, 29, 281, 365, 34, 371, 449, 454, 455, 533, 538, 539, 617, 622, 701, 706, 785, 790, 869, 874, 953, 958, 1037, 1042, 1121, 182, 433, 97, 434, 517, 14, 518, 265, 260, 92, 512, 596, 601, 13, 266, 602, 680, 8, 685, 686, 764, 344, 769, 181, 770, 848, 349, 853, 98, 854, 932, 937, 350, 938, 1016, 176, 1021, 1100, 428, 1105, 1184, 1205, 39, 123, 207, 291, 375, 459, 543, 36, 119, 120, 203, 204, 287, 288, 371, 198, 114, 30, 282, 366, 35, 372, 450, 455, 456, 534, 539, 540, 618, 623, 627, 702, 707, 786, 791, 870, 875, 954, 959, 1038, 1043, 1122, 183, 434, 98, 435, 518, 15, 519, 266, 261, 93, 513, 597, 602, 14, 267, 603, 681, 9, 686, 687, 765, 345, 770, 182, 771, 849, 350, 854, 99, 855, 933, 938, 351, 939, 1017, 177, 1022, 1101, 429, 1106, 1185, 1206, 40, 124, 208, 292, 376, 460, 544, 37, 120, 121, 204, 205, 288, 289, 372, 199, 115, 31, 283, 367, 36, 373, 451, 456, 457, 535, 540, 541, 619, 624, 628, 703, 708, 787, 792, 871, 876, 955, 960, 1039, 1044, 1123, 184, 435, 99, 436, 519, 16, 520, 267, 262, 94, 514, 598, 603, 15, 268, 604, 682, 10, 687, 688, 766, 346, 771, 183, 772, 850, 351, 855, 100, 856, 934, 939, 352, 940, 1018, 178, 1023, 1102, 430, 1107, 1186, 1207, 41, 125, 209, 293, 377, 461, 545, 38, 121, 122, 205, 206, 289, 290, 373, 200, 116, 32, 284, 368, 37, 374, 452, 457, 458, 536, 541, 542, 620, 625, 629, 704, 709, 788, 793, 872, 877, 956, 961, 1040, 1045, 1124, 185, 436, 100, 437, 520, 17, 521, 268, 263, 95, 515, 599, 604, 16, 269, 605, 683, 11, 688, 689, 767, 347, 772, 184, 773, 851, 352, 856, 101, 857, 935, 940, 353, 941, 1019, 179, 1024, 1103, 431, 1108, 1187, 1208, 42, 126, 210, 294, 378, 462, 546, 122, 206, 290, 374, 201, 117, 33, 285, 369, 38, 453, 458, 537, 542, 621, 626, 630, 705, 710, 789, 794, 873, 878, 957, 962, 1041, 1046, 1125, 437, 101, 521, 269, 264, 96, 516, 600, 605, 17, 684, 12, 689, 768, 348, 773, 185, 852, 353, 857, 936, 941, 1020, 180, 1025, 1104, 432, 1109, 1188, 1209, 39, 123, 207, 291, 202, 118, 34, 286, 370, 375, 454, 459, 538, 543, 622, 706, 790, 874, 958, 1042, 1126, 186, 438, 18, 522, 265, 97, 517, 601, 270, 606, 685, 13, 690, 769, 349, 774, 853, 102, 858, 937, 354, 942, 1021, 181, 1105, 433, 1189, 1210, 40, 123, 124, 207, 208, 291, 292, 375, 203, 119, 35, 287, 371, 39, 376, 455, 459, 460, 539, 543, 544, 623, 627, 707, 711, 791, 795, 875, 879, 959, 963, 1043, 1047, 1127, 187, 438, 102, 439, 522, 19, 523, 270, 266, 98, 518, 602, 606, 18, 271, 607, 686, 14, 690, 691, 770, 350, 774, 186, 775, 854, 354, 858, 103, 859, 938, 942, 355, 943, 1022, 182, 1026, 1106, 434, 1110, 1190, 1211, 43, 127, 211, 295, 379, 463, 547, 41, 124, 125, 208, 209, 292, 293, 376, 204, 120, 36, 288, 372, 40, 377, 456, 460, 461, 540, 544, 545, 624, 628, 631, 708, 712, 792, 796, 876, 880, 960, 964, 1044, 1048, 1128, 188, 439, 103, 440, 523, 20, 524, 271, 267, 99, 519, 603, 607, 19, 272, 608, 687, 15, 691, 692, 771, 351, 775, 187, 776, 855, 355, 859, 104, 860, 939, 943, 356, 944, 1023, 183, 1027, 1107, 435, 1111, 1191, 1212, 44, 128, 212, 296, 380, 464, 548, 42, 125, 126, 209, 210, 293, 294, 377, 205, 121, 37, 289, 373, 41, 378, 457, 461, 462, 541, 545, 546, 625, 629, 632, 709, 713, 793, 797, 877, 881, 961, 965, 1045, 1049, 1129, 189, 440, 104, 441, 524, 21, 525, 272, 268, 100, 520, 604, 608, 20, 273, 609, 688, 16, 692, 693, 772, 352, 776, 188, 777, 856, 356, 860, 105, 861, 940, 944, 357, 945, 1024, 184, 1028, 1108, 436, 1112, 1192, 1213, 45, 129, 213, 297, 381, 465, 549, 126, 210, 294, 378, 206, 122, 38, 290, 374, 42, 458, 462, 542, 546, 626, 630, 633, 710, 714, 794, 798, 878, 882, 962, 966, 1046, 1050, 1130, 441, 105, 525, 273, 269, 101, 521, 605, 609, 21, 689, 17, 693, 773, 353, 777, 189, 857, 357, 861, 941, 945, 1025, 185, 1029, 1109, 437, 1113, 1193, 1214, 43, 127, 211, 295, 207, 123, 39, 291, 375, 379, 459, 463, 543, 547, 627, 711, 795, 879, 963, 1047, 1131, 190, 442, 22, 526, 270, 102, 522, 606, 274, 610, 690, 18, 694, 774, 354, 778, 858, 106, 862, 942, 358, 946, 1026, 186, 1110, 438, 1194, 1215, 44, 127, 128, 211, 212, 295, 296, 379, 208, 124, 40, 292, 376, 43, 380, 460, 463, 464, 544, 547, 548, 628, 631, 712, 715, 796, 799, 880, 883, 964, 967, 1048, 1051, 1132, 191, 442, 106, 443, 526, 23, 527, 274, 271, 103, 523, 607, 610, 22, 275, 611, 691, 19, 694, 695, 775, 355, 778, 190, 779, 859, 358, 862, 107, 863, 943, 946, 359, 947, 1027, 187, 1030, 1111, 439, 1114, 1195, 1216, 46, 130, 214, 298, 382, 466, 550, 45, 128, 129, 212, 213, 296, 297, 380, 209, 125, 41, 293, 377, 44, 381, 461, 464, 465, 545, 548, 549, 629, 632, 634, 713, 716, 797, 800, 881, 884, 965, 968, 1049, 1052, 1133, 192, 443, 107, 444, 527, 24, 528, 275, 272, 104, 524, 608, 611, 23, 276, 612, 692, 20, 695, 696, 776, 356, 779, 191, 780, 860, 359, 863, 108, 864, 944, 947, 360, 948, 1028, 188, 1031, 1112, 440, 1115, 1196, 1217, 196, 448, 28, 532, 280, 616, 700, 784, 112, 672, 84, 504, 0, 840, 336, 588, 756, 252, 868, 924, 168, 364, 952, 1008, 420, 1092, 1176, 197, 448, 112, 449, 532, 29, 533, 280, 616, 28, 281, 617, 700, 701, 784, 196, 785, 364, 868, 113, 673, 85, 505, 1, 841, 337, 589, 757, 253, 869, 925, 169, 952, 365, 953, 1009, 1036, 421, 1093, 1120, 1177, 49, 133, 217, 301, 385, 469, 553, 637, 198, 449, 113, 450, 533, 30, 534, 281, 617, 29, 282, 618, 701, 702, 785, 197, 786, 365, 869, 114, 674, 86, 506, 2, 842, 338, 590, 758, 254, 870, 926, 170, 953, 366, 954, 1010, 1037, 422, 1094, 1121, 1178, 50, 134, 218, 302, 386, 470, 554, 638, 199, 450, 114, 451, 534, 31, 535, 282, 618, 30, 283, 619, 702, 703, 786, 198, 787, 366, 870, 115, 675, 87, 507, 3, 843, 339, 591, 759, 255, 871, 927, 171, 954, 367, 955, 1011, 1038, 423, 1095, 1122, 1179, 51, 135, 219, 303, 387, 471, 555, 639, 200, 451, 115, 452, 535, 32, 536, 283, 619, 31, 284, 620, 703, 704, 787, 199, 788, 367, 871, 116, 676, 88, 508, 4, 844, 340, 592, 760, 256, 872, 928, 172, 955, 368, 956, 1012, 1039, 424, 1096, 1123, 1180, 52, 136, 220, 304, 388, 472, 556, 640, 201, 452, 116, 453, 536, 33, 537, 284, 620, 32, 285, 621, 704, 705, 788, 200, 789, 368, 872, 117, 677, 89, 509, 5, 845, 341, 593, 761, 257, 873, 929, 173, 956, 369, 957, 1013, 1040, 425, 1097, 1124, 1181, 53, 137, 221, 305, 389, 473, 557, 641, 453, 117, 537, 285, 621, 33, 705, 789, 201, 369, 873, 678, 90, 510, 6, 846, 342, 594, 762, 258, 930, 174, 957, 1014, 1041, 426, 1098, 1125, 1182, 49, 133, 217, 301, 385, 469, 553, 202, 454, 34, 538, 280, 112, 532, 616, 286, 622, 700, 28, 706, 784, 364, 790, 868, 118, 679, 91, 511, 7, 847, 343, 595, 763, 259, 874, 931, 952, 175, 370, 958, 1015, 1036, 196, 427, 1099, 1120, 448, 1183, 1204, 50, 133, 134, 217, 218, 301, 302, 385, 49, 386, 469, 470, 553, 554, 637, 721, 805, 889, 973, 1057, 203, 454, 118, 455, 538, 35, 539, 286, 281, 113, 533, 617, 622, 34, 287, 623, 701, 29, 706, 707, 785, 365, 790, 202, 791, 869, 370, 874, 119, 680, 92, 512, 8, 848, 344, 596, 764, 260, 875, 932, 953, 176, 958, 371, 959, 1016, 1037, 197, 1042, 428, 1100, 1121, 449, 1126, 1184, 1205, 54, 138, 222, 306, 217, 133, 49, 301, 385, 390, 469, 474, 553, 558, 637, 721, 805, 889, 973, 1057, 1141, 207, 459, 39, 543, 286, 118, 538, 622, 291, 627, 706, 34, 711, 790, 370, 795, 874, 123, 685, 97, 517, 13, 853, 349, 601, 769, 265, 879, 937, 958, 181, 375, 963, 1021, 1042, 202, 433, 1105, 1126, 454, 1189, 1210, 1225, 54, 138, 222, 306, 390, 474, 558, 51, 134, 135, 218, 219, 302, 303, 386, 50, 387, 470, 471, 554, 555, 638, 642, 722, 806, 890, 974, 1058, 204, 455, 119, 456, 539, 36, 540, 287, 282, 114, 534, 618, 623, 35, 288, 624, 702, 30, 707, 708, 786, 366, 791, 203, 792, 870, 371, 875, 120, 681, 93, 513, 9, 849, 345, 597, 765, 261, 876, 933, 954, 177, 959, 372, 960, 1017, 1038, 198, 1043, 429, 1101, 1122, 450, 1127, 1185, 1206, 55, 138, 139, 222, 223, 306, 307, 390, 218, 134, 50, 302, 386, 54, 391, 470, 474, 475, 554, 558, 559, 638, 642, 722, 726, 806, 810, 890, 894, 974, 978, 1058, 1062, 1142, 208, 459, 123, 460, 543, 40, 544, 291, 287, 119, 539, 623, 627, 39, 292, 628, 707, 35, 711, 712, 791, 371, 795, 207, 796, 875, 375, 879, 124, 686, 98, 518, 14, 854, 350, 602, 770, 266, 880, 938, 959, 182, 963, 376, 964, 1022, 1043, 203, 1047, 434, 1106, 1127, 455, 1131, 1190, 1211, 1226, 58, 142, 226, 310, 222, 138, 54, 306, 390, 394, 474, 478, 558, 562, 642, 726, 810, 894, 978, 1062, 1146, 211, 463, 43, 547, 291, 123, 543, 627, 295, 631, 711, 39, 715, 795, 375, 799, 879, 127, 690, 102, 522, 18, 858, 354, 606, 774, 270, 883, 942, 963, 186, 379, 967, 1026, 1047, 207, 438, 1110, 1131, 459, 1194, 1215, 1230, 55, 139, 223, 307, 391, 475, 559, 52, 135, 136, 219, 220, 303, 304, 387, 51, 388, 471, 472, 555, 556, 639, 643, 723, 807, 891, 975, 1059, 205, 456, 120, 457, 540, 37, 541, 288, 283, 115, 535, 619, 624, 36, 289, 625, 703, 31, 708, 709, 787, 367, 792, 204, 793, 871, 372, 876, 121, 682, 94, 514, 10, 850, 346, 598, 766, 262, 877, 934, 955, 178, 960, 373, 961, 1018, 1039, 199, 1044, 430, 1102, 1123, 451, 1128, 1186, 1207, 58, 142, 226, 310, 394, 478, 562, 56, 139, 140, 223, 224, 307, 308, 391, 219, 135, 51, 303, 387, 55, 392, 471, 475, 476, 555, 559, 560, 639, 643, 646, 723, 727, 807, 811, 891, 895, 975, 979, 1059, 1063, 1143, 209, 460, 124, 461, 544, 41, 545, 292, 288, 120, 540, 624, 628, 40, 293, 629, 708, 36, 712, 713, 792, 372, 796, 208, 797, 876, 376, 880, 125, 687, 99, 519, 15, 855, 351, 603, 771, 267, 881, 939, 960, 183, 964, 377, 965, 1023, 1044, 204, 1048, 435, 1107, 1128, 456, 1132, 1191, 1212, 1227, 59, 142, 143, 226, 227, 310, 311, 394, 223, 139, 55, 307, 391, 58, 395, 475, 478, 479, 559, 562, 563, 643, 646, 727, 730, 811, 814, 895, 898, 979, 982, 1063, 1066, 1147, 212, 463, 127, 464, 547, 44, 548, 295, 292, 124, 544, 628, 631, 43, 296, 632, 712, 40, 715, 716, 796, 376, 799, 211, 800, 880, 379, 883, 128, 691, 103, 523, 19, 859, 355, 607, 775, 271, 884, 943, 964, 187, 967, 380, 968, 1027, 1048, 208, 1051, 439, 1111, 1132, 460, 1135, 1195, 1216, 1231, 46, 130, 214, 298, 211, 127, 43, 295, 379, 382, 463, 466, 547, 550, 631, 715, 799, 883, 967, 1051, 1135, 193, 445, 25, 529, 274, 106, 526, 610, 277, 613, 694, 22, 697, 778, 358, 781, 862, 109, 865, 946, 361, 949, 1030, 190, 1114, 442, 1198, 1219, 56, 140, 224, 308, 392, 476, 560, 53, 136, 137, 220, 221, 304, 305, 388, 52, 389, 472, 473, 556, 557, 640, 644, 724, 808, 892, 976, 1060, 206, 457, 121, 458, 541, 38, 542, 289, 284, 116, 536, 620, 625, 37, 290, 626, 704, 32, 709, 710, 788, 368, 793, 205, 794, 872, 373, 877, 122, 683, 95, 515, 11, 851, 347, 599, 767, 263, 878, 935, 956, 179, 961, 374, 962, 1019, 1040, 200, 1045, 431, 1103, 1124, 452, 1129, 1187, 1208, 59, 143, 227, 311, 395, 479, 563, 57, 140, 141, 224, 225, 308, 309, 392, 220, 136, 52, 304, 388, 56, 393, 472, 476, 477, 556, 560, 561, 640, 644, 647, 724, 728, 808, 812, 892, 896, 976, 980, 1060, 1064, 1144, 210, 461, 125, 462, 545, 42, 546, 293, 289, 121, 541, 625, 629, 41, 294, 630, 709, 37, 713, 714, 793, 373, 797, 209, 798, 877, 377, 881, 126, 688, 100, 520, 16, 856, 352, 604, 772, 268, 882, 940, 961, 184, 965, 378, 966, 1024, 1045, 205, 1049, 436, 1108, 1129, 457, 1133, 1192, 1213, 1228, 61, 145, 229, 313, 397, 481, 565, 60, 143, 144, 227, 228, 311, 312, 395, 224, 140, 56, 308, 392, 59, 396, 476, 479, 480, 560, 563, 564, 644, 647, 649, 728, 731, 812, 815, 896, 899, 980, 983, 1064, 1067, 1148, 213, 464, 128, 465, 548, 45, 549, 296, 293, 125, 545, 629, 632, 44, 297, 633, 713, 41, 716, 717, 797, 377, 800, 212, 801, 881, 380, 884, 129, 692, 104, 524, 20, 860, 356, 608, 776, 272, 885, 944, 965, 188, 968, 381, 969, 1028, 1049, 209, 1052, 440, 1112, 1133, 461, 1136, 1196, 1217, 1232, 47, 130, 131, 214, 215, 298, 299, 382, 212, 128, 44, 296, 380, 46, 383, 464, 466, 467, 548, 550, 551, 632, 634, 716, 718, 800, 802, 884, 886, 968, 970, 1052, 1054, 1136, 194, 445, 109, 446, 529, 26, 530, 277, 275, 107, 527, 611, 613, 25, 278, 614, 695, 23, 697, 698, 779, 359, 781, 193, 782, 863, 361, 865, 110, 866, 947, 949, 362, 950, 1031, 191, 1033, 1115, 443, 1117, 1199, 1220, 48, 132, 216, 300, 214, 130, 46, 298, 382, 384, 466, 468, 550, 552, 634, 718, 802, 886, 970, 1054, 1138, 195, 447, 27, 531, 277, 109, 529, 613, 279, 615, 697, 25, 699, 781, 361, 783, 865, 111, 867, 949, 363, 951, 1033, 193, 1117, 445, 1201, 1222, 57, 141, 225, 309, 393, 477, 561, 137, 221, 305, 389, 53, 473, 557, 641, 645, 725, 809, 893, 977, 1061, 458, 122, 542, 290, 285, 117, 537, 621, 626, 38, 705, 33, 710, 789, 369, 794, 206, 873, 374, 878, 684, 96, 516, 12, 852, 348, 600, 768, 264, 936, 957, 180, 962, 1020, 1041, 201, 1046, 432, 1104, 1125, 453, 1130, 1188, 1209, 60, 144, 228, 312, 396, 480, 564, 141, 225, 309, 393, 221, 137, 53, 305, 389, 57, 473, 477, 557, 561, 641, 645, 648, 725, 729, 809, 813, 893, 897, 977, 981, 1061, 1065, 1145, 462, 126, 546, 294, 290, 122, 542, 626, 630, 42, 710, 38, 714, 794, 374, 798, 210, 878, 378, 882, 689, 101, 521, 17, 857, 353, 605, 773, 269, 941, 962, 185, 966, 1025, 1046, 206, 1050, 437, 1109, 1130, 458, 1134, 1193, 1214, 1229, 47, 131, 215, 299, 383, 467, 551, 129, 213, 297, 381, 210, 126, 42, 294, 378, 45, 462, 465, 546, 549, 630, 633, 635, 714, 717, 798, 801, 882, 885, 966, 969, 1050, 1053, 1134, 444, 108, 528, 276, 273, 105, 525, 609, 612, 24, 693, 21, 696, 777, 357, 780, 192, 861, 360, 864, 945, 948, 1029, 189, 1032, 1113, 441, 1116, 1197, 1218, 48, 132, 216, 300, 384, 468, 552, 131, 215, 299, 383, 213, 129, 45, 297, 381, 47, 465, 467, 549, 551, 633, 635, 636, 717, 719, 801, 803, 885, 887, 969, 971, 1053, 1055, 1137, 446, 110, 530, 278, 276, 108, 528, 612, 614, 26, 696, 24, 698, 780, 360, 782, 194, 864, 362, 866, 948, 950, 1032, 192, 1034, 1116, 444, 1118, 1200, 1221, 132, 216, 300, 384, 215, 131, 47, 299, 383, 48, 467, 468, 551, 552, 635, 636, 719, 720, 803, 804, 887, 888, 971, 972, 1055, 1056, 1139, 447, 111, 531, 279, 278, 110, 530, 614, 615, 27, 698, 26, 699, 782, 362, 783, 195, 866, 363, 867, 950, 951, 1034, 194, 1035, 1118, 446, 1119, 1202, 1223, 195, 111, 27, 279, 363, 447, 531, 615, 699, 783, 867, 951, 1035, 1119, 1203}; static const int coeffs1_ind[] = { 755, 167, 587, 83, 923, 419, 671, 839, 335, 1007, 251, 1091, 503, 1175, 1259, 251, 503, 83, 587, 335, 671, 755, 839, 167, 752, 164, 584, 80, 920, 416, 668, 836, 332, 923, 1004, 248, 419, 1007, 1088, 500, 1172, 1256, 248, 500, 80, 584, 332, 668, 752, 836, 164, 746, 158, 578, 74, 914, 410, 662, 830, 326, 920, 998, 242, 416, 1004, 1082, 494, 1166, 1250, 242, 494, 74, 578, 326, 662, 746, 830, 158, 736, 148, 568, 64, 904, 400, 652, 820, 316, 914, 988, 232, 410, 998, 1072, 484, 1156, 1240, 232, 484, 64, 568, 316, 652, 736, 820, 148, 721, 133, 553, 49, 889, 385, 637, 805, 301, 904, 973, 217, 400, 988, 1057, 469, 1141, 1225, 217, 469, 49, 553, 301, 637, 721, 805, 133, 700, 112, 532, 28, 868, 364, 616, 784, 280, 889, 952, 196, 385, 973, 1036, 448, 1120, 1204, 218, 469, 133, 470, 553, 50, 554, 301, 637, 49, 302, 638, 721, 722, 805, 217, 806, 385, 889, 134, 701, 113, 533, 29, 869, 365, 617, 785, 281, 890, 953, 197, 973, 386, 974, 1037, 1057, 449, 1121, 1141, 1205, 64, 148, 232, 316, 400, 484, 568, 222, 474, 54, 558, 301, 133, 553, 637, 306, 642, 721, 49, 726, 805, 385, 810, 889, 138, 706, 118, 538, 34, 874, 370, 622, 790, 286, 894, 958, 973, 202, 390, 978, 1042, 1057, 217, 454, 1126, 1141, 469, 1210, 1225, 233, 484, 148, 485, 568, 65, 569, 316, 652, 64, 317, 653, 736, 737, 820, 232, 821, 400, 904, 149, 722, 134, 554, 50, 890, 386, 638, 806, 302, 905, 974, 218, 988, 401, 989, 1058, 1072, 470, 1142, 1156, 1226, 64, 148, 232, 316, 400, 484, 568, 652, 219, 470, 134, 471, 554, 51, 555, 302, 638, 50, 303, 639, 722, 723, 806, 218, 807, 386, 890, 135, 702, 114, 534, 30, 870, 366, 618, 786, 282, 891, 954, 198, 974, 387, 975, 1038, 1058, 450, 1122, 1142, 1206, 65, 148, 149, 232, 233, 316, 317, 400, 64, 401, 484, 485, 568, 569, 652, 736, 820, 904, 988, 1072, 223, 474, 138, 475, 558, 55, 559, 306, 302, 134, 554, 638, 642, 54, 307, 643, 722, 50, 726, 727, 806, 386, 810, 222, 811, 890, 390, 894, 139, 707, 119, 539, 35, 875, 371, 623, 791, 287, 895, 959, 974, 203, 978, 391, 979, 1043, 1058, 218, 1062, 455, 1127, 1142, 470, 1146, 1211, 1226, 74, 158, 242, 326, 410, 494, 578, 236, 488, 68, 572, 316, 148, 568, 652, 320, 656, 736, 64, 740, 820, 400, 824, 904, 152, 726, 138, 558, 54, 894, 390, 642, 810, 306, 908, 978, 988, 222, 404, 992, 1062, 1072, 232, 474, 1146, 1156, 484, 1230, 1240, 68, 152, 236, 320, 232, 148, 64, 316, 400, 404, 484, 488, 568, 572, 652, 736, 820, 904, 988, 1072, 1156, 226, 478, 58, 562, 306, 138, 558, 642, 310, 646, 726, 54, 730, 810, 390, 814, 894, 142, 711, 123, 543, 39, 879, 375, 627, 795, 291, 898, 963, 978, 207, 394, 982, 1047, 1062, 222, 459, 1131, 1146, 474, 1215, 1230, 1240, 243, 494, 158, 495, 578, 75, 579, 326, 662, 74, 327, 663, 746, 747, 830, 242, 831, 410, 914, 159, 737, 149, 569, 65, 905, 401, 653, 821, 317, 915, 989, 233, 998, 411, 999, 1073, 1082, 485, 1157, 1166, 1241, 74, 158, 242, 326, 410, 494, 578, 662, 234, 485, 149, 486, 569, 66, 570, 317, 653, 65, 318, 654, 737, 738, 821, 233, 822, 401, 905, 150, 723, 135, 555, 51, 891, 387, 639, 807, 303, 906, 975, 219, 989, 402, 990, 1059, 1073, 471, 1143, 1157, 1227, 65, 149, 233, 317, 401, 485, 569, 653, 220, 471, 135, 472, 555, 52, 556, 303, 639, 51, 304, 640, 723, 724, 807, 219, 808, 387, 891, 136, 703, 115, 535, 31, 871, 367, 619, 787, 283, 892, 955, 199, 975, 388, 976, 1039, 1059, 451, 1123, 1143, 1207, 68, 152, 236, 320, 404, 488, 572, 66, 149, 150, 233, 234, 317, 318, 401, 65, 402, 485, 486, 569, 570, 653, 656, 737, 821, 905, 989, 1073, 224, 475, 139, 476, 559, 56, 560, 307, 303, 135, 555, 639, 643, 55, 308, 644, 723, 51, 727, 728, 807, 387, 811, 223, 812, 891, 391, 895, 140, 708, 120, 540, 36, 876, 372, 624, 792, 288, 896, 960, 975, 204, 979, 392, 980, 1044, 1059, 219, 1063, 456, 1128, 1143, 471, 1147, 1212, 1227, 75, 158, 159, 242, 243, 326, 327, 410, 74, 411, 494, 495, 578, 579, 662, 746, 830, 914, 998, 1082, 237, 488, 152, 489, 572, 69, 573, 320, 317, 149, 569, 653, 656, 68, 321, 657, 737, 65, 740, 741, 821, 401, 824, 236, 825, 905, 404, 908, 153, 727, 139, 559, 55, 895, 391, 643, 811, 307, 909, 979, 989, 223, 992, 405, 993, 1063, 1073, 233, 1076, 475, 1147, 1157, 485, 1160, 1231, 1241, 69, 152, 153, 236, 237, 320, 321, 404, 233, 149, 65, 317, 401, 68, 405, 485, 488, 489, 569, 572, 573, 653, 656, 737, 740, 821, 824, 905, 908, 989, 992, 1073, 1076, 1157, 227, 478, 142, 479, 562, 59, 563, 310, 307, 139, 559, 643, 646, 58, 311, 647, 727, 55, 730, 731, 811, 391, 814, 226, 815, 895, 394, 898, 143, 712, 124, 544, 40, 880, 376, 628, 796, 292, 899, 964, 979, 208, 982, 395, 983, 1048, 1063, 223, 1066, 460, 1132, 1147, 475, 1150, 1216, 1231, 1241, 80, 164, 248, 332, 416, 500, 584, 245, 497, 77, 581, 326, 158, 578, 662, 329, 665, 746, 74, 749, 830, 410, 833, 914, 161, 740, 152, 572, 68, 908, 404, 656, 824, 320, 917, 992, 998, 236, 413, 1001, 1076, 1082, 242, 488, 1160, 1166, 494, 1244, 1250, 77, 161, 245, 329, 242, 158, 74, 326, 410, 413, 494, 497, 578, 581, 662, 746, 830, 914, 998, 1082, 1166, 239, 491, 71, 575, 320, 152, 572, 656, 323, 659, 740, 68, 743, 824, 404, 827, 908, 155, 730, 142, 562, 58, 898, 394, 646, 814, 310, 911, 982, 992, 226, 407, 995, 1066, 1076, 236, 478, 1150, 1160, 488, 1234, 1244, 1250, 71, 155, 239, 323, 236, 152, 68, 320, 404, 407, 488, 491, 572, 575, 656, 740, 824, 908, 992, 1076, 1160, 229, 481, 61, 565, 310, 142, 562, 646, 313, 649, 730, 58, 733, 814, 394, 817, 898, 145, 715, 127, 547, 43, 883, 379, 631, 799, 295, 901, 967, 982, 211, 397, 985, 1051, 1066, 226, 463, 1135, 1150, 478, 1219, 1234, 1244, 61, 145, 229, 313, 226, 142, 58, 310, 394, 397, 478, 481, 562, 565, 646, 730, 814, 898, 982, 1066, 1150, 214, 466, 46, 550, 295, 127, 547, 631, 298, 634, 715, 43, 718, 799, 379, 802, 883, 130, 694, 106, 526, 22, 862, 358, 610, 778, 274, 886, 946, 967, 190, 382, 970, 1030, 1051, 211, 442, 1114, 1135, 463, 1198, 1219, 1234, 249, 500, 164, 501, 584, 81, 585, 332, 668, 80, 333, 669, 752, 753, 836, 248, 837, 416, 920, 165, 747, 159, 579, 75, 915, 411, 663, 831, 327, 921, 999, 243, 1004, 417, 1005, 1083, 1088, 495, 1167, 1172, 1251, 80, 164, 248, 332, 416, 500, 584, 668, 244, 495, 159, 496, 579, 76, 580, 327, 663, 75, 328, 664, 747, 748, 831, 243, 832, 411, 915, 160, 738, 150, 570, 66, 906, 402, 654, 822, 318, 916, 990, 234, 999, 412, 1000, 1074, 1083, 486, 1158, 1167, 1242, 75, 159, 243, 327, 411, 495, 579, 663, 235, 486, 150, 487, 570, 67, 571, 318, 654, 66, 319, 655, 738, 739, 822, 234, 823, 402, 906, 151, 724, 136, 556, 52, 892, 388, 640, 808, 304, 907, 976, 220, 990, 403, 991, 1060, 1074, 472, 1144, 1158, 1228, 66, 150, 234, 318, 402, 486, 570, 654, 221, 472, 136, 473, 556, 53, 557, 304, 640, 52, 305, 641, 724, 725, 808, 220, 809, 388, 892, 137, 704, 116, 536, 32, 872, 368, 620, 788, 284, 893, 956, 200, 976, 389, 977, 1040, 1060, 452, 1124, 1144, 1208, 69, 153, 237, 321, 405, 489, 573, 67, 150, 151, 234, 235, 318, 319, 402, 66, 403, 486, 487, 570, 571, 654, 657, 738, 822, 906, 990, 1074, 225, 476, 140, 477, 560, 57, 561, 308, 304, 136, 556, 640, 644, 56, 309, 645, 724, 52, 728, 729, 808, 388, 812, 224, 813, 892, 392, 896, 141, 709, 121, 541, 37, 877, 373, 625, 793, 289, 897, 961, 976, 205, 980, 393, 981, 1045, 1060, 220, 1064, 457, 1129, 1144, 472, 1148, 1213, 1228, 77, 161, 245, 329, 413, 497, 581, 76, 159, 160, 243, 244, 327, 328, 411, 75, 412, 495, 496, 579, 580, 663, 665, 747, 831, 915, 999, 1083, 238, 489, 153, 490, 573, 70, 574, 321, 318, 150, 570, 654, 657, 69, 322, 658, 738, 66, 741, 742, 822, 402, 825, 237, 826, 906, 405, 909, 154, 728, 140, 560, 56, 896, 392, 644, 812, 308, 910, 980, 990, 224, 993, 406, 994, 1064, 1074, 234, 1077, 476, 1148, 1158, 486, 1161, 1232, 1242, 71, 155, 239, 323, 407, 491, 575, 70, 153, 154, 237, 238, 321, 322, 405, 234, 150, 66, 318, 402, 69, 406, 486, 489, 490, 570, 573, 574, 654, 657, 659, 738, 741, 822, 825, 906, 909, 990, 993, 1074, 1077, 1158, 228, 479, 143, 480, 563, 60, 564, 311, 308, 140, 560, 644, 647, 59, 312, 648, 728, 56, 731, 732, 812, 392, 815, 227, 816, 896, 395, 899, 144, 713, 125, 545, 41, 881, 377, 629, 797, 293, 900, 965, 980, 209, 983, 396, 984, 1049, 1064, 224, 1067, 461, 1133, 1148, 476, 1151, 1217, 1232, 1242, 81, 164, 165, 248, 249, 332, 333, 416, 80, 417, 500, 501, 584, 585, 668, 752, 836, 920, 1004, 1088, 246, 497, 161, 498, 581, 78, 582, 329, 327, 159, 579, 663, 665, 77, 330, 666, 747, 75, 749, 750, 831, 411, 833, 245, 834, 915, 413, 917, 162, 741, 153, 573, 69, 909, 405, 657, 825, 321, 918, 993, 999, 237, 1001, 414, 1002, 1077, 1083, 243, 1085, 489, 1161, 1167, 495, 1169, 1245, 1251, 78, 161, 162, 245, 246, 329, 330, 413, 243, 159, 75, 327, 411, 77, 414, 495, 497, 498, 579, 581, 582, 663, 665, 747, 749, 831, 833, 915, 917, 999, 1001, 1083, 1085, 1167, 240, 491, 155, 492, 575, 72, 576, 323, 321, 153, 573, 657, 659, 71, 324, 660, 741, 69, 743, 744, 825, 405, 827, 239, 828, 909, 407, 911, 156, 731, 143, 563, 59, 899, 395, 647, 815, 311, 912, 983, 993, 227, 995, 408, 996, 1067, 1077, 237, 1079, 479, 1151, 1161, 489, 1163, 1235, 1245, 1251, 72, 155, 156, 239, 240, 323, 324, 407, 237, 153, 69, 321, 405, 71, 408, 489, 491, 492, 573, 575, 576, 657, 659, 741, 743, 825, 827, 909, 911, 993, 995, 1077, 1079, 1161, 230, 481, 145, 482, 565, 62, 566, 313, 311, 143, 563, 647, 649, 61, 314, 650, 731, 59, 733, 734, 815, 395, 817, 229, 818, 899, 397, 901, 146, 716, 128, 548, 44, 884, 380, 632, 800, 296, 902, 968, 983, 212, 985, 398, 986, 1052, 1067, 227, 1069, 464, 1136, 1151, 479, 1153, 1220, 1235, 1245, 62, 145, 146, 229, 230, 313, 314, 397, 227, 143, 59, 311, 395, 61, 398, 479, 481, 482, 563, 565, 566, 647, 649, 731, 733, 815, 817, 899, 901, 983, 985, 1067, 1069, 1151, 215, 466, 130, 467, 550, 47, 551, 298, 296, 128, 548, 632, 634, 46, 299, 635, 716, 44, 718, 719, 800, 380, 802, 214, 803, 884, 382, 886, 131, 695, 107, 527, 23, 863, 359, 611, 779, 275, 887, 947, 968, 191, 970, 383, 971, 1031, 1052, 212, 1054, 443, 1115, 1136, 464, 1138, 1199, 1220, 1235, 83, 167, 251, 335, 419, 503, 587, 250, 502, 82, 586, 332, 164, 584, 668, 334, 670, 752, 80, 754, 836, 416, 838, 920, 166, 749, 161, 581, 77, 917, 413, 665, 833, 329, 922, 1001, 1004, 245, 418, 1006, 1085, 1088, 248, 497, 1169, 1172, 500, 1253, 1256, 82, 166, 250, 334, 248, 164, 80, 332, 416, 418, 500, 502, 584, 586, 668, 752, 836, 920, 1004, 1088, 1172, 247, 499, 79, 583, 329, 161, 581, 665, 331, 667, 749, 77, 751, 833, 413, 835, 917, 163, 743, 155, 575, 71, 911, 407, 659, 827, 323, 919, 995, 1001, 239, 415, 1003, 1079, 1085, 245, 491, 1163, 1169, 497, 1247, 1253, 1256, 79, 163, 247, 331, 245, 161, 77, 329, 413, 415, 497, 499, 581, 583, 665, 749, 833, 917, 1001, 1085, 1169, 241, 493, 73, 577, 323, 155, 575, 659, 325, 661, 743, 71, 745, 827, 407, 829, 911, 157, 733, 145, 565, 61, 901, 397, 649, 817, 313, 913, 985, 995, 229, 409, 997, 1069, 1079, 239, 481, 1153, 1163, 491, 1237, 1247, 1253, 73, 157, 241, 325, 239, 155, 71, 323, 407, 409, 491, 493, 575, 577, 659, 743, 827, 911, 995, 1079, 1163, 231, 483, 63, 567, 313, 145, 565, 649, 315, 651, 733, 61, 735, 817, 397, 819, 901, 147, 718, 130, 550, 46, 886, 382, 634, 802, 298, 903, 970, 985, 214, 399, 987, 1054, 1069, 229, 466, 1138, 1153, 481, 1222, 1237, 1247, 63, 147, 231, 315, 229, 145, 61, 313, 397, 399, 481, 483, 565, 567, 649, 733, 817, 901, 985, 1069, 1153, 216, 468, 48, 552, 298, 130, 550, 634, 300, 636, 718, 46, 720, 802, 382, 804, 886, 132, 697, 109, 529, 25, 865, 361, 613, 781, 277, 888, 949, 970, 193, 384, 972, 1033, 1054, 214, 445, 1117, 1138, 466, 1201, 1222, 1237, 503, 167, 587, 335, 671, 83, 755, 839, 251, 419, 923, 753, 165, 585, 81, 921, 417, 669, 837, 333, 1005, 249, 1007, 1089, 1091, 501, 1173, 1175, 1257, 83, 167, 251, 335, 419, 503, 587, 671, 501, 165, 585, 333, 669, 81, 753, 837, 249, 417, 921, 748, 160, 580, 76, 916, 412, 664, 832, 328, 1000, 244, 1005, 1084, 1089, 496, 1168, 1173, 1252, 81, 165, 249, 333, 417, 501, 585, 669, 496, 160, 580, 328, 664, 76, 748, 832, 244, 412, 916, 739, 151, 571, 67, 907, 403, 655, 823, 319, 991, 235, 1000, 1075, 1084, 487, 1159, 1168, 1243, 76, 160, 244, 328, 412, 496, 580, 664, 487, 151, 571, 319, 655, 67, 739, 823, 235, 403, 907, 725, 137, 557, 53, 893, 389, 641, 809, 305, 977, 221, 991, 1061, 1075, 473, 1145, 1159, 1229, 67, 151, 235, 319, 403, 487, 571, 655, 473, 137, 557, 305, 641, 53, 725, 809, 221, 389, 893, 705, 117, 537, 33, 873, 369, 621, 789, 285, 957, 201, 977, 1041, 1061, 453, 1125, 1145, 1209, 70, 154, 238, 322, 406, 490, 574, 151, 235, 319, 403, 67, 487, 571, 655, 658, 739, 823, 907, 991, 1075, 477, 141, 561, 309, 305, 137, 557, 641, 645, 57, 725, 53, 729, 809, 389, 813, 225, 893, 393, 897, 710, 122, 542, 38, 878, 374, 626, 794, 290, 962, 977, 206, 981, 1046, 1061, 221, 1065, 458, 1130, 1145, 473, 1149, 1214, 1229, 78, 162, 246, 330, 414, 498, 582, 160, 244, 328, 412, 76, 496, 580, 664, 666, 748, 832, 916, 1000, 1084, 490, 154, 574, 322, 319, 151, 571, 655, 658, 70, 739, 67, 742, 823, 403, 826, 238, 907, 406, 910, 729, 141, 561, 57, 897, 393, 645, 813, 309, 981, 991, 225, 994, 1065, 1075, 235, 1078, 477, 1149, 1159, 487, 1162, 1233, 1243, 72, 156, 240, 324, 408, 492, 576, 154, 238, 322, 406, 235, 151, 67, 319, 403, 70, 487, 490, 571, 574, 655, 658, 660, 739, 742, 823, 826, 907, 910, 991, 994, 1075, 1078, 1159, 480, 144, 564, 312, 309, 141, 561, 645, 648, 60, 729, 57, 732, 813, 393, 816, 228, 897, 396, 900, 714, 126, 546, 42, 882, 378, 630, 798, 294, 966, 981, 210, 984, 1050, 1065, 225, 1068, 462, 1134, 1149, 477, 1152, 1218, 1233, 1243, 62, 146, 230, 314, 398, 482, 566, 144, 228, 312, 396, 225, 141, 57, 309, 393, 60, 477, 480, 561, 564, 645, 648, 650, 729, 732, 813, 816, 897, 900, 981, 984, 1065, 1068, 1149, 465, 129, 549, 297, 294, 126, 546, 630, 633, 45, 714, 42, 717, 798, 378, 801, 213, 882, 381, 885, 693, 105, 525, 21, 861, 357, 609, 777, 273, 945, 966, 189, 969, 1029, 1050, 210, 1053, 441, 1113, 1134, 462, 1137, 1197, 1218, 1233, 82, 166, 250, 334, 418, 502, 586, 165, 249, 333, 417, 81, 501, 585, 669, 670, 753, 837, 921, 1005, 1089, 498, 162, 582, 330, 328, 160, 580, 664, 666, 78, 748, 76, 750, 832, 412, 834, 246, 916, 414, 918, 742, 154, 574, 70, 910, 406, 658, 826, 322, 994, 1000, 238, 1002, 1078, 1084, 244, 1086, 490, 1162, 1168, 496, 1170, 1246, 1252, 79, 163, 247, 331, 415, 499, 583, 162, 246, 330, 414, 244, 160, 76, 328, 412, 78, 496, 498, 580, 582, 664, 666, 667, 748, 750, 832, 834, 916, 918, 1000, 1002, 1084, 1086, 1168, 492, 156, 576, 324, 322, 154, 574, 658, 660, 72, 742, 70, 744, 826, 406, 828, 240, 910, 408, 912, 732, 144, 564, 60, 900, 396, 648, 816, 312, 984, 994, 228, 996, 1068, 1078, 238, 1080, 480, 1152, 1162, 490, 1164, 1236, 1246, 1252, 73, 157, 241, 325, 409, 493, 577, 156, 240, 324, 408, 238, 154, 70, 322, 406, 72, 490, 492, 574, 576, 658, 660, 661, 742, 744, 826, 828, 910, 912, 994, 996, 1078, 1080, 1162, 482, 146, 566, 314, 312, 144, 564, 648, 650, 62, 732, 60, 734, 816, 396, 818, 230, 900, 398, 902, 717, 129, 549, 45, 885, 381, 633, 801, 297, 969, 984, 213, 986, 1053, 1068, 228, 1070, 465, 1137, 1152, 480, 1154, 1221, 1236, 1246, 63, 147, 231, 315, 399, 483, 567, 146, 230, 314, 398, 228, 144, 60, 312, 396, 62, 480, 482, 564, 566, 648, 650, 651, 732, 734, 816, 818, 900, 902, 984, 986, 1068, 1070, 1152, 467, 131, 551, 299, 297, 129, 549, 633, 635, 47, 717, 45, 719, 801, 381, 803, 215, 885, 383, 887, 696, 108, 528, 24, 864, 360, 612, 780, 276, 948, 969, 192, 971, 1032, 1053, 213, 1055, 444, 1116, 1137, 465, 1139, 1200, 1221, 1236, 167, 251, 335, 419, 83, 503, 587, 671, 755, 839, 923, 1007, 1091, 502, 166, 586, 334, 333, 165, 585, 669, 670, 82, 753, 81, 754, 837, 417, 838, 250, 921, 418, 922, 750, 162, 582, 78, 918, 414, 666, 834, 330, 1002, 1005, 246, 1006, 1086, 1089, 249, 1090, 498, 1170, 1173, 501, 1174, 1254, 1257, 166, 250, 334, 418, 249, 165, 81, 333, 417, 82, 501, 502, 585, 586, 669, 670, 753, 754, 837, 838, 921, 922, 1005, 1006, 1089, 1090, 1173, 499, 163, 583, 331, 330, 162, 582, 666, 667, 79, 750, 78, 751, 834, 414, 835, 247, 918, 415, 919, 744, 156, 576, 72, 912, 408, 660, 828, 324, 996, 1002, 240, 1003, 1080, 1086, 246, 1087, 492, 1164, 1170, 498, 1171, 1248, 1254, 1257, 163, 247, 331, 415, 246, 162, 78, 330, 414, 79, 498, 499, 582, 583, 666, 667, 750, 751, 834, 835, 918, 919, 1002, 1003, 1086, 1087, 1170, 493, 157, 577, 325, 324, 156, 576, 660, 661, 73, 744, 72, 745, 828, 408, 829, 241, 912, 409, 913, 734, 146, 566, 62, 902, 398, 650, 818, 314, 986, 996, 230, 997, 1070, 1080, 240, 1081, 482, 1154, 1164, 492, 1165, 1238, 1248, 1254, 157, 241, 325, 409, 240, 156, 72, 324, 408, 73, 492, 493, 576, 577, 660, 661, 744, 745, 828, 829, 912, 913, 996, 997, 1080, 1081, 1164, 483, 147, 567, 315, 314, 146, 566, 650, 651, 63, 734, 62, 735, 818, 398, 819, 231, 902, 399, 903, 719, 131, 551, 47, 887, 383, 635, 803, 299, 971, 986, 215, 987, 1055, 1070, 230, 1071, 467, 1139, 1154, 482, 1155, 1223, 1238, 1248, 147, 231, 315, 399, 230, 146, 62, 314, 398, 63, 482, 483, 566, 567, 650, 651, 734, 735, 818, 819, 902, 903, 986, 987, 1070, 1071, 1154, 468, 132, 552, 300, 299, 131, 551, 635, 636, 48, 719, 47, 720, 803, 383, 804, 216, 887, 384, 888, 698, 110, 530, 26, 866, 362, 614, 782, 278, 950, 971, 194, 972, 1034, 1055, 215, 1056, 446, 1118, 1139, 467, 1140, 1202, 1223, 1238, 335, 167, 587, 671, 755, 83, 839, 419, 923, 754, 166, 586, 82, 922, 418, 670, 838, 334, 1006, 1007, 250, 1090, 1091, 251, 502, 1174, 1175, 503, 1258, 1259, 251, 167, 83, 335, 419, 503, 587, 671, 755, 839, 923, 1007, 1091, 1175, 334, 166, 586, 670, 754, 82, 838, 418, 922, 751, 163, 583, 79, 919, 415, 667, 835, 331, 1003, 1006, 247, 1087, 1090, 250, 499, 1171, 1174, 502, 1255, 1258, 1259, 250, 166, 82, 334, 418, 502, 586, 670, 754, 838, 922, 1006, 1090, 1174, 331, 163, 583, 667, 751, 79, 835, 415, 919, 745, 157, 577, 73, 913, 409, 661, 829, 325, 997, 1003, 241, 1081, 1087, 247, 493, 1165, 1171, 499, 1249, 1255, 1258, 247, 163, 79, 331, 415, 499, 583, 667, 751, 835, 919, 1003, 1087, 1171, 325, 157, 577, 661, 745, 73, 829, 409, 913, 735, 147, 567, 63, 903, 399, 651, 819, 315, 987, 997, 231, 1071, 1081, 241, 483, 1155, 1165, 493, 1239, 1249, 1255, 241, 157, 73, 325, 409, 493, 577, 661, 745, 829, 913, 997, 1081, 1165, 315, 147, 567, 651, 735, 63, 819, 399, 903, 720, 132, 552, 48, 888, 384, 636, 804, 300, 972, 987, 216, 1056, 1071, 231, 468, 1140, 1155, 483, 1224, 1239, 1249, 231, 147, 63, 315, 399, 483, 567, 651, 735, 819, 903, 987, 1071, 1155, 300, 132, 552, 636, 720, 48, 804, 384, 888, 699, 111, 531, 27, 867, 363, 615, 783, 279, 951, 972, 195, 1035, 1056, 216, 447, 1119, 1140, 468, 1203, 1224, 1239, 216, 132, 48, 300, 384, 468, 552, 636, 720, 804, 888, 972, 1056, 1140, 279, 111, 531, 615, 699, 27, 783, 363, 867, 951, 1035, 195, 1119, 447, 1203, 1224}; static const int C0_ind[] = { 0, 1, 2, 3, 4, 5, 6, 30, 99, 100, 101, 102, 103, 104, 105, 129, 198, 199, 200, 201, 202, 203, 204, 228, 297, 298, 299, 300, 301, 302, 303, 327, 396, 397, 398, 399, 400, 401, 402, 426, 495, 496, 497, 498, 499, 500, 501, 525, 594, 595, 596, 597, 598, 599, 600, 624, 700, 702, 704, 706, 714, 717, 720, 799, 800, 801, 802, 803, 804, 805, 806, 812, 813, 815, 816, 818, 819, 821, 824, 826, 828, 830, 832, 891, 892, 893, 894, 895, 896, 897, 898, 899, 900, 901, 902, 903, 904, 905, 911, 912, 914, 915, 917, 918, 920, 921, 923, 925, 927, 929, 931, 990, 991, 992, 993, 994, 995, 996, 997, 998, 999, 1000, 1001, 1002, 1003, 1004, 1010, 1011, 1013, 1014, 1016, 1017, 1019, 1020, 1022, 1024, 1026, 1028, 1030, 1089, 1090, 1091, 1092, 1093, 1094, 1095, 1096, 1097, 1098, 1099, 1100, 1101, 1102, 1103, 1109, 1110, 1112, 1113, 1115, 1116, 1118, 1119, 1121, 1123, 1125, 1127, 1129, 1188, 1189, 1190, 1191, 1192, 1193, 1194, 1195, 1196, 1197, 1198, 1199, 1200, 1201, 1202, 1208, 1209, 1211, 1212, 1214, 1215, 1217, 1218, 1220, 1222, 1224, 1226, 1228, 1287, 1288, 1289, 1290, 1291, 1292, 1293, 1294, 1295, 1296, 1297, 1298, 1299, 1300, 1301, 1307, 1308, 1310, 1311, 1313, 1314, 1316, 1317, 1319, 1321, 1323, 1325, 1327, 1386, 1387, 1388, 1389, 1390, 1391, 1392, 1394, 1396, 1398, 1400, 1406, 1409, 1412, 1415, 1416, 1418, 1420, 1422, 1424, 1426, 1492, 1494, 1496, 1498, 1500, 1501, 1502, 1503, 1504, 1506, 1507, 1509, 1510, 1512, 1513, 1516, 1518, 1520, 1522, 1524, 1526, 1583, 1591, 1592, 1593, 1594, 1595, 1596, 1597, 1598, 1599, 1600, 1601, 1602, 1603, 1604, 1605, 1606, 1607, 1608, 1609, 1610, 1611, 1612, 1613, 1615, 1616, 1617, 1618, 1619, 1620, 1621, 1622, 1623, 1624, 1625, 1682, 1683, 1684, 1685, 1686, 1687, 1688, 1689, 1690, 1691, 1692, 1693, 1694, 1695, 1696, 1697, 1698, 1699, 1700, 1701, 1702, 1703, 1704, 1705, 1706, 1707, 1708, 1709, 1710, 1711, 1712, 1713, 1714, 1715, 1716, 1717, 1718, 1719, 1720, 1721, 1722, 1723, 1724, 1781, 1782, 1783, 1784, 1785, 1786, 1787, 1788, 1789, 1790, 1791, 1792, 1793, 1794, 1795, 1796, 1797, 1798, 1799, 1800, 1801, 1802, 1803, 1804, 1805, 1806, 1807, 1808, 1809, 1810, 1811, 1812, 1813, 1814, 1815, 1816, 1817, 1818, 1819, 1820, 1821, 1822, 1823, 1880, 1881, 1882, 1883, 1884, 1885, 1886, 1887, 1888, 1889, 1890, 1891, 1892, 1893, 1894, 1895, 1896, 1897, 1898, 1899, 1900, 1901, 1902, 1903, 1904, 1905, 1906, 1907, 1908, 1909, 1910, 1911, 1912, 1913, 1914, 1915, 1916, 1917, 1918, 1919, 1920, 1921, 1922, 1979, 1980, 1981, 1982, 1983, 1984, 1985, 1986, 1987, 1988, 1989, 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, 2020, 2021, 2078, 2079, 2080, 2081, 2082, 2083, 2084, 2085, 2087, 2089, 2091, 2093, 2094, 2095, 2096, 2097, 2098, 2099, 2101, 2102, 2104, 2105, 2107, 2108, 2109, 2110, 2111, 2112, 2113, 2114, 2115, 2116, 2117, 2118, 2119, 2120, 2177, 2185, 2187, 2189, 2191, 2193, 2194, 2195, 2196, 2197, 2199, 2200, 2202, 2203, 2205, 2206, 2209, 2211, 2213, 2215, 2217, 2219, 2276, 2284, 2285, 2286, 2287, 2288, 2289, 2290, 2291, 2292, 2293, 2294, 2295, 2296, 2297, 2298, 2299, 2300, 2301, 2302, 2303, 2304, 2305, 2306, 2308, 2309, 2310, 2311, 2312, 2313, 2314, 2315, 2316, 2317, 2318, 2375, 2376, 2377, 2378, 2379, 2380, 2381, 2382, 2383, 2384, 2385, 2386, 2387, 2388, 2389, 2390, 2391, 2392, 2393, 2394, 2395, 2396, 2397, 2398, 2399, 2400, 2401, 2402, 2403, 2404, 2405, 2406, 2407, 2408, 2409, 2410, 2411, 2412, 2413, 2414, 2415, 2416, 2417, 2474, 2475, 2476, 2477, 2478, 2479, 2480, 2481, 2482, 2483, 2484, 2485, 2486, 2487, 2488, 2489, 2490, 2491, 2492, 2493, 2494, 2495, 2496, 2497, 2498, 2499, 2500, 2501, 2502, 2503, 2504, 2505, 2506, 2507, 2508, 2509, 2510, 2511, 2512, 2513, 2514, 2515, 2516, 2573, 2574, 2575, 2576, 2577, 2578, 2579, 2580, 2581, 2582, 2583, 2584, 2585, 2586, 2587, 2588, 2589, 2590, 2591, 2592, 2593, 2594, 2595, 2596, 2597, 2598, 2599, 2600, 2601, 2602, 2603, 2604, 2605, 2606, 2607, 2608, 2609, 2610, 2611, 2612, 2613, 2614, 2615, 2672, 2673, 2674, 2675, 2676, 2677, 2678, 2679, 2681, 2683, 2685, 2687, 2688, 2689, 2690, 2691, 2692, 2693, 2695, 2696, 2698, 2699, 2701, 2702, 2703, 2704, 2705, 2706, 2707, 2708, 2709, 2710, 2711, 2712, 2713, 2714, 2771, 2779, 2781, 2783, 2785, 2787, 2788, 2789, 2790, 2791, 2793, 2794, 2796, 2797, 2799, 2800, 2803, 2805, 2807, 2809, 2811, 2813, 2870, 2878, 2879, 2880, 2881, 2882, 2883, 2884, 2885, 2886, 2887, 2888, 2889, 2890, 2891, 2892, 2893, 2894, 2895, 2896, 2897, 2898, 2899, 2900, 2902, 2903, 2904, 2905, 2906, 2907, 2908, 2909, 2910, 2911, 2912, 2969, 2970, 2971, 2972, 2973, 2974, 2975, 2976, 2977, 2978, 2979, 2980, 2981, 2982, 2983, 2984, 2985, 2986, 2987, 2988, 2989, 2990, 2991, 2992, 2993, 2994, 2995, 2996, 2997, 2998, 2999, 3000, 3001, 3002, 3003, 3004, 3005, 3006, 3007, 3008, 3009, 3010, 3011, 3068, 3069, 3070, 3071, 3072, 3073, 3074, 3075, 3076, 3077, 3078, 3079, 3080, 3081, 3082, 3083, 3084, 3085, 3086, 3087, 3088, 3089, 3090, 3091, 3092, 3093, 3094, 3095, 3096, 3097, 3098, 3099, 3100, 3101, 3102, 3103, 3104, 3105, 3106, 3107, 3108, 3109, 3110, 3167, 3168, 3169, 3170, 3171, 3172, 3173, 3174, 3176, 3178, 3180, 3182, 3183, 3184, 3185, 3186, 3187, 3188, 3190, 3191, 3193, 3194, 3196, 3197, 3198, 3199, 3200, 3201, 3202, 3203, 3204, 3205, 3206, 3207, 3208, 3209, 3266, 3274, 3276, 3278, 3280, 3282, 3283, 3284, 3285, 3286, 3288, 3289, 3291, 3292, 3294, 3295, 3298, 3300, 3302, 3304, 3306, 3308, 3365, 3373, 3374, 3375, 3376, 3377, 3378, 3379, 3380, 3381, 3382, 3383, 3384, 3385, 3386, 3387, 3388, 3389, 3390, 3391, 3392, 3393, 3394, 3395, 3397, 3398, 3399, 3400, 3401, 3402, 3403, 3404, 3405, 3406, 3407, 3464, 3465, 3466, 3467, 3468, 3469, 3470, 3471, 3472, 3473, 3474, 3475, 3476, 3477, 3478, 3479, 3480, 3481, 3482, 3483, 3484, 3485, 3486, 3487, 3488, 3489, 3490, 3491, 3492, 3493, 3494, 3495, 3496, 3497, 3498, 3499, 3500, 3501, 3502, 3503, 3504, 3505, 3506, 3563, 3564, 3565, 3566, 3567, 3568, 3569, 3570, 3572, 3574, 3576, 3578, 3579, 3580, 3581, 3582, 3583, 3584, 3586, 3587, 3589, 3590, 3592, 3593, 3594, 3595, 3596, 3597, 3598, 3599, 3600, 3601, 3602, 3603, 3604, 3605, 3662, 3670, 3672, 3674, 3676, 3678, 3679, 3680, 3681, 3682, 3684, 3685, 3687, 3688, 3690, 3691, 3694, 3696, 3698, 3700, 3702, 3704, 3761, 3769, 3770, 3771, 3772, 3773, 3774, 3775, 3776, 3777, 3778, 3779, 3780, 3781, 3782, 3783, 3784, 3785, 3786, 3787, 3788, 3789, 3790, 3791, 3793, 3794, 3795, 3796, 3797, 3798, 3799, 3800, 3801, 3802, 3803, 3860, 3861, 3862, 3863, 3864, 3865, 3866, 3867, 3869, 3871, 3873, 3875, 3876, 3877, 3878, 3879, 3880, 3881, 3883, 3884, 3886, 3887, 3889, 3890, 3891, 3892, 3893, 3894, 3895, 3896, 3897, 3898, 3899, 3900, 3901, 3902, 3959, 3967, 3969, 3971, 3973, 3975, 3976, 3977, 3978, 3979, 3981, 3982, 3984, 3985, 3987, 3988, 3991, 3993, 3995, 3997, 3999, 4001, 4058, 4067, 4069, 4071, 4073, 4074, 4075, 4076, 4077, 4078, 4079, 4081, 4082, 4084, 4085, 4087, 4088, 4090, 4091, 4092, 4093, 4094, 4095, 4096, 4097, 4098, 4099, 4100, 4157, 4200, 4203, 4205, 4206, 4214, 4215, 4219, 4224, 4228, 4238, 4243, 4244, 4299, 4300, 4301, 4302, 4303, 4304, 4305, 4306, 4311, 4312, 4313, 4314, 4317, 4318, 4321, 4322, 4323, 4325, 4326, 4327, 4337, 4341, 4342, 4343, 4347, 4352, 4356, 4357, 4358, 4359, 4360, 4361, 4362, 4386, 4398, 4399, 4400, 4401, 4402, 4403, 4404, 4405, 4410, 4411, 4412, 4413, 4416, 4417, 4420, 4421, 4422, 4424, 4425, 4426, 4436, 4440, 4441, 4442, 4446, 4451, 4455, 4456, 4457, 4458, 4459, 4460, 4461, 4485, 4497, 4498, 4499, 4500, 4501, 4502, 4503, 4504, 4509, 4510, 4511, 4512, 4515, 4516, 4519, 4520, 4521, 4523, 4524, 4525, 4535, 4539, 4540, 4541, 4545, 4550, 4554, 4555, 4556, 4557, 4558, 4559, 4560, 4584, 4596, 4597, 4598, 4599, 4600, 4601, 4602, 4603, 4608, 4609, 4610, 4611, 4614, 4615, 4618, 4619, 4620, 4622, 4623, 4624, 4634, 4638, 4639, 4640, 4644, 4649, 4653, 4654, 4655, 4656, 4657, 4658, 4659, 4683, 4695, 4696, 4697, 4698, 4699, 4700, 4701, 4702, 4707, 4708, 4709, 4710, 4713, 4714, 4717, 4718, 4719, 4721, 4722, 4723, 4733, 4737, 4738, 4739, 4743, 4748, 4752, 4753, 4754, 4755, 4756, 4757, 4758, 4782, 4794, 4795, 4796, 4797, 4798, 4799, 4800, 4801, 4806, 4807, 4808, 4809, 4812, 4813, 4816, 4817, 4818, 4820, 4821, 4822, 4832, 4836, 4837, 4838, 4842, 4847, 4851, 4852, 4853, 4854, 4855, 4856, 4857, 4881, 4894, 4895, 4897, 4900, 4905, 4906, 4911, 4915, 4916, 4919, 4920, 4935, 4941, 4946, 4957, 4959, 4961, 4963, 4971, 4974, 4977, 4992, 4995, 4997, 4998, 5000, 5001, 5002, 5003, 5006, 5007, 5008, 5009, 5011, 5012, 5013, 5016, 5017, 5020, 5030, 5032, 5035, 5036, 5038, 5039, 5043, 5044, 5047, 5056, 5057, 5058, 5059, 5060, 5061, 5062, 5063, 5069, 5070, 5072, 5073, 5075, 5076, 5078, 5081, 5083, 5085, 5087, 5089, 5091, 5092, 5093, 5094, 5095, 5096, 5097, 5098, 5099, 5100, 5101, 5102, 5103, 5104, 5105, 5106, 5107, 5108, 5109, 5110, 5111, 5112, 5113, 5114, 5115, 5116, 5117, 5118, 5119, 5129, 5131, 5133, 5134, 5135, 5137, 5138, 5139, 5142, 5143, 5144, 5146, 5148, 5149, 5150, 5151, 5152, 5153, 5154, 5155, 5156, 5157, 5158, 5159, 5160, 5161, 5162, 5168, 5169, 5171, 5172, 5174, 5175, 5177, 5178, 5180, 5182, 5184, 5186, 5188, 5190, 5191, 5192, 5193, 5194, 5195, 5196, 5197, 5198, 5199, 5200, 5201, 5202, 5203, 5204, 5205, 5206, 5207, 5208, 5209, 5210, 5211, 5212, 5213, 5214, 5215, 5216, 5217, 5218, 5228, 5230, 5232, 5233, 5234, 5236, 5237, 5238, 5241, 5242, 5243, 5245, 5247, 5248, 5249, 5250, 5251, 5252, 5253, 5254, 5255, 5256, 5257, 5258, 5259, 5260, 5261, 5267, 5268, 5270, 5271, 5273, 5274, 5276, 5277, 5279, 5281, 5283, 5285, 5287, 5289, 5290, 5291, 5292, 5293, 5294, 5295, 5296, 5297, 5298, 5299, 5300, 5301, 5302, 5303, 5304, 5305, 5306, 5307, 5308, 5309, 5310, 5311, 5312, 5313, 5314, 5315, 5316, 5317, 5327, 5329, 5331, 5332, 5333, 5335, 5336, 5337, 5340, 5341, 5342, 5344, 5346, 5347, 5348, 5349, 5350, 5351, 5352, 5353, 5354, 5355, 5356, 5357, 5358, 5359, 5360, 5366, 5367, 5369, 5370, 5372, 5373, 5375, 5376, 5378, 5380, 5382, 5384, 5386, 5388, 5389, 5390, 5391, 5392, 5393, 5394, 5395, 5396, 5397, 5398, 5399, 5400, 5401, 5402, 5403, 5404, 5405, 5406, 5407, 5408, 5409, 5410, 5411, 5412, 5413, 5414, 5415, 5416, 5426, 5428, 5430, 5431, 5432, 5434, 5435, 5436, 5439, 5440, 5441, 5443, 5445, 5446, 5447, 5448, 5449, 5450, 5451, 5452, 5453, 5454, 5455, 5456, 5457, 5458, 5459, 5465, 5466, 5468, 5469, 5471, 5472, 5474, 5475, 5477, 5479, 5481, 5483, 5485, 5487, 5488, 5489, 5490, 5491, 5492, 5493, 5494, 5495, 5496, 5497, 5498, 5499, 5500, 5501, 5502, 5503, 5504, 5505, 5506, 5507, 5508, 5509, 5510, 5511, 5512, 5513, 5514, 5515, 5525, 5527, 5529, 5530, 5531, 5533, 5534, 5535, 5538, 5539, 5540, 5542, 5544, 5545, 5546, 5547, 5548, 5549, 5550, 5552, 5554, 5556, 5558, 5564, 5567, 5570, 5573, 5574, 5576, 5578, 5580, 5582, 5584, 5587, 5588, 5590, 5593, 5594, 5595, 5596, 5597, 5598, 5599, 5602, 5603, 5604, 5606, 5607, 5608, 5609, 5611, 5612, 5613, 5626, 5628, 5632, 5633, 5634, 5637, 5638, 5639, 5641, 5650, 5652, 5654, 5656, 5658, 5659, 5660, 5661, 5662, 5664, 5665, 5667, 5668, 5670, 5671, 5674, 5676, 5678, 5680, 5682, 5684, 5685, 5688, 5690, 5691, 5693, 5694, 5695, 5696, 5699, 5700, 5701, 5702, 5704, 5705, 5706, 5709, 5710, 5713, 5723, 5725, 5728, 5729, 5731, 5732, 5736, 5737, 5740, 5741, 5749, 5750, 5751, 5752, 5753, 5754, 5755, 5756, 5757, 5758, 5759, 5760, 5761, 5762, 5763, 5764, 5765, 5766, 5767, 5768, 5769, 5770, 5771, 5773, 5774, 5775, 5776, 5777, 5778, 5779, 5780, 5781, 5782, 5783, 5784, 5785, 5786, 5787, 5788, 5789, 5790, 5791, 5792, 5793, 5794, 5795, 5796, 5797, 5798, 5799, 5800, 5801, 5802, 5803, 5804, 5805, 5806, 5807, 5808, 5809, 5810, 5811, 5812, 5822, 5824, 5826, 5827, 5828, 5830, 5831, 5832, 5835, 5836, 5837, 5839, 5840, 5841, 5842, 5843, 5844, 5845, 5846, 5847, 5848, 5849, 5850, 5851, 5852, 5853, 5854, 5855, 5856, 5857, 5858, 5859, 5860, 5861, 5862, 5863, 5864, 5865, 5866, 5867, 5868, 5869, 5870, 5871, 5872, 5873, 5874, 5875, 5876, 5877, 5878, 5879, 5880, 5881, 5882, 5883, 5884, 5885, 5886, 5887, 5888, 5889, 5890, 5891, 5892, 5893, 5894, 5895, 5896, 5897, 5898, 5899, 5900, 5901, 5902, 5903, 5904, 5905, 5906, 5907, 5908, 5909, 5910, 5911, 5921, 5923, 5925, 5926, 5927, 5929, 5930, 5931, 5934, 5935, 5936, 5938, 5939, 5940, 5941, 5942, 5943, 5944, 5945, 5946, 5947, 5948, 5949, 5950, 5951, 5952, 5953, 5954, 5955, 5956, 5957, 5958, 5959, 5960, 5961, 5962, 5963, 5964, 5965, 5966, 5967, 5968, 5969, 5970, 5971, 5972, 5973, 5974, 5975, 5976, 5977, 5978, 5979, 5980, 5981, 5982, 5983, 5984, 5985, 5986, 5987, 5988, 5989, 5990, 5991, 5992, 5993, 5994, 5995, 5996, 5997, 5998, 5999, 6000, 6001, 6002, 6003, 6004, 6005, 6006, 6007, 6008, 6009, 6010, 6020, 6022, 6024, 6025, 6026, 6028, 6029, 6030, 6033, 6034, 6035, 6037, 6038, 6039, 6040, 6041, 6042, 6043, 6044, 6045, 6046, 6047, 6048, 6049, 6050, 6051, 6052, 6053, 6054, 6055, 6056, 6057, 6058, 6059, 6060, 6061, 6062, 6063, 6064, 6065, 6066, 6067, 6068, 6069, 6070, 6071, 6072, 6073, 6074, 6075, 6076, 6077, 6078, 6079, 6080, 6081, 6082, 6083, 6084, 6085, 6086, 6087, 6088, 6089, 6090, 6091, 6092, 6093, 6094, 6095, 6096, 6097, 6098, 6099, 6100, 6101, 6102, 6103, 6104, 6105, 6106, 6107, 6108, 6109, 6119, 6121, 6123, 6124, 6125, 6127, 6128, 6129, 6132, 6133, 6134, 6136, 6137, 6138, 6139, 6140, 6141, 6142, 6143, 6144, 6146, 6148, 6150, 6152, 6153, 6154, 6155, 6156, 6157, 6158, 6160, 6161, 6163, 6164, 6166, 6167, 6168, 6169, 6170, 6171, 6172, 6173, 6174, 6175, 6176, 6177, 6178, 6179, 6181, 6182, 6184, 6187, 6188, 6189, 6190, 6191, 6192, 6193, 6196, 6197, 6198, 6200, 6201, 6202, 6203, 6205, 6206, 6207, 6220, 6222, 6226, 6227, 6228, 6231, 6232, 6233, 6235, 6236, 6244, 6246, 6248, 6250, 6252, 6253, 6254, 6255, 6256, 6258, 6259, 6261, 6262, 6264, 6265, 6268, 6270, 6272, 6274, 6276, 6278, 6279, 6282, 6284, 6285, 6287, 6288, 6289, 6290, 6293, 6294, 6295, 6296, 6298, 6299, 6300, 6303, 6304, 6307, 6317, 6319, 6322, 6323, 6325, 6326, 6330, 6331, 6334, 6335, 6343, 6344, 6345, 6346, 6347, 6348, 6349, 6350, 6351, 6352, 6353, 6354, 6355, 6356, 6357, 6358, 6359, 6360, 6361, 6362, 6363, 6364, 6365, 6367, 6368, 6369, 6370, 6371, 6372, 6373, 6374, 6375, 6376, 6377, 6378, 6379, 6380, 6381, 6382, 6383, 6384, 6385, 6386, 6387, 6388, 6389, 6390, 6391, 6392, 6393, 6394, 6395, 6396, 6397, 6398, 6399, 6400, 6401, 6402, 6403, 6404, 6405, 6406, 6416, 6418, 6420, 6421, 6422, 6424, 6425, 6426, 6429, 6430, 6431, 6433, 6434, 6435, 6436, 6437, 6438, 6439, 6440, 6441, 6442, 6443, 6444, 6445, 6446, 6447, 6448, 6449, 6450, 6451, 6452, 6453, 6454, 6455, 6456, 6457, 6458, 6459, 6460, 6461, 6462, 6463, 6464, 6465, 6466, 6467, 6468, 6469, 6470, 6471, 6472, 6473, 6474, 6475, 6476, 6477, 6478, 6479, 6480, 6481, 6482, 6483, 6484, 6485, 6486, 6487, 6488, 6489, 6490, 6491, 6492, 6493, 6494, 6495, 6496, 6497, 6498, 6499, 6500, 6501, 6502, 6503, 6504, 6505, 6515, 6517, 6519, 6520, 6521, 6523, 6524, 6525, 6528, 6529, 6530, 6532, 6533, 6534, 6535, 6536, 6537, 6538, 6539, 6540, 6541, 6542, 6543, 6544, 6545, 6546, 6547, 6548, 6549, 6550, 6551, 6552, 6553, 6554, 6555, 6556, 6557, 6558, 6559, 6560, 6561, 6562, 6563, 6564, 6565, 6566, 6567, 6568, 6569, 6570, 6571, 6572, 6573, 6574, 6575, 6576, 6577, 6578, 6579, 6580, 6581, 6582, 6583, 6584, 6585, 6586, 6587, 6588, 6589, 6590, 6591, 6592, 6593, 6594, 6595, 6596, 6597, 6598, 6599, 6600, 6601, 6602, 6603, 6604, 6614, 6616, 6618, 6619, 6620, 6622, 6623, 6624, 6627, 6628, 6629, 6631, 6632, 6633, 6634, 6635, 6636, 6637, 6638, 6639, 6641, 6643, 6645, 6647, 6648, 6649, 6650, 6651, 6652, 6653, 6655, 6656, 6658, 6659, 6661, 6662, 6663, 6664, 6665, 6666, 6667, 6668, 6669, 6670, 6671, 6672, 6673, 6674, 6676, 6677, 6679, 6682, 6683, 6684, 6685, 6686, 6687, 6688, 6691, 6692, 6693, 6695, 6696, 6697, 6698, 6700, 6701, 6702, 6715, 6717, 6721, 6722, 6723, 6726, 6727, 6728, 6730, 6731, 6739, 6741, 6743, 6745, 6747, 6748, 6749, 6750, 6751, 6753, 6754, 6756, 6757, 6759, 6760, 6763, 6765, 6767, 6769, 6771, 6773, 6774, 6777, 6779, 6780, 6782, 6783, 6784, 6785, 6788, 6789, 6790, 6791, 6793, 6794, 6795, 6798, 6799, 6802, 6812, 6814, 6817, 6818, 6820, 6821, 6825, 6826, 6829, 6830, 6838, 6839, 6840, 6841, 6842, 6843, 6844, 6845, 6846, 6847, 6848, 6849, 6850, 6851, 6852, 6853, 6854, 6855, 6856, 6857, 6858, 6859, 6860, 6862, 6863, 6864, 6865, 6866, 6867, 6868, 6869, 6870, 6871, 6872, 6873, 6874, 6875, 6876, 6877, 6878, 6879, 6880, 6881, 6882, 6883, 6884, 6885, 6886, 6887, 6888, 6889, 6890, 6891, 6892, 6893, 6894, 6895, 6896, 6897, 6898, 6899, 6900, 6901, 6911, 6913, 6915, 6916, 6917, 6919, 6920, 6921, 6924, 6925, 6926, 6928, 6929, 6930, 6931, 6932, 6933, 6934, 6935, 6936, 6937, 6938, 6939, 6940, 6941, 6942, 6943, 6944, 6945, 6946, 6947, 6948, 6949, 6950, 6951, 6952, 6953, 6954, 6955, 6956, 6957, 6958, 6959, 6960, 6961, 6962, 6963, 6964, 6965, 6966, 6967, 6968, 6969, 6970, 6971, 6972, 6973, 6974, 6975, 6976, 6977, 6978, 6979, 6980, 6981, 6982, 6983, 6984, 6985, 6986, 6987, 6988, 6989, 6990, 6991, 6992, 6993, 6994, 6995, 6996, 6997, 6998, 6999, 7000, 7010, 7012, 7014, 7015, 7016, 7018, 7019, 7020, 7023, 7024, 7025, 7027, 7028, 7071, 7074, 7076, 7077, 7085, 7086, 7090, 7095, 7099, 7100, 7101, 7102, 7103, 7104, 7105, 7106, 7107, 7108, 7109, 7110, 7112, 7114, 7115, 7116, 7120, 7121, 7125, 7170, 7171, 7172, 7173, 7174, 7175, 7176, 7177, 7182, 7183, 7184, 7185, 7188, 7189, 7192, 7193, 7194, 7196, 7197, 7198, 7199, 7200, 7201, 7202, 7203, 7204, 7205, 7206, 7207, 7208, 7209, 7211, 7212, 7213, 7214, 7215, 7218, 7219, 7220, 7223, 7224, 7227, 7228, 7229, 7230, 7231, 7232, 7233, 7257, 7269, 7270, 7271, 7272, 7273, 7274, 7275, 7276, 7281, 7282, 7283, 7284, 7287, 7288, 7291, 7292, 7293, 7295, 7296, 7297, 7298, 7299, 7300, 7301, 7302, 7303, 7304, 7305, 7306, 7307, 7308, 7310, 7311, 7312, 7313, 7314, 7317, 7318, 7319, 7322, 7323, 7326, 7327, 7328, 7329, 7330, 7331, 7332, 7356, 7368, 7369, 7370, 7371, 7372, 7373, 7374, 7375, 7380, 7381, 7382, 7383, 7386, 7387, 7390, 7391, 7392, 7394, 7395, 7396, 7397, 7398, 7399, 7400, 7401, 7402, 7403, 7404, 7405, 7406, 7407, 7409, 7410, 7411, 7412, 7413, 7416, 7417, 7418, 7421, 7422, 7425, 7426, 7427, 7428, 7429, 7430, 7431, 7455, 7467, 7468, 7469, 7470, 7471, 7472, 7473, 7474, 7479, 7480, 7481, 7482, 7485, 7486, 7489, 7490, 7491, 7493, 7494, 7495, 7496, 7497, 7498, 7499, 7500, 7501, 7502, 7503, 7504, 7505, 7506, 7508, 7509, 7510, 7511, 7512, 7515, 7516, 7517, 7520, 7521, 7524, 7525, 7526, 7527, 7528, 7529, 7530, 7554, 7566, 7567, 7568, 7569, 7570, 7571, 7572, 7573, 7578, 7579, 7580, 7581, 7584, 7585, 7588, 7589, 7590, 7592, 7593, 7594, 7595, 7596, 7597, 7598, 7599, 7600, 7601, 7602, 7603, 7604, 7605, 7607, 7608, 7609, 7610, 7611, 7614, 7615, 7616, 7619, 7620, 7623, 7624, 7625, 7626, 7627, 7628, 7629, 7653, 7666, 7667, 7669, 7672, 7677, 7678, 7683, 7687, 7688, 7691, 7692, 7694, 7695, 7696, 7697, 7698, 7699, 7700, 7701, 7702, 7704, 7706, 7707, 7710, 7713, 7714, 7715, 7718, 7719, 7729, 7731, 7733, 7735, 7743, 7746, 7749, 7764, 7767, 7769, 7770, 7772, 7773, 7774, 7775, 7778, 7779, 7780, 7781, 7783, 7784, 7785, 7788, 7789, 7792, 7793, 7794, 7795, 7796, 7797, 7798, 7799, 7800, 7801, 7802, 7803, 7804, 7805, 7807, 7808, 7809, 7810, 7811, 7813, 7814, 7815, 7816, 7818, 7819, 7828, 7829, 7830, 7831, 7832, 7833, 7834, 7835, 7841, 7842, 7844, 7845, 7847, 7848, 7850, 7853, 7855, 7857, 7859, 7861, 7863, 7864, 7865, 7866, 7867, 7868, 7869, 7870, 7871, 7872, 7873, 7874, 7875, 7876, 7877, 7878, 7879, 7880, 7881, 7882, 7883, 7884, 7885, 7886, 7887, 7888, 7889, 7890, 7891, 7892, 7893, 7894, 7895, 7896, 7897, 7898, 7899, 7900, 7901, 7902, 7903, 7904, 7905, 7906, 7907, 7908, 7909, 7910, 7911, 7912, 7913, 7914, 7915, 7916, 7917, 7918, 7927, 7929, 7931, 7933, 7935, 7936, 7937, 7938, 7939, 7941, 7942, 7944, 7945, 7947, 7948, 7951, 7953, 7955, 7957, 7959, 7961, 7962, 7965, 7967, 7968, 7970, 7971, 7972, 7973, 7976, 7977, 7978, 7979, 7981, 7982, 7983, 7986, 7987, 7990, 7991, 7992, 7993, 7994, 7995, 7996, 7997, 7998, 7999, 8000, 8001, 8002, 8003, 8005, 8006, 8007, 8008, 8009, 8011, 8012, 8013, 8014, 8016, 8017, 8018, 8019, 8020, 8021, 8022, 8023, 8024, 8025, 8026, 8027, 8028, 8029, 8030, 8031, 8032, 8033, 8039, 8040, 8042, 8043, 8045, 8046, 8048, 8049, 8051, 8053, 8055, 8057, 8059, 8061, 8062, 8063, 8064, 8065, 8066, 8067, 8068, 8069, 8070, 8071, 8072, 8073, 8074, 8075, 8076, 8077, 8078, 8079, 8080, 8081, 8082, 8083, 8084, 8085, 8086, 8087, 8088, 8089, 8090, 8091, 8092, 8093, 8094, 8095, 8096, 8097, 8098, 8099, 8100, 8101, 8102, 8103, 8104, 8105, 8106, 8107, 8108, 8109, 8110, 8111, 8112, 8113, 8114, 8115, 8116, 8125, 8126, 8127, 8128, 8129, 8130, 8131, 8132, 8133, 8134, 8135, 8136, 8137, 8138, 8139, 8140, 8141, 8142, 8143, 8144, 8145, 8146, 8147, 8149, 8150, 8151, 8152, 8153, 8154, 8155, 8156, 8157, 8158, 8159, 8160, 8161, 8162, 8163, 8164, 8165, 8166, 8167, 8168, 8169, 8170, 8171, 8172, 8173, 8174, 8175, 8176, 8177, 8178, 8179, 8180, 8181, 8182, 8183, 8184, 8185, 8186, 8187, 8188, 8189, 8190, 8191, 8192, 8193, 8194, 8195, 8196, 8197, 8198, 8199, 8200, 8201, 8202, 8203, 8204, 8205, 8206, 8207, 8208, 8209, 8210, 8211, 8212, 8213, 8214, 8215, 8216, 8224, 8226, 8228, 8230, 8232, 8233, 8234, 8235, 8236, 8238, 8239, 8241, 8242, 8244, 8245, 8248, 8250, 8252, 8254, 8256, 8258, 8259, 8262, 8264, 8265, 8267, 8268, 8269, 8270, 8273, 8274, 8275, 8276, 8278, 8279, 8280, 8283, 8284, 8287, 8288, 8289, 8290, 8291, 8292, 8293, 8294, 8295, 8296, 8297, 8298, 8299, 8300, 8302, 8303, 8304, 8305, 8306, 8308, 8309, 8310, 8311, 8313, 8314, 8315, 8316, 8317, 8318, 8319, 8320, 8321, 8322, 8323, 8324, 8325, 8326, 8327, 8328, 8329, 8330, 8336, 8337, 8339, 8340, 8342, 8343, 8345, 8346, 8348, 8350, 8352, 8354, 8356, 8358, 8359, 8360, 8361, 8362, 8363, 8364, 8365, 8366, 8367, 8368, 8369, 8370, 8371, 8372, 8373, 8374, 8375, 8376, 8377, 8378, 8379, 8380, 8381, 8382, 8383, 8384, 8385, 8386, 8387, 8388, 8389, 8390, 8391, 8392, 8393, 8394, 8395, 8396, 8397, 8398, 8399, 8400, 8401, 8402, 8403, 8404, 8405, 8406, 8407, 8408, 8409, 8410, 8411, 8412, 8413, 8415, 8416, 8417, 8418, 8419, 8420, 8421, 8422, 8423, 8424, 8425, 8426, 8427, 8428, 8429, 8430, 8431, 8432, 8433, 8434, 8435, 8436, 8437, 8438, 8439, 8440, 8441, 8442, 8443, 8444, 8445, 8446, 8447, 8448, 8449, 8450, 8451, 8452, 8453, 8454, 8455, 8456, 8457, 8458, 8459, 8460, 8461, 8462, 8463, 8464, 8465, 8466, 8467, 8468, 8469, 8470, 8471, 8472, 8473, 8474, 8475, 8476, 8477, 8478, 8479, 8480, 8481, 8482, 8483, 8484, 8485, 8486, 8487, 8488, 8489, 8490, 8491, 8492, 8493, 8494, 8495, 8496, 8497, 8498, 8499, 8500, 8501, 8502, 8503, 8504, 8505, 8506, 8507, 8508, 8509, 8510, 8511, 8512, 8513, 8521, 8522, 8523, 8524, 8525, 8526, 8527, 8528, 8529, 8530, 8531, 8532, 8533, 8534, 8535, 8536, 8537, 8538, 8539, 8540, 8541, 8542, 8543, 8545, 8546, 8547, 8548, 8549, 8550, 8551, 8552, 8553, 8554, 8555, 8556, 8557, 8558, 8559, 8560, 8561, 8562, 8563, 8564, 8565, 8566, 8567, 8568, 8569, 8570, 8571, 8572, 8573, 8574, 8575, 8576, 8577, 8578, 8579, 8580, 8581, 8582, 8583, 8584, 8585, 8586, 8587, 8588, 8589, 8590, 8591, 8592, 8593, 8594, 8595, 8596, 8597, 8598, 8599, 8600, 8601, 8602, 8603, 8604, 8605, 8606, 8607, 8608, 8609, 8610, 8611, 8612, 8620, 8622, 8624, 8626, 8628, 8629, 8630, 8631, 8632, 8634, 8635, 8637, 8638, 8640, 8641, 8644, 8646, 8648, 8650, 8652, 8654, 8655, 8658, 8660, 8661, 8663, 8664, 8665, 8666, 8669, 8670, 8671, 8672, 8674, 8675, 8676, 8679, 8680, 8683, 8693, 8695, 8698, 8699, 8701, 8702, 8706, 8707, 8710, 8711, 8712, 8713, 8714, 8715, 8716, 8717, 8718, 8719, 8720, 8721, 8722, 8723, 8724, 8725, 8726, 8732, 8733, 8735, 8736, 8738, 8739, 8741, 8742, 8744, 8746, 8748, 8750, 8752, 8754, 8755, 8756, 8757, 8758, 8759, 8760, 8761, 8762, 8763, 8764, 8765, 8766, 8767, 8768, 8769, 8770, 8771, 8772, 8773, 8774, 8775, 8776, 8777, 8778, 8779, 8780, 8781, 8782, 8783, 8784, 8785, 8786, 8787, 8788, 8789, 8790, 8791, 8792, 8793, 8794, 8795, 8796, 8797, 8798, 8799, 8800, 8801, 8802, 8803, 8804, 8805, 8806, 8807, 8808, 8809, 8811, 8812, 8813, 8814, 8815, 8816, 8817, 8818, 8819, 8820, 8821, 8822, 8823, 8824, 8825, 8826, 8827, 8828, 8829, 8830, 8831, 8832, 8833, 8834, 8835, 8836, 8837, 8838, 8839, 8840, 8841, 8842, 8843, 8844, 8845, 8846, 8847, 8848, 8849, 8850, 8851, 8852, 8853, 8854, 8855, 8856, 8857, 8858, 8859, 8860, 8861, 8862, 8863, 8864, 8865, 8866, 8867, 8868, 8869, 8870, 8871, 8872, 8873, 8874, 8875, 8876, 8877, 8878, 8879, 8880, 8881, 8882, 8883, 8884, 8885, 8886, 8887, 8888, 8889, 8890, 8891, 8892, 8893, 8894, 8895, 8896, 8897, 8898, 8899, 8900, 8901, 8902, 8903, 8904, 8905, 8906, 8907, 8908, 8909, 8910, 8911, 8912, 8913, 8914, 8915, 8916, 8917, 8918, 8919, 8920, 8921, 8922, 8923, 8924, 8925, 8926, 8927, 8928, 8929, 8930, 8931, 8932, 8933, 8934, 8935, 8936, 8937, 8938, 8939, 8940, 8941, 8942, 8943, 8944, 8945, 8946, 8947, 8948, 8949, 8950, 8951, 8952, 8953, 8954, 8955, 8956, 8957, 8958, 8959, 8960, 8961, 8962, 8963, 8964, 8965, 8966, 8967, 8968, 8969, 8970, 8971, 8972, 8973, 8974, 8975, 8976, 8977, 8978, 8979, 8980, 8981, 8982, 8983, 8984, 8985, 8986, 8987, 8988, 8989, 8990, 8991, 8992, 8993, 8994, 8995, 8996, 8997, 8998, 8999, 9000, 9001, 9002, 9003, 9004, 9005, 9006, 9007, 9008, 9016, 9017, 9018, 9019, 9020, 9021, 9022, 9023, 9024, 9025, 9026, 9027, 9028, 9029, 9030, 9031, 9032, 9033, 9034, 9035, 9036, 9037, 9038, 9040, 9041, 9042, 9043, 9044, 9045, 9046, 9047, 9048, 9049, 9050, 9051, 9052, 9053, 9054, 9055, 9056, 9057, 9058, 9059, 9060, 9061, 9062, 9063, 9064, 9065, 9066, 9067, 9068, 9069, 9070, 9071, 9072, 9073, 9074, 9075, 9076, 9077, 9078, 9079, 9089, 9091, 9093, 9094, 9095, 9097, 9098, 9099, 9102, 9103, 9104, 9106, 9107, 9115, 9117, 9119, 9121, 9123, 9124, 9125, 9126, 9127, 9129, 9130, 9132, 9133, 9135, 9136, 9139, 9141, 9143, 9145, 9147, 9149, 9150, 9153, 9155, 9156, 9158, 9159, 9160, 9161, 9164, 9165, 9166, 9167, 9169, 9170, 9171, 9174, 9175, 9178, 9188, 9190, 9193, 9194, 9196, 9197, 9201, 9202, 9205, 9206, 9207, 9208, 9209, 9210, 9211, 9212, 9213, 9215, 9217, 9219, 9221, 9227, 9230, 9233, 9236, 9237, 9239, 9241, 9243, 9245, 9247, 9250, 9251, 9253, 9256, 9257, 9258, 9259, 9260, 9261, 9262, 9265, 9266, 9267, 9269, 9270, 9271, 9272, 9274, 9275, 9276, 9278, 9279, 9280, 9281, 9282, 9283, 9284, 9285, 9286, 9288, 9289, 9290, 9291, 9294, 9295, 9296, 9297, 9298, 9299, 9300, 9301, 9302, 9303, 9304, 9306, 9307, 9308, 9309, 9310, 9311, 9312, 9314, 9316, 9318, 9320, 9321, 9322, 9323, 9324, 9325, 9326, 9328, 9329, 9331, 9332, 9334, 9335, 9336, 9337, 9338, 9339, 9340, 9341, 9342, 9343, 9344, 9345, 9346, 9347, 9349, 9350, 9352, 9355, 9356, 9357, 9358, 9359, 9360, 9361, 9364, 9365, 9366, 9368, 9369, 9370, 9371, 9373, 9374, 9375, 9377, 9378, 9379, 9380, 9381, 9382, 9383, 9384, 9385, 9387, 9388, 9389, 9390, 9393, 9394, 9395, 9396, 9397, 9398, 9399, 9400, 9401, 9402, 9403, 9404, 9405, 9406, 9407, 9408, 9409, 9410, 9411, 9413, 9415, 9417, 9419, 9420, 9421, 9422, 9423, 9424, 9425, 9427, 9428, 9430, 9431, 9433, 9434, 9435, 9436, 9437, 9438, 9439, 9440, 9441, 9442, 9443, 9444, 9445, 9446, 9448, 9449, 9451, 9454, 9455, 9456, 9457, 9458, 9459, 9460, 9463, 9464, 9465, 9467, 9468, 9469, 9470, 9472, 9473, 9474, 9487, 9489, 9493, 9494, 9495, 9498, 9499, 9500, 9502, 9503, 9504, 9505, 9506, 9507, 9508, 9509, 9510, 9512, 9514, 9516, 9518, 9519, 9520, 9521, 9522, 9523, 9524, 9526, 9527, 9529, 9530, 9532, 9533, 9534, 9535, 9536, 9537, 9538, 9539, 9540, 9541, 9542, 9543, 9544, 9545, 9547, 9548, 9550, 9553, 9554, 9555, 9556, 9557, 9558, 9559, 9562, 9563, 9564, 9566, 9567, 9568, 9569, 9571, 9572, 9573, 9586, 9588, 9592, 9593, 9594, 9597, 9598, 9599, 9601, 9602, 9611, 9613, 9615, 9617, 9618, 9619, 9620, 9621, 9622, 9623, 9625, 9626, 9628, 9629, 9631, 9632, 9634, 9635, 9636, 9637, 9638, 9639, 9640, 9641, 9642, 9643, 9644, 9646, 9647, 9649, 9652, 9653, 9654, 9655, 9656, 9657, 9658, 9661, 9662, 9663, 9665, 9666, 9667, 9668, 9670, 9671, 9672, 9685, 9687, 9691, 9692, 9693, 9696, 9697, 9698, 9700, 9701, 9717, 9718, 9719, 9720, 9721, 9724, 9727, 9730, 9733, 9735, 9737, 9739, 9741, 9743, 9800}; static const int C1_ind[] = { 71, 72, 73, 74, 75, 76, 77, 78, 79, 81, 83, 87, 91, 92, 96, 141, 144, 146, 147, 155, 156, 160, 165, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 182, 184, 185, 186, 190, 191, 195, 240, 243, 245, 246, 254, 255, 259, 264, 268, 269, 270, 271, 272, 273, 274, 275, 276, 277, 278, 279, 281, 283, 284, 285, 289, 290, 294, 339, 342, 344, 345, 353, 354, 358, 363, 367, 368, 369, 370, 371, 372, 373, 374, 375, 376, 377, 378, 380, 382, 383, 384, 388, 389, 393, 438, 441, 443, 444, 452, 453, 457, 462, 466, 467, 468, 469, 470, 471, 472, 473, 474, 475, 476, 477, 479, 481, 482, 483, 487, 488, 492, 537, 540, 542, 543, 551, 552, 556, 561, 565, 566, 567, 568, 569, 570, 571, 572, 573, 574, 575, 576, 578, 580, 581, 582, 586, 587, 591, 636, 637, 638, 639, 640, 641, 642, 643, 648, 649, 650, 651, 654, 655, 658, 659, 660, 662, 663, 664, 665, 666, 667, 668, 669, 670, 671, 672, 673, 674, 675, 677, 678, 679, 680, 681, 684, 685, 686, 689, 690, 700, 702, 704, 706, 714, 717, 720, 735, 738, 740, 741, 743, 744, 745, 746, 749, 750, 751, 752, 754, 755, 756, 759, 760, 763, 764, 765, 766, 767, 768, 769, 770, 771, 772, 773, 774, 775, 776, 778, 779, 780, 781, 782, 784, 785, 786, 787, 789, 790, 834, 835, 836, 837, 838, 839, 840, 841, 846, 847, 848, 849, 852, 853, 856, 857, 858, 860, 861, 862, 863, 864, 865, 866, 867, 868, 869, 870, 871, 872, 873, 875, 876, 877, 878, 879, 882, 883, 884, 887, 888, 891, 892, 893, 894, 895, 896, 897, 921, 933, 934, 935, 936, 937, 938, 939, 940, 945, 946, 947, 948, 951, 952, 955, 956, 957, 959, 960, 961, 962, 963, 964, 965, 966, 967, 968, 969, 970, 971, 972, 974, 975, 976, 977, 978, 981, 982, 983, 986, 987, 997, 998, 999, 1000, 1001, 1002, 1003, 1004, 1010, 1011, 1013, 1014, 1016, 1017, 1019, 1022, 1024, 1026, 1028, 1030, 1032, 1033, 1034, 1035, 1036, 1037, 1038, 1039, 1040, 1041, 1042, 1043, 1044, 1045, 1046, 1047, 1048, 1049, 1050, 1051, 1052, 1053, 1054, 1055, 1056, 1057, 1058, 1059, 1060, 1061, 1062, 1063, 1064, 1065, 1066, 1067, 1068, 1069, 1070, 1071, 1072, 1073, 1074, 1075, 1076, 1077, 1078, 1079, 1080, 1081, 1082, 1083, 1084, 1085, 1086, 1087, 1096, 1098, 1100, 1102, 1110, 1113, 1116, 1131, 1134, 1136, 1137, 1139, 1140, 1141, 1142, 1145, 1146, 1147, 1148, 1150, 1151, 1152, 1155, 1156, 1159, 1160, 1161, 1162, 1163, 1164, 1165, 1166, 1167, 1168, 1169, 1170, 1171, 1172, 1174, 1175, 1176, 1177, 1178, 1180, 1181, 1182, 1183, 1185, 1186, 1195, 1197, 1199, 1201, 1203, 1204, 1205, 1206, 1207, 1209, 1210, 1212, 1213, 1215, 1216, 1219, 1221, 1223, 1225, 1227, 1229, 1230, 1233, 1235, 1236, 1238, 1239, 1240, 1241, 1244, 1245, 1246, 1247, 1249, 1250, 1251, 1254, 1255, 1258, 1259, 1260, 1261, 1262, 1263, 1264, 1265, 1266, 1267, 1268, 1269, 1270, 1271, 1273, 1274, 1275, 1276, 1277, 1279, 1280, 1281, 1282, 1284, 1285, 1286, 1329, 1330, 1331, 1332, 1333, 1334, 1335, 1336, 1341, 1342, 1343, 1344, 1347, 1348, 1351, 1352, 1353, 1355, 1356, 1357, 1358, 1359, 1360, 1361, 1362, 1363, 1364, 1365, 1366, 1367, 1368, 1370, 1371, 1372, 1373, 1374, 1377, 1378, 1379, 1382, 1383, 1386, 1387, 1388, 1389, 1390, 1391, 1392, 1416, 1428, 1429, 1430, 1431, 1432, 1433, 1434, 1435, 1440, 1441, 1442, 1443, 1446, 1447, 1450, 1451, 1452, 1454, 1455, 1456, 1457, 1458, 1459, 1460, 1461, 1462, 1463, 1464, 1465, 1466, 1467, 1469, 1470, 1471, 1472, 1473, 1476, 1477, 1478, 1481, 1482, 1485, 1486, 1487, 1488, 1489, 1490, 1491, 1515, 1527, 1528, 1529, 1530, 1531, 1532, 1533, 1534, 1539, 1540, 1541, 1542, 1545, 1546, 1549, 1550, 1551, 1553, 1554, 1555, 1556, 1557, 1558, 1559, 1560, 1561, 1562, 1563, 1564, 1565, 1566, 1568, 1569, 1570, 1571, 1572, 1575, 1576, 1577, 1580, 1581, 1584, 1585, 1586, 1587, 1588, 1589, 1590, 1591, 1592, 1593, 1594, 1595, 1596, 1597, 1598, 1604, 1605, 1607, 1608, 1610, 1611, 1613, 1614, 1616, 1618, 1620, 1622, 1624, 1626, 1627, 1628, 1629, 1630, 1631, 1632, 1633, 1634, 1635, 1636, 1637, 1638, 1639, 1640, 1641, 1642, 1643, 1644, 1645, 1646, 1647, 1648, 1649, 1650, 1651, 1652, 1653, 1654, 1655, 1656, 1657, 1658, 1659, 1660, 1661, 1662, 1663, 1664, 1665, 1666, 1667, 1668, 1669, 1670, 1671, 1672, 1673, 1674, 1675, 1676, 1677, 1678, 1679, 1680, 1681, 1690, 1691, 1692, 1693, 1694, 1695, 1696, 1697, 1703, 1704, 1706, 1707, 1709, 1710, 1712, 1715, 1717, 1719, 1721, 1723, 1725, 1726, 1727, 1728, 1729, 1730, 1731, 1732, 1733, 1734, 1735, 1736, 1737, 1738, 1739, 1740, 1741, 1742, 1743, 1744, 1745, 1746, 1747, 1748, 1749, 1750, 1751, 1752, 1753, 1754, 1755, 1756, 1757, 1758, 1759, 1760, 1761, 1762, 1763, 1764, 1765, 1766, 1767, 1768, 1769, 1770, 1771, 1772, 1773, 1774, 1775, 1776, 1777, 1778, 1779, 1780, 1789, 1790, 1791, 1792, 1793, 1794, 1795, 1796, 1797, 1798, 1799, 1800, 1801, 1802, 1803, 1804, 1805, 1806, 1807, 1808, 1809, 1810, 1811, 1813, 1814, 1815, 1816, 1817, 1818, 1819, 1820, 1821, 1822, 1823, 1824, 1825, 1826, 1827, 1828, 1829, 1830, 1831, 1832, 1833, 1834, 1835, 1836, 1837, 1838, 1839, 1840, 1841, 1842, 1843, 1844, 1845, 1846, 1847, 1848, 1849, 1850, 1851, 1852, 1853, 1854, 1855, 1856, 1857, 1858, 1859, 1860, 1861, 1862, 1863, 1864, 1865, 1866, 1867, 1868, 1869, 1870, 1871, 1872, 1873, 1874, 1875, 1876, 1877, 1878, 1879, 1880, 1888, 1890, 1892, 1894, 1902, 1905, 1908, 1923, 1926, 1928, 1929, 1931, 1932, 1933, 1934, 1937, 1938, 1939, 1940, 1942, 1943, 1944, 1947, 1948, 1951, 1952, 1953, 1954, 1955, 1956, 1957, 1958, 1959, 1960, 1961, 1962, 1963, 1964, 1966, 1967, 1968, 1969, 1970, 1972, 1973, 1974, 1975, 1977, 1978, 1987, 1989, 1991, 1993, 1995, 1996, 1997, 1998, 1999, 2001, 2002, 2004, 2005, 2007, 2008, 2011, 2013, 2015, 2017, 2019, 2021, 2022, 2025, 2027, 2028, 2030, 2031, 2032, 2033, 2036, 2037, 2038, 2039, 2041, 2042, 2043, 2046, 2047, 2050, 2051, 2052, 2053, 2054, 2055, 2056, 2057, 2058, 2059, 2060, 2061, 2062, 2063, 2065, 2066, 2067, 2068, 2069, 2071, 2072, 2073, 2074, 2076, 2077, 2078, 2086, 2088, 2090, 2092, 2094, 2095, 2096, 2097, 2098, 2100, 2101, 2103, 2104, 2106, 2107, 2110, 2112, 2114, 2116, 2118, 2120, 2121, 2124, 2126, 2127, 2129, 2130, 2131, 2132, 2135, 2136, 2137, 2138, 2140, 2141, 2142, 2145, 2146, 2149, 2150, 2151, 2152, 2153, 2154, 2155, 2156, 2157, 2158, 2159, 2160, 2161, 2162, 2164, 2165, 2166, 2167, 2168, 2170, 2171, 2172, 2173, 2175, 2176, 2177, 2185, 2187, 2189, 2191, 2193, 2194, 2195, 2196, 2197, 2199, 2200, 2202, 2203, 2205, 2206, 2209, 2211, 2213, 2215, 2217, 2219, 2220, 2223, 2225, 2226, 2228, 2229, 2230, 2231, 2234, 2235, 2236, 2237, 2239, 2240, 2241, 2244, 2245, 2248, 2249, 2250, 2251, 2252, 2253, 2254, 2255, 2256, 2257, 2258, 2259, 2260, 2261, 2263, 2264, 2265, 2266, 2267, 2269, 2270, 2271, 2272, 2274, 2275, 2276, 2319, 2320, 2321, 2322, 2323, 2324, 2325, 2326, 2331, 2332, 2333, 2334, 2337, 2338, 2341, 2342, 2343, 2345, 2346, 2347, 2348, 2349, 2350, 2351, 2352, 2353, 2354, 2355, 2356, 2357, 2358, 2360, 2361, 2362, 2363, 2364, 2367, 2368, 2369, 2372, 2373, 2376, 2377, 2378, 2379, 2380, 2381, 2382, 2406, 2418, 2419, 2420, 2421, 2422, 2423, 2424, 2425, 2430, 2431, 2432, 2433, 2436, 2437, 2440, 2441, 2442, 2444, 2445, 2446, 2447, 2448, 2449, 2450, 2451, 2452, 2453, 2454, 2455, 2456, 2457, 2459, 2460, 2461, 2462, 2463, 2466, 2467, 2468, 2471, 2472, 2475, 2476, 2477, 2478, 2479, 2480, 2481, 2505, 2517, 2518, 2519, 2520, 2521, 2522, 2523, 2524, 2529, 2530, 2531, 2532, 2535, 2536, 2539, 2540, 2541, 2543, 2544, 2545, 2546, 2547, 2548, 2549, 2550, 2551, 2552, 2553, 2554, 2555, 2556, 2558, 2559, 2560, 2561, 2562, 2565, 2566, 2567, 2570, 2571, 2574, 2575, 2576, 2577, 2578, 2579, 2580, 2604, 2616, 2617, 2618, 2619, 2620, 2621, 2622, 2623, 2628, 2629, 2630, 2631, 2634, 2635, 2638, 2639, 2640, 2642, 2643, 2644, 2645, 2646, 2647, 2648, 2649, 2650, 2651, 2652, 2653, 2654, 2655, 2657, 2658, 2659, 2660, 2661, 2664, 2665, 2666, 2669, 2670, 2673, 2674, 2675, 2676, 2677, 2678, 2679, 2680, 2681, 2682, 2683, 2684, 2685, 2686, 2687, 2693, 2694, 2696, 2697, 2699, 2700, 2702, 2703, 2705, 2707, 2709, 2711, 2713, 2715, 2716, 2717, 2718, 2719, 2720, 2721, 2722, 2723, 2724, 2725, 2726, 2727, 2728, 2729, 2730, 2731, 2732, 2733, 2734, 2735, 2736, 2737, 2738, 2739, 2740, 2741, 2742, 2743, 2744, 2745, 2746, 2747, 2748, 2749, 2750, 2751, 2752, 2753, 2754, 2755, 2756, 2757, 2758, 2759, 2760, 2761, 2762, 2763, 2764, 2765, 2766, 2767, 2768, 2769, 2770, 2772, 2773, 2774, 2775, 2776, 2777, 2778, 2779, 2780, 2781, 2782, 2783, 2784, 2785, 2786, 2792, 2793, 2795, 2796, 2798, 2799, 2801, 2802, 2804, 2806, 2808, 2810, 2812, 2814, 2815, 2816, 2817, 2818, 2819, 2820, 2821, 2822, 2823, 2824, 2825, 2826, 2827, 2828, 2829, 2830, 2831, 2832, 2833, 2834, 2835, 2836, 2837, 2838, 2839, 2840, 2841, 2842, 2843, 2844, 2845, 2846, 2847, 2848, 2849, 2850, 2851, 2852, 2853, 2854, 2855, 2856, 2857, 2858, 2859, 2860, 2861, 2862, 2863, 2864, 2865, 2866, 2867, 2868, 2869, 2871, 2872, 2873, 2874, 2875, 2876, 2877, 2878, 2879, 2880, 2881, 2882, 2883, 2884, 2885, 2886, 2887, 2888, 2889, 2890, 2891, 2892, 2893, 2894, 2895, 2896, 2897, 2898, 2899, 2900, 2901, 2902, 2903, 2904, 2905, 2906, 2907, 2908, 2909, 2910, 2911, 2912, 2913, 2914, 2915, 2916, 2917, 2918, 2919, 2920, 2921, 2922, 2923, 2924, 2925, 2926, 2927, 2928, 2929, 2930, 2931, 2932, 2933, 2934, 2935, 2936, 2937, 2938, 2939, 2940, 2941, 2942, 2943, 2944, 2945, 2946, 2947, 2948, 2949, 2950, 2951, 2952, 2953, 2954, 2955, 2956, 2957, 2958, 2959, 2960, 2961, 2962, 2963, 2964, 2965, 2966, 2967, 2968, 2969, 2977, 2978, 2979, 2980, 2981, 2982, 2983, 2984, 2990, 2991, 2993, 2994, 2996, 2997, 2999, 3002, 3004, 3006, 3008, 3010, 3012, 3013, 3014, 3015, 3016, 3017, 3018, 3019, 3020, 3021, 3022, 3023, 3024, 3025, 3026, 3027, 3028, 3029, 3030, 3031, 3032, 3033, 3034, 3035, 3036, 3037, 3038, 3039, 3040, 3041, 3042, 3043, 3044, 3045, 3046, 3047, 3048, 3049, 3050, 3051, 3052, 3053, 3054, 3055, 3056, 3057, 3058, 3059, 3060, 3061, 3062, 3063, 3064, 3065, 3066, 3067, 3076, 3077, 3078, 3079, 3080, 3081, 3082, 3083, 3084, 3085, 3086, 3087, 3088, 3089, 3090, 3091, 3092, 3093, 3094, 3095, 3096, 3097, 3098, 3100, 3101, 3102, 3103, 3104, 3105, 3106, 3107, 3108, 3109, 3110, 3111, 3112, 3113, 3114, 3115, 3116, 3117, 3118, 3119, 3120, 3121, 3122, 3123, 3124, 3125, 3126, 3127, 3128, 3129, 3130, 3131, 3132, 3133, 3134, 3135, 3136, 3137, 3138, 3139, 3140, 3141, 3142, 3143, 3144, 3145, 3146, 3147, 3148, 3149, 3150, 3151, 3152, 3153, 3154, 3155, 3156, 3157, 3158, 3159, 3160, 3161, 3162, 3163, 3164, 3165, 3166, 3167, 3175, 3176, 3177, 3178, 3179, 3180, 3181, 3182, 3183, 3184, 3185, 3186, 3187, 3188, 3189, 3190, 3191, 3192, 3193, 3194, 3195, 3196, 3197, 3199, 3200, 3201, 3202, 3203, 3204, 3205, 3206, 3207, 3208, 3209, 3210, 3211, 3212, 3213, 3214, 3215, 3216, 3217, 3218, 3219, 3220, 3221, 3222, 3223, 3224, 3225, 3226, 3227, 3228, 3229, 3230, 3231, 3232, 3233, 3234, 3235, 3236, 3237, 3238, 3239, 3240, 3241, 3242, 3243, 3244, 3245, 3246, 3247, 3248, 3249, 3250, 3251, 3252, 3253, 3254, 3255, 3256, 3257, 3258, 3259, 3260, 3261, 3262, 3263, 3264, 3265, 3266, 3274, 3275, 3276, 3277, 3278, 3279, 3280, 3281, 3282, 3283, 3284, 3285, 3286, 3287, 3288, 3289, 3290, 3291, 3292, 3293, 3294, 3295, 3296, 3298, 3299, 3300, 3301, 3302, 3303, 3304, 3305, 3306, 3307, 3308, 3309, 3310, 3311, 3312, 3313, 3314, 3315, 3316, 3317, 3318, 3319, 3320, 3321, 3322, 3323, 3324, 3325, 3326, 3327, 3328, 3329, 3330, 3331, 3332, 3333, 3334, 3335, 3336, 3337, 3338, 3339, 3340, 3341, 3342, 3343, 3344, 3345, 3346, 3347, 3348, 3349, 3350, 3351, 3352, 3353, 3354, 3355, 3356, 3357, 3358, 3359, 3360, 3361, 3362, 3363, 3364, 3365, 3373, 3375, 3377, 3379, 3387, 3390, 3393, 3408, 3411, 3413, 3414, 3416, 3417, 3418, 3419, 3422, 3423, 3424, 3425, 3427, 3428, 3429, 3432, 3433, 3436, 3437, 3438, 3439, 3440, 3441, 3442, 3443, 3444, 3445, 3446, 3447, 3448, 3449, 3451, 3452, 3453, 3454, 3455, 3457, 3458, 3459, 3460, 3462, 3463, 3472, 3474, 3476, 3478, 3480, 3481, 3482, 3483, 3484, 3486, 3487, 3489, 3490, 3492, 3493, 3496, 3498, 3500, 3502, 3504, 3506, 3507, 3510, 3512, 3513, 3515, 3516, 3517, 3518, 3521, 3522, 3523, 3524, 3526, 3527, 3528, 3531, 3532, 3535, 3536, 3537, 3538, 3539, 3540, 3541, 3542, 3543, 3544, 3545, 3546, 3547, 3548, 3550, 3551, 3552, 3553, 3554, 3556, 3557, 3558, 3559, 3561, 3562, 3563, 3571, 3573, 3575, 3577, 3579, 3580, 3581, 3582, 3583, 3585, 3586, 3588, 3589, 3591, 3592, 3595, 3597, 3599, 3601, 3603, 3605, 3606, 3609, 3611, 3612, 3614, 3615, 3616, 3617, 3620, 3621, 3622, 3623, 3625, 3626, 3627, 3630, 3631, 3634, 3635, 3636, 3637, 3638, 3639, 3640, 3641, 3642, 3643, 3644, 3645, 3646, 3647, 3649, 3650, 3651, 3652, 3653, 3655, 3656, 3657, 3658, 3660, 3661, 3662, 3670, 3672, 3674, 3676, 3678, 3679, 3680, 3681, 3682, 3684, 3685, 3687, 3688, 3690, 3691, 3694, 3696, 3698, 3700, 3702, 3704, 3705, 3708, 3710, 3711, 3713, 3714, 3715, 3716, 3719, 3720, 3721, 3722, 3724, 3725, 3726, 3729, 3730, 3733, 3734, 3735, 3736, 3737, 3738, 3739, 3740, 3741, 3742, 3743, 3744, 3745, 3746, 3748, 3749, 3750, 3751, 3752, 3754, 3755, 3756, 3757, 3759, 3760, 3761, 3769, 3771, 3773, 3775, 3777, 3778, 3779, 3780, 3781, 3783, 3784, 3786, 3787, 3789, 3790, 3793, 3795, 3797, 3799, 3801, 3803, 3804, 3807, 3809, 3810, 3812, 3813, 3814, 3815, 3818, 3819, 3820, 3821, 3823, 3824, 3825, 3828, 3829, 3832, 3833, 3834, 3835, 3836, 3837, 3838, 3839, 3840, 3841, 3842, 3843, 3844, 3845, 3847, 3848, 3849, 3850, 3851, 3853, 3854, 3855, 3856, 3858, 3859, 3860, 3904, 3905, 3907, 3910, 3915, 3916, 3921, 3925, 3926, 3929, 3930, 3932, 3933, 3934, 3935, 3936, 3937, 3938, 3939, 3940, 3942, 3944, 3945, 3948, 3951, 3952, 3953, 3956, 3957, 3960, 3961, 3962, 3963, 3964, 3965, 3966, 3990, 4003, 4004, 4006, 4009, 4014, 4015, 4020, 4024, 4025, 4028, 4029, 4031, 4032, 4033, 4034, 4035, 4036, 4037, 4038, 4039, 4041, 4043, 4044, 4047, 4050, 4051, 4052, 4055, 4056, 4059, 4060, 4061, 4062, 4063, 4064, 4065, 4089, 4102, 4103, 4105, 4108, 4113, 4114, 4119, 4123, 4124, 4127, 4128, 4130, 4131, 4132, 4133, 4134, 4135, 4136, 4137, 4138, 4140, 4142, 4143, 4146, 4149, 4150, 4151, 4154, 4155, 4158, 4159, 4160, 4161, 4162, 4163, 4164, 4188, 4201, 4202, 4204, 4207, 4212, 4213, 4218, 4222, 4223, 4226, 4227, 4229, 4230, 4231, 4232, 4233, 4234, 4235, 4236, 4237, 4239, 4241, 4242, 4245, 4248, 4249, 4250, 4253, 4254, 4257, 4258, 4259, 4260, 4261, 4262, 4263, 4287, 4300, 4301, 4303, 4306, 4311, 4312, 4317, 4321, 4322, 4325, 4326, 4328, 4329, 4330, 4331, 4332, 4333, 4334, 4335, 4336, 4338, 4340, 4341, 4344, 4347, 4348, 4349, 4352, 4353, 4356, 4357, 4358, 4359, 4360, 4361, 4362, 4364, 4366, 4368, 4370, 4376, 4379, 4382, 4385, 4386, 4388, 4390, 4392, 4394, 4396, 4399, 4400, 4402, 4405, 4406, 4407, 4408, 4409, 4410, 4411, 4414, 4415, 4416, 4418, 4419, 4420, 4421, 4423, 4424, 4425, 4427, 4428, 4429, 4430, 4431, 4432, 4433, 4434, 4435, 4437, 4438, 4439, 4440, 4443, 4444, 4445, 4446, 4447, 4448, 4449, 4450, 4451, 4452, 4453, 4455, 4456, 4457, 4458, 4459, 4460, 4461, 4463, 4465, 4467, 4469, 4475, 4478, 4481, 4484, 4485, 4487, 4489, 4491, 4493, 4495, 4498, 4499, 4501, 4504, 4505, 4506, 4507, 4508, 4509, 4510, 4513, 4514, 4515, 4517, 4518, 4519, 4520, 4522, 4523, 4524, 4526, 4527, 4528, 4529, 4530, 4531, 4532, 4533, 4534, 4536, 4537, 4538, 4539, 4542, 4543, 4544, 4545, 4546, 4547, 4548, 4549, 4550, 4551, 4552, 4554, 4555, 4556, 4557, 4558, 4559, 4560, 4562, 4564, 4566, 4568, 4569, 4570, 4571, 4572, 4573, 4574, 4576, 4577, 4579, 4580, 4582, 4583, 4584, 4585, 4586, 4587, 4588, 4589, 4590, 4591, 4592, 4593, 4594, 4595, 4597, 4598, 4600, 4603, 4604, 4605, 4606, 4607, 4608, 4609, 4612, 4613, 4614, 4616, 4617, 4618, 4619, 4621, 4622, 4623, 4625, 4626, 4627, 4628, 4629, 4630, 4631, 4632, 4633, 4635, 4636, 4637, 4638, 4641, 4642, 4643, 4644, 4645, 4646, 4647, 4648, 4649, 4650, 4651, 4652, 4653, 4654, 4655, 4656, 4657, 4658, 4659, 4661, 4663, 4665, 4667, 4668, 4669, 4670, 4671, 4672, 4673, 4675, 4676, 4678, 4679, 4681, 4682, 4683, 4684, 4685, 4686, 4687, 4688, 4689, 4690, 4691, 4692, 4693, 4694, 4696, 4697, 4699, 4702, 4703, 4704, 4705, 4706, 4707, 4708, 4711, 4712, 4713, 4715, 4716, 4717, 4718, 4720, 4721, 4722, 4724, 4725, 4726, 4727, 4728, 4729, 4730, 4731, 4732, 4734, 4735, 4736, 4737, 4740, 4741, 4742, 4743, 4744, 4745, 4746, 4747, 4748, 4749, 4750, 4751, 4752, 4753, 4754, 4755, 4756, 4757, 4758, 4760, 4762, 4764, 4766, 4772, 4775, 4778, 4781, 4782, 4784, 4786, 4788, 4790, 4792, 4795, 4796, 4798, 4801, 4802, 4803, 4804, 4805, 4806, 4807, 4810, 4811, 4812, 4814, 4815, 4816, 4817, 4819, 4820, 4821, 4823, 4824, 4825, 4826, 4827, 4828, 4829, 4830, 4831, 4833, 4834, 4835, 4836, 4839, 4840, 4841, 4842, 4843, 4844, 4845, 4846, 4847, 4848, 4849, 4851, 4852, 4853, 4854, 4855, 4856, 4857, 4859, 4861, 4863, 4865, 4866, 4867, 4868, 4869, 4870, 4871, 4873, 4874, 4876, 4877, 4879, 4880, 4881, 4882, 4883, 4884, 4885, 4886, 4887, 4888, 4889, 4890, 4891, 4892, 4894, 4895, 4897, 4900, 4901, 4902, 4903, 4904, 4905, 4906, 4909, 4910, 4911, 4913, 4914, 4915, 4916, 4918, 4919, 4920, 4922, 4923, 4924, 4925, 4926, 4927, 4928, 4929, 4930, 4932, 4933, 4934, 4935, 4938, 4939, 4940, 4941, 4942, 4943, 4944, 4945, 4946, 4947, 4948, 4949, 4950, 4951, 4952, 4953, 4954, 4955, 4956, 4958, 4960, 4962, 4964, 4965, 4966, 4967, 4968, 4969, 4970, 4972, 4973, 4975, 4976, 4978, 4979, 4980, 4981, 4982, 4983, 4984, 4985, 4986, 4987, 4988, 4989, 4990, 4991, 4993, 4994, 4996, 4999, 5000, 5001, 5002, 5003, 5004, 5005, 5008, 5009, 5010, 5012, 5013, 5014, 5015, 5017, 5018, 5019, 5021, 5022, 5023, 5024, 5025, 5026, 5027, 5028, 5029, 5031, 5032, 5033, 5034, 5037, 5038, 5039, 5040, 5041, 5042, 5043, 5044, 5045, 5046, 5047, 5048, 5049, 5050, 5051, 5052, 5053, 5054, 5055, 5057, 5059, 5061, 5063, 5064, 5065, 5066, 5067, 5068, 5069, 5071, 5072, 5074, 5075, 5077, 5078, 5079, 5080, 5081, 5082, 5083, 5084, 5085, 5086, 5087, 5088, 5089, 5090, 5092, 5093, 5095, 5098, 5099, 5100, 5101, 5102, 5103, 5104, 5107, 5108, 5109, 5111, 5112, 5113, 5114, 5116, 5117, 5118, 5120, 5121, 5122, 5123, 5124, 5125, 5126, 5127, 5128, 5130, 5131, 5132, 5133, 5136, 5137, 5138, 5139, 5140, 5141, 5142, 5143, 5144, 5145, 5146, 5147, 5156, 5158, 5160, 5162, 5168, 5171, 5174, 5177, 5180, 5182, 5184, 5186, 5188, 5191, 5192, 5194, 5197, 5198, 5199, 5200, 5201, 5202, 5203, 5206, 5207, 5208, 5210, 5211, 5212, 5213, 5215, 5216, 5217, 5219, 5220, 5221, 5222, 5223, 5224, 5225, 5226, 5227, 5229, 5230, 5231, 5232, 5235, 5236, 5237, 5238, 5239, 5240, 5241, 5242, 5243, 5244, 5245, 5255, 5257, 5259, 5261, 5262, 5263, 5264, 5265, 5266, 5267, 5269, 5270, 5272, 5273, 5275, 5276, 5278, 5279, 5280, 5281, 5282, 5283, 5284, 5285, 5286, 5287, 5288, 5290, 5291, 5293, 5296, 5297, 5298, 5299, 5300, 5301, 5302, 5305, 5306, 5307, 5309, 5310, 5311, 5312, 5314, 5315, 5316, 5318, 5319, 5320, 5321, 5322, 5323, 5324, 5325, 5326, 5328, 5329, 5330, 5331, 5334, 5335, 5336, 5337, 5338, 5339, 5340, 5341, 5342, 5343, 5344, 5345, 5354, 5356, 5358, 5360, 5361, 5362, 5363, 5364, 5365, 5366, 5368, 5369, 5371, 5372, 5374, 5375, 5377, 5378, 5379, 5380, 5381, 5382, 5383, 5384, 5385, 5386, 5387, 5389, 5390, 5392, 5395, 5396, 5397, 5398, 5399, 5400, 5401, 5404, 5405, 5406, 5408, 5409, 5410, 5411, 5413, 5414, 5415, 5417, 5418, 5419, 5420, 5421, 5422, 5423, 5424, 5425, 5427, 5428, 5429, 5430, 5433, 5434, 5435, 5436, 5437, 5438, 5439, 5440, 5441, 5442, 5443, 5444, 5453, 5455, 5457, 5459, 5460, 5461, 5462, 5463, 5464, 5465, 5467, 5468, 5470, 5471, 5473, 5474, 5476, 5477, 5478, 5479, 5480, 5481, 5482, 5483, 5484, 5485, 5486, 5488, 5489, 5491, 5494, 5495, 5496, 5497, 5498, 5499, 5500, 5503, 5504, 5505, 5507, 5508, 5509, 5510, 5512, 5513, 5514, 5516, 5517, 5518, 5519, 5520, 5521, 5522, 5523, 5524, 5526, 5527, 5528, 5529, 5532, 5533, 5534, 5535, 5536, 5537, 5538, 5539, 5540, 5541, 5542, 5543, 5552, 5554, 5556, 5558, 5559, 5560, 5561, 5562, 5563, 5564, 5566, 5567, 5569, 5570, 5572, 5573, 5575, 5576, 5577, 5578, 5579, 5580, 5581, 5582, 5583, 5584, 5585, 5587, 5588, 5590, 5593, 5594, 5595, 5596, 5597, 5598, 5599, 5602, 5603, 5604, 5606, 5607, 5608, 5609, 5611, 5612, 5613, 5615, 5616, 5617, 5618, 5619, 5620, 5621, 5622, 5623, 5625, 5626, 5627, 5628, 5631, 5632, 5633, 5634, 5635, 5636, 5637, 5638, 5639, 5640, 5641, 5642, 5693, 5694, 5695, 5696, 5701, 5702, 5705, 5706, 5710, 5714, 5715, 5716, 5717, 5718, 5719, 5720, 5721, 5722, 5724, 5725, 5726, 5730, 5731, 5732, 5734, 5735, 5736, 5737, 5739, 5740, 5757, 5758, 5759, 5760, 5761, 5764, 5767, 5770, 5773, 5775, 5777, 5779, 5781, 5783, 5792, 5793, 5794, 5795, 5800, 5801, 5804, 5805, 5809, 5813, 5814, 5815, 5816, 5817, 5818, 5819, 5820, 5821, 5823, 5824, 5825, 5829, 5830, 5831, 5833, 5834, 5835, 5836, 5838, 5839, 5840, 5856, 5857, 5858, 5859, 5860, 5863, 5866, 5869, 5872, 5874, 5876, 5878, 5880, 5882, 5891, 5892, 5893, 5894, 5899, 5900, 5903, 5904, 5908, 5912, 5913, 5914, 5915, 5916, 5917, 5918, 5919, 5920, 5922, 5923, 5924, 5928, 5929, 5930, 5932, 5933, 5934, 5935, 5937, 5938, 5939, 5955, 5956, 5957, 5958, 5959, 5962, 5965, 5968, 5971, 5973, 5975, 5977, 5979, 5981, 5990, 5991, 5992, 5993, 5998, 5999, 6002, 6003, 6007, 6011, 6012, 6013, 6014, 6015, 6016, 6017, 6018, 6019, 6021, 6022, 6023, 6027, 6028, 6029, 6031, 6032, 6033, 6034, 6036, 6037, 6038, 6054, 6055, 6056, 6057, 6058, 6061, 6064, 6067, 6070, 6072, 6074, 6076, 6078, 6080, 6089, 6090, 6091, 6092, 6097, 6098, 6101, 6102, 6106, 6110, 6111, 6112, 6113, 6114, 6115, 6116, 6117, 6118, 6120, 6121, 6122, 6126, 6127, 6128, 6130, 6131, 6132, 6133, 6135, 6136, 6137, 6153, 6154, 6155, 6156, 6157, 6160, 6163, 6166, 6169, 6171, 6173, 6175, 6177, 6179, 6188, 6189, 6190, 6191, 6196, 6197, 6200, 6201, 6205, 6209, 6210, 6211, 6212, 6213, 6214, 6215, 6216, 6217, 6219, 6220, 6221, 6225, 6226, 6227, 6229, 6230, 6231, 6232, 6234, 6235, 6236, 6252, 6253, 6254, 6255, 6256, 6259, 6262, 6265, 6268, 6270, 6272, 6274, 6276, 6278, 6287, 6288, 6289, 6290, 6295, 6296, 6299, 6300, 6304, 6319, 6325, 6326, 6330, 6331, 6334, 6335}; // which combinations of 4 points used to generate the 15 equations used in the solver // clang-format off static const int pt_index[] = { 0,1,2,3, 0,1,2,4, 0,1,2,5, 0,1,3,4, 0,1,3,5, 0,1,4,5, 0,2,3,4, 0,2,3,5, 0,2,4,5, 0,3,4,5, 1,2,3,4, 1,2,3,5, 1,2,4,5, 1,3,4,5, 2,3,4,5, }; // clang-format on // Multiplies a deg 2 poly with a deg 2 poly void mul2_2(double *a, double *b, double *c) { c[0] = a[0] * b[0]; c[1] = a[0] * b[1] + a[1] * b[0]; c[2] = a[0] * b[3] + a[3] * b[0]; c[3] = a[0] * b[6] + a[6] * b[0]; c[4] = a[0] * b[2] + a[1] * b[1] + a[2] * b[0]; c[5] = a[0] * b[4] + a[1] * b[3] + a[3] * b[1] + a[4] * b[0]; c[6] = a[0] * b[7] + a[1] * b[6] + a[6] * b[1] + a[7] * b[0]; c[7] = a[0] * b[5] + a[5] * b[0] + a[3] * b[3]; c[8] = a[0] * b[8] + a[8] * b[0] + a[3] * b[6] + a[6] * b[3]; c[9] = a[0] * b[9] + a[9] * b[0] + a[6] * b[6]; c[10] = a[1] * b[2] + a[2] * b[1]; c[11] = a[1] * b[4] + a[2] * b[3] + a[3] * b[2] + a[4] * b[1]; c[12] = a[1] * b[7] + a[2] * b[6] + a[6] * b[2] + a[7] * b[1]; c[13] = a[1] * b[5] + a[5] * b[1] + a[3] * b[4] + a[4] * b[3]; c[14] = a[1] * b[8] + a[8] * b[1] + a[3] * b[7] + a[4] * b[6] + a[6] * b[4] + a[7] * b[3]; c[15] = a[1] * b[9] + a[9] * b[1] + a[6] * b[7] + a[7] * b[6]; c[16] = a[3] * b[5] + a[5] * b[3]; c[17] = a[3] * b[8] + a[5] * b[6] + a[6] * b[5] + a[8] * b[3]; c[18] = a[3] * b[9] + a[9] * b[3] + a[6] * b[8] + a[8] * b[6]; c[19] = a[6] * b[9] + a[9] * b[6]; c[20] = a[2] * b[2]; c[21] = a[2] * b[4] + a[4] * b[2]; c[22] = a[2] * b[7] + a[7] * b[2]; c[23] = a[2] * b[5] + a[5] * b[2] + a[4] * b[4]; c[24] = a[2] * b[8] + a[8] * b[2] + a[4] * b[7] + a[7] * b[4]; c[25] = a[2] * b[9] + a[9] * b[2] + a[7] * b[7]; c[26] = a[4] * b[5] + a[5] * b[4]; c[27] = a[4] * b[8] + a[5] * b[7] + a[7] * b[5] + a[8] * b[4]; c[28] = a[4] * b[9] + a[9] * b[4] + a[7] * b[8] + a[8] * b[7]; c[29] = a[7] * b[9] + a[9] * b[7]; c[30] = a[5] * b[5]; c[31] = a[5] * b[8] + a[8] * b[5]; c[32] = a[5] * b[9] + a[9] * b[5] + a[8] * b[8]; c[33] = a[8] * b[9] + a[9] * b[8]; c[34] = a[9] * b[9]; } // Multiplies a deg 2 poly with a deg 2 poly and subtracts it from c void mul2_2m(double *a, double *b, double *c) { c[0] -= a[0] * b[0]; c[1] -= a[0] * b[1] + a[1] * b[0]; c[2] -= a[0] * b[3] + a[3] * b[0]; c[3] -= a[0] * b[6] + a[6] * b[0]; c[4] -= a[0] * b[2] + a[1] * b[1] + a[2] * b[0]; c[5] -= a[0] * b[4] + a[1] * b[3] + a[3] * b[1] + a[4] * b[0]; c[6] -= a[0] * b[7] + a[1] * b[6] + a[6] * b[1] + a[7] * b[0]; c[7] -= a[0] * b[5] + a[5] * b[0] + a[3] * b[3]; c[8] -= a[0] * b[8] + a[8] * b[0] + a[3] * b[6] + a[6] * b[3]; c[9] -= a[0] * b[9] + a[9] * b[0] + a[6] * b[6]; c[10] -= a[1] * b[2] + a[2] * b[1]; c[11] -= a[1] * b[4] + a[2] * b[3] + a[3] * b[2] + a[4] * b[1]; c[12] -= a[1] * b[7] + a[2] * b[6] + a[6] * b[2] + a[7] * b[1]; c[13] -= a[1] * b[5] + a[5] * b[1] + a[3] * b[4] + a[4] * b[3]; c[14] -= a[1] * b[8] + a[8] * b[1] + a[3] * b[7] + a[4] * b[6] + a[6] * b[4] + a[7] * b[3]; c[15] -= a[1] * b[9] + a[9] * b[1] + a[6] * b[7] + a[7] * b[6]; c[16] -= a[3] * b[5] + a[5] * b[3]; c[17] -= a[3] * b[8] + a[5] * b[6] + a[6] * b[5] + a[8] * b[3]; c[18] -= a[3] * b[9] + a[9] * b[3] + a[6] * b[8] + a[8] * b[6]; c[19] -= a[6] * b[9] + a[9] * b[6]; c[20] -= a[2] * b[2]; c[21] -= a[2] * b[4] + a[4] * b[2]; c[22] -= a[2] * b[7] + a[7] * b[2]; c[23] -= a[2] * b[5] + a[5] * b[2] + a[4] * b[4]; c[24] -= a[2] * b[8] + a[8] * b[2] + a[4] * b[7] + a[7] * b[4]; c[25] -= a[2] * b[9] + a[9] * b[2] + a[7] * b[7]; c[26] -= a[4] * b[5] + a[5] * b[4]; c[27] -= a[4] * b[8] + a[5] * b[7] + a[7] * b[5] + a[8] * b[4]; c[28] -= a[4] * b[9] + a[9] * b[4] + a[7] * b[8] + a[8] * b[7]; c[29] -= a[7] * b[9] + a[9] * b[7]; c[30] -= a[5] * b[5]; c[31] -= a[5] * b[8] + a[8] * b[5]; c[32] -= a[5] * b[9] + a[9] * b[5] + a[8] * b[8]; c[33] -= a[8] * b[9] + a[9] * b[8]; c[34] -= a[9] * b[9]; } // Multiplies a deg 2 poly with a deg 4 poly and adds it to c void mul2_4p(double *a, double *b, double *c) { c[0] += a[0] * b[0]; c[1] += a[0] * b[1] + a[1] * b[0]; c[2] += a[1] * b[1] + a[2] * b[0] + a[0] * b[4]; c[3] += a[2] * b[1] + a[1] * b[4] + a[0] * b[10]; c[4] += a[2] * b[4] + a[1] * b[10] + a[0] * b[20]; c[5] += a[2] * b[10] + a[1] * b[20]; c[6] += a[2] * b[20]; c[7] += a[0] * b[2] + a[3] * b[0]; c[8] += a[1] * b[2] + a[3] * b[1] + a[4] * b[0] + a[0] * b[5]; c[9] += a[2] * b[2] + a[4] * b[1] + a[1] * b[5] + a[3] * b[4] + a[0] * b[11]; c[10] += a[2] * b[5] + a[4] * b[4] + a[1] * b[11] + a[3] * b[10] + a[0] * b[21]; c[11] += a[2] * b[11] + a[4] * b[10] + a[1] * b[21] + a[3] * b[20]; c[12] += a[2] * b[21] + a[4] * b[20]; c[13] += a[3] * b[2] + a[5] * b[0] + a[0] * b[7]; c[14] += a[4] * b[2] + a[5] * b[1] + a[1] * b[7] + a[3] * b[5] + a[0] * b[13]; c[15] += a[2] * b[7] + a[4] * b[5] + a[5] * b[4] + a[1] * b[13] + a[3] * b[11] + a[0] * b[23]; c[16] += a[2] * b[13] + a[4] * b[11] + a[5] * b[10] + a[1] * b[23] + a[3] * b[21]; c[17] += a[2] * b[23] + a[4] * b[21] + a[5] * b[20]; c[18] += a[5] * b[2] + a[3] * b[7] + a[0] * b[16]; c[19] += a[5] * b[5] + a[4] * b[7] + a[3] * b[13] + a[1] * b[16] + a[0] * b[26]; c[20] += a[5] * b[11] + a[4] * b[13] + a[2] * b[16] + a[3] * b[23] + a[1] * b[26]; c[21] += a[5] * b[21] + a[4] * b[23] + a[2] * b[26]; c[22] += a[5] * b[7] + a[3] * b[16] + a[0] * b[30]; c[23] += a[5] * b[13] + a[4] * b[16] + a[3] * b[26] + a[1] * b[30]; c[24] += a[5] * b[23] + a[4] * b[26] + a[2] * b[30]; c[25] += a[5] * b[16] + a[3] * b[30]; c[26] += a[5] * b[26] + a[4] * b[30]; c[27] += a[5] * b[30]; c[28] += a[0] * b[3] + a[6] * b[0]; c[29] += a[1] * b[3] + a[0] * b[6] + a[6] * b[1] + a[7] * b[0]; c[30] += a[2] * b[3] + a[1] * b[6] + a[7] * b[1] + a[6] * b[4] + a[0] * b[12]; c[31] += a[2] * b[6] + a[7] * b[4] + a[1] * b[12] + a[6] * b[10] + a[0] * b[22]; c[32] += a[2] * b[12] + a[7] * b[10] + a[1] * b[22] + a[6] * b[20]; c[33] += a[2] * b[22] + a[7] * b[20]; c[34] += a[3] * b[3] + a[0] * b[8] + a[6] * b[2] + a[8] * b[0]; c[35] += a[4] * b[3] + a[1] * b[8] + a[3] * b[6] + a[7] * b[2] + a[8] * b[1] + a[6] * b[5] + a[0] * b[14]; c[36] += a[2] * b[8] + a[4] * b[6] + a[7] * b[5] + a[8] * b[4] + a[1] * b[14] + a[3] * b[12] + a[6] * b[11] + a[0] * b[24]; c[37] += a[2] * b[14] + a[4] * b[12] + a[7] * b[11] + a[8] * b[10] + a[1] * b[24] + a[3] * b[22] + a[6] * b[21]; c[38] += a[2] * b[24] + a[4] * b[22] + a[7] * b[21] + a[8] * b[20]; c[39] += a[5] * b[3] + a[8] * b[2] + a[3] * b[8] + a[6] * b[7] + a[0] * b[17]; c[40] += a[5] * b[6] + a[4] * b[8] + a[8] * b[5] + a[7] * b[7] + a[3] * b[14] + a[1] * b[17] + a[6] * b[13] + a[0] * b[27]; c[41] += a[5] * b[12] + a[4] * b[14] + a[2] * b[17] + a[8] * b[11] + a[7] * b[13] + a[3] * b[24] + a[1] * b[27] + a[6] * b[23]; c[42] += a[5] * b[22] + a[4] * b[24] + a[2] * b[27] + a[8] * b[21] + a[7] * b[23]; c[43] += a[5] * b[8] + a[8] * b[7] + a[3] * b[17] + a[6] * b[16] + a[0] * b[31]; c[44] += a[5] * b[14] + a[4] * b[17] + a[8] * b[13] + a[7] * b[16] + a[3] * b[27] + a[1] * b[31] + a[6] * b[26]; c[45] += a[5] * b[24] + a[4] * b[27] + a[8] * b[23] + a[2] * b[31] + a[7] * b[26]; c[46] += a[5] * b[17] + a[8] * b[16] + a[3] * b[31] + a[6] * b[30]; c[47] += a[5] * b[27] + a[8] * b[26] + a[4] * b[31] + a[7] * b[30]; c[48] += a[5] * b[31] + a[8] * b[30]; c[49] += a[0] * b[9] + a[6] * b[3] + a[9] * b[0]; c[50] += a[1] * b[9] + a[7] * b[3] + a[9] * b[1] + a[6] * b[6] + a[0] * b[15]; c[51] += a[2] * b[9] + a[7] * b[6] + a[9] * b[4] + a[1] * b[15] + a[6] * b[12] + a[0] * b[25]; c[52] += a[2] * b[15] + a[7] * b[12] + a[9] * b[10] + a[1] * b[25] + a[6] * b[22]; c[53] += a[2] * b[25] + a[7] * b[22] + a[9] * b[20]; c[54] += a[8] * b[3] + a[9] * b[2] + a[3] * b[9] + a[6] * b[8] + a[0] * b[18]; c[55] += a[4] * b[9] + a[8] * b[6] + a[9] * b[5] + a[7] * b[8] + a[3] * b[15] + a[1] * b[18] + a[6] * b[14] + a[0] * b[28]; c[56] += a[4] * b[15] + a[2] * b[18] + a[8] * b[12] + a[9] * b[11] + a[7] * b[14] + a[3] * b[25] + a[1] * b[28] + a[6] * b[24]; c[57] += a[4] * b[25] + a[2] * b[28] + a[8] * b[22] + a[9] * b[21] + a[7] * b[24]; c[58] += a[5] * b[9] + a[8] * b[8] + a[9] * b[7] + a[3] * b[18] + a[6] * b[17] + a[0] * b[32]; c[59] += a[5] * b[15] + a[4] * b[18] + a[8] * b[14] + a[9] * b[13] + a[7] * b[17] + a[3] * b[28] + a[1] * b[32] + a[6] * b[27]; c[60] += a[5] * b[25] + a[4] * b[28] + a[8] * b[24] + a[9] * b[23] + a[2] * b[32] + a[7] * b[27]; c[61] += a[5] * b[18] + a[8] * b[17] + a[9] * b[16] + a[3] * b[32] + a[6] * b[31]; c[62] += a[5] * b[28] + a[8] * b[27] + a[9] * b[26] + a[4] * b[32] + a[7] * b[31]; c[63] += a[5] * b[32] + a[8] * b[31] + a[9] * b[30]; c[64] += a[9] * b[3] + a[6] * b[9] + a[0] * b[19]; c[65] += a[9] * b[6] + a[7] * b[9] + a[1] * b[19] + a[6] * b[15] + a[0] * b[29]; c[66] += a[2] * b[19] + a[9] * b[12] + a[7] * b[15] + a[1] * b[29] + a[6] * b[25]; c[67] += a[2] * b[29] + a[9] * b[22] + a[7] * b[25]; c[68] += a[8] * b[9] + a[9] * b[8] + a[3] * b[19] + a[6] * b[18] + a[0] * b[33]; c[69] += a[4] * b[19] + a[8] * b[15] + a[9] * b[14] + a[7] * b[18] + a[3] * b[29] + a[1] * b[33] + a[6] * b[28]; c[70] += a[4] * b[29] + a[8] * b[25] + a[9] * b[24] + a[2] * b[33] + a[7] * b[28]; c[71] += a[5] * b[19] + a[8] * b[18] + a[9] * b[17] + a[3] * b[33] + a[6] * b[32]; c[72] += a[5] * b[29] + a[8] * b[28] + a[9] * b[27] + a[4] * b[33] + a[7] * b[32]; c[73] += a[5] * b[33] + a[8] * b[32] + a[9] * b[31]; c[74] += a[9] * b[9] + a[6] * b[19] + a[0] * b[34]; c[75] += a[9] * b[15] + a[7] * b[19] + a[1] * b[34] + a[6] * b[29]; c[76] += a[9] * b[25] + a[2] * b[34] + a[7] * b[29]; c[77] += a[8] * b[19] + a[9] * b[18] + a[3] * b[34] + a[6] * b[33]; c[78] += a[8] * b[29] + a[9] * b[28] + a[4] * b[34] + a[7] * b[33]; c[79] += a[5] * b[34] + a[8] * b[33] + a[9] * b[32]; c[80] += a[9] * b[19] + a[6] * b[34]; c[81] += a[9] * b[29] + a[7] * b[34]; c[82] += a[8] * b[34] + a[9] * b[33]; c[83] += a[9] * b[34]; } // Computes the matrix of coefficients for the 15 equations (in 84 monomials) void setup_coeff_matrix(const std::vector &pp1, const std::vector &xx1, const std::vector &pp2, const std::vector &xx2, Eigen::Matrix *M) { Eigen::Matrix F1, F2, F3; double *f1 = F1.data(); double *f2 = F2.data(); double *f3 = F3.data(); std::vector qq1(6, Eigen::Vector3d::Zero()); std::vector qq2(6, Eigen::Vector3d::Zero()); for (size_t k = 0; k < 6; ++k) { qq1[k] = xx1[k].cross(pp1[k]); qq2[k] = xx2[k].cross(pp2[k]); } M->setZero(); for (size_t eq_k = 0; eq_k < 15; ++eq_k) { int i0 = pt_index[4 * eq_k]; Eigen::Vector3d x1 = xx1[i0]; Eigen::Vector3d p1 = pp1[i0]; Eigen::Vector3d x2 = xx2[i0]; Eigen::Vector3d p2 = pp2[i0]; // Compute 3x3 matrix where each element is quadratic in cayley parameters // F1 is the first column of the matrix, etc.. // This is for eliminating the translation. for (size_t i = 0; i < 3; ++i) { int i1 = pt_index[4 * eq_k + i + 1]; Eigen::Vector3d xp1 = xx1[i1]; Eigen::Vector3d qp1 = qq1[i1]; Eigen::Vector3d xp2 = xx2[i1]; Eigen::Vector3d qp2 = qq2[i1]; F1(0, i) = qp1(0) * xp2(0) + qp2(0) * xp1(0) - qp1(1) * xp2(1) - qp2(1) * xp1(1) - qp1(2) * xp2(2) - qp2(2) * xp1(2) + xp1(0) * (xp2(2) * (p1(1) + p2(1)) - xp2(1) * (p1(2) + p2(2))) + xp1(2) * (xp2(0) * (p1(1) + p2(1)) + xp2(1) * (p1(0) - p2(0))) - xp1(1) * (xp2(0) * (p1(2) + p2(2)) + xp2(2) * (p1(0) - p2(0))); F1(1, i) = 2 * qp1(0) * xp2(1) + 2 * qp1(1) * xp2(0) + 2 * qp2(0) * xp1(1) + 2 * qp2(1) * xp1(0) - xp1(0) * (2 * p2(0) * xp2(2) - xp2(0) * (2 * p1(2) + 2 * p2(2))) + xp1(1) * (2 * p2(1) * xp2(2) - xp2(1) * (2 * p1(2) + 2 * p2(2))) - xp1(2) * (2 * p1(0) * xp2(0) - 2 * p1(1) * xp2(1)); F1(2, i) = qp1(1) * xp2(1) - qp2(0) * xp1(0) - qp1(0) * xp2(0) + qp2(1) * xp1(1) - qp1(2) * xp2(2) - qp2(2) * xp1(2) - xp1(1) * (xp2(2) * (p1(0) + p2(0)) - xp2(0) * (p1(2) + p2(2))) - xp1(2) * (xp2(1) * (p1(0) + p2(0)) + xp2(0) * (p1(1) - p2(1))) + xp1(0) * (xp2(1) * (p1(2) + p2(2)) + xp2(2) * (p1(1) - p2(1))); F1(3, i) = 2 * qp1(0) * xp2(2) + 2 * qp1(2) * xp2(0) + 2 * qp2(0) * xp1(2) + 2 * qp2(2) * xp1(0) + xp1(0) * (2 * p2(0) * xp2(1) - xp2(0) * (2 * p1(1) + 2 * p2(1))) - xp1(2) * (2 * p2(2) * xp2(1) - xp2(2) * (2 * p1(1) + 2 * p2(1))) + xp1(1) * (2 * p1(0) * xp2(0) - 2 * p1(2) * xp2(2)); F1(4, i) = 2 * qp1(1) * xp2(2) + 2 * qp1(2) * xp2(1) + 2 * qp2(1) * xp1(2) + 2 * qp2(2) * xp1(1) - xp1(1) * (2 * p2(1) * xp2(0) - xp2(1) * (2 * p1(0) + 2 * p2(0))) + xp1(2) * (2 * p2(2) * xp2(0) - xp2(2) * (2 * p1(0) + 2 * p2(0))) - xp1(0) * (2 * p1(1) * xp2(1) - 2 * p1(2) * xp2(2)); F1(5, i) = qp1(2) * xp2(2) - qp2(0) * xp1(0) - qp1(1) * xp2(1) - qp2(1) * xp1(1) - qp1(0) * xp2(0) + qp2(2) * xp1(2) + xp1(2) * (xp2(1) * (p1(0) + p2(0)) - xp2(0) * (p1(1) + p2(1))) + xp1(1) * (xp2(2) * (p1(0) + p2(0)) + xp2(0) * (p1(2) - p2(2))) - xp1(0) * (xp2(2) * (p1(1) + p2(1)) + xp2(1) * (p1(2) - p2(2))); F1(6, i) = 2 * qp1(1) * xp2(2) - 2 * qp1(2) * xp2(1) - 2 * qp2(1) * xp1(2) + 2 * qp2(2) * xp1(1) - xp1(1) * (2 * p2(1) * xp2(0) + xp2(1) * (2 * p1(0) - 2 * p2(0))) - xp1(2) * (2 * p2(2) * xp2(0) + xp2(2) * (2 * p1(0) - 2 * p2(0))) + xp1(0) * (2 * p1(1) * xp2(1) + 2 * p1(2) * xp2(2)); F1(7, i) = 2 * qp1(2) * xp2(0) - 2 * qp1(0) * xp2(2) + 2 * qp2(0) * xp1(2) - 2 * qp2(2) * xp1(0) - xp1(0) * (2 * p2(0) * xp2(1) + xp2(0) * (2 * p1(1) - 2 * p2(1))) - xp1(2) * (2 * p2(2) * xp2(1) + xp2(2) * (2 * p1(1) - 2 * p2(1))) + xp1(1) * (2 * p1(0) * xp2(0) + 2 * p1(2) * xp2(2)); F1(8, i) = 2 * qp1(0) * xp2(1) - 2 * qp1(1) * xp2(0) - 2 * qp2(0) * xp1(1) + 2 * qp2(1) * xp1(0) - xp1(0) * (2 * p2(0) * xp2(2) + xp2(0) * (2 * p1(2) - 2 * p2(2))) - xp1(1) * (2 * p2(1) * xp2(2) + xp2(1) * (2 * p1(2) - 2 * p2(2))) + xp1(2) * (2 * p1(0) * xp2(0) + 2 * p1(1) * xp2(1)); F1(9, i) = xp1(1) * (xp2(2) * (p1(0) - p2(0)) - xp2(0) * (p1(2) - p2(2))) - xp1(2) * (xp2(1) * (p1(0) - p2(0)) - xp2(0) * (p1(1) - p2(1))) - xp1(0) * (xp2(2) * (p1(1) - p2(1)) - xp2(1) * (p1(2) - p2(2))) + qp1(0) * xp2(0) + qp2(0) * xp1(0) + qp1(1) * xp2(1) + qp2(1) * xp1(1) + qp1(2) * xp2(2) + qp2(2) * xp1(2); F2(0, i) = xp1(2) * (x1(0) * xp2(1) + x1(1) * xp2(0)) - xp1(1) * (x1(0) * xp2(2) + x1(2) * xp2(0)) + xp1(0) * (x1(1) * xp2(2) - x1(2) * xp2(1)); F2(1, i) = 2 * x1(2) * xp1(0) * xp2(0) - xp1(2) * (2 * x1(0) * xp2(0) - 2 * x1(1) * xp2(1)) - 2 * x1(2) * xp1(1) * xp2(1); F2(2, i) = xp1(0) * (x1(1) * xp2(2) + x1(2) * xp2(1)) - xp1(2) * (x1(0) * xp2(1) + x1(1) * xp2(0)) - xp1(1) * (x1(0) * xp2(2) - x1(2) * xp2(0)); F2(3, i) = xp1(1) * (2 * x1(0) * xp2(0) - 2 * x1(2) * xp2(2)) - 2 * x1(1) * xp1(0) * xp2(0) + 2 * x1(1) * xp1(2) * xp2(2); F2(4, i) = 2 * x1(0) * xp1(1) * xp2(1) - xp1(0) * (2 * x1(1) * xp2(1) - 2 * x1(2) * xp2(2)) - 2 * x1(0) * xp1(2) * xp2(2); F2(5, i) = xp1(1) * (x1(0) * xp2(2) + x1(2) * xp2(0)) + xp1(2) * (x1(0) * xp2(1) - x1(1) * xp2(0)) - xp1(0) * (x1(1) * xp2(2) + x1(2) * xp2(1)); F2(6, i) = xp1(0) * (2 * x1(1) * xp2(1) + 2 * x1(2) * xp2(2)) - 2 * x1(0) * xp1(1) * xp2(1) - 2 * x1(0) * xp1(2) * xp2(2); F2(7, i) = xp1(1) * (2 * x1(0) * xp2(0) + 2 * x1(2) * xp2(2)) - 2 * x1(1) * xp1(0) * xp2(0) - 2 * x1(1) * xp1(2) * xp2(2); F2(8, i) = xp1(2) * (2 * x1(0) * xp2(0) + 2 * x1(1) * xp2(1)) - 2 * x1(2) * xp1(0) * xp2(0) - 2 * x1(2) * xp1(1) * xp2(1); F2(9, i) = xp1(1) * (x1(0) * xp2(2) - x1(2) * xp2(0)) - xp1(2) * (x1(0) * xp2(1) - x1(1) * xp2(0)) - xp1(0) * (x1(1) * xp2(2) - x1(2) * xp2(1)); F3(0, i) = xp1(1) * (x2(0) * xp2(2) - x2(2) * xp2(0)) - xp1(2) * (x2(0) * xp2(1) - x2(1) * xp2(0)) + xp1(0) * (x2(1) * xp2(2) - x2(2) * xp2(1)); F3(1, i) = xp1(1) * (2 * x2(1) * xp2(2) - 2 * x2(2) * xp2(1)) - xp1(0) * (2 * x2(0) * xp2(2) - 2 * x2(2) * xp2(0)); F3(2, i) = -xp1(2) * (x2(0) * xp2(1) - x2(1) * xp2(0)) - xp1(1) * (x2(0) * xp2(2) - x2(2) * xp2(0)) - xp1(0) * (x2(1) * xp2(2) - x2(2) * xp2(1)); F3(3, i) = xp1(0) * (2 * x2(0) * xp2(1) - 2 * x2(1) * xp2(0)) + xp1(2) * (2 * x2(1) * xp2(2) - 2 * x2(2) * xp2(1)); F3(4, i) = xp1(1) * (2 * x2(0) * xp2(1) - 2 * x2(1) * xp2(0)) - xp1(2) * (2 * x2(0) * xp2(2) - 2 * x2(2) * xp2(0)); F3(5, i) = xp1(2) * (x2(0) * xp2(1) - x2(1) * xp2(0)) + xp1(1) * (x2(0) * xp2(2) - x2(2) * xp2(0)) - xp1(0) * (x2(1) * xp2(2) - x2(2) * xp2(1)); F3(6, i) = xp1(1) * (2 * x2(0) * xp2(1) - 2 * x2(1) * xp2(0)) + xp1(2) * (2 * x2(0) * xp2(2) - 2 * x2(2) * xp2(0)); F3(7, i) = xp1(2) * (2 * x2(1) * xp2(2) - 2 * x2(2) * xp2(1)) - xp1(0) * (2 * x2(0) * xp2(1) - 2 * x2(1) * xp2(0)); F3(8, i) = -xp1(0) * (2 * x2(0) * xp2(2) - 2 * x2(2) * xp2(0)) - xp1(1) * (2 * x2(1) * xp2(2) - 2 * x2(2) * xp2(1)); F3(9, i) = xp1(2) * (x2(0) * xp2(1) - x2(1) * xp2(0)) - xp1(1) * (x2(0) * xp2(2) - x2(2) * xp2(0)) + xp1(0) * (x2(1) * xp2(2) - x2(2) * xp2(1)); } double p4[35]; double *c = M->data() + 84 * eq_k; // Compute the determinant by expansion along first column mul2_2(f2 + 10, f3 + 20, p4); mul2_2m(f2 + 20, f3 + 10, p4); mul2_4p(f1, p4, c); mul2_2(f2 + 20, f3, p4); mul2_2m(f2, f3 + 20, p4); mul2_4p(f1 + 10, p4, c); mul2_2(f2, f3 + 10, p4); mul2_2m(f2 + 10, f3, p4); mul2_4p(f1 + 20, p4, c); } } #ifdef USE_FAST_EIGENVECTOR_SOLVER // Solves for the eigenvector by using structured backsubstitution // (i.e. substituting the eigenvalue into the eigenvector using the known structure to get a reduced linear system) void fast_eigenvector_solver(double *eigv, int neig, const Eigen::Matrix &AM, Eigen::Matrix &sols) { static const int ind[] = {5, 6, 7, 9, 10, 12, 15, 16, 18, 22, 26, 27, 29, 33, 38, 43, 44, 47, 51, 56, 63}; // Truncated action matrix containing non-trivial rows Eigen::Matrix AMs; double zi[8]; for (int i = 0; i < 21; i++) { AMs.row(i) = AM.row(ind[i]); } for (int i = 0; i < neig; i++) { zi[0] = eigv[i]; for (int j = 1; j < 8; j++) { zi[j] = zi[j - 1] * eigv[i]; } Eigen::Matrix AA; AA.col(0) = AMs.col(5); AA.col(1) = AMs.col(6); AA.col(2) = AMs.col(4) + zi[0] * AMs.col(7); AA.col(3) = AMs.col(9); AA.col(4) = AMs.col(8) + zi[0] * AMs.col(10); AA.col(5) = AMs.col(3) + zi[0] * AMs.col(11) + zi[1] * AMs.col(12); AA.col(6) = AMs.col(15); AA.col(7) = AMs.col(14) + zi[0] * AMs.col(16); AA.col(8) = AMs.col(13) + zi[0] * AMs.col(17) + zi[1] * AMs.col(18); AA.col(9) = AMs.col(2) + zi[0] * AMs.col(19) + zi[1] * AMs.col(20) + zi[2] * AMs.col(21) + zi[3] * AMs.col(22); AA.col(10) = AMs.col(26); AA.col(11) = AMs.col(25) + zi[0] * AMs.col(27); AA.col(12) = AMs.col(24) + zi[0] * AMs.col(28) + zi[1] * AMs.col(29); AA.col(13) = AMs.col(23) + zi[0] * AMs.col(30) + zi[1] * AMs.col(31) + zi[2] * AMs.col(32) + zi[3] * AMs.col(33); AA.col(14) = AMs.col(1) + zi[0] * AMs.col(34) + zi[1] * AMs.col(35) + zi[2] * AMs.col(36) + zi[3] * AMs.col(37) + zi[4] * AMs.col(38); AA.col(15) = AMs.col(43); AA.col(16) = AMs.col(42) + zi[0] * AMs.col(44); AA.col(17) = AMs.col(41) + zi[0] * AMs.col(45) + zi[1] * AMs.col(46) + zi[2] * AMs.col(47); AA.col(18) = AMs.col(40) + zi[0] * AMs.col(48) + zi[1] * AMs.col(49) + zi[2] * AMs.col(50) + zi[3] * AMs.col(51); AA.col(19) = AMs.col(39) + zi[0] * AMs.col(52) + zi[1] * AMs.col(53) + zi[2] * AMs.col(54) + zi[3] * AMs.col(55) + zi[4] * AMs.col(56); AA.col(20) = AMs.col(0) + zi[0] * AMs.col(57) + zi[1] * AMs.col(58) + zi[2] * AMs.col(59) + zi[3] * AMs.col(60) + zi[4] * AMs.col(61) + zi[5] * AMs.col(62) + zi[6] * AMs.col(63); AA(0, 0) = AA(0, 0) - zi[0]; AA(1, 1) = AA(1, 1) - zi[0]; AA(2, 2) = AA(2, 2) - zi[1]; AA(3, 3) = AA(3, 3) - zi[0]; AA(4, 4) = AA(4, 4) - zi[1]; AA(5, 5) = AA(5, 5) - zi[2]; AA(6, 6) = AA(6, 6) - zi[0]; AA(7, 7) = AA(7, 7) - zi[1]; AA(8, 8) = AA(8, 8) - zi[2]; AA(9, 9) = AA(9, 9) - zi[4]; AA(10, 10) = AA(10, 10) - zi[0]; AA(11, 11) = AA(11, 11) - zi[1]; AA(12, 12) = AA(12, 12) - zi[2]; AA(13, 13) = AA(13, 13) - zi[4]; AA(14, 14) = AA(14, 14) - zi[5]; AA(15, 15) = AA(15, 15) - zi[0]; AA(16, 16) = AA(16, 16) - zi[1]; AA(17, 17) = AA(17, 17) - zi[3]; AA(18, 18) = AA(18, 18) - zi[4]; AA(19, 19) = AA(19, 19) - zi[5]; AA(20, 20) = AA(20, 20) - zi[7]; Eigen::Matrix s = AA.leftCols(20).householderQr().solve(-AA.col(20)); sols(0, i) = s(14); sols(1, i) = s(19); sols(2, i) = zi[0]; } } #endif // Performs Newton iterations on the epipolar constraints void root_refinement(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, std::vector *output) { Eigen::Matrix J; Eigen::Matrix res; Eigen::Matrix dp; Eigen::Matrix sw; sw.setZero(); std::vector qq1(6, Eigen::Vector3d::Zero()), qq2(6, Eigen::Vector3d::Zero()); for (size_t pt_k = 0; pt_k < 6; ++pt_k) { qq1[pt_k] = x1[pt_k].cross(p1[pt_k]); qq2[pt_k] = x2[pt_k].cross(p2[pt_k]); } for (size_t pose_k = 0; pose_k < output->size(); ++pose_k) { CameraPose &pose = (*output)[pose_k]; for (size_t iter = 0; iter < 5; ++iter) { // compute residual and jacobian for (size_t pt_k = 0; pt_k < 6; ++pt_k) { Eigen::Vector3d x2t = x2[pt_k].cross(pose.t); Eigen::Vector3d Rx1 = pose.rotate(x1[pt_k]); Eigen::Vector3d Rqq1 = pose.rotate(qq1[pt_k]); res(pt_k) = (x2t - qq2[pt_k]).dot(Rx1) - x2[pt_k].dot(Rqq1); J.block<1, 3>(pt_k, 0) = -x2t.cross(Rx1) + qq2[pt_k].cross(Rx1) + x2[pt_k].cross(Rqq1); J.block<1, 3>(pt_k, 3) = -x2[pt_k].cross(Rx1); } if (res.norm() < 1e-12) { break; } dp = J.partialPivLu().solve(res); Eigen::Vector3d w = -dp.block<3, 1>(0, 0); pose.q = quat_step_pre(pose.q, w); pose.t = pose.t - dp.block<3, 1>(3, 0); } } } int gen_relpose_6pt(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, std::vector *output) { Eigen::Matrix M; setup_coeff_matrix(p1, x1, p2, x2, &M); double *coeffs = M.data(); Eigen::MatrixXd C0 = Eigen::MatrixXd::Zero(99, 99); Eigen::MatrixXd C1 = Eigen::MatrixXd::Zero(99, 64); for (int i = 0; i < 4655; i++) { C0(C0_ind[i]) = coeffs[coeffs0_ind[i]]; } for (int i = 0; i < 3661; i++) { C1(C1_ind[i]) = coeffs[coeffs1_ind[i]]; } Eigen::MatrixXd C12 = C0.partialPivLu().solve(C1); // Setup action matrix Eigen::Matrix AM; AM.setZero(); AM(0, 57) = 1.0; AM(1, 34) = 1.0; AM(2, 19) = 1.0; AM(3, 11) = 1.0; AM(4, 7) = 1.0; AM.row(5) = -C12.row(78); AM.row(6) = -C12.row(79); AM.row(7) = -C12.row(80); AM(8, 10) = 1.0; AM.row(9) = -C12.row(81); AM.row(10) = -C12.row(82); AM(11, 12) = 1.0; AM.row(12) = -C12.row(83); AM(13, 17) = 1.0; AM(14, 16) = 1.0; AM.row(15) = -C12.row(84); AM.row(16) = -C12.row(85); AM(17, 18) = 1.0; AM.row(18) = -C12.row(86); AM(19, 20) = 1.0; AM(20, 21) = 1.0; AM(21, 22) = 1.0; AM.row(22) = -C12.row(87); AM(23, 30) = 1.0; AM(24, 28) = 1.0; AM(25, 27) = 1.0; AM.row(26) = -C12.row(88); AM.row(27) = -C12.row(89); AM(28, 29) = 1.0; AM.row(29) = -C12.row(90); AM(30, 31) = 1.0; AM(31, 32) = 1.0; AM(32, 33) = 1.0; AM.row(33) = -C12.row(91); AM(34, 35) = 1.0; AM(35, 36) = 1.0; AM(36, 37) = 1.0; AM(37, 38) = 1.0; AM.row(38) = -C12.row(92); AM(39, 52) = 1.0; AM(40, 48) = 1.0; AM(41, 45) = 1.0; AM(42, 44) = 1.0; AM.row(43) = -C12.row(93); AM.row(44) = -C12.row(94); AM(45, 46) = 1.0; AM(46, 47) = 1.0; AM.row(47) = -C12.row(95); AM(48, 49) = 1.0; AM(49, 50) = 1.0; AM(50, 51) = 1.0; AM.row(51) = -C12.row(96); AM(52, 53) = 1.0; AM(53, 54) = 1.0; AM(54, 55) = 1.0; AM(55, 56) = 1.0; AM.row(56) = -C12.row(97); AM(57, 58) = 1.0; AM(58, 59) = 1.0; AM(59, 60) = 1.0; AM(60, 61) = 1.0; AM(61, 62) = 1.0; AM(62, 63) = 1.0; AM.row(63) = -C12.row(98); Eigen::Matrix sols; sols.setZero(); int n_roots = 0; #ifdef USE_FAST_EIGENVECTOR_SOLVER // Here we only compute eigenvalues and we use the structured backsubsitution to // solve for the eigenvectors Eigen::EigenSolver> es(AM, false); Eigen::Matrix, 64, 1> D = es.eigenvalues(); double eigv[64]; for (int i = 0; i < 64; i++) { if (std::abs(D(i).imag()) < 1e-6) eigv[n_roots++] = D(i).real(); } fast_eigenvector_solver(eigv, n_roots, AM, sols); #else // Solve eigenvalue problem Eigen::EigenSolver> es(AM); Eigen::ArrayXcd D = es.eigenvalues(); Eigen::ArrayXXcd V = es.eigenvectors(); // Extract solutions from eigenvectors for (size_t k = 0; k < 64; ++k) { if (std::abs(D(k).imag()) < 1e-6) { sols(0, n_roots) = V(1, k).real() / V(0, k).real(); sols(1, n_roots) = V(39, k).real() / V(0, k).real(); sols(2, n_roots) = D(k).real(); n_roots++; } } #endif output->clear(); output->reserve(n_roots); for (int sol_k = 0; sol_k < n_roots; ++sol_k) { CameraPose pose; // From each solution we compute the rotation and solve for the translation Eigen::Vector3d w = sols.col(sol_k); pose.q << 1.0, w(0), w(1), w(2); pose.q.normalize(); Eigen::Matrix3d R = quat_to_rotmat(pose.q); // Solve for the translation Eigen::Matrix3d A; Eigen::Vector3d b; A.setZero(); b.setZero(); for (size_t i = 0; i < 6; ++i) { Eigen::Vector3d u = (R * x1[i]).cross(x2[i]); Eigen::Vector3d v = p2[i] - R * p1[i]; A += u * u.transpose(); b += u * (u.dot(v)); } pose.t = A.llt().solve(b); // Filter solution using cheirality bool cheiral_ok = true; for (size_t pt_k = 0; pt_k < 6; ++pt_k) { if (!check_cheirality(pose, p1[pt_k], x1[pt_k], p2[pt_k], x2[pt_k])) { cheiral_ok = false; break; } } if (!cheiral_ok) { continue; } output->push_back(pose); } root_refinement(p1, x1, p2, x2, output); return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/gen_relpose_6pt.h000066400000000000000000000041521504452766400213700ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_GEN_RELPOSE_6PT_H #define POSELIB_GEN_RELPOSE_6PT_H #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for generalized relative pose from 6 correspondences // The solver is created using Larsson et al. CVPR 2017 int gen_relpose_6pt(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, std::vector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/gen_relpose_upright_4pt.cc000066400000000000000000000202541504452766400232670ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "gen_relpose_upright_4pt.h" #include "PoseLib/misc/qep.h" int poselib::gen_relpose_upright_4pt(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, CameraPoseVector *output) { Eigen::Matrix M, C, K; Eigen::Matrix VX; for (int i = 0; i < 4; ++i) VX.col(i) = x1[i].cross(p1[i]); M(0, 0) = -x1[0](1) * x2[0](2) - x1[0](2) * x2[0](1); M(0, 1) = x1[0](2) * x2[0](0) - x1[0](0) * x2[0](2); M(0, 2) = x1[0](0) * x2[0](1) + x1[0](1) * x2[0](0); M(0, 3) = VX(1, 0) * x2[0](1) - VX(0, 0) * x2[0](0) - VX(2, 0) * x2[0](2) + x1[0](0) * p2[0](1) * x2[0](2) - x1[0](0) * p2[0](2) * x2[0](1) + x1[0](1) * p2[0](0) * x2[0](2) - x1[0](1) * p2[0](2) * x2[0](0) + x1[0](2) * p2[0](0) * x2[0](1) - x1[0](2) * p2[0](1) * x2[0](0); M(1, 0) = -x1[1](1) * x2[1](2) - x1[1](2) * x2[1](1); M(1, 1) = x1[1](2) * x2[1](0) - x1[1](0) * x2[1](2); M(1, 2) = x1[1](0) * x2[1](1) + x1[1](1) * x2[1](0); M(1, 3) = VX(1, 1) * x2[1](1) - VX(0, 1) * x2[1](0) - VX(2, 1) * x2[1](2) + x1[1](0) * p2[1](1) * x2[1](2) - x1[1](0) * p2[1](2) * x2[1](1) + x1[1](1) * p2[1](0) * x2[1](2) - x1[1](1) * p2[1](2) * x2[1](0) + x1[1](2) * p2[1](0) * x2[1](1) - x1[1](2) * p2[1](1) * x2[1](0); M(2, 0) = -x1[2](1) * x2[2](2) - x1[2](2) * x2[2](1); M(2, 1) = x1[2](2) * x2[2](0) - x1[2](0) * x2[2](2); M(2, 2) = x1[2](0) * x2[2](1) + x1[2](1) * x2[2](0); M(2, 3) = VX(1, 2) * x2[2](1) - VX(0, 2) * x2[2](0) - VX(2, 2) * x2[2](2) + x1[2](0) * p2[2](1) * x2[2](2) - x1[2](0) * p2[2](2) * x2[2](1) + x1[2](1) * p2[2](0) * x2[2](2) - x1[2](1) * p2[2](2) * x2[2](0) + x1[2](2) * p2[2](0) * x2[2](1) - x1[2](2) * p2[2](1) * x2[2](0); M(3, 0) = -x1[3](1) * x2[3](2) - x1[3](2) * x2[3](1); M(3, 1) = x1[3](2) * x2[3](0) - x1[3](0) * x2[3](2); M(3, 2) = x1[3](0) * x2[3](1) + x1[3](1) * x2[3](0); M(3, 3) = VX(1, 3) * x2[3](1) - VX(0, 3) * x2[3](0) - VX(2, 3) * x2[3](2) + x1[3](0) * p2[3](1) * x2[3](2) - x1[3](0) * p2[3](2) * x2[3](1) + x1[3](1) * p2[3](0) * x2[3](2) - x1[3](1) * p2[3](2) * x2[3](0) + x1[3](2) * p2[3](0) * x2[3](1) - x1[3](2) * p2[3](1) * x2[3](0); C(0, 0) = -2 * x1[0](0) * x2[0](1); C(0, 1) = 2 * x1[0](0) * x2[0](0) + 2 * x1[0](2) * x2[0](2); C(0, 2) = -2 * x1[0](2) * x2[0](1); C(0, 3) = 2 * VX(2, 0) * x2[0](0) - 2 * VX(0, 0) * x2[0](2) + 2 * x1[0](0) * p2[0](0) * x2[0](1) - 2 * x1[0](0) * p2[0](1) * x2[0](0) - 2 * x1[0](2) * p2[0](1) * x2[0](2) + 2 * x1[0](2) * p2[0](2) * x2[0](1); C(1, 0) = -2 * x1[1](0) * x2[1](1); C(1, 1) = 2 * x1[1](0) * x2[1](0) + 2 * x1[1](2) * x2[1](2); C(1, 2) = -2 * x1[1](2) * x2[1](1); C(1, 3) = 2 * VX(2, 1) * x2[1](0) - 2 * VX(0, 1) * x2[1](2) + 2 * x1[1](0) * p2[1](0) * x2[1](1) - 2 * x1[1](0) * p2[1](1) * x2[1](0) - 2 * x1[1](2) * p2[1](1) * x2[1](2) + 2 * x1[1](2) * p2[1](2) * x2[1](1); C(2, 0) = -2 * x1[2](0) * x2[2](1); C(2, 1) = 2 * x1[2](0) * x2[2](0) + 2 * x1[2](2) * x2[2](2); C(2, 2) = -2 * x1[2](2) * x2[2](1); C(2, 3) = 2 * VX(2, 2) * x2[2](0) - 2 * VX(0, 2) * x2[2](2) + 2 * x1[2](0) * p2[2](0) * x2[2](1) - 2 * x1[2](0) * p2[2](1) * x2[2](0) - 2 * x1[2](2) * p2[2](1) * x2[2](2) + 2 * x1[2](2) * p2[2](2) * x2[2](1); C(3, 0) = -2 * x1[3](0) * x2[3](1); C(3, 1) = 2 * x1[3](0) * x2[3](0) + 2 * x1[3](2) * x2[3](2); C(3, 2) = -2 * x1[3](2) * x2[3](1); C(3, 3) = 2 * VX(2, 3) * x2[3](0) - 2 * VX(0, 3) * x2[3](2) + 2 * x1[3](0) * p2[3](0) * x2[3](1) - 2 * x1[3](0) * p2[3](1) * x2[3](0) - 2 * x1[3](2) * p2[3](1) * x2[3](2) + 2 * x1[3](2) * p2[3](2) * x2[3](1); K(0, 0) = x1[0](2) * x2[0](1) - x1[0](1) * x2[0](2); K(0, 1) = x1[0](0) * x2[0](2) - x1[0](2) * x2[0](0); K(0, 2) = x1[0](1) * x2[0](0) - x1[0](0) * x2[0](1); K(0, 3) = VX(0, 0) * x2[0](0) + VX(1, 0) * x2[0](1) + VX(2, 0) * x2[0](2) - x1[0](0) * p2[0](1) * x2[0](2) + x1[0](0) * p2[0](2) * x2[0](1) + x1[0](1) * p2[0](0) * x2[0](2) - x1[0](1) * p2[0](2) * x2[0](0) - x1[0](2) * p2[0](0) * x2[0](1) + x1[0](2) * p2[0](1) * x2[0](0); K(1, 0) = x1[1](2) * x2[1](1) - x1[1](1) * x2[1](2); K(1, 1) = x1[1](0) * x2[1](2) - x1[1](2) * x2[1](0); K(1, 2) = x1[1](1) * x2[1](0) - x1[1](0) * x2[1](1); K(1, 3) = VX(0, 1) * x2[1](0) + VX(1, 1) * x2[1](1) + VX(2, 1) * x2[1](2) - x1[1](0) * p2[1](1) * x2[1](2) + x1[1](0) * p2[1](2) * x2[1](1) + x1[1](1) * p2[1](0) * x2[1](2) - x1[1](1) * p2[1](2) * x2[1](0) - x1[1](2) * p2[1](0) * x2[1](1) + x1[1](2) * p2[1](1) * x2[1](0); K(2, 0) = x1[2](2) * x2[2](1) - x1[2](1) * x2[2](2); K(2, 1) = x1[2](0) * x2[2](2) - x1[2](2) * x2[2](0); K(2, 2) = x1[2](1) * x2[2](0) - x1[2](0) * x2[2](1); K(2, 3) = VX(0, 2) * x2[2](0) + VX(1, 2) * x2[2](1) + VX(2, 2) * x2[2](2) - x1[2](0) * p2[2](1) * x2[2](2) + x1[2](0) * p2[2](2) * x2[2](1) + x1[2](1) * p2[2](0) * x2[2](2) - x1[2](1) * p2[2](2) * x2[2](0) - x1[2](2) * p2[2](0) * x2[2](1) + x1[2](2) * p2[2](1) * x2[2](0); K(3, 0) = x1[3](2) * x2[3](1) - x1[3](1) * x2[3](2); K(3, 1) = x1[3](0) * x2[3](2) - x1[3](2) * x2[3](0); K(3, 2) = x1[3](1) * x2[3](0) - x1[3](0) * x2[3](1); K(3, 3) = VX(0, 3) * x2[3](0) + VX(1, 3) * x2[3](1) + VX(2, 3) * x2[3](2) - x1[3](0) * p2[3](1) * x2[3](2) + x1[3](0) * p2[3](2) * x2[3](1) + x1[3](1) * p2[3](0) * x2[3](2) - x1[3](1) * p2[3](2) * x2[3](0) - x1[3](2) * p2[3](0) * x2[3](1) + x1[3](2) * p2[3](1) * x2[3](0); /* Eigen::Matrix eig_vecs; double eig_vals[8]; const int n_roots = qep::qep_sturm(M, C, K, eig_vals, &eig_vecs); */ // We know that (1+q^2) is a factor. Dividing by this gives degree 6 poly. Eigen::Matrix eig_vecs; double eig_vals[6]; const int n_roots = qep::qep_sturm_div_1_q2(M, C, K, eig_vals, &eig_vecs); output->clear(); for (int i = 0; i < n_roots; ++i) { poselib::CameraPose pose; const double q = eig_vals[i]; const double q2 = q * q; const double inv_norm = 1.0 / (1 + q2); const double cq = (1 - q2) * inv_norm; const double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; output->emplace_back(R, eig_vecs.col(i)); } return n_roots; } PoseLib-2.0.5/PoseLib/solvers/gen_relpose_upright_4pt.h000066400000000000000000000044031504452766400231270ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_GEN_RELPOSE_UPRIGHT_4PT_H #define POSELIB_GEN_RELPOSE_UPRIGHT_4PT_H #include "PoseLib/camera_pose.h" #include namespace poselib { // Upright generalized relative pose from four point correspondences, i.e. // R * (p1 + lambda1 * x1) + t = p2 + lambda2 * x2 // Sweeney et al., Solving for Relative Pose with a Partially Known Rotation is a Quadratic Eigenvalue Problem, 3DV // 2014 int gen_relpose_upright_4pt(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/gp3p.cc000066400000000000000000000060231504452766400173030ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "gp3p.h" #include "PoseLib/misc/re3q3.h" namespace poselib { // Solves for camera pose such that: p+lambda*x = R*X+t int gp3p(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output) { Eigen::Matrix A; for (int i = 0; i < 3; ++i) { // xx = [x3 0 -x1; 0 x3 -x2] // eqs = [xx kron(X',xx), -xx*p] * [t; vec(R); 1] A.row(2 * i) << x[i](2), 0.0, -x[i](0), X[i](0) * x[i](2), 0.0, -X[i](0) * x[i](0), X[i](1) * x[i](2), 0.0, -X[i](1) * x[i](0), X[i](2) * x[i](2), 0.0, -X[i](2) * x[i](0), -p[i](0) * x[i](2) + p[i](2) * x[i](0); A.row(2 * i + 1) << 0.0, x[i](2), -x[i](1), 0.0, X[i](0) * x[i](2), -X[i](0) * x[i](1), 0.0, X[i](1) * x[i](2), -X[i](1) * x[i](1), 0.0, X[i](2) * x[i](2), -X[i](2) * x[i](1), -p[i](1) * x[i](2) + p[i](2) * x[i](1); } Eigen::Matrix3d B = A.block<3, 3>(0, 0).inverse(); Eigen::Matrix AR = A.block<3, 10>(3, 3) - A.block<3, 3>(3, 0) * B * A.block<3, 10>(0, 3); Eigen::Matrix solutions; int n_sols = re3q3::re3q3_rotation(AR, &solutions); output->clear(); for (int i = 0; i < n_sols; ++i) { CameraPose pose; pose.q = solutions.col(i); pose.t = -B * (A.block<3, 9>(0, 3) * quat_to_rotmatvec(pose.q) + A.block<3, 1>(0, 12)); output->push_back(pose); } return n_sols; } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/gp3p.h000066400000000000000000000041351504452766400171470ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_GP3P_H_ #define POSELIB_GP3P_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: p+lambda*x = R*X+t // Re-implementation of the gP3P solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 int gp3p(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/gp4ps.cc000066400000000000000000000212701504452766400174700ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "gp4ps.h" #include "PoseLib/misc/re3q3.h" #include "PoseLib/misc/univariate.h" namespace poselib { // Solves for camera pose such that: p+lambda*x = R*X+t // Note: This function assumes that the bearing vectors (x) are normalized! int gp4ps(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_scales, bool filter_solutions) { for (int i = 0; i < 4; ++i) { for (int j = i + 1; j < 4; ++j) { if ((X[i] - X[j]).squaredNorm() < 1e-10) { // we have a duplicated 3d point std::vector pp = p; std::vector xp = x; std::vector Xp = X; std::swap(pp[0], pp[i]); std::swap(xp[0], xp[i]); std::swap(Xp[0], Xp[i]); std::swap(pp[1], pp[j]); std::swap(xp[1], xp[j]); std::swap(Xp[1], Xp[j]); return gp4ps_camposeco(pp, xp, Xp, output, output_scales); } } } return gp4ps_kukelova(p, x, X, output, output_scales, filter_solutions); } // Solves for camera pose such that: scale*p+lambda*x = R*X+t int gp4ps_kukelova(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_scales, bool filter_solutions) { Eigen::Matrix A; for (int i = 0; i < 4; ++i) { // xx = [x3 0 -x1; 0 x3 -x2] // eqs = [xx kron(X',xx), -xx*p] * [t; scale; vec(R)] A.row(2 * i) << x[i](2), 0.0, -x[i](0), -p[i](0) * x[i](2) + p[i](2) * x[i](0), X[i](0) * x[i](2), 0.0, -X[i](0) * x[i](0), X[i](1) * x[i](2), 0.0, -X[i](1) * x[i](0), X[i](2) * x[i](2), 0.0, -X[i](2) * x[i](0); A.row(2 * i + 1) << 0.0, x[i](2), -x[i](1), -p[i](1) * x[i](2) + p[i](2) * x[i](1), 0.0, X[i](0) * x[i](2), -X[i](0) * x[i](1), 0.0, X[i](1) * x[i](2), -X[i](1) * x[i](1), 0.0, X[i](2) * x[i](2), -X[i](2) * x[i](1); } Eigen::Matrix4d B = A.block<4, 4>(0, 0).inverse(); Eigen::Matrix AR = A.block<3, 9>(4, 4) - A.block<3, 4>(4, 0) * B * A.block<4, 9>(0, 4); Eigen::Matrix solutions; int n_sols = re3q3::re3q3_rotation(AR, &solutions); Eigen::Vector4d ts; output->clear(); output_scales->clear(); CameraPose best_pose; double best_scale = 1.0; double best_res = 0.0; for (int i = 0; i < n_sols; ++i) { CameraPose pose; pose.q = solutions.col(i); ts = -B * (A.block<4, 9>(0, 4) * quat_to_rotmatvec(pose.q)); pose.t = ts.block<3, 1>(0, 0); double scale = ts(3); if (filter_solutions) { double res = std::abs(x[3].dot((pose.R() * X[3] + pose.t - scale * p[3]).normalized())); if (res > best_res) { best_pose = pose; best_scale = scale; best_res = res; } } else { output->push_back(pose); output_scales->push_back(scale); } } if (filter_solutions && best_res > 0.0) { output->push_back(best_pose); output_scales->push_back(best_scale); } return output->size(); } // Solves for camera pose such that: scale*p+lambda*x = R*X+t // Assumes that X[0] == X[1] ! int gp4ps_camposeco(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_scales) { // Locally triangulate the 3D point const double a = x[0].dot(x[1]); const double b1 = x[0].dot(p[1] - p[0]); const double b2 = x[1].dot(p[1] - p[0]); const double lambda = (a * b2 - b1) / (a * a - 1); const Eigen::Vector3d Xc = p[0] + lambda * x[0]; // Shift rig coordinate system by Xc Eigen::Vector3d q0 = p[2] - Xc; Eigen::Vector3d q1 = p[3] - Xc; // Ensure q is orthogonal to x q0 -= q0.dot(x[2]) * x[2]; q1 -= q1.dot(x[3]) * x[3]; const double D21 = (X[2] - X[0]).squaredNorm(); const double D31 = (X[3] - X[0]).squaredNorm(); const double D23 = (X[3] - X[2]).squaredNorm(); const double inv1 = 1.0 / D31; const double k1 = -inv1 * D21; const double k2 = inv1 * (D31 * (q0(0) * q0(0) + q0(1) * q0(1) + q0(2) * q0(2)) - D21 * (q1(0) * q1(0) + q1(1) * q1(1) + q1(2) * q1(2))); const double inv2 = 1.0 / (D21 * (x[2](0) * x[2](0) + x[2](1) * x[2](1) + x[2](2) * x[2](2)) - D23 * (x[2](0) * x[2](0) + x[2](1) * x[2](1) + x[2](2) * x[2](2))); const double k3 = inv2 * (-D21 * (2 * x[2](0) * x[3](0) + 2 * x[2](1) * x[3](1) + 2 * x[2](2) * x[3](2))); const double k4 = inv2 * (D21 * (x[3](0) * x[3](0) + x[3](1) * x[3](1) + x[3](2) * x[3](2))); const double k5 = inv2 * (D21 * (2 * x[2](0) * (q0(0) - q1(0)) + 2 * x[2](1) * (q0(1) - q1(1)) + 2 * x[2](2) * (q0(2) - q1(2))) - D23 * (2 * q0(0) * x[2](0) + 2 * q0(1) * x[2](1) + 2 * q0(2) * x[2](2))); const double k6 = inv2 * (-D21 * (2 * x[3](0) * (q0(0) - q1(0)) + 2 * x[3](1) * (q0(1) - q1(1)) + 2 * x[3](2) * (q0(2) - q1(2)))); const double k7 = inv2 * (D21 * ((q0(0) - q1(0)) * (q0(0) - q1(0)) + (q0(1) - q1(1)) * (q0(1) - q1(1)) + (q0(2) - q1(2)) * (q0(2) - q1(2))) - D23 * (q0(0) * q0(0) + q0(1) * q0(1) + q0(2) * q0(2))); // Quartic in lambda3 const double inv_c4 = 1.0 / (k1 * k1 + k3 * k3 * k1 - 2 * k4 * k1 + k4 * k4); const double c3 = inv_c4 * 2.0 * (k1 * k3 * k5 - k1 * k6 + k4 * k6); const double c2 = inv_c4 * (k2 * k3 * k3 + k1 * k5 * k5 + k6 * k6 + 2.0 * k1 * k2 - 2.0 * k2 * k4 - 2.0 * k1 * k7 + 2.0 * k4 * k7); const double c1 = inv_c4 * (2.0 * k2 * k3 * k5 - 2.0 * k2 * k6 + 2.0 * k6 * k7); const double c0 = inv_c4 * (k2 * k2 + k2 * k5 * k5 + k7 * k7 - 2.0 * k2 * k7); double roots[4]; const int n_sols = univariate::solve_quartic_real(c3, c2, c1, c0, roots); Eigen::Matrix3d YY; YY.col(0) = X[2] - X[0]; YY.col(1) = X[3] - X[0]; YY.col(2) = YY.col(0).cross(YY.col(1)); const double sY = YY.col(0).norm(); YY = YY.inverse().eval(); Eigen::Matrix3d XX; output->clear(); output_scales->clear(); for (int i = 0; i < n_sols; ++i) { const double lambda3 = roots[i]; const double lambda2 = (k2 - k7 + (k1 - k4) * lambda3 * lambda3 - k6 * lambda3) / (k3 * lambda3 + k5); XX.col(0) = q0 + lambda2 * x[2]; XX.col(1) = q1 + lambda3 * x[3]; CameraPose pose; double scale = sY / (XX.col(0)).norm(); XX.col(0) *= scale; XX.col(1) *= scale; XX.col(2) = XX.col(0).cross(XX.col(1)); pose.q = rotmat_to_quat(XX * YY); pose.t = scale * Xc - pose.R() * X[0]; output->push_back(pose); output_scales->push_back(scale); } return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/gp4ps.h000066400000000000000000000067631504452766400173440ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_GP4PS_H_ #define POSELIB_GP4PS_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solver the generalized absolute pose and scale problem. // The solver automagically identifies the quasi-degenerate case where two 3D points coincides, // and then either calls gp4ps_kukelova or gp4ps_camposeco. // If you know that you never have duplicate observations (e.g. non-overlapping FoV) you can directly call // gp4ps_kukelova int gp4ps(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_scales, bool filter_solutions = true); // Solves for camera pose such that: scale*p+lambda*x = R*X+t // Re-implementation of the gP4P solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 // Note: this impl. assumes that x has been normalized and that the 3D points are distinct! int gp4ps_kukelova(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_scales, bool filter_solutions = true); // Solves for camera pose such that: scale*p+lambda*x = R*X+t // Re-implementation of the gP4P solver from // Camposeco et al., Minimal solvers for generalized pose and scale estimation from two rays and one point, ECCV 2016 // Note: This solver assumes that the first two points correspond to the same 3D point! // This is a minimal problem and it is not possible to filter solutions! int gp4ps_camposeco(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_scales); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/homography_4pt.cc000066400000000000000000000127251504452766400214040ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // // S. Cai, Z. Wu, L. Guo, J. Wang, S. Zhang, J. Yan, S. Shen, Fast and interpretable 2d homography decomposition: // Similarity-kernel-similarity and affine-core-affine transformations. PAMI 2025. #include "homography_4pt.h" namespace poselib { int homography_4pt(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *H, bool check_cheirality) { if (check_cheirality) { Eigen::Vector3d p = x1[0].cross(x1[1]); Eigen::Vector3d q = x2[0].cross(x2[1]); if (p.dot(x1[2]) * q.dot(x2[2]) < 0) return 0; if (p.dot(x1[3]) * q.dot(x2[3]) < 0) return 0; p = x1[2].cross(x1[3]); q = x2[2].cross(x2[3]); if (p.dot(x1[0]) * q.dot(x2[0]) < 0) return 0; if (p.dot(x1[1]) * q.dot(x2[1]) < 0) return 0; } std::array xa; std::array xb; for (int i = 0; i < 4; i++) { xa[i] = x1[i] / x1[i].z(); xb[i] = x2[i] / x2[i].z(); } double M1N1_x = xa[1].x() - xa[0].x(), M1P1_x = xa[2].x() - xa[0].x(), M1Q1_x = xa[3].x() - xa[0].x(); double M1N1_y = xa[1].y() - xa[0].y(), M1P1_y = xa[2].y() - xa[0].y(), M1Q1_y = xa[3].y() - xa[0].y(); double fA1 = M1N1_x * M1P1_y - M1N1_y * M1P1_x; double Q3_x = M1P1_y * M1Q1_x - M1P1_x * M1Q1_y; double Q3_y = M1N1_x * M1Q1_y - M1N1_y * M1Q1_x; // [ M1P1_y -M1P1_x ] [1 -src[0] ] // H_A1 = [ -M1N1_y M1N1_x ] * [ 1 -src[1] ] // [ f_A1 ] [ 1 ] // compute H_A2 and other variables on target plane double M2N2_x = xb[1].x() - xb[0].x(), M2P2_x = xb[2].x() - xb[0].x(), M2Q2_x = xb[3].x() - xb[0].x(); double M2N2_y = xb[1].y() - xb[0].y(), M2P2_y = xb[2].y() - xb[0].y(), M2Q2_y = xb[3].y() - xb[0].y(); double fA2 = M2N2_x * M2P2_y - M2N2_y * M2P2_x; double Q4_x = M2P2_y * M2Q2_x - M2P2_x * M2Q2_y; double Q4_y = M2N2_x * M2Q2_y - M2N2_y * M2Q2_x; // [ M2N2_x M2P2_x tar[0] ] // H_A2_inv = [ M2N2_y M2P2_y tar[1] ] // [ 1 ] // obtain the core transformation H_C double tt1 = fA1 - Q3_x - Q3_y; double C11 = Q3_y * Q4_x * tt1; double C22 = Q3_x * Q4_y * tt1; double C33 = Q3_x * Q3_y * (fA2 - Q4_x - Q4_y); double C31 = C11 - C33; double C32 = C22 - C33; // [ C11 0 0 ] // H_C = [ 0 C22 0 ] // [ C11-C33 C22-C33 C33 ] // obtain some intermediate variables double tt3 = xb[0].x() * C33; double tt4 = xb[0].y() * C33; double H1_11 = xb[1].x() * C11 - tt3; double H1_12 = xb[2].x() * C22 - tt3; double H1_21 = xb[1].y() * C11 - tt4; double H1_22 = xb[2].y() * C22 - tt4; // H1 = H_A2_inv * H_C // only compute the 2*2 upper-left elements. C_33*M2 are repeated twice. no need to compute the last row // [ C11*tar[2]-C33*tar[0] C22*tar[4]-C33*tar[0] C33*tar[0] ] // H1 = [ C11*tar[3]-C33*tar[1] C22*tar[5]-C33*tar[1] C33*tar[1] ] // [ C31 C32 C33 ] Eigen::Matrix h; // obtain H h[0] = H1_11 * M1P1_y - H1_12 * M1N1_y; h[1] = H1_12 * M1N1_x - H1_11 * M1P1_x; h[3] = H1_21 * M1P1_y - H1_22 * M1N1_y; h[4] = H1_22 * M1N1_x - H1_21 * M1P1_x; h[6] = C31 * M1P1_y - C32 * M1N1_y; h[7] = C32 * M1N1_x - C31 * M1P1_x; h[2] = tt3 * fA1 - h[0] * xa[0].x() - h[1] * xa[0].y(); h[5] = tt4 * fA1 - h[3] * xa[0].x() - h[4] * xa[0].y(); h[8] = C33 * fA1 - h[6] * xa[0].x() - h[7] * xa[0].y(); *H = Eigen::Map(h.data()).transpose(); // Check for degenerate homography H->normalize(); double det = H->determinant(); if (std::abs(det) < 1e-8) { return 0; } return 1; } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/homography_4pt.h000066400000000000000000000036771504452766400212540ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_HOMOGRAPHY_4PT_H_ #define POSELIB_HOMOGRAPHY_4PT_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { // Homography estimation from 4 points (x2 = H*x1) int homography_4pt(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *H, bool check_cheirality = true); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p1p2ll.cc000066400000000000000000000065051504452766400175510ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p1p2ll.h" #include "PoseLib/misc/re3q3.h" namespace poselib { int p1p2ll(const std::vector &xp, const std::vector &Xp, const std::vector &l, const std::vector &X, const std::vector &V, std::vector *output) { // We center coordinate system on Xp // Point-point equation then yield: t = lambda*xp // Inserting into l'*(R*X + t) = l'*(R(X-Xp) + lambda*xp) // From the two equations we get // lambda * l1'*xp + l1'*R*(X1-Xp) = 0 // lambda * l2'*xp + l2'*R*(X2-Xp) = 0 // Eliminating lambda // [(l1'*xp) * kron(X2-Xp,l2') - (l2'*xp) * kron(X1-Xp,l1')] * R(:) = 0 double l1xp = l[0].dot(xp[0]); double l2xp = l[1].dot(xp[0]); Eigen::Matrix B; Eigen::Vector3d z1 = l2xp * (X[0] - Xp[0]); Eigen::Vector3d z2 = l1xp * (X[1] - Xp[0]); // Two equations from l'*R*V = 0 and finally the equation from above B << V[0](0) * l[0].transpose(), V[0](1) * l[0].transpose(), V[0](2) * l[0].transpose(), V[1](0) * l[1].transpose(), V[1](1) * l[1].transpose(), V[1](2) * l[1].transpose(), z1(0) * l[0].transpose() - z2(0) * l[1].transpose(), z1(1) * l[0].transpose() - z2(1) * l[1].transpose(), z1(2) * l[0].transpose() - z2(2) * l[1].transpose(); Eigen::Matrix solutions; int n_sols = re3q3::re3q3_rotation(B, &solutions); output->clear(); for (int i = 0; i < n_sols; ++i) { CameraPose pose; pose.q = solutions.col(i); Eigen::Matrix3d R = pose.R(); double lambda = -l[0].dot(R * (X[0] - Xp[0])) / l1xp; pose.t = lambda * xp[0] - R * Xp[0]; output->push_back(pose); } return n_sols; } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/p1p2ll.h000066400000000000000000000043211504452766400174050ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P1P2LL_H_ #define POSELIB_P1P2LL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: l'*(R*(X+mu*V)+t) = 0 and lambda*xp = R*Xp + t // Relies on the E3Q3 solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 int p1p2ll(const std::vector &xp, const std::vector &Xp, const std::vector &l, const std::vector &X, const std::vector &V, std::vector *output); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p2p1ll.cc000066400000000000000000000065161504452766400175530ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p2p1ll.h" #include "PoseLib/misc/re3q3.h" namespace poselib { int p2p1ll(const std::vector &xp, const std::vector &Xp, const std::vector &l, const std::vector &X, const std::vector &V, std::vector *output) { // By some calculation we get that // x2 ~ [(l'*x1)*kron(Xp2'-Xp1',I_3) - x1 * kron(X-Xp1,l')] * R(:) // From this we can extract two constraints on the rotation + the constraint l'*R*V = 0 Eigen::Vector3d dX21 = Xp[1] - Xp[0]; Eigen::Vector3d dX01 = X[0] - Xp[0]; double lxp1 = l[0].dot(xp[0]); dX21 *= lxp1; Eigen::Matrix B; Eigen::Matrix b; b << -dX01(0) * l[0].transpose(), -dX01(1) * l[0].transpose(), -dX01(2) * l[0].transpose(); B.row(0) = xp[0](0) * b; B.row(1) = xp[0](1) * b; B.row(2) = xp[0](2) * b; B(0, 0) += dX21(0); B(1, 1) += dX21(0); B(2, 2) += dX21(0); B(0, 3) += dX21(1); B(1, 4) += dX21(1); B(2, 5) += dX21(1); B(0, 6) += dX21(2); B(1, 7) += dX21(2); B(2, 8) += dX21(2); B.row(0) = xp[1](2) * B.row(0) - xp[1](0) * B.row(2); B.row(1) = xp[1](2) * B.row(1) - xp[1](1) * B.row(2); B.row(2) << V[0](0) * l[0].transpose(), V[0](1) * l[0].transpose(), V[0](2) * l[0].transpose(); Eigen::Matrix solutions; int n_sols = re3q3::re3q3_rotation(B, &solutions); output->clear(); for (int i = 0; i < n_sols; ++i) { CameraPose pose; pose.q = solutions.col(i); Eigen::Matrix3d R = pose.R(); double lambda = -l[0].dot(R * (X[0] - Xp[0])) / lxp1; pose.t = lambda * xp[0] - R * Xp[0]; output->push_back(pose); } return n_sols; } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/p2p1ll.h000066400000000000000000000043211504452766400174050ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P2P1LL_H_ #define POSELIB_P2P1LL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: l'*(R*(X+mu*V)+t) = 0 and lambda*xp = R*Xp + t // Relies on the E3Q3 solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 int p2p1ll(const std::vector &xp, const std::vector &Xp, const std::vector &l, const std::vector &X, const std::vector &V, std::vector *output); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p2p2pl.cc000066400000000000000000000404611504452766400175550ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p2p2pl.h" void p2p2l_fast_eigenvector_solver(double *eigv, int neig, Eigen::Matrix &AM, Eigen::Matrix &sols) { static const int ind[] = {4, 5, 7, 10, 15}; // Truncated action matrix containing non-trivial rows Eigen::Matrix AMs; double zi[6]; for (int i = 0; i < 5; i++) { AMs.row(i) = AM.row(ind[i]); } for (int i = 0; i < neig; i++) { zi[0] = eigv[i]; for (int j = 1; j < 6; j++) { zi[j] = zi[j - 1] * eigv[i]; } Eigen::Matrix AA; AA.col(0) = AMs.col(4); AA.col(1) = AMs.col(3) + zi[0] * AMs.col(5); AA.col(2) = AMs.col(2) + zi[0] * AMs.col(6) + zi[1] * AMs.col(7); AA.col(3) = AMs.col(1) + zi[0] * AMs.col(8) + zi[1] * AMs.col(9) + zi[2] * AMs.col(10); AA.col(4) = AMs.col(0) + zi[0] * AMs.col(11) + zi[1] * AMs.col(12) + zi[2] * AMs.col(13) + zi[3] * AMs.col(14) + zi[4] * AMs.col(15); AA(0, 0) = AA(0, 0) - zi[0]; AA(1, 1) = AA(1, 1) - zi[1]; AA(2, 2) = AA(2, 2) - zi[2]; AA(3, 3) = AA(3, 3) - zi[3]; AA(4, 4) = AA(4, 4) - zi[5]; Eigen::Matrix s = AA.leftCols(4).colPivHouseholderQr().solve(-AA.col(4)); sols(0, i) = s(3); sols(1, i) = zi[0]; } } int poselib::p2p2pl(const std::vector &xp0, const std::vector &Xp0, const std::vector &x0, const std::vector &X0, const std::vector &V0, CameraPoseVector *output) { // Change world coordinate system Eigen::Vector3d t0 = Xp0[0]; Eigen::Matrix X; X << X0[0] - t0, X0[1] - t0; Eigen::Matrix Xp; Xp << Xp0[0] - t0, Xp0[1] - t0; Eigen::Matrix V; V << V0[0], V0[1]; double s0 = Xp.col(1).norm(); Xp /= s0; X /= s0; Eigen::Matrix3d R0 = Eigen::Quaternion::FromTwoVectors(Xp.col(1), Eigen::Vector3d(1.0, 0.0, 0.0)).toRotationMatrix(); Xp = R0 * Xp; X = R0 * X; V = R0 * V; // Change image coordinate system Eigen::Matrix x; x << x0[0].normalized(), x0[1].normalized(); Eigen::Matrix xp; xp << xp0[0].normalized(), xp0[1].normalized(); Eigen::Matrix3d R1 = Eigen::Quaternion::FromTwoVectors(xp.col(0), Eigen::Vector3d(0.0, 0.0, 1.0)).toRotationMatrix(); xp = R1 * xp; x = R1 * x; Eigen::Matrix3d R2; R2.setIdentity(); Eigen::Vector2d a = xp.block<2, 1>(0, 1).normalized(); R2(0, 0) = a(0); R2(0, 1) = a(1); R2(1, 0) = -a(1); R2(1, 1) = a(0); xp = R2 * xp; x = R2 * x; double u = xp(2, 1) / xp(0, 1); double coeffs[30]; coeffs[0] = V(1, 0) * X(0, 0) * x(2, 0) - V(0, 0) * X(2, 0) * x(1, 0) - V(0, 0) * X(1, 0) * x(2, 0) + V(1, 0) * X(2, 0) * x(0, 0) + V(2, 0) * X(0, 0) * x(1, 0) - V(2, 0) * X(1, 0) * x(0, 0) + V(0, 0) * u * x(1, 0) - V(1, 0) * u * x(0, 0); coeffs[1] = 2 * V(1, 0) * x(0, 0) - 2 * V(0, 0) * x(1, 0) + 2 * V(0, 0) * X(1, 0) * x(0, 0) - 2 * V(1, 0) * X(0, 0) * x(0, 0) + 2 * V(1, 0) * X(2, 0) * x(2, 0) - 2 * V(2, 0) * X(1, 0) * x(2, 0) + 2 * V(2, 0) * u * x(1, 0); coeffs[2] = 2 * V(0, 0) * X(1, 0) * x(1, 0) - 2 * V(1, 0) * X(0, 0) * x(1, 0) - 2 * V(0, 0) * X(2, 0) * x(2, 0) + 2 * V(2, 0) * X(0, 0) * x(2, 0) - 2 * V(2, 0) * u * x(0, 0); coeffs[3] = 2 * V(2, 0) * X(0, 0) * x(1, 0) - 2 * V(0, 0) * X(2, 0) * x(1, 0) - 4 * V(2, 0) * x(1, 0) - 2 * V(0, 0) * u * x(1, 0); coeffs[4] = 4 * V(2, 0) * x(0, 0) + 4 * V(0, 0) * X(2, 0) * x(0, 0) - 4 * V(2, 0) * X(0, 0) * x(0, 0) - 4 * V(1, 0) * u * x(1, 0); coeffs[5] = V(0, 0) * X(1, 0) * x(2, 0) + V(0, 0) * X(2, 0) * x(1, 0) - V(1, 0) * X(0, 0) * x(2, 0) + V(1, 0) * X(2, 0) * x(0, 0) - V(2, 0) * X(0, 0) * x(1, 0) - V(2, 0) * X(1, 0) * x(0, 0) + V(0, 0) * u * x(1, 0) + V(1, 0) * u * x(0, 0); coeffs[6] = 2 * V(0, 0) * x(1, 0) + 2 * V(1, 0) * x(0, 0) + 2 * V(0, 0) * X(1, 0) * x(0, 0) - 2 * V(1, 0) * X(0, 0) * x(0, 0) + 2 * V(1, 0) * X(2, 0) * x(2, 0) - 2 * V(2, 0) * X(1, 0) * x(2, 0) - 2 * V(2, 0) * u * x(1, 0); coeffs[7] = 8 * V(1, 0) * x(1, 0) + 4 * V(0, 0) * X(1, 0) * x(1, 0) - 4 * V(1, 0) * X(0, 0) * x(1, 0); coeffs[8] = 2 * V(1, 0) * X(0, 0) * x(0, 0) - 2 * V(1, 0) * x(0, 0) - 2 * V(0, 0) * X(1, 0) * x(0, 0) - 2 * V(0, 0) * x(1, 0) + 2 * V(1, 0) * X(2, 0) * x(2, 0) - 2 * V(2, 0) * X(1, 0) * x(2, 0) - 2 * V(2, 0) * u * x(1, 0); coeffs[9] = V(0, 0) * X(1, 0) * x(2, 0) - V(0, 0) * X(2, 0) * x(1, 0) - V(1, 0) * X(0, 0) * x(2, 0) - V(1, 0) * X(2, 0) * x(0, 0) + V(2, 0) * X(0, 0) * x(1, 0) + V(2, 0) * X(1, 0) * x(0, 0) + V(0, 0) * u * x(1, 0) + V(1, 0) * u * x(0, 0); coeffs[10] = 4 * V(2, 0) * x(0, 0) + 4 * V(0, 0) * X(2, 0) * x(0, 0) - 4 * V(2, 0) * X(0, 0) * x(0, 0) + 4 * V(1, 0) * u * x(1, 0); coeffs[11] = 4 * V(2, 0) * x(1, 0) + 2 * V(0, 0) * X(2, 0) * x(1, 0) - 2 * V(2, 0) * X(0, 0) * x(1, 0) - 2 * V(0, 0) * u * x(1, 0); coeffs[12] = 2 * V(0, 0) * X(1, 0) * x(1, 0) - 2 * V(1, 0) * X(0, 0) * x(1, 0) + 2 * V(0, 0) * X(2, 0) * x(2, 0) - 2 * V(2, 0) * X(0, 0) * x(2, 0) + 2 * V(2, 0) * u * x(0, 0); coeffs[13] = 2 * V(0, 0) * x(1, 0) - 2 * V(1, 0) * x(0, 0) - 2 * V(0, 0) * X(1, 0) * x(0, 0) + 2 * V(1, 0) * X(0, 0) * x(0, 0) + 2 * V(1, 0) * X(2, 0) * x(2, 0) - 2 * V(2, 0) * X(1, 0) * x(2, 0) + 2 * V(2, 0) * u * x(1, 0); coeffs[14] = V(0, 0) * X(2, 0) * x(1, 0) - V(0, 0) * X(1, 0) * x(2, 0) + V(1, 0) * X(0, 0) * x(2, 0) - V(1, 0) * X(2, 0) * x(0, 0) - V(2, 0) * X(0, 0) * x(1, 0) + V(2, 0) * X(1, 0) * x(0, 0) + V(0, 0) * u * x(1, 0) - V(1, 0) * u * x(0, 0); coeffs[15] = V(1, 1) * X(0, 1) * x(2, 1) - V(0, 1) * X(2, 1) * x(1, 1) - V(0, 1) * X(1, 1) * x(2, 1) + V(1, 1) * X(2, 1) * x(0, 1) + V(2, 1) * X(0, 1) * x(1, 1) - V(2, 1) * X(1, 1) * x(0, 1) + V(0, 1) * u * x(1, 1) - V(1, 1) * u * x(0, 1); coeffs[16] = 2 * V(1, 1) * x(0, 1) - 2 * V(0, 1) * x(1, 1) + 2 * V(0, 1) * X(1, 1) * x(0, 1) - 2 * V(1, 1) * X(0, 1) * x(0, 1) + 2 * V(1, 1) * X(2, 1) * x(2, 1) - 2 * V(2, 1) * X(1, 1) * x(2, 1) + 2 * V(2, 1) * u * x(1, 1); coeffs[17] = 2 * V(0, 1) * X(1, 1) * x(1, 1) - 2 * V(1, 1) * X(0, 1) * x(1, 1) - 2 * V(0, 1) * X(2, 1) * x(2, 1) + 2 * V(2, 1) * X(0, 1) * x(2, 1) - 2 * V(2, 1) * u * x(0, 1); coeffs[18] = 2 * V(2, 1) * X(0, 1) * x(1, 1) - 2 * V(0, 1) * X(2, 1) * x(1, 1) - 4 * V(2, 1) * x(1, 1) - 2 * V(0, 1) * u * x(1, 1); coeffs[19] = 4 * V(2, 1) * x(0, 1) + 4 * V(0, 1) * X(2, 1) * x(0, 1) - 4 * V(2, 1) * X(0, 1) * x(0, 1) - 4 * V(1, 1) * u * x(1, 1); coeffs[20] = V(0, 1) * X(1, 1) * x(2, 1) + V(0, 1) * X(2, 1) * x(1, 1) - V(1, 1) * X(0, 1) * x(2, 1) + V(1, 1) * X(2, 1) * x(0, 1) - V(2, 1) * X(0, 1) * x(1, 1) - V(2, 1) * X(1, 1) * x(0, 1) + V(0, 1) * u * x(1, 1) + V(1, 1) * u * x(0, 1); coeffs[21] = 2 * V(0, 1) * x(1, 1) + 2 * V(1, 1) * x(0, 1) + 2 * V(0, 1) * X(1, 1) * x(0, 1) - 2 * V(1, 1) * X(0, 1) * x(0, 1) + 2 * V(1, 1) * X(2, 1) * x(2, 1) - 2 * V(2, 1) * X(1, 1) * x(2, 1) - 2 * V(2, 1) * u * x(1, 1); coeffs[22] = 8 * V(1, 1) * x(1, 1) + 4 * V(0, 1) * X(1, 1) * x(1, 1) - 4 * V(1, 1) * X(0, 1) * x(1, 1); coeffs[23] = 2 * V(1, 1) * X(0, 1) * x(0, 1) - 2 * V(1, 1) * x(0, 1) - 2 * V(0, 1) * X(1, 1) * x(0, 1) - 2 * V(0, 1) * x(1, 1) + 2 * V(1, 1) * X(2, 1) * x(2, 1) - 2 * V(2, 1) * X(1, 1) * x(2, 1) - 2 * V(2, 1) * u * x(1, 1); coeffs[24] = V(0, 1) * X(1, 1) * x(2, 1) - V(0, 1) * X(2, 1) * x(1, 1) - V(1, 1) * X(0, 1) * x(2, 1) - V(1, 1) * X(2, 1) * x(0, 1) + V(2, 1) * X(0, 1) * x(1, 1) + V(2, 1) * X(1, 1) * x(0, 1) + V(0, 1) * u * x(1, 1) + V(1, 1) * u * x(0, 1); coeffs[25] = 4 * V(2, 1) * x(0, 1) + 4 * V(0, 1) * X(2, 1) * x(0, 1) - 4 * V(2, 1) * X(0, 1) * x(0, 1) + 4 * V(1, 1) * u * x(1, 1); coeffs[26] = 4 * V(2, 1) * x(1, 1) + 2 * V(0, 1) * X(2, 1) * x(1, 1) - 2 * V(2, 1) * X(0, 1) * x(1, 1) - 2 * V(0, 1) * u * x(1, 1); coeffs[27] = 2 * V(0, 1) * X(1, 1) * x(1, 1) - 2 * V(1, 1) * X(0, 1) * x(1, 1) + 2 * V(0, 1) * X(2, 1) * x(2, 1) - 2 * V(2, 1) * X(0, 1) * x(2, 1) + 2 * V(2, 1) * u * x(0, 1); coeffs[28] = 2 * V(0, 1) * x(1, 1) - 2 * V(1, 1) * x(0, 1) - 2 * V(0, 1) * X(1, 1) * x(0, 1) + 2 * V(1, 1) * X(0, 1) * x(0, 1) + 2 * V(1, 1) * X(2, 1) * x(2, 1) - 2 * V(2, 1) * X(1, 1) * x(2, 1) + 2 * V(2, 1) * u * x(1, 1); coeffs[29] = V(0, 1) * X(2, 1) * x(1, 1) - V(0, 1) * X(1, 1) * x(2, 1) + V(1, 1) * X(0, 1) * x(2, 1) - V(1, 1) * X(2, 1) * x(0, 1) - V(2, 1) * X(0, 1) * x(1, 1) + V(2, 1) * X(1, 1) * x(0, 1) + V(0, 1) * u * x(1, 1) - V(1, 1) * u * x(0, 1); // Setup elimination template static const int coeffs0_ind[] = { 0, 15, 1, 0, 15, 16, 2, 0, 15, 17, 3, 1, 0, 15, 16, 18, 4, 2, 1, 0, 16, 15, 17, 19, 5, 2, 0, 15, 17, 20, 6, 3, 1, 0, 15, 16, 18, 21, 7, 4, 3, 2, 1, 0, 18, 17, 15, 16, 19, 22, 8, 5, 4, 2, 1, 0, 16, 15, 19, 17, 20, 23, 5, 2, 17, 20, 9, 6, 3, 1, 16, 18, 21, 24, 10, 7, 6, 4, 3, 2, 1, 17, 0, 21, 19, 15, 16, 18, 22, 25, 11, 8, 7, 5, 4, 3, 2, 1, 18, 15, 16, 22, 20, 17, 19, 23, 0, 26, 8, 5, 4, 2, 19, 17, 23, 20, 5, 20, 9, 6, 3, 18, 21, 24, 12, 10, 9, 7, 6, 4, 3, 19, 1, 24, 22, 16, 18, 21, 25, 27, 13, 11, 10, 8, 7, 6, 5, 4, 3, 21, 20, 2, 16, 18, 25, 23, 17, 19, 22, 26, 1, 0, 15, 28, 11, 8, 7, 5, 4, 22, 17, 19, 26, 20, 23, 2, 9, 6, 21, 24, 12, 10, 9, 7, 6, 22, 3, 25, 18, 21, 24, 27, 14, 13, 12, 11, 10, 9, 8, 7, 6, 24, 23, 4, 18, 21, 27, 26, 19, 22, 25, 28, 3, 1, 16, 29, 13, 11, 10, 8, 7, 25, 5, 19, 22, 28, 20, 23, 26, 4, 2, 17, 8, 5, 23, 20}; static const int coeffs1_ind[] = { 14, 29, 14, 29, 12, 27, 14, 29, 12, 27, 9, 24, 12, 27, 9, 24, 9, 24, 12, 10, 9, 25, 6, 27, 21, 24, 14, 13, 12, 28, 10, 24, 29, 25, 27, 9, 6, 21, 14, 13, 12, 11, 10, 9, 26, 7, 21, 24, 28, 22, 25, 27, 29, 6, 3, 18, 14, 13, 27, 28, 29, 12, 10, 25, 14, 13, 12, 11, 25, 27, 26, 28, 29, 10, 7, 22, 14, 13, 12, 11, 10, 27, 8, 22, 25, 29, 23, 26, 28, 7, 4, 19, 29, 14, 13, 28, 14, 28, 29, 13, 11, 26, 14, 13, 29, 26, 28, 11, 8, 23, 13, 11, 28, 23, 26, 8, 5, 20, 11, 8, 26, 20, 23, 5}; static const int C0_ind[] = { 0, 23, 24, 25, 43, 47, 48, 50, 62, 71, 72, 73, 75, 87, 91, 95, 96, 97, 98, 100, 110, 114, 115, 119, 120, 122, 125, 129, 134, 143, 144, 145, 147, 150, 154, 159, 163, 167, 168, 169, 170, 171, 172, 175, 182, 183, 185, 186, 187, 191, 192, 193, 194, 196, 197, 200, 201, 205, 206, 210, 211, 215, 218, 221, 225, 230, 240, 241, 243, 246, 250, 255, 259, 263, 264, 265, 266, 267, 268, 270, 271, 274, 275, 278, 279, 280, 281, 282, 283, 287, 288, 289, 290, 291, 292, 293, 295, 296, 297, 300, 301, 302, 303, 305, 306, 307, 308, 311, 314, 316, 317, 320, 321, 325, 326, 330, 341, 345, 361, 363, 366, 370, 375, 379, 384, 385, 386, 387, 388, 390, 391, 394, 395, 398, 399, 400, 401, 402, 403, 407, 408, 409, 410, 411, 412, 413, 414, 415, 416, 417, 418, 419, 420, 421, 422, 423, 424, 425, 426, 427, 428, 429, 430, 431, 434, 436, 437, 439, 440, 441, 444, 445, 446, 449, 450, 452, 459, 462, 466, 471, 481, 483, 484, 486, 487, 490, 491, 495, 496, 497, 498, 499, 504, 505, 506, 507, 508, 509, 510, 511, 512, 513, 514, 515, 516, 517, 518, 519, 520, 521, 522, 523, 524, 525, 526, 527, 530, 532, 533, 535, 536, 537, 539, 540, 541, 542, 544, 545, 546, 548, 549, 550, 557, 560, 561, 565}; static const int C1_ind[] = { 21, 22, 35, 40, 45, 46, 54, 58, 59, 64, 69, 70, 78, 82, 83, 88, 102, 106, 123, 126, 127, 130, 131, 135, 136, 137, 147, 150, 151, 154, 155, 156, 159, 160, 161, 164, 165, 166, 169, 171, 172, 174, 175, 176, 178, 179, 180, 181, 183, 184, 185, 186, 187, 188, 189, 190, 199, 203, 204, 208, 209, 212, 213, 214, 220, 223, 224, 227, 228, 229, 232, 233, 234, 236, 237, 238, 242, 244, 245, 247, 248, 249, 251, 252, 253, 254, 256, 257, 258, 260, 261, 262, 276, 284, 285, 286, 296, 300, 301, 308, 309, 310, 317, 320, 321, 324, 325, 332, 333, 334, 341, 344, 345, 348, 349, 356, 357, 358, 365, 368, 369, 372, 373, 380}; Eigen::Matrix C0; C0.setZero(); Eigen::Matrix C1; C1.setZero(); for (int i = 0; i < 236; i++) { C0(C0_ind[i]) = coeffs[coeffs0_ind[i]]; } for (int i = 0; i < 124; i++) { C1(C1_ind[i]) = coeffs[coeffs1_ind[i]]; } Eigen::Matrix C12 = C0.partialPivLu().solve(C1); // Setup action matrix Eigen::Matrix AM; AM.setZero(); AM(0, 11) = 1.0; AM(1, 8) = 1.0; AM(2, 6) = 1.0; AM(3, 5) = 1.0; AM.row(4) = -C12.row(19); AM.row(5) = -C12.row(20); AM(6, 7) = 1.0; AM.row(7) = -C12.row(21); AM(8, 9) = 1.0; AM(9, 10) = 1.0; AM.row(10) = -C12.row(22); AM(11, 12) = 1.0; AM(12, 13) = 1.0; AM(13, 14) = 1.0; AM(14, 15) = 1.0; AM.row(15) = -C12.row(23); // Solve for eigenvalues Eigen::EigenSolver> es(AM, false); Eigen::Array, 16, 1> D = es.eigenvalues(); int nroots = 0; double eigv[16]; for (int i = 0; i < 16; i++) { if (std::abs(D(i).imag()) < 1e-6) eigv[nroots++] = D(i).real(); } // Solve for the eigenvectors (exploiting their structure) Eigen::Matrix sols; p2p2l_fast_eigenvector_solver(eigv, nroots, AM, sols); output->clear(); for (int i = 0; i < nroots; ++i) { double a = 1.0; double b = sols(0, i); double c = sols(1, i); double d = -b * c; Eigen::Quaternion q(a, b, c, d); CameraPose pose; pose.q << a, b, c, d; pose.t << 0.0, 0.0, u * (1.0 + b * b - c * c - d * d) - 2 * b * d + 2 * a * c; pose.t(2) /= pose.q.squaredNorm(); pose.q.normalize(); // Revert change of variable pose.q = rotmat_to_quat(R1.transpose() * R2.transpose() * pose.R() * R0); pose.t = R1.transpose() * R2.transpose() * pose.t; pose.t = pose.t * s0 - pose.R() * t0; output->push_back(pose); } return nroots; } PoseLib-2.0.5/PoseLib/solvers/p2p2pl.h000066400000000000000000000043741504452766400174220ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P2P2PL_H_ #define POSELIB_P2P2PL_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { // Absolute pose from two point-point and two point-line constraints. // lambda * xp = R*Xp + t and lambda * x = R*(X + mu*V) + t // This solver is based on the formulation from the paper // Josephson et al., Image-Based Localization Using Hybrid Feature Correspondences, CVPR 2007 int p2p2pl(const std::vector &xp, const std::vector &Xp, const std::vector &x, const std::vector &X, const std::vector &V, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p3ll.cc000066400000000000000000000056741504452766400173170ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p3ll.h" #include "PoseLib/misc/re3q3.h" namespace poselib { int p3ll(const std::vector &l, const std::vector &X, const std::vector &V, std::vector *output) { Eigen::Matrix3d A; Eigen::Matrix B1, B2; // l'*R*V = 0 A << l[0].transpose(), l[1].transpose(), l[2].transpose(); B1 << V[0](0) * l[0].transpose(), V[0](1) * l[0].transpose(), V[0](2) * l[0].transpose(), V[1](0) * l[1].transpose(), V[1](1) * l[1].transpose(), V[1](2) * l[1].transpose(), V[2](0) * l[2].transpose(), V[2](1) * l[2].transpose(), V[2](2) * l[2].transpose(); // l'*(R*X+t) = 0 B2 << X[0](0) * l[0].transpose(), X[0](1) * l[0].transpose(), X[0](2) * l[0].transpose(), X[1](0) * l[1].transpose(), X[1](1) * l[1].transpose(), X[1](2) * l[1].transpose(), X[2](0) * l[2].transpose(), X[2](1) * l[2].transpose(), X[2](2) * l[2].transpose(); // B1*R(:) = 0 // A*t + B2*R(:) = 0; // t + B1*R(:) = 0 B2 = A.inverse() * B2; Eigen::Matrix solutions; int n_sols = re3q3::re3q3_rotation(B1, &solutions); output->clear(); for (int i = 0; i < n_sols; ++i) { CameraPose pose; pose.q = solutions.col(i); pose.t = -B2 * quat_to_rotmatvec(pose.q); output->push_back(pose); } return n_sols; } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/p3ll.h000066400000000000000000000041251504452766400171470ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P3LL_H_ #define POSELIB_P3LL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: l'*(R*(X+mu*V)+t) = 0 // Relies on the E3Q3 solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 int p3ll(const std::vector &l, const std::vector &X, const std::vector &V, std::vector *output); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p3p.cc000066400000000000000000000156631504452766400171460ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Author: Yaqing Ding // Mark Shachkov #include "p3p.h" #include "PoseLib/misc/univariate.h" #include "p3p_common.h" namespace poselib { int p3p(const std::vector &x_copy, const std::vector &X_copy, std::vector *output) { if (output == nullptr) { return 0; } output->clear(); output->reserve(4); Eigen::Vector3d X01 = X_copy[0] - X_copy[1]; Eigen::Vector3d X02 = X_copy[0] - X_copy[2]; Eigen::Vector3d X12 = X_copy[1] - X_copy[2]; double a01 = X01.squaredNorm(); double a02 = X02.squaredNorm(); double a12 = X12.squaredNorm(); std::array X = {X_copy[0], X_copy[1], X_copy[2]}; std::array x = {x_copy[0], x_copy[1], x_copy[2]}; // Switch X,x so that BC is the largest distance among {X01, X02, X12} if (a01 > a02) { if (a01 > a12) { std::swap(x[0], x[2]); std::swap(X[0], X[2]); std::swap(a01, a12); X01 = -X12; X02 = -X02; } } else if (a02 > a12) { std::swap(x[0], x[1]); std::swap(X[0], X[1]); std::swap(a02, a12); X01 = -X01; X02 = X12; } const double a12d = 1.0 / a12; const double a = a01 * a12d; const double b = a02 * a12d; const double m01 = x[0].dot(x[1]); const double m02 = x[0].dot(x[2]); const double m12 = x[1].dot(x[2]); // Ugly parameters to simplify the calculation const double m12sq = -m12 * m12 + 1.0; const double m02sq = -1.0 + m02 * m02; const double m01sq = -1.0 + m01 * m01; const double ab = a * b; const double bsq = b * b; const double asq = a * a; const double m013 = -2.0 + 2.0 * m01 * m02 * m12; const double bsqm12sq = bsq * m12sq; const double asqm12sq = asq * m12sq; const double abm12sq = 2.0 * ab * m12sq; const double k3_inv = 1.0 / (bsqm12sq + b * m02sq); const double k2 = k3_inv * ((-1.0 + a) * m02sq + abm12sq + bsqm12sq + b * m013); const double k1 = k3_inv * (asqm12sq + abm12sq + a * m013 + (-1.0 + b) * m01sq); const double k0 = k3_inv * (asqm12sq + a * m01sq); double s; bool G = univariate::solve_cubic_single_real(k2, k1, k0, s); Eigen::Matrix3d C; C(0, 0) = -a + s * (1 - b); C(0, 1) = -m02 * s; C(0, 2) = a * m12 + b * m12 * s; C(1, 0) = C(0, 1); C(1, 1) = s + 1; C(1, 2) = -m01; C(2, 0) = C(0, 2); C(2, 1) = C(1, 2); C(2, 2) = -a - b * s + 1; std::array pq = compute_pq(C); double d0, d1, d2; CameraPose pose; output->clear(); Eigen::Matrix3d XX; XX << X01, X02, X01.cross(X02); XX = XX.inverse().eval(); Eigen::Vector3d v1, v2; Eigen::Matrix3d YY; int n_sols = 0; for (int i = 0; i < 2; ++i) { // [p0 p1 p2] * [1; x; y] = 0, or [p0 p1 p2] * [d2; d0; d1] = 0 double p0 = pq[i](0); double p1 = pq[i](1); double p2 = pq[i](2); // here we run into trouble if p0 is zero, // so depending on which is larger, we solve for either d0 or d1 // The case p0 = p1 = 0 is degenerate and can be ignored bool switch_12 = std::abs(p0) <= std::abs(p1); if (switch_12) { // eliminate d0 double w0 = -p0 / p1; double w1 = -p2 / p1; double ca = 1.0 / (w1 * w1 - b); double cb = 2.0 * (b * m12 - m02 * w1 + w0 * w1) * ca; double cc = (w0 * w0 - 2 * m02 * w0 - b + 1.0) * ca; double taus[2]; if (!root2real(cb, cc, taus[0], taus[1])) continue; for (double tau : taus) { if (tau <= 0) continue; // positive only d2 = std::sqrt(a12 / (tau * (tau - 2.0 * m12) + 1.0)); d1 = tau * d2; d0 = (w0 * d2 + w1 * d1); if (d0 < 0) continue; refine_lambda(d0, d1, d2, a01, a02, a12, m01, m02, m12); v1 = d0 * x[0] - d1 * x[1]; v2 = d0 * x[0] - d2 * x[2]; YY << v1, v2, v1.cross(v2); Eigen::Matrix3d R = YY * XX; output->emplace_back(R, d0 * x[0] - R * X[0]); ++n_sols; } } else { double w0 = -p1 / p0; double w1 = -p2 / p0; double ca = 1.0 / (-a * w1 * w1 + 2 * a * m12 * w1 - a + 1); double cb = 2 * (a * m12 * w0 - m01 - a * w0 * w1) * ca; double cc = (1 - a * w0 * w0) * ca; double taus[2]; if (!root2real(cb, cc, taus[0], taus[1])) continue; for (double tau : taus) { if (tau <= 0) continue; d0 = std::sqrt(a01 / (tau * (tau - 2.0 * m01) + 1.0)); d1 = tau * d0; d2 = w0 * d0 + w1 * d1; if (d2 < 0) continue; refine_lambda(d0, d1, d2, a01, a02, a12, m01, m02, m12); v1 = d0 * x[0] - d1 * x[1]; v2 = d0 * x[0] - d2 * x[2]; YY << v1, v2, v1.cross(v2); Eigen::Matrix3d R = YY * XX; output->emplace_back(R, d0 * x[0] - R * X[0]); ++n_sols; } } if (n_sols > 0 && G) break; } return output->size(); } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/p3p.h000066400000000000000000000041521504452766400167770ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P3P_H_ #define POSELIB_P3P_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: lambda*x = R*X+t with positive lambda. // Re-implementation of the P3P solver from // Y. Ding, J. Yang, V. Larsson, C. Olsson, K. Ã…ström, Revisiting the P3P Problem, CVPR 2023 // Note: this impl. assumes that x has been normalized. int p3p(const std::vector &x, const std::vector &X, std::vector *output); } // namespace poselib #endif PoseLib-2.0.5/PoseLib/solvers/p3p_common.h000066400000000000000000000063361504452766400203550ustar00rootroot00000000000000#ifndef POSELIB_P3P_COMMON_H #define POSELIB_P3P_COMMON_H #include namespace poselib { bool inline root2real(double b, double c, double &r1, double &r2) { double THRESHOLD = -1.0e-12; double v = b * b - 4.0 * c; if (v < THRESHOLD) { r1 = r2 = -0.5 * b; return v >= 0; } if (v > THRESHOLD && v < 0.0) { r1 = -0.5 * b; r2 = -2; return true; } double y = std::sqrt(v); if (b < 0) { r1 = 0.5 * (-b + y); r2 = 0.5 * (-b - y); } else { r1 = 2.0 * c / (-b + y); r2 = 2.0 * c / (-b - y); } return true; } inline std::array compute_pq(Eigen::Matrix3d C) { std::array pq; Eigen::Matrix3d C_adj; C_adj(0, 0) = C(1, 2) * C(2, 1) - C(1, 1) * C(2, 2); C_adj(1, 1) = C(0, 2) * C(2, 0) - C(0, 0) * C(2, 2); C_adj(2, 2) = C(0, 1) * C(1, 0) - C(0, 0) * C(1, 1); C_adj(0, 1) = C(0, 1) * C(2, 2) - C(0, 2) * C(2, 1); C_adj(0, 2) = C(0, 2) * C(1, 1) - C(0, 1) * C(1, 2); C_adj(1, 0) = C_adj(0, 1); C_adj(1, 2) = C(0, 0) * C(1, 2) - C(0, 2) * C(1, 0); C_adj(2, 0) = C_adj(0, 2); C_adj(2, 1) = C_adj(1, 2); Eigen::Vector3d v; if (C_adj(0, 0) > C_adj(1, 1)) { if (C_adj(0, 0) > C_adj(2, 2)) { v = C_adj.col(0) / std::sqrt(C_adj(0, 0)); } else { v = C_adj.col(2) / std::sqrt(C_adj(2, 2)); } } else if (C_adj(1, 1) > C_adj(2, 2)) { v = C_adj.col(1) / std::sqrt(C_adj(1, 1)); } else { v = C_adj.col(2) / std::sqrt(C_adj(2, 2)); } C(0, 1) -= v(2); C(0, 2) += v(1); C(1, 2) -= v(0); C(1, 0) += v(2); C(2, 0) -= v(1); C(2, 1) += v(0); pq[0] = C.col(0); pq[1] = C.row(0); return pq; } // Performs a few newton steps on the equations inline void refine_lambda(double &lambda1, double &lambda2, double &lambda3, const double a12, const double a13, const double a23, const double b12, const double b13, const double b23) { for (int iter = 0; iter < 5; ++iter) { double r1 = (lambda1 * lambda1 - 2.0 * lambda1 * lambda2 * b12 + lambda2 * lambda2 - a12); double r2 = (lambda1 * lambda1 - 2.0 * lambda1 * lambda3 * b13 + lambda3 * lambda3 - a13); double r3 = (lambda2 * lambda2 - 2.0 * lambda2 * lambda3 * b23 + lambda3 * lambda3 - a23); if (std::abs(r1) + std::abs(r2) + std::abs(r3) < 1e-10) return; double x11 = lambda1 - lambda2 * b12; double x12 = lambda2 - lambda1 * b12; double x21 = lambda1 - lambda3 * b13; double x23 = lambda3 - lambda1 * b13; double x32 = lambda2 - lambda3 * b23; double x33 = lambda3 - lambda2 * b23; double detJ = 0.5 / (x11 * x23 * x32 + x12 * x21 * x33); // half minus inverse determinant // This uses the closed form of the inverse for the jacobean. // Due to the zero elements this actually becomes quite nice. lambda1 += (-x23 * x32 * r1 - x12 * x33 * r2 + x12 * x23 * r3) * detJ; lambda2 += (-x21 * x33 * r1 + x11 * x33 * r2 - x11 * x23 * r3) * detJ; lambda3 += (x21 * x32 * r1 - x11 * x32 * r2 - x12 * x21 * r3) * detJ; } } } // namespace poselib #endif // POSELIB_P3P_COMMON_H PoseLib-2.0.5/PoseLib/solvers/p3p_lambdatwist.cc000066400000000000000000000227061504452766400215350ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p3p_lambdatwist.h" #include "p3p_common.h" namespace poselib { // Computes the eigen decomposition of a 3x3 matrix given that one eigenvalue is zero. void compute_eig3x3known0(const Eigen::Matrix3d &M, Eigen::Matrix3d &E, double &sig1, double &sig2) { // In the original paper there is a missing minus sign here (for M(0,0)) double p1 = -M(0, 0) - M(1, 1) - M(2, 2); double p0 = -M(0, 1) * M(0, 1) - M(0, 2) * M(0, 2) - M(1, 2) * M(1, 2) + M(0, 0) * (M(1, 1) + M(2, 2)) + M(1, 1) * M(2, 2); double disc = std::sqrt(p1 * p1 / 4.0 - p0); double tmp = -p1 / 2.0; sig1 = tmp + disc; sig2 = tmp - disc; if (std::abs(sig1) < std::abs(sig2)) std::swap(sig1, sig2); double c = sig1 * sig1 + M(0, 0) * M(1, 1) - sig1 * (M(0, 0) + M(1, 1)) - M(0, 1) * M(0, 1); double a1 = (sig1 * M(0, 2) + M(0, 1) * M(1, 2) - M(0, 2) * M(1, 1)) / c; double a2 = (sig1 * M(1, 2) + M(0, 1) * M(0, 2) - M(0, 0) * M(1, 2)) / c; double n = 1.0 / std::sqrt(1 + a1 * a1 + a2 * a2); E.col(0) << a1 * n, a2 * n, n; c = sig2 * sig2 + M(0, 0) * M(1, 1) - sig2 * (M(0, 0) + M(1, 1)) - M(0, 1) * M(0, 1); a1 = (sig2 * M(0, 2) + M(0, 1) * M(1, 2) - M(0, 2) * M(1, 1)) / c; a2 = (sig2 * M(1, 2) + M(0, 1) * M(0, 2) - M(0, 0) * M(1, 2)) / c; n = 1.0 / std::sqrt(1 + a1 * a1 + a2 * a2); E.col(1) << a1 * n, a2 * n, n; // This is never used so we don't compute it // E.col(2) = M.col(1).cross(M.col(2)).normalized(); } // Solves for camera pose such that: lambda*x = R*X+t with positive lambda. int p3p_lambdatwist(const std::vector &x, const std::vector &X, std::vector *output) { Eigen::Vector3d dX12 = X[0] - X[1]; Eigen::Vector3d dX13 = X[0] - X[2]; Eigen::Vector3d dX23 = X[1] - X[2]; double a12 = dX12.squaredNorm(); double b12 = x[0].dot(x[1]); double a13 = dX13.squaredNorm(); double b13 = x[0].dot(x[2]); double a23 = dX23.squaredNorm(); double b23 = x[1].dot(x[2]); double a23b12 = a23 * b12; double a12b23 = a12 * b23; double a23b13 = a23 * b13; double a13b23 = a13 * b23; Eigen::Matrix3d D1, D2; D1 << a23, -a23b12, 0.0, -a23b12, a23 - a12, a12b23, 0.0, a12b23, -a12; D2 << a23, 0.0, -a23b13, 0.0, -a13, a13b23, -a23b13, a13b23, a23 - a13; Eigen::Matrix3d DX1, DX2; DX1 << D1.col(1).cross(D1.col(2)), D1.col(2).cross(D1.col(0)), D1.col(0).cross(D1.col(1)); DX2 << D2.col(1).cross(D2.col(2)), D2.col(2).cross(D2.col(0)), D2.col(0).cross(D2.col(1)); // Coefficients of p(gamma) = det(D1 + gamma*D2) // In the original paper c2 and c1 are switched. double c3 = D2.col(0).dot(DX2.col(0)); double c2 = (D1.array() * DX2.array()).sum(); double c1 = (D2.array() * DX1.array()).sum(); double c0 = D1.col(0).dot(DX1.col(0)); // closed root solver for cubic root const double c3inv = 1.0 / c3; c2 *= c3inv; c1 *= c3inv; c0 *= c3inv; double a = c1 - c2 * c2 / 3.0; double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0; double c = b * b / 4.0 + a * a * a / 27.0; double gamma; if (c > 0) { c = std::sqrt(c); b *= -0.5; gamma = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0; } else { c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a); gamma = 2.0 * std::sqrt(-a / 3.0) * std::cos(std::acos(c) / 3.0) - c2 / 3.0; } // We do a single newton step on the cubic equation double f = gamma * gamma * gamma + c2 * gamma * gamma + c1 * gamma + c0; double df = 3.0 * gamma * gamma + 2.0 * c2 * gamma + c1; gamma = gamma - f / df; Eigen::Matrix3d D0 = D1 + gamma * D2; Eigen::Matrix3d E; double sig1, sig2; compute_eig3x3known0(D0, E, sig1, sig2); double s = std::sqrt(-sig2 / sig1); double lambda1, lambda2, lambda3; CameraPose pose; output->clear(); Eigen::Matrix3d XX; XX << dX12, dX13, dX12.cross(dX13); XX = XX.inverse().eval(); Eigen::Vector3d v1, v2; Eigen::Matrix3d YY; const double TOL_DOUBLE_ROOT = 1e-12; for (int s_flip = 0; s_flip < 2; ++s_flip, s = -s) { // [u1 u2 u3] * [lambda1; lambda2; lambda3] = 0 double u1 = E(0, 0) - s * E(0, 1); double u2 = E(1, 0) - s * E(1, 1); double u3 = E(2, 0) - s * E(2, 1); // here we run into trouble if u1 is zero, // so depending on which is larger, we solve for either lambda1 or lambda2 // The case u1 = u2 = 0 is degenerate and can be ignored bool switch_12 = std::abs(u1) < std::abs(u2); double a, b, c, w0, w1; if (switch_12) { // solve for lambda2 w0 = -u1 / u2; w1 = -u3 / u2; a = -a13 * w1 * w1 + 2 * a13b23 * w1 - a13 + a23; b = 2 * a13b23 * w0 - 2 * a23b13 - 2 * a13 * w0 * w1; c = -a13 * w0 * w0 + a23; double b2m4ac = b * b - 4.0 * a * c; // if b2m4ac is zero we have a double root // to handle this case we allow slightly negative discriminants here if (b2m4ac < -TOL_DOUBLE_ROOT) continue; // clip to zero here in case we have double root double sq = std::sqrt(std::max(0.0, b2m4ac)); // first root of tau double tau = (b > 0) ? (2.0 * c) / (-b - sq) : (2.0 * c) / (-b + sq); for (int tau_flip = 0; tau_flip < 2; ++tau_flip, tau = c / (a * tau)) { if (tau > 0) { lambda1 = std::sqrt(a13 / (tau * (tau - 2.0 * b13) + 1.0)); lambda3 = tau * lambda1; lambda2 = w0 * lambda1 + w1 * lambda3; // since tau > 0 and lambda1 > 0 we only need to check lambda2 here if (lambda2 < 0) continue; refine_lambda(lambda1, lambda2, lambda3, a12, a13, a23, b12, b13, b23); v1 = lambda1 * x[0] - lambda2 * x[1]; v2 = lambda1 * x[0] - lambda3 * x[2]; YY << v1, v2, v1.cross(v2); Eigen::Matrix3d R = YY * XX; output->emplace_back(R, lambda1 * x[0] - R * X[0]); } if (b2m4ac < TOL_DOUBLE_ROOT) { // double root we can skip the second tau break; } } } else { // Same as except we solve for lambda1 as a combination of lambda2 and lambda3 // (default case in the paper) w0 = -u2 / u1; w1 = -u3 / u1; a = (a13 - a12) * w1 * w1 + 2.0 * a12 * b13 * w1 - a12; b = -2.0 * a13 * b12 * w1 + 2.0 * a12 * b13 * w0 - 2.0 * w0 * w1 * (a12 - a13); c = (a13 - a12) * w0 * w0 - 2.0 * a13 * b12 * w0 + a13; double b2m4ac = b * b - 4.0 * a * c; if (b2m4ac < -TOL_DOUBLE_ROOT) continue; double sq = std::sqrt(std::max(0.0, b2m4ac)); double tau = (b > 0) ? (2.0 * c) / (-b - sq) : (2.0 * c) / (-b + sq); for (int tau_flip = 0; tau_flip < 2; ++tau_flip, tau = c / (a * tau)) { if (tau > 0) { lambda2 = std::sqrt(a23 / (tau * (tau - 2.0 * b23) + 1.0)); lambda3 = tau * lambda2; lambda1 = w0 * lambda2 + w1 * lambda3; if (lambda1 < 0) continue; refine_lambda(lambda1, lambda2, lambda3, a12, a13, a23, b12, b13, b23); v1 = lambda1 * x[0] - lambda2 * x[1]; v2 = lambda1 * x[0] - lambda3 * x[2]; YY << v1, v2, v1.cross(v2); Eigen::Matrix3d R = YY * XX; output->emplace_back(R, lambda1 * x[0] - R * X[0]); } if (b2m4ac < TOL_DOUBLE_ROOT) { break; } } } } return output->size(); } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/p3p_lambdatwist.h000066400000000000000000000043011504452766400213660ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P3P_LAMBDATWIST_H_ #define POSELIB_P3P_LAMBDATWIST_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: lambda*x = R*X+t with positive lambda. // Re-implementation of the Lambdatwist P3P solver from // M. Persson, K. Nordberg, Lambda Twist: An Accurate Fast Robust Perspective Three Point (P3P) Solver, ECCV 2018 // Note: this impl. assumes that x has been normalized. int p3p_lambdatwist(const std::vector &x, const std::vector &X, std::vector *output); } // namespace poselib #endif PoseLib-2.0.5/PoseLib/solvers/p4pf.cc000066400000000000000000000246461504452766400173160ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p4pf.h" #include "PoseLib/misc/re3q3.h" namespace poselib { int p4pf(const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_focal, bool filter_solutions) { std::vector poses; std::vector fx; std::vector fy; int n = p4pf(x, X, &poses, &fx, &fy, filter_solutions); if (filter_solutions) { int best_ind = -1; double best_err = 1.0; for (int i = 0; i < n; ++i) { double a = fx[i] / fy[i]; double err = std::max(std::abs(a - 1.0), std::abs(1 / a - 1.0)); if (err < best_err) { best_err = err; best_ind = i; } } if (best_err < 1.0 && best_ind > -1) { double focal = (fx[best_ind] + fy[best_ind]) / 2.0; output_focal->push_back(focal); output->push_back(poses[best_ind]); } } else { *output = poses; output_focal->resize(n); for (int i = 0; i < n; ++i) { (*output_focal)[i] = (fx[i] + fy[i]) / 2.0; } } return output->size(); } int p4pf(const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_fx, std::vector *output_fy, bool filter_solutions) { Eigen::Matrix points2d; for (int i = 0; i < 4; ++i) { points2d.col(i) = x[i]; } double f0 = points2d.colwise().norm().mean(); points2d /= f0; double d[48]; // Setup nullspace Eigen::Matrix M; Eigen::Matrix A; Eigen::Map> N(d); Eigen::Map> B(d + 32); for (int i = 0; i < 4; i++) { M(0, i) = -points2d(1, i) * X[i](0); M(2, i) = -points2d(1, i) * X[i](1); M(4, i) = -points2d(1, i) * X[i](2); M(6, i) = -points2d(1, i); M(1, i) = points2d(0, i) * X[i](0); M(3, i) = points2d(0, i) * X[i](1); M(5, i) = points2d(0, i) * X[i](2); M(7, i) = points2d(0, i); } // Compute nullspace using QR Eigen::Matrix Q = M.householderQr().householderQ(); N = Q.rightCols(4); // Setup matrices A and B (see paper for definition) for (int i = 0; i < 4; ++i) { if (std::abs(points2d(0, i)) < std::abs(points2d(1, i))) { A(i, 0) = points2d(1, i) * X[i](0); A(i, 1) = points2d(1, i) * X[i](1); A(i, 2) = points2d(1, i) * X[i](2); A(i, 3) = points2d(1, i); B(i, 0) = X[i](0) * N(1, 0) + X[i](1) * N(3, 0) + X[i](2) * N(5, 0) + N(7, 0); // alpha1 B(i, 1) = X[i](0) * N(1, 1) + X[i](1) * N(3, 1) + X[i](2) * N(5, 1) + N(7, 1); // alpha2 B(i, 2) = X[i](0) * N(1, 2) + X[i](1) * N(3, 2) + X[i](2) * N(5, 2) + N(7, 2); // alpha3 B(i, 3) = X[i](0) * N(1, 3) + X[i](1) * N(3, 3) + X[i](2) * N(5, 3) + N(7, 3); // 1 } else { A(i, 0) = points2d(0, i) * X[i](0); A(i, 1) = points2d(0, i) * X[i](1); A(i, 2) = points2d(0, i) * X[i](2); A(i, 3) = points2d(0, i); B(i, 0) = X[i](0) * N(0, 0) + X[i](1) * N(2, 0) + X[i](2) * N(4, 0) + N(6, 0); // alpha1 B(i, 1) = X[i](0) * N(0, 1) + X[i](1) * N(2, 1) + X[i](2) * N(4, 1) + N(6, 1); // alpha2 B(i, 2) = X[i](0) * N(0, 2) + X[i](1) * N(2, 2) + X[i](2) * N(4, 2) + N(6, 2); // alpha3 B(i, 3) = X[i](0) * N(0, 3) + X[i](1) * N(2, 3) + X[i](2) * N(4, 3) + N(6, 3); // 1 } } // [p31,p32,p33,p34] = B * [alpha; 1] B = A.inverse() * B; Eigen::Matrix coeffs; Eigen::Matrix solutions; // Orthogonality constraints coeffs.row(0) << d[0] * d[1] + d[2] * d[3] + d[4] * d[5], d[0] * d[9] + d[1] * d[8] + d[2] * d[11] + d[3] * d[10] + d[4] * d[13] + d[5] * d[12], d[0] * d[17] + d[1] * d[16] + d[2] * d[19] + d[3] * d[18] + d[4] * d[21] + d[5] * d[20], d[8] * d[9] + d[10] * d[11] + d[12] * d[13], d[8] * d[17] + d[9] * d[16] + d[10] * d[19] + d[11] * d[18] + d[12] * d[21] + d[13] * d[20], d[16] * d[17] + d[18] * d[19] + d[20] * d[21], d[0] * d[25] + d[1] * d[24] + d[2] * d[27] + d[3] * d[26] + d[4] * d[29] + d[5] * d[28], d[8] * d[25] + d[9] * d[24] + d[10] * d[27] + d[11] * d[26] + d[12] * d[29] + d[13] * d[28], d[16] * d[25] + d[17] * d[24] + d[18] * d[27] + d[19] * d[26] + d[20] * d[29] + d[21] * d[28], d[24] * d[25] + d[26] * d[27] + d[28] * d[29]; coeffs.row(1) << d[0] * d[32] + d[2] * d[33] + d[4] * d[34], d[0] * d[36] + d[2] * d[37] + d[8] * d[32] + d[4] * d[38] + d[10] * d[33] + d[12] * d[34], d[0] * d[40] + d[2] * d[41] + d[4] * d[42] + d[16] * d[32] + d[18] * d[33] + d[20] * d[34], d[8] * d[36] + d[10] * d[37] + d[12] * d[38], d[8] * d[40] + d[10] * d[41] + d[16] * d[36] + d[12] * d[42] + d[18] * d[37] + d[20] * d[38], d[16] * d[40] + d[18] * d[41] + d[20] * d[42], d[0] * d[44] + d[2] * d[45] + d[4] * d[46] + d[24] * d[32] + d[26] * d[33] + d[28] * d[34], d[8] * d[44] + d[10] * d[45] + d[12] * d[46] + d[24] * d[36] + d[26] * d[37] + d[28] * d[38], d[16] * d[44] + d[18] * d[45] + d[24] * d[40] + d[20] * d[46] + d[26] * d[41] + d[28] * d[42], d[24] * d[44] + d[26] * d[45] + d[28] * d[46]; coeffs.row(2) << d[1] * d[32] + d[3] * d[33] + d[5] * d[34], d[1] * d[36] + d[3] * d[37] + d[9] * d[32] + d[5] * d[38] + d[11] * d[33] + d[13] * d[34], d[1] * d[40] + d[3] * d[41] + d[5] * d[42] + d[17] * d[32] + d[19] * d[33] + d[21] * d[34], d[9] * d[36] + d[11] * d[37] + d[13] * d[38], d[9] * d[40] + d[11] * d[41] + d[17] * d[36] + d[13] * d[42] + d[19] * d[37] + d[21] * d[38], d[17] * d[40] + d[19] * d[41] + d[21] * d[42], d[1] * d[44] + d[3] * d[45] + d[5] * d[46] + d[25] * d[32] + d[27] * d[33] + d[29] * d[34], d[9] * d[44] + d[11] * d[45] + d[13] * d[46] + d[25] * d[36] + d[27] * d[37] + d[29] * d[38], d[17] * d[44] + d[19] * d[45] + d[25] * d[40] + d[21] * d[46] + d[27] * d[41] + d[29] * d[42], d[25] * d[44] + d[27] * d[45] + d[29] * d[46]; // The fourth unused constraint (norms of two first rows equal) // d[0] * d[0] - d[1] * d[1] + d[2] * d[2] - d[3] * d[3] + d[4] * d[4] - d[5] * d[5], 2 * d[0] * d[8] - 2 * d[1] * // d[9] + 2 * d[2] * d[10] - 2 * d[3] * d[11] + 2 * d[4] * d[12] - 2 * d[5] * d[13], 2 * d[0] * d[16] - 2 * d[1] * // d[17] + 2 * d[2] * d[18] - 2 * d[3] * d[19] + 2 * d[4] * d[20] - 2 * d[5] * d[21], d[8] * d[8] - d[9] * d[9] + // d[10] * d[10] - d[11] * d[11] + d[12] * d[12] - d[13] * d[13], 2 * d[8] * d[16] - 2 * d[9] * d[17] + 2 * d[10] * // d[18] - 2 * d[11] * d[19] + 2 * d[12] * d[20] - 2 * d[13] * d[21], d[16] * d[16] - d[17] * d[17] + d[18] * d[18] // - d[19] * d[19] + d[20] * d[20] - d[21] * d[21], 2 * d[0] * d[24] - 2 * d[1] * d[25] + 2 * d[2] * d[26] - 2 * // d[3] // * d[27] + 2 * d[4] * d[28] - 2 * d[5] * d[29], 2 * d[8] * d[24] - 2 * d[9] * d[25] + 2 * d[10] * d[26] - 2 * // d[11] // * d[27] + 2 * d[12] * d[28] - 2 * d[13] * d[29], 2 * d[16] * d[24] - 2 * d[17] * d[25] + 2 * d[18] * d[26] - 2 * // d[19] * d[27] + 2 * d[20] * d[28] - 2 * d[21] * d[29], d[24] * d[24] - d[25] * d[25] + d[26] * d[26] - d[27] * // d[27] + d[28] * d[28] - d[29] * d[29]; int n_sols = re3q3::re3q3(coeffs, &solutions); CameraPose best_pose; output->clear(); output->reserve(n_sols); output_fx->clear(); output_fx->reserve(n_sols); output_fy->clear(); output_fy->reserve(n_sols); for (int i = 0; i < n_sols; ++i) { Eigen::Matrix P; Eigen::Vector4d alpha; alpha << solutions.col(i), 1.0; Eigen::Matrix P12 = N * alpha; P.block<2, 4>(0, 0) = Eigen::Map>(P12.data()); P.row(2) = B * alpha; if (P.block<3, 3>(0, 0).determinant() < 0) P = -P; P = P / P.block<1, 3>(2, 0).norm(); double fx = P.block<1, 3>(0, 0).norm(); double fy = P.block<1, 3>(1, 0).norm(); P.row(0) = P.row(0) / fx; P.row(1) = P.row(1) / fy; Eigen::Matrix3d R = P.block<3, 3>(0, 0); Eigen::Vector3d t = P.block<3, 1>(0, 3); fx *= f0; fy *= f0; CameraPose pose(R, t); if (filter_solutions) { // Check cheirality bool ok = true; for (int k = 0; k < 4; ++k) { if (R.row(2) * X[k] + t(2) < 0.0) { ok = false; break; } } if (!ok) { continue; } } output->push_back(pose); output_fx->push_back(fx); output_fy->push_back(fy); } return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/p4pf.h000066400000000000000000000052461504452766400171530ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P4PF_H_ #define POSELIB_P4PF_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose and focal lengths (fx,fy) such that: lambda*diag(1/fx,1/fy,1)*[x;1] = R*X+t // Re-implementation of the p4pf solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 // This solver returns a separate focal length for x and y // If filter_solutions is true it only returns solutions with positive focal length int p4pf(const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_fx, std::vector *output_fy, bool filter_solutions = true); // Wrapper that returns the average focal length instead. // filter_solutions also removes instances where the aspect ratio (fx/fy) is far from 1 int p4pf(const std::vector &x, const std::vector &X, std::vector *output, std::vector *output_focal, bool filter_solutions = true); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p5lp_radial.cc000066400000000000000000000212151504452766400206260ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p5lp_radial.h" #include "PoseLib/misc/univariate.h" namespace poselib { int p5lp_radial(const std::vector &l, const std::vector &X, std::vector *output) { std::vector x(5, Eigen::Vector2d::Zero()); for (size_t i = 0; i < 5; ++i) { x[i] << l[i](1), -l[i](0); } // Note: Assumes that l[i](2) = 0 ! return p5lp_radial(x, X, output); } int p5lp_radial(const std::vector &x, const std::vector &X, std::vector *output) { // Setup nullspace Eigen::Matrix cc; for (int i = 0; i < 5; i++) { cc(0, i) = -x[i](1) * X[i](0); cc(1, i) = -x[i](1) * X[i](1); cc(2, i) = -x[i](1) * X[i](2); cc(3, i) = -x[i](1); cc(4, i) = x[i](0) * X[i](0); cc(5, i) = x[i](0) * X[i](1); cc(6, i) = x[i](0) * X[i](2); cc(7, i) = x[i](0); } Eigen::Matrix Q = cc.householderQr().householderQ(); Eigen::Matrix N = Q.rightCols(3); // Compute coefficients for sylvester resultant double c11_1 = N(0, 1) * N(4, 1) + N(1, 1) * N(5, 1) + N(2, 1) * N(6, 1); double c12_1 = N(0, 1) * N(4, 2) + N(0, 2) * N(4, 1) + N(1, 1) * N(5, 2) + N(1, 2) * N(5, 1) + N(2, 1) * N(6, 2) + N(2, 2) * N(6, 1); double c12_2 = N(0, 0) * N(4, 1) + N(0, 1) * N(4, 0) + N(1, 0) * N(5, 1) + N(1, 1) * N(5, 0) + N(2, 0) * N(6, 1) + N(2, 1) * N(6, 0); double c13_1 = N(0, 2) * N(4, 2) + N(1, 2) * N(5, 2) + N(2, 2) * N(6, 2); double c13_2 = N(0, 0) * N(4, 2) + N(0, 2) * N(4, 0) + N(1, 0) * N(5, 2) + N(1, 2) * N(5, 0) + N(2, 0) * N(6, 2) + N(2, 2) * N(6, 0); double c13_3 = N(0, 0) * N(4, 0) + N(1, 0) * N(5, 0) + N(2, 0) * N(6, 0); double c21_1 = N(0, 1) * N(0, 1) + N(1, 1) * N(1, 1) + N(2, 1) * N(2, 1) - N(4, 1) * N(4, 1) - N(5, 1) * N(5, 1) - N(6, 1) * N(6, 1); double c22_1 = 2 * N(0, 1) * N(0, 2) + 2 * N(1, 1) * N(1, 2) + 2 * N(2, 1) * N(2, 2) - 2 * N(4, 1) * N(4, 2) - 2 * N(5, 1) * N(5, 2) - 2 * N(6, 1) * N(6, 2); double c22_2 = 2 * N(0, 0) * N(0, 1) + 2 * N(1, 0) * N(1, 1) + 2 * N(2, 0) * N(2, 1) - 2 * N(4, 0) * N(4, 1) - 2 * N(5, 0) * N(5, 1) - 2 * N(6, 0) * N(6, 1); double c23_1 = N(0, 2) * N(0, 2) + N(1, 2) * N(1, 2) + N(2, 2) * N(2, 2) - N(4, 2) * N(4, 2) - N(5, 2) * N(5, 2) - N(6, 2) * N(6, 2); double c23_2 = 2 * N(0, 0) * N(0, 2) + 2 * N(1, 0) * N(1, 2) + 2 * N(2, 0) * N(2, 2) - 2 * N(4, 0) * N(4, 2) - 2 * N(5, 0) * N(5, 2) - 2 * N(6, 0) * N(6, 2); double c23_3 = N(0, 0) * N(0, 0) + N(1, 0) * N(1, 0) + N(2, 0) * N(2, 0) - N(4, 0) * N(4, 0) - N(5, 0) * N(5, 0) - N(6, 0) * N(6, 0); double a4 = c11_1 * c11_1 * c23_3 * c23_3 - c11_1 * c12_2 * c22_2 * c23_3 - 2 * c11_1 * c13_3 * c21_1 * c23_3 + c11_1 * c13_3 * c22_2 * c22_2 + c12_2 * c12_2 * c21_1 * c23_3 - c12_2 * c13_3 * c21_1 * c22_2 + c13_3 * c13_3 * c21_1 * c21_1; double a3 = c11_1 * c13_2 * c22_2 * c22_2 + 2 * c13_2 * c13_3 * c21_1 * c21_1 + c12_2 * c12_2 * c21_1 * c23_2 + 2 * c11_1 * c11_1 * c23_2 * c23_3 - c11_1 * c12_1 * c22_2 * c23_3 - c11_1 * c12_2 * c22_1 * c23_3 - c11_1 * c12_2 * c22_2 * c23_2 - 2 * c11_1 * c13_2 * c21_1 * c23_3 - 2 * c11_1 * c13_3 * c21_1 * c23_2 + 2 * c11_1 * c13_3 * c22_1 * c22_2 + 2 * c12_1 * c12_2 * c21_1 * c23_3 - c12_1 * c13_3 * c21_1 * c22_2 - c12_2 * c13_2 * c21_1 * c22_2 - c12_2 * c13_3 * c21_1 * c22_1; double a2 = c11_1 * c11_1 * c23_2 * c23_2 + c13_2 * c13_2 * c21_1 * c21_1 + c11_1 * c13_1 * c22_2 * c22_2 + c11_1 * c13_3 * c22_1 * c22_1 + 2 * c13_1 * c13_3 * c21_1 * c21_1 + c12_2 * c12_2 * c21_1 * c23_1 + c12_1 * c12_1 * c21_1 * c23_3 + 2 * c11_1 * c11_1 * c23_1 * c23_3 - c11_1 * c12_1 * c22_1 * c23_3 - c11_1 * c12_1 * c22_2 * c23_2 - c11_1 * c12_2 * c22_1 * c23_2 - c11_1 * c12_2 * c22_2 * c23_1 - 2 * c11_1 * c13_1 * c21_1 * c23_3 - 2 * c11_1 * c13_2 * c21_1 * c23_2 + 2 * c11_1 * c13_2 * c22_1 * c22_2 - 2 * c11_1 * c13_3 * c21_1 * c23_1 + 2 * c12_1 * c12_2 * c21_1 * c23_2 - c12_1 * c13_2 * c21_1 * c22_2 - c12_1 * c13_3 * c21_1 * c22_1 - c12_2 * c13_1 * c21_1 * c22_2 - c12_2 * c13_2 * c21_1 * c22_1; double a1 = c11_1 * c13_2 * c22_1 * c22_1 + 2 * c13_1 * c13_2 * c21_1 * c21_1 + c12_1 * c12_1 * c21_1 * c23_2 + 2 * c11_1 * c11_1 * c23_1 * c23_2 - c11_1 * c12_1 * c22_1 * c23_2 - c11_1 * c12_1 * c22_2 * c23_1 - c11_1 * c12_2 * c22_1 * c23_1 - 2 * c11_1 * c13_1 * c21_1 * c23_2 + 2 * c11_1 * c13_1 * c22_1 * c22_2 - 2 * c11_1 * c13_2 * c21_1 * c23_1 + 2 * c12_1 * c12_2 * c21_1 * c23_1 - c12_1 * c13_1 * c21_1 * c22_2 - c12_1 * c13_2 * c21_1 * c22_1 - c12_2 * c13_1 * c21_1 * c22_1; double a0 = c11_1 * c11_1 * c23_1 * c23_1 - c11_1 * c12_1 * c22_1 * c23_1 - 2 * c11_1 * c13_1 * c21_1 * c23_1 + c11_1 * c13_1 * c22_1 * c22_1 + c12_1 * c12_1 * c21_1 * c23_1 - c12_1 * c13_1 * c21_1 * c22_1 + c13_1 * c13_1 * c21_1 * c21_1; a4 = 1.0 / a4; a3 *= a4; a2 *= a4; a1 *= a4; a0 *= a4; double roots[4]; int n_roots = univariate::solve_quartic_real(a3, a2, a1, a0, roots); CameraPose pose; output->clear(); for (int i = 0; i < n_roots; i++) { // We have two quadratic polynomials in y after substituting x double a = roots[i]; double c1a = c11_1; double c1b = c12_1 + c12_2 * a; double c1c = c13_1 + c13_2 * a + c13_3 * a * a; double c2a = c21_1; double c2b = c22_1 + c22_2 * a; double c2c = c23_1 + c23_2 * a + c23_3 * a * a; // we solve the first one double bb[2]; if (!univariate::solve_quadratic_real(c1a, c1b, c1c, bb)) continue; // and check the residuals of the other double res1 = c2a * bb[0] * bb[0] + c2b * bb[0] + c2c; double res2; // For data where X(3,:) = 0 there is only a single solution // In this case the second solution will be NaN if (std::isnan(bb[1])) res2 = std::numeric_limits::max(); else res2 = c2a * bb[1] * bb[1] + c2b * bb[1] + c2c; double b = (std::abs(res1) > std::abs(res2)) ? bb[1] : bb[0]; Eigen::Matrix p = N.col(0) * a + N.col(1) * b + N.col(2); Eigen::Matrix3d R; R.row(0) << p(0), p(1), p(2); R.row(1) << p(4), p(5), p(6); Eigen::Vector3d t(p(3), p(7), 0.0); double scale = R.row(0).norm(); R.row(0) /= scale; R.row(1) /= scale; t /= scale; R.row(2) = R.row(0).cross(R.row(1)); // Select sign using first point if ((R * X[0] + t).topRows<2>().dot(x[0]) < 0) { R.block<2, 3>(0, 0) = -R.block<2, 3>(0, 0); t = -t; } output->emplace_back(R, t); } return n_roots; } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/p5lp_radial.h000066400000000000000000000051171504452766400204730ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P5LP_RADIAL_H_ #define POSELIB_P5LP_RADIAL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: l'*(R*X+t) = 0 // Assumes that all lines pass through the image center, i.e. l = [l1,l2,0] // This is equivalent to the 1D Radial pose solver from // Kukelova et al., Real-Time Solution to the Absolute Pose Problem with Unknown Radial Distortion and Focal Length, // ICCV 2013 // Converting the 2D points to lines l = [-y,x,0] // Note that this solver always returns tz = 0 since it is not observable from these constraints. int p5lp_radial(const std::vector &l, const std::vector &X, std::vector *output); // Helper function using the 2D points. Corrects the sign of the camera using the first point correspondence. int p5lp_radial(const std::vector &x, const std::vector &X, std::vector *output); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/p6lp.cc000066400000000000000000000057231504452766400173210ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "p6lp.h" #include "PoseLib/misc/re3q3.h" namespace poselib { int p6lp(const std::vector &l, const std::vector &X, std::vector *output) { Eigen::Matrix3d A1, A2; Eigen::Matrix B1, B2; A1 << l[0].transpose(), l[1].transpose(), l[2].transpose(); B1 << X[0](0) * l[0].transpose(), X[0](1) * l[0].transpose(), X[0](2) * l[0].transpose(), X[1](0) * l[1].transpose(), X[1](1) * l[1].transpose(), X[1](2) * l[1].transpose(), X[2](0) * l[2].transpose(), X[2](1) * l[2].transpose(), X[2](2) * l[2].transpose(); A2 << l[3].transpose(), l[4].transpose(), l[5].transpose(); B2 << X[3](0) * l[3].transpose(), X[3](1) * l[3].transpose(), X[3](2) * l[3].transpose(), X[4](0) * l[4].transpose(), X[4](1) * l[4].transpose(), X[4](2) * l[4].transpose(), X[5](0) * l[5].transpose(), X[5](1) * l[5].transpose(), X[5](2) * l[5].transpose(); // t + B1*R(:) = 0 B1 = A1.inverse() * B1; // A2*t + B2*R(:) = 0 ==> (B2 - A2*B1) * R(:) = 0 B2 -= A2 * B1; Eigen::Matrix solutions; int n_sols = re3q3::re3q3_rotation(B2, &solutions); output->clear(); for (int i = 0; i < n_sols; ++i) { CameraPose pose; pose.q = solutions.col(i); pose.t = -B1 * quat_to_rotmatvec(pose.q); output->push_back(pose); } return n_sols; } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/p6lp.h000066400000000000000000000040361504452766400171570ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_P6LP_H_ #define POSELIB_P6LP_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: l'*(R*X+t) = 0 // Relies on the E3Q3 solver from // Kukelova et al., Efficient Intersection of Three Quadrics and Applications in Computer Vision, CVPR 2016 int p6lp(const std::vector &l, const std::vector &X, std::vector *output); } // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/relpose_5pt.cc000066400000000000000000000610641504452766400207010ustar00rootroot00000000000000#include "relpose_5pt.h" #include "PoseLib/misc/essential.h" #include "PoseLib/misc/sturm.h" #include namespace poselib { // a, b are first order polys [x,y,z,1] // c is degree 2 poly with order // [ x^2, x*y, x*z, x, y^2, y*z, y, z^2, z, 1] inline void o1(const double a[4], const double b[4], double c[10]) { c[0] = a[0] * b[0]; c[1] = a[0] * b[1] + a[1] * b[0]; c[2] = a[0] * b[2] + a[2] * b[0]; c[3] = a[0] * b[3] + a[3] * b[0]; c[4] = a[1] * b[1]; c[5] = a[1] * b[2] + a[2] * b[1]; c[6] = a[1] * b[3] + a[3] * b[1]; c[7] = a[2] * b[2]; c[8] = a[2] * b[3] + a[3] * b[2]; c[9] = a[3] * b[3]; } inline void o1p(const double a[4], const double b[4], double c[10]) { c[0] += a[0] * b[0]; c[1] += a[0] * b[1] + a[1] * b[0]; c[2] += a[0] * b[2] + a[2] * b[0]; c[3] += a[0] * b[3] + a[3] * b[0]; c[4] += a[1] * b[1]; c[5] += a[1] * b[2] + a[2] * b[1]; c[6] += a[1] * b[3] + a[3] * b[1]; c[7] += a[2] * b[2]; c[8] += a[2] * b[3] + a[3] * b[2]; c[9] += a[3] * b[3]; } inline void o1m(const double a[4], const double b[4], double c[10]) { c[0] -= a[0] * b[0]; c[1] -= a[0] * b[1] + a[1] * b[0]; c[2] -= a[0] * b[2] + a[2] * b[0]; c[3] -= a[0] * b[3] + a[3] * b[0]; c[4] -= a[1] * b[1]; c[5] -= a[1] * b[2] + a[2] * b[1]; c[6] -= a[1] * b[3] + a[3] * b[1]; c[7] -= a[2] * b[2]; c[8] -= a[2] * b[3] + a[3] * b[2]; c[9] -= a[3] * b[3]; } // a is second degree poly with order // [ x^2, x*y, x*z, x, y^2, y*z, y, z^2, z, 1] // b is first degree with order // [x y z 1] // c is third degree with order (same as nister's paper) // [ x^3, y^3, x^2*y, x*y^2, x^2*z, x^2, y^2*z, y^2, x*y*z, x*y, x*z^2, x*z, x, y*z^2, y*z, y, z^3, z^2, z, 1] inline void o2(const double a[10], const double b[4], double c[20]) { c[0] = a[0] * b[0]; c[1] = a[4] * b[1]; c[2] = a[0] * b[1] + a[1] * b[0]; c[3] = a[1] * b[1] + a[4] * b[0]; c[4] = a[0] * b[2] + a[2] * b[0]; c[5] = a[0] * b[3] + a[3] * b[0]; c[6] = a[4] * b[2] + a[5] * b[1]; c[7] = a[4] * b[3] + a[6] * b[1]; c[8] = a[1] * b[2] + a[2] * b[1] + a[5] * b[0]; c[9] = a[1] * b[3] + a[3] * b[1] + a[6] * b[0]; c[10] = a[2] * b[2] + a[7] * b[0]; c[11] = a[2] * b[3] + a[3] * b[2] + a[8] * b[0]; c[12] = a[3] * b[3] + a[9] * b[0]; c[13] = a[5] * b[2] + a[7] * b[1]; c[14] = a[5] * b[3] + a[6] * b[2] + a[8] * b[1]; c[15] = a[6] * b[3] + a[9] * b[1]; c[16] = a[7] * b[2]; c[17] = a[7] * b[3] + a[8] * b[2]; c[18] = a[8] * b[3] + a[9] * b[2]; c[19] = a[9] * b[3]; } inline void o2p(const double a[10], const double b[4], double c[20]) { c[0] += a[0] * b[0]; c[1] += a[4] * b[1]; c[2] += a[0] * b[1] + a[1] * b[0]; c[3] += a[1] * b[1] + a[4] * b[0]; c[4] += a[0] * b[2] + a[2] * b[0]; c[5] += a[0] * b[3] + a[3] * b[0]; c[6] += a[4] * b[2] + a[5] * b[1]; c[7] += a[4] * b[3] + a[6] * b[1]; c[8] += a[1] * b[2] + a[2] * b[1] + a[5] * b[0]; c[9] += a[1] * b[3] + a[3] * b[1] + a[6] * b[0]; c[10] += a[2] * b[2] + a[7] * b[0]; c[11] += a[2] * b[3] + a[3] * b[2] + a[8] * b[0]; c[12] += a[3] * b[3] + a[9] * b[0]; c[13] += a[5] * b[2] + a[7] * b[1]; c[14] += a[5] * b[3] + a[6] * b[2] + a[8] * b[1]; c[15] += a[6] * b[3] + a[9] * b[1]; c[16] += a[7] * b[2]; c[17] += a[7] * b[3] + a[8] * b[2]; c[18] += a[8] * b[3] + a[9] * b[2]; c[19] += a[9] * b[3]; } void compute_trace_constraints(const Eigen::Matrix &N, Eigen::Matrix &coeffs) { double const *N_ptr = N.data(); #define EE(i, j) N_ptr + 4 * (3 * j + i) double d[60]; // Determinant constraint Eigen::Matrix row; double *c_data = row.data(); o1(EE(0, 1), EE(1, 2), d); o1m(EE(0, 2), EE(1, 1), d); o2(d, EE(2, 0), c_data); o1(EE(0, 2), EE(1, 0), d); o1m(EE(0, 0), EE(1, 2), d); o2p(d, EE(2, 1), c_data); o1(EE(0, 0), EE(1, 1), d); o1m(EE(0, 1), EE(1, 0), d); o2p(d, EE(2, 2), c_data); coeffs.row(9) = row; double *EET[3][3] = {{d, d + 10, d + 20}, {d + 10, d + 40, d + 30}, {d + 20, d + 30, d + 50}}; // Compute EE^T (equation 20 in paper) for (int i = 0; i < 3; ++i) { for (int j = i; j < 3; ++j) { o1(EE(i, 0), EE(j, 0), EET[i][j]); o1p(EE(i, 1), EE(j, 1), EET[i][j]); o1p(EE(i, 2), EE(j, 2), EET[i][j]); } } // Subtract trace (equation 22 in paper) for (int i = 0; i < 10; ++i) { double t = 0.5 * (EET[0][0][i] + EET[1][1][i] + EET[2][2][i]); EET[0][0][i] -= t; EET[1][1][i] -= t; EET[2][2][i] -= t; } int cnt = 0; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { o2(EET[i][0], EE(0, j), c_data); o2p(EET[i][1], EE(1, j), c_data); o2p(EET[i][2], EE(2, j), c_data); coeffs.row(cnt++) = row; } } #undef EE } int relpose_5pt(const std::vector &x1, const std::vector &x2, std::vector *essential_matrices) { // Compute nullspace to epipolar constraints Eigen::Matrix epipolar_constraints; for (int i = 0; i < 5; ++i) { epipolar_constraints.col(i) << x1[i](0) * x2[i], x1[i](1) * x2[i], x1[i](2) * x2[i]; } Eigen::Matrix Q = epipolar_constraints.fullPivHouseholderQr().matrixQ(); Eigen::Matrix N = Q.rightCols(4).transpose(); // Compute equation coefficients for the trace constraints + determinant Eigen::Matrix coeffs; compute_trace_constraints(N, coeffs); coeffs.block<10, 10>(0, 10) = coeffs.block<10, 10>(0, 0).partialPivLu().solve(coeffs.block<10, 10>(0, 10)); // Perform eliminations using the 6 bottom rows Eigen::Matrix A; for (int i = 0; i < 3; ++i) { A(i, 0) = 0.0; A.block<1, 3>(i, 1) = coeffs.block<1, 3>(4 + 2 * i, 10); A.block<1, 3>(i, 0) -= coeffs.block<1, 3>(5 + 2 * i, 10); A(i, 4) = 0.0; A.block<1, 3>(i, 5) = coeffs.block<1, 3>(4 + 2 * i, 13); A.block<1, 3>(i, 4) -= coeffs.block<1, 3>(5 + 2 * i, 13); A(i, 8) = 0.0; A.block<1, 4>(i, 9) = coeffs.block<1, 4>(4 + 2 * i, 16); A.block<1, 4>(i, 8) -= coeffs.block<1, 4>(5 + 2 * i, 16); } // Compute degree 10 poly representing determinant (equation 14 in the paper) double c[11]; c[0] = A(0, 12) * A(1, 3) * A(2, 7) - A(0, 12) * A(1, 7) * A(2, 3) - A(0, 3) * A(2, 7) * A(1, 12) + A(0, 7) * A(2, 3) * A(1, 12) + A(0, 3) * A(1, 7) * A(2, 12) - A(0, 7) * A(1, 3) * A(2, 12); c[1] = A(0, 11) * A(1, 3) * A(2, 7) - A(0, 11) * A(1, 7) * A(2, 3) + A(0, 12) * A(1, 2) * A(2, 7) + A(0, 12) * A(1, 3) * A(2, 6) - A(0, 12) * A(1, 6) * A(2, 3) - A(0, 12) * A(1, 7) * A(2, 2) - A(0, 2) * A(2, 7) * A(1, 12) - A(0, 3) * A(2, 6) * A(1, 12) - A(0, 3) * A(2, 7) * A(1, 11) + A(0, 6) * A(2, 3) * A(1, 12) + A(0, 7) * A(2, 2) * A(1, 12) + A(0, 7) * A(2, 3) * A(1, 11) + A(0, 2) * A(1, 7) * A(2, 12) + A(0, 3) * A(1, 6) * A(2, 12) + A(0, 3) * A(1, 7) * A(2, 11) - A(0, 6) * A(1, 3) * A(2, 12) - A(0, 7) * A(1, 2) * A(2, 12) - A(0, 7) * A(1, 3) * A(2, 11); c[2] = A(0, 10) * A(1, 3) * A(2, 7) - A(0, 10) * A(1, 7) * A(2, 3) + A(0, 11) * A(1, 2) * A(2, 7) + A(0, 11) * A(1, 3) * A(2, 6) - A(0, 11) * A(1, 6) * A(2, 3) - A(0, 11) * A(1, 7) * A(2, 2) + A(1, 1) * A(0, 12) * A(2, 7) + A(0, 12) * A(1, 2) * A(2, 6) + A(0, 12) * A(1, 3) * A(2, 5) - A(0, 12) * A(1, 5) * A(2, 3) - A(0, 12) * A(1, 6) * A(2, 2) - A(0, 12) * A(1, 7) * A(2, 1) - A(0, 1) * A(2, 7) * A(1, 12) - A(0, 2) * A(2, 6) * A(1, 12) - A(0, 2) * A(2, 7) * A(1, 11) - A(0, 3) * A(2, 5) * A(1, 12) - A(0, 3) * A(2, 6) * A(1, 11) - A(0, 3) * A(2, 7) * A(1, 10) + A(0, 5) * A(2, 3) * A(1, 12) + A(0, 6) * A(2, 2) * A(1, 12) + A(0, 6) * A(2, 3) * A(1, 11) + A(0, 7) * A(2, 1) * A(1, 12) + A(0, 7) * A(2, 2) * A(1, 11) + A(0, 7) * A(2, 3) * A(1, 10) + A(0, 1) * A(1, 7) * A(2, 12) + A(0, 2) * A(1, 6) * A(2, 12) + A(0, 2) * A(1, 7) * A(2, 11) + A(0, 3) * A(1, 5) * A(2, 12) + A(0, 3) * A(1, 6) * A(2, 11) + A(0, 3) * A(1, 7) * A(2, 10) - A(0, 5) * A(1, 3) * A(2, 12) - A(0, 6) * A(1, 2) * A(2, 12) - A(0, 6) * A(1, 3) * A(2, 11) - A(0, 7) * A(1, 1) * A(2, 12) - A(0, 7) * A(1, 2) * A(2, 11) - A(0, 7) * A(1, 3) * A(2, 10); c[3] = A(0, 3) * A(1, 7) * A(2, 9) - A(0, 3) * A(1, 9) * A(2, 7) - A(0, 7) * A(1, 3) * A(2, 9) + A(0, 7) * A(1, 9) * A(2, 3) + A(0, 9) * A(1, 3) * A(2, 7) - A(0, 9) * A(1, 7) * A(2, 3) + A(0, 10) * A(1, 2) * A(2, 7) + A(0, 10) * A(1, 3) * A(2, 6) - A(0, 10) * A(1, 6) * A(2, 3) - A(0, 10) * A(1, 7) * A(2, 2) + A(1, 0) * A(0, 12) * A(2, 7) + A(0, 11) * A(1, 1) * A(2, 7) + A(0, 11) * A(1, 2) * A(2, 6) + A(0, 11) * A(1, 3) * A(2, 5) - A(0, 11) * A(1, 5) * A(2, 3) - A(0, 11) * A(1, 6) * A(2, 2) - A(0, 11) * A(1, 7) * A(2, 1) + A(1, 1) * A(0, 12) * A(2, 6) + A(0, 12) * A(1, 2) * A(2, 5) + A(0, 12) * A(1, 3) * A(2, 4) - A(0, 12) * A(1, 4) * A(2, 3) - A(0, 12) * A(1, 5) * A(2, 2) - A(0, 12) * A(1, 6) * A(2, 1) - A(0, 12) * A(1, 7) * A(2, 0) - A(0, 0) * A(2, 7) * A(1, 12) - A(0, 1) * A(2, 6) * A(1, 12) - A(0, 1) * A(2, 7) * A(1, 11) - A(0, 2) * A(2, 5) * A(1, 12) - A(0, 2) * A(2, 6) * A(1, 11) - A(0, 2) * A(2, 7) * A(1, 10) - A(0, 3) * A(2, 4) * A(1, 12) - A(0, 3) * A(2, 5) * A(1, 11) - A(0, 3) * A(2, 6) * A(1, 10) + A(0, 4) * A(2, 3) * A(1, 12) + A(0, 5) * A(2, 2) * A(1, 12) + A(0, 5) * A(2, 3) * A(1, 11) + A(0, 6) * A(2, 1) * A(1, 12) + A(0, 6) * A(2, 2) * A(1, 11) + A(0, 6) * A(2, 3) * A(1, 10) + A(0, 7) * A(2, 0) * A(1, 12) + A(0, 7) * A(2, 1) * A(1, 11) + A(0, 7) * A(2, 2) * A(1, 10) + A(0, 0) * A(1, 7) * A(2, 12) + A(0, 1) * A(1, 6) * A(2, 12) + A(0, 1) * A(1, 7) * A(2, 11) + A(0, 2) * A(1, 5) * A(2, 12) + A(0, 2) * A(1, 6) * A(2, 11) + A(0, 2) * A(1, 7) * A(2, 10) + A(0, 3) * A(1, 4) * A(2, 12) + A(0, 3) * A(1, 5) * A(2, 11) + A(0, 3) * A(1, 6) * A(2, 10) - A(0, 4) * A(1, 3) * A(2, 12) - A(0, 5) * A(1, 2) * A(2, 12) - A(0, 5) * A(1, 3) * A(2, 11) - A(0, 6) * A(1, 1) * A(2, 12) - A(0, 6) * A(1, 2) * A(2, 11) - A(0, 6) * A(1, 3) * A(2, 10) - A(0, 7) * A(1, 0) * A(2, 12) - A(0, 7) * A(1, 1) * A(2, 11) - A(0, 7) * A(1, 2) * A(2, 10); c[4] = A(0, 2) * A(1, 7) * A(2, 9) - A(0, 2) * A(1, 9) * A(2, 7) + A(0, 3) * A(1, 6) * A(2, 9) + A(0, 3) * A(1, 7) * A(2, 8) - A(0, 3) * A(1, 8) * A(2, 7) - A(0, 3) * A(1, 9) * A(2, 6) - A(0, 6) * A(1, 3) * A(2, 9) + A(0, 6) * A(1, 9) * A(2, 3) - A(0, 7) * A(1, 2) * A(2, 9) - A(0, 7) * A(1, 3) * A(2, 8) + A(0, 7) * A(1, 8) * A(2, 3) + A(0, 7) * A(1, 9) * A(2, 2) + A(0, 8) * A(1, 3) * A(2, 7) - A(0, 8) * A(1, 7) * A(2, 3) + A(0, 9) * A(1, 2) * A(2, 7) + A(0, 9) * A(1, 3) * A(2, 6) - A(0, 9) * A(1, 6) * A(2, 3) - A(0, 9) * A(1, 7) * A(2, 2) + A(0, 10) * A(1, 1) * A(2, 7) + A(0, 10) * A(1, 2) * A(2, 6) + A(0, 10) * A(1, 3) * A(2, 5) - A(0, 10) * A(1, 5) * A(2, 3) - A(0, 10) * A(1, 6) * A(2, 2) - A(0, 10) * A(1, 7) * A(2, 1) + A(1, 0) * A(0, 11) * A(2, 7) + A(1, 0) * A(0, 12) * A(2, 6) + A(0, 11) * A(1, 1) * A(2, 6) + A(0, 11) * A(1, 2) * A(2, 5) + A(0, 11) * A(1, 3) * A(2, 4) - A(0, 11) * A(1, 4) * A(2, 3) - A(0, 11) * A(1, 5) * A(2, 2) - A(0, 11) * A(1, 6) * A(2, 1) - A(0, 11) * A(1, 7) * A(2, 0) + A(1, 1) * A(0, 12) * A(2, 5) + A(0, 12) * A(1, 2) * A(2, 4) - A(0, 12) * A(1, 4) * A(2, 2) - A(0, 12) * A(1, 5) * A(2, 1) - A(0, 12) * A(1, 6) * A(2, 0) - A(0, 0) * A(2, 6) * A(1, 12) - A(0, 0) * A(2, 7) * A(1, 11) - A(0, 1) * A(2, 5) * A(1, 12) - A(0, 1) * A(2, 6) * A(1, 11) - A(0, 1) * A(2, 7) * A(1, 10) - A(0, 2) * A(2, 4) * A(1, 12) - A(0, 2) * A(2, 5) * A(1, 11) - A(0, 2) * A(2, 6) * A(1, 10) - A(0, 3) * A(2, 4) * A(1, 11) - A(0, 3) * A(2, 5) * A(1, 10) + A(0, 4) * A(2, 2) * A(1, 12) + A(0, 4) * A(2, 3) * A(1, 11) + A(0, 5) * A(2, 1) * A(1, 12) + A(0, 5) * A(2, 2) * A(1, 11) + A(0, 5) * A(2, 3) * A(1, 10) + A(0, 6) * A(2, 0) * A(1, 12) + A(0, 6) * A(2, 1) * A(1, 11) + A(0, 6) * A(2, 2) * A(1, 10) + A(0, 7) * A(2, 0) * A(1, 11) + A(0, 7) * A(2, 1) * A(1, 10) + A(0, 0) * A(1, 6) * A(2, 12) + A(0, 0) * A(1, 7) * A(2, 11) + A(0, 1) * A(1, 5) * A(2, 12) + A(0, 1) * A(1, 6) * A(2, 11) + A(0, 1) * A(1, 7) * A(2, 10) + A(0, 2) * A(1, 4) * A(2, 12) + A(0, 2) * A(1, 5) * A(2, 11) + A(0, 2) * A(1, 6) * A(2, 10) + A(0, 3) * A(1, 4) * A(2, 11) + A(0, 3) * A(1, 5) * A(2, 10) - A(0, 4) * A(1, 2) * A(2, 12) - A(0, 4) * A(1, 3) * A(2, 11) - A(0, 5) * A(1, 1) * A(2, 12) - A(0, 5) * A(1, 2) * A(2, 11) - A(0, 5) * A(1, 3) * A(2, 10) - A(0, 6) * A(1, 0) * A(2, 12) - A(0, 6) * A(1, 1) * A(2, 11) - A(0, 6) * A(1, 2) * A(2, 10) - A(0, 7) * A(1, 0) * A(2, 11) - A(0, 7) * A(1, 1) * A(2, 10); c[5] = A(0, 1) * A(1, 7) * A(2, 9) - A(0, 1) * A(1, 9) * A(2, 7) + A(0, 2) * A(1, 6) * A(2, 9) + A(0, 2) * A(1, 7) * A(2, 8) - A(0, 2) * A(1, 8) * A(2, 7) - A(0, 2) * A(1, 9) * A(2, 6) + A(0, 3) * A(1, 5) * A(2, 9) + A(0, 3) * A(1, 6) * A(2, 8) - A(0, 3) * A(1, 8) * A(2, 6) - A(0, 3) * A(1, 9) * A(2, 5) - A(0, 5) * A(1, 3) * A(2, 9) + A(0, 5) * A(1, 9) * A(2, 3) - A(0, 6) * A(1, 2) * A(2, 9) - A(0, 6) * A(1, 3) * A(2, 8) + A(0, 6) * A(1, 8) * A(2, 3) + A(0, 6) * A(1, 9) * A(2, 2) - A(0, 7) * A(1, 1) * A(2, 9) - A(0, 7) * A(1, 2) * A(2, 8) + A(0, 7) * A(1, 8) * A(2, 2) + A(0, 7) * A(1, 9) * A(2, 1) + A(0, 8) * A(1, 2) * A(2, 7) + A(0, 8) * A(1, 3) * A(2, 6) - A(0, 8) * A(1, 6) * A(2, 3) - A(0, 8) * A(1, 7) * A(2, 2) + A(0, 9) * A(1, 1) * A(2, 7) + A(0, 9) * A(1, 2) * A(2, 6) + A(0, 9) * A(1, 3) * A(2, 5) - A(0, 9) * A(1, 5) * A(2, 3) - A(0, 9) * A(1, 6) * A(2, 2) - A(0, 9) * A(1, 7) * A(2, 1) + A(0, 10) * A(1, 0) * A(2, 7) + A(0, 10) * A(1, 1) * A(2, 6) + A(0, 10) * A(1, 2) * A(2, 5) + A(0, 10) * A(1, 3) * A(2, 4) - A(0, 10) * A(1, 4) * A(2, 3) - A(0, 10) * A(1, 5) * A(2, 2) - A(0, 10) * A(1, 6) * A(2, 1) - A(0, 10) * A(1, 7) * A(2, 0) + A(1, 0) * A(0, 11) * A(2, 6) + A(1, 0) * A(0, 12) * A(2, 5) + A(0, 11) * A(1, 1) * A(2, 5) + A(0, 11) * A(1, 2) * A(2, 4) - A(0, 11) * A(1, 4) * A(2, 2) - A(0, 11) * A(1, 5) * A(2, 1) - A(0, 11) * A(1, 6) * A(2, 0) + A(1, 1) * A(0, 12) * A(2, 4) - A(0, 12) * A(1, 4) * A(2, 1) - A(0, 12) * A(1, 5) * A(2, 0) - A(0, 0) * A(2, 5) * A(1, 12) - A(0, 0) * A(2, 6) * A(1, 11) - A(0, 0) * A(2, 7) * A(1, 10) - A(0, 1) * A(2, 4) * A(1, 12) - A(0, 1) * A(2, 5) * A(1, 11) - A(0, 1) * A(2, 6) * A(1, 10) - A(0, 2) * A(2, 4) * A(1, 11) - A(0, 2) * A(2, 5) * A(1, 10) - A(0, 3) * A(2, 4) * A(1, 10) + A(0, 4) * A(2, 1) * A(1, 12) + A(0, 4) * A(2, 2) * A(1, 11) + A(0, 4) * A(2, 3) * A(1, 10) + A(0, 5) * A(2, 0) * A(1, 12) + A(0, 5) * A(2, 1) * A(1, 11) + A(0, 5) * A(2, 2) * A(1, 10) + A(0, 6) * A(2, 0) * A(1, 11) + A(0, 6) * A(2, 1) * A(1, 10) + A(0, 7) * A(2, 0) * A(1, 10) + A(0, 0) * A(1, 5) * A(2, 12) + A(0, 0) * A(1, 6) * A(2, 11) + A(0, 0) * A(1, 7) * A(2, 10) + A(0, 1) * A(1, 4) * A(2, 12) + A(0, 1) * A(1, 5) * A(2, 11) + A(0, 1) * A(1, 6) * A(2, 10) + A(0, 2) * A(1, 4) * A(2, 11) + A(0, 2) * A(1, 5) * A(2, 10) + A(0, 3) * A(1, 4) * A(2, 10) - A(0, 4) * A(1, 1) * A(2, 12) - A(0, 4) * A(1, 2) * A(2, 11) - A(0, 4) * A(1, 3) * A(2, 10) - A(0, 5) * A(1, 0) * A(2, 12) - A(0, 5) * A(1, 1) * A(2, 11) - A(0, 5) * A(1, 2) * A(2, 10) - A(0, 6) * A(1, 0) * A(2, 11) - A(0, 6) * A(1, 1) * A(2, 10) - A(0, 7) * A(1, 0) * A(2, 10); c[6] = A(0, 0) * A(1, 7) * A(2, 9) - A(0, 0) * A(1, 9) * A(2, 7) + A(0, 1) * A(1, 6) * A(2, 9) + A(0, 1) * A(1, 7) * A(2, 8) - A(0, 1) * A(1, 8) * A(2, 7) - A(0, 1) * A(1, 9) * A(2, 6) + A(0, 2) * A(1, 5) * A(2, 9) + A(0, 2) * A(1, 6) * A(2, 8) - A(0, 2) * A(1, 8) * A(2, 6) - A(0, 2) * A(1, 9) * A(2, 5) + A(0, 3) * A(1, 4) * A(2, 9) + A(0, 3) * A(1, 5) * A(2, 8) - A(0, 3) * A(1, 8) * A(2, 5) - A(0, 3) * A(1, 9) * A(2, 4) - A(0, 4) * A(1, 3) * A(2, 9) + A(0, 4) * A(1, 9) * A(2, 3) - A(0, 5) * A(1, 2) * A(2, 9) - A(0, 5) * A(1, 3) * A(2, 8) + A(0, 5) * A(1, 8) * A(2, 3) + A(0, 5) * A(1, 9) * A(2, 2) - A(0, 6) * A(1, 1) * A(2, 9) - A(0, 6) * A(1, 2) * A(2, 8) + A(0, 6) * A(1, 8) * A(2, 2) + A(0, 6) * A(1, 9) * A(2, 1) - A(0, 7) * A(1, 0) * A(2, 9) - A(0, 7) * A(1, 1) * A(2, 8) + A(0, 7) * A(1, 8) * A(2, 1) + A(0, 7) * A(1, 9) * A(2, 0) + A(0, 8) * A(1, 1) * A(2, 7) + A(0, 8) * A(1, 2) * A(2, 6) + A(0, 8) * A(1, 3) * A(2, 5) - A(0, 8) * A(1, 5) * A(2, 3) - A(0, 8) * A(1, 6) * A(2, 2) - A(0, 8) * A(1, 7) * A(2, 1) + A(0, 9) * A(1, 0) * A(2, 7) + A(0, 9) * A(1, 1) * A(2, 6) + A(0, 9) * A(1, 2) * A(2, 5) + A(0, 9) * A(1, 3) * A(2, 4) - A(0, 9) * A(1, 4) * A(2, 3) - A(0, 9) * A(1, 5) * A(2, 2) - A(0, 9) * A(1, 6) * A(2, 1) - A(0, 9) * A(1, 7) * A(2, 0) + A(0, 10) * A(1, 0) * A(2, 6) + A(0, 10) * A(1, 1) * A(2, 5) + A(0, 10) * A(1, 2) * A(2, 4) - A(0, 10) * A(1, 4) * A(2, 2) - A(0, 10) * A(1, 5) * A(2, 1) - A(0, 10) * A(1, 6) * A(2, 0) + A(1, 0) * A(0, 11) * A(2, 5) + A(1, 0) * A(0, 12) * A(2, 4) + A(0, 11) * A(1, 1) * A(2, 4) - A(0, 11) * A(1, 4) * A(2, 1) - A(0, 11) * A(1, 5) * A(2, 0) - A(0, 12) * A(1, 4) * A(2, 0) - A(0, 0) * A(2, 4) * A(1, 12) - A(0, 0) * A(2, 5) * A(1, 11) - A(0, 0) * A(2, 6) * A(1, 10) - A(0, 1) * A(2, 4) * A(1, 11) - A(0, 1) * A(2, 5) * A(1, 10) - A(0, 2) * A(2, 4) * A(1, 10) + A(0, 4) * A(2, 0) * A(1, 12) + A(0, 4) * A(2, 1) * A(1, 11) + A(0, 4) * A(2, 2) * A(1, 10) + A(0, 5) * A(2, 0) * A(1, 11) + A(0, 5) * A(2, 1) * A(1, 10) + A(0, 6) * A(2, 0) * A(1, 10) + A(0, 0) * A(1, 4) * A(2, 12) + A(0, 0) * A(1, 5) * A(2, 11) + A(0, 0) * A(1, 6) * A(2, 10) + A(0, 1) * A(1, 4) * A(2, 11) + A(0, 1) * A(1, 5) * A(2, 10) + A(0, 2) * A(1, 4) * A(2, 10) - A(0, 4) * A(1, 0) * A(2, 12) - A(0, 4) * A(1, 1) * A(2, 11) - A(0, 4) * A(1, 2) * A(2, 10) - A(0, 5) * A(1, 0) * A(2, 11) - A(0, 5) * A(1, 1) * A(2, 10) - A(0, 6) * A(1, 0) * A(2, 10); c[7] = A(0, 0) * A(1, 6) * A(2, 9) + A(0, 0) * A(1, 7) * A(2, 8) - A(0, 0) * A(1, 8) * A(2, 7) - A(0, 0) * A(1, 9) * A(2, 6) + A(0, 1) * A(1, 5) * A(2, 9) + A(0, 1) * A(1, 6) * A(2, 8) - A(0, 1) * A(1, 8) * A(2, 6) - A(0, 1) * A(1, 9) * A(2, 5) + A(0, 2) * A(1, 4) * A(2, 9) + A(0, 2) * A(1, 5) * A(2, 8) - A(0, 2) * A(1, 8) * A(2, 5) - A(0, 2) * A(1, 9) * A(2, 4) + A(0, 3) * A(1, 4) * A(2, 8) - A(0, 3) * A(1, 8) * A(2, 4) - A(0, 4) * A(1, 2) * A(2, 9) - A(0, 4) * A(1, 3) * A(2, 8) + A(0, 4) * A(1, 8) * A(2, 3) + A(0, 4) * A(1, 9) * A(2, 2) - A(0, 5) * A(1, 1) * A(2, 9) - A(0, 5) * A(1, 2) * A(2, 8) + A(0, 5) * A(1, 8) * A(2, 2) + A(0, 5) * A(1, 9) * A(2, 1) - A(0, 6) * A(1, 0) * A(2, 9) - A(0, 6) * A(1, 1) * A(2, 8) + A(0, 6) * A(1, 8) * A(2, 1) + A(0, 6) * A(1, 9) * A(2, 0) - A(0, 7) * A(1, 0) * A(2, 8) + A(0, 7) * A(1, 8) * A(2, 0) + A(0, 8) * A(1, 0) * A(2, 7) + A(0, 8) * A(1, 1) * A(2, 6) + A(0, 8) * A(1, 2) * A(2, 5) + A(0, 8) * A(1, 3) * A(2, 4) - A(0, 8) * A(1, 4) * A(2, 3) - A(0, 8) * A(1, 5) * A(2, 2) - A(0, 8) * A(1, 6) * A(2, 1) - A(0, 8) * A(1, 7) * A(2, 0) + A(0, 9) * A(1, 0) * A(2, 6) + A(0, 9) * A(1, 1) * A(2, 5) + A(0, 9) * A(1, 2) * A(2, 4) - A(0, 9) * A(1, 4) * A(2, 2) - A(0, 9) * A(1, 5) * A(2, 1) - A(0, 9) * A(1, 6) * A(2, 0) + A(0, 10) * A(1, 0) * A(2, 5) + A(0, 10) * A(1, 1) * A(2, 4) - A(0, 10) * A(1, 4) * A(2, 1) - A(0, 10) * A(1, 5) * A(2, 0) + A(1, 0) * A(0, 11) * A(2, 4) - A(0, 11) * A(1, 4) * A(2, 0) - A(0, 0) * A(2, 4) * A(1, 11) - A(0, 0) * A(2, 5) * A(1, 10) - A(0, 1) * A(2, 4) * A(1, 10) + A(0, 4) * A(2, 0) * A(1, 11) + A(0, 4) * A(2, 1) * A(1, 10) + A(0, 5) * A(2, 0) * A(1, 10) + A(0, 0) * A(1, 4) * A(2, 11) + A(0, 0) * A(1, 5) * A(2, 10) + A(0, 1) * A(1, 4) * A(2, 10) - A(0, 4) * A(1, 0) * A(2, 11) - A(0, 4) * A(1, 1) * A(2, 10) - A(0, 5) * A(1, 0) * A(2, 10); c[8] = A(0, 0) * A(1, 5) * A(2, 9) + A(0, 0) * A(1, 6) * A(2, 8) - A(0, 0) * A(1, 8) * A(2, 6) - A(0, 0) * A(1, 9) * A(2, 5) + A(0, 1) * A(1, 4) * A(2, 9) + A(0, 1) * A(1, 5) * A(2, 8) - A(0, 1) * A(1, 8) * A(2, 5) - A(0, 1) * A(1, 9) * A(2, 4) + A(0, 2) * A(1, 4) * A(2, 8) - A(0, 2) * A(1, 8) * A(2, 4) - A(0, 4) * A(1, 1) * A(2, 9) - A(0, 4) * A(1, 2) * A(2, 8) + A(0, 4) * A(1, 8) * A(2, 2) + A(0, 4) * A(1, 9) * A(2, 1) - A(0, 5) * A(1, 0) * A(2, 9) - A(0, 5) * A(1, 1) * A(2, 8) + A(0, 5) * A(1, 8) * A(2, 1) + A(0, 5) * A(1, 9) * A(2, 0) - A(0, 6) * A(1, 0) * A(2, 8) + A(0, 6) * A(1, 8) * A(2, 0) + A(0, 8) * A(1, 0) * A(2, 6) + A(0, 8) * A(1, 1) * A(2, 5) + A(0, 8) * A(1, 2) * A(2, 4) - A(0, 8) * A(1, 4) * A(2, 2) - A(0, 8) * A(1, 5) * A(2, 1) - A(0, 8) * A(1, 6) * A(2, 0) + A(0, 9) * A(1, 0) * A(2, 5) + A(0, 9) * A(1, 1) * A(2, 4) - A(0, 9) * A(1, 4) * A(2, 1) - A(0, 9) * A(1, 5) * A(2, 0) + A(0, 10) * A(1, 0) * A(2, 4) - A(0, 10) * A(1, 4) * A(2, 0) - A(0, 0) * A(2, 4) * A(1, 10) + A(0, 4) * A(2, 0) * A(1, 10) + A(0, 0) * A(1, 4) * A(2, 10) - A(0, 4) * A(1, 0) * A(2, 10); c[9] = A(0, 0) * A(1, 4) * A(2, 9) + A(0, 0) * A(1, 5) * A(2, 8) - A(0, 0) * A(1, 8) * A(2, 5) - A(0, 0) * A(1, 9) * A(2, 4) + A(0, 1) * A(1, 4) * A(2, 8) - A(0, 1) * A(1, 8) * A(2, 4) - A(0, 4) * A(1, 0) * A(2, 9) - A(0, 4) * A(1, 1) * A(2, 8) + A(0, 4) * A(1, 8) * A(2, 1) + A(0, 4) * A(1, 9) * A(2, 0) - A(0, 5) * A(1, 0) * A(2, 8) + A(0, 5) * A(1, 8) * A(2, 0) + A(0, 8) * A(1, 0) * A(2, 5) + A(0, 8) * A(1, 1) * A(2, 4) - A(0, 8) * A(1, 4) * A(2, 1) - A(0, 8) * A(1, 5) * A(2, 0) + A(0, 9) * A(1, 0) * A(2, 4) - A(0, 9) * A(1, 4) * A(2, 0); c[10] = A(0, 0) * A(1, 4) * A(2, 8) - A(0, 0) * A(1, 8) * A(2, 4) - A(0, 4) * A(1, 0) * A(2, 8) + A(0, 4) * A(1, 8) * A(2, 0) + A(0, 8) * A(1, 0) * A(2, 4) - A(0, 8) * A(1, 4) * A(2, 0); // Solve for the roots using sturm bracketing double roots[10]; int n_sols = poselib::sturm::bisect_sturm<10>(c, roots); // Back substitution to recover essential matrices Eigen::Matrix B; Eigen::Matrix b; Eigen::Matrix xz; Eigen::Matrix E; Eigen::Map> e(E.data()); essential_matrices->reserve(n_sols); for (int i = 0; i < n_sols; ++i) { const double z = roots[i]; const double z2 = z * z; const double z3 = z2 * z; const double z4 = z2 * z2; B.col(0) = A.block<3, 1>(0, 0) * z3 + A.block<3, 1>(0, 1) * z2 + A.block<3, 1>(0, 2) * z + A.block<3, 1>(0, 3); B.col(1) = A.block<3, 1>(0, 4) * z3 + A.block<3, 1>(0, 5) * z2 + A.block<3, 1>(0, 6) * z + A.block<3, 1>(0, 7); b = A.block<3, 1>(0, 8) * z4 + A.block<3, 1>(0, 9) * z3 + A.block<3, 1>(0, 10) * z2 + A.block<3, 1>(0, 11) * z + A.block<3, 1>(0, 12); // We try to solve using top two rows xz = B.block<2, 2>(0, 0).inverse() * b.block<2, 1>(0, 0); // If this fails we revert to more expensive QR solver using all three rows if (std::abs(B.row(2) * xz - b(2)) > 1e-6) { xz = B.colPivHouseholderQr().solve(b); } const double x = -xz(0), y = -xz(1); e = N.row(0) * x + N.row(1) * y + N.row(2) * z + N.row(3); // Since the rows of N are orthogonal unit vectors, we can normalize the coefficients instead const double inv_norm = 1.0 / std::sqrt(x * x + y * y + z * z + 1.0); e *= inv_norm; essential_matrices->push_back(E); } return n_sols; } int relpose_5pt(const std::vector &x1, const std::vector &x2, std::vector *output) { std::vector essential_matrices; int n_sols = relpose_5pt(x1, x2, &essential_matrices); output->clear(); output->reserve(n_sols); for (int i = 0; i < n_sols; ++i) { motion_from_essential(essential_matrices[i], x1, x2, output); } return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/relpose_5pt.h000066400000000000000000000042751504452766400205440ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RELPOSE_5PT_H_ #define POSELIB_RELPOSE_5PT_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Computes the essential matrix from five point correspondences. // Nister, An Efficient Solution to the Five-Point Relative Pose Problem, PAMI 2004 int relpose_5pt(const std::vector &x1, const std::vector &x2, std::vector *essential_matrices); int relpose_5pt(const std::vector &x1, const std::vector &x2, std::vector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/relpose_6pt_focal.cc000066400000000000000000002754451504452766400220600ustar00rootroot00000000000000#include "PoseLib/misc/sturm.h" #include #include #include #include #include namespace poselib { void shared_focal_relpose_fast_eigenvector_solver(double *eigv, int neig, Eigen::Matrix &AM, Eigen::Matrix, 3, 15> &sols) { static const int ind[] = {2, 3, 4, 6, 8, 9, 11, 14}; // Truncated action matrix containing non-trivial rows Eigen::Matrix AMs; double zi[3]; for (int i = 0; i < 8; i++) { AMs.row(i) = AM.row(ind[i]); } for (int i = 0; i < neig; i++) { zi[0] = eigv[i]; for (int j = 1; j < 3; j++) { zi[j] = zi[j - 1] * eigv[i]; } Eigen::Matrix AA; AA.col(0) = AMs.col(2); AA.col(1) = AMs.col(6); AA.col(2) = zi[0] * AMs.col(4) + AMs.col(5); AA.col(3) = AMs.col(1) + zi[0] * AMs.col(3); AA.col(4) = AMs.col(14); AA.col(5) = zi[0] * AMs.col(11) + AMs.col(13); AA.col(6) = zi[1] * AMs.col(9) + zi[0] * AMs.col(10) + AMs.col(12); AA.col(7) = AMs.col(0) + zi[0] * AMs.col(7) + zi[1] * AMs.col(8); AA(0, 0) = AA(0, 0) - zi[0]; AA(3, 1) = AA(3, 1) - zi[0]; AA(2, 2) = AA(2, 2) - zi[1]; AA(1, 3) = AA(1, 3) - zi[1]; AA(7, 4) = AA(7, 4) - zi[0]; AA(6, 5) = AA(6, 5) - zi[1]; AA(5, 6) = AA(5, 6) - zi[2]; AA(4, 7) = AA(4, 7) - zi[2]; // Using column pivoting leads to unstable numerics // Eigen::Matrix s = AA.leftCols(7).colPivHouseholderQr().solve(-AA.col(7)); Eigen::Matrix s = AA.block<7, 7>(0, 0).householderQr().solve(-AA.block<7, 1>(0, 7)); sols(0, i) = s(3); sols(1, i) = zi[0]; sols(2, i) = s(6); } } int solver_shared_focal_relpose_6pt(const Eigen::VectorXd &data, Eigen::Matrix, 3, 15> &sols) { // Compute coefficients const double *d = data.data(); Eigen::VectorXd coeffs(280); coeffs[0] = 2 * d[11] * d[15] * d[17] - d[9] * std::pow(d[17], 2); coeffs[1] = -std::pow(d[17], 2) * d[18] + 2 * d[15] * d[17] * d[20] + 2 * d[11] * d[17] * d[24] + 2 * d[11] * d[15] * d[26] - 2 * d[9] * d[17] * d[26]; coeffs[2] = 2 * d[17] * d[20] * d[24] - 2 * d[17] * d[18] * d[26] + 2 * d[15] * d[20] * d[26] + 2 * d[11] * d[24] * d[26] - d[9] * std::pow(d[26], 2); coeffs[3] = 2 * d[20] * d[24] * d[26] - d[18] * std::pow(d[26], 2); coeffs[4] = d[9] * std::pow(d[11], 2) + 2 * d[11] * d[12] * d[14] - d[9] * std::pow(d[14], 2) + d[9] * std::pow(d[15], 2) + 2 * d[10] * d[15] * d[16] - d[9] * std::pow(d[16], 2); coeffs[5] = std::pow(d[11], 2) * d[18] - std::pow(d[14], 2) * d[18] + std::pow(d[15], 2) * d[18] - std::pow(d[16], 2) * d[18] + 2 * d[15] * d[16] * d[19] + 2 * d[9] * d[11] * d[20] + 2 * d[12] * d[14] * d[20] + 2 * d[11] * d[14] * d[21] + 2 * d[11] * d[12] * d[23] - 2 * d[9] * d[14] * d[23] + 2 * d[9] * d[15] * d[24] + 2 * d[10] * d[16] * d[24] + 2 * d[10] * d[15] * d[25] - 2 * d[9] * d[16] * d[25]; coeffs[6] = 2 * d[11] * d[18] * d[20] + d[9] * std::pow(d[20], 2) + 2 * d[14] * d[20] * d[21] - 2 * d[14] * d[18] * d[23] + 2 * d[12] * d[20] * d[23] + 2 * d[11] * d[21] * d[23] - d[9] * std::pow(d[23], 2) + 2 * d[15] * d[18] * d[24] + 2 * d[16] * d[19] * d[24] + d[9] * std::pow(d[24], 2) - 2 * d[16] * d[18] * d[25] + 2 * d[15] * d[19] * d[25] + 2 * d[10] * d[24] * d[25] - d[9] * std::pow(d[25], 2); coeffs[7] = d[18] * std::pow(d[20], 2) + 2 * d[20] * d[21] * d[23] - d[18] * std::pow(d[23], 2) + d[18] * std::pow(d[24], 2) + 2 * d[19] * d[24] * d[25] - d[18] * std::pow(d[25], 2); coeffs[8] = 2 * d[8] * d[11] * d[15] - 2 * d[8] * d[9] * d[17] + 2 * d[6] * d[11] * d[17] + 2 * d[2] * d[15] * d[17] - d[0] * std::pow(d[17], 2); coeffs[9] = -2 * d[8] * d[17] * d[18] + 2 * d[8] * d[15] * d[20] + 2 * d[6] * d[17] * d[20] + 2 * d[8] * d[11] * d[24] + 2 * d[2] * d[17] * d[24] - 2 * d[8] * d[9] * d[26] + 2 * d[6] * d[11] * d[26] + 2 * d[2] * d[15] * d[26] - 2 * d[0] * d[17] * d[26]; coeffs[10] = 2 * d[8] * d[20] * d[24] - 2 * d[8] * d[18] * d[26] + 2 * d[6] * d[20] * d[26] + 2 * d[2] * d[24] * d[26] - d[0] * std::pow(d[26], 2); coeffs[11] = std::pow(d[9], 3) + d[9] * std::pow(d[10], 2) + d[9] * std::pow(d[12], 2) + 2 * d[10] * d[12] * d[13] - d[9] * std::pow(d[13], 2); coeffs[12] = 3 * std::pow(d[9], 2) * d[18] + std::pow(d[10], 2) * d[18] + std::pow(d[12], 2) * d[18] - std::pow(d[13], 2) * d[18] + 2 * d[9] * d[10] * d[19] + 2 * d[12] * d[13] * d[19] + 2 * d[9] * d[12] * d[21] + 2 * d[10] * d[13] * d[21] + 2 * d[10] * d[12] * d[22] - 2 * d[9] * d[13] * d[22]; coeffs[13] = 3 * d[9] * std::pow(d[18], 2) + 2 * d[10] * d[18] * d[19] + d[9] * std::pow(d[19], 2) + 2 * d[12] * d[18] * d[21] + 2 * d[13] * d[19] * d[21] + d[9] * std::pow(d[21], 2) - 2 * d[13] * d[18] * d[22] + 2 * d[12] * d[19] * d[22] + 2 * d[10] * d[21] * d[22] - d[9] * std::pow(d[22], 2); coeffs[14] = std::pow(d[18], 3) + d[18] * std::pow(d[19], 2) + d[18] * std::pow(d[21], 2) + 2 * d[19] * d[21] * d[22] - d[18] * std::pow(d[22], 2); coeffs[15] = 2 * d[2] * d[9] * d[11] + d[0] * std::pow(d[11], 2) + 2 * d[5] * d[11] * d[12] - 2 * d[5] * d[9] * d[14] + 2 * d[3] * d[11] * d[14] + 2 * d[2] * d[12] * d[14] - d[0] * std::pow(d[14], 2) + 2 * d[6] * d[9] * d[15] + 2 * d[7] * d[10] * d[15] + d[0] * std::pow(d[15], 2) - 2 * d[7] * d[9] * d[16] + 2 * d[6] * d[10] * d[16] + 2 * d[1] * d[15] * d[16] - d[0] * std::pow(d[16], 2); coeffs[16] = 2 * d[2] * d[11] * d[18] - 2 * d[5] * d[14] * d[18] + 2 * d[6] * d[15] * d[18] - 2 * d[7] * d[16] * d[18] + 2 * d[7] * d[15] * d[19] + 2 * d[6] * d[16] * d[19] + 2 * d[2] * d[9] * d[20] + 2 * d[0] * d[11] * d[20] + 2 * d[5] * d[12] * d[20] + 2 * d[3] * d[14] * d[20] + 2 * d[5] * d[11] * d[21] + 2 * d[2] * d[14] * d[21] - 2 * d[5] * d[9] * d[23] + 2 * d[3] * d[11] * d[23] + 2 * d[2] * d[12] * d[23] - 2 * d[0] * d[14] * d[23] + 2 * d[6] * d[9] * d[24] + 2 * d[7] * d[10] * d[24] + 2 * d[0] * d[15] * d[24] + 2 * d[1] * d[16] * d[24] - 2 * d[7] * d[9] * d[25] + 2 * d[6] * d[10] * d[25] + 2 * d[1] * d[15] * d[25] - 2 * d[0] * d[16] * d[25]; coeffs[17] = 2 * d[2] * d[18] * d[20] + d[0] * std::pow(d[20], 2) + 2 * d[5] * d[20] * d[21] - 2 * d[5] * d[18] * d[23] + 2 * d[3] * d[20] * d[23] + 2 * d[2] * d[21] * d[23] - d[0] * std::pow(d[23], 2) + 2 * d[6] * d[18] * d[24] + 2 * d[7] * d[19] * d[24] + d[0] * std::pow(d[24], 2) - 2 * d[7] * d[18] * d[25] + 2 * d[6] * d[19] * d[25] + 2 * d[1] * d[24] * d[25] - d[0] * std::pow(d[25], 2); coeffs[18] = -std::pow(d[8], 2) * d[9] + 2 * d[6] * d[8] * d[11] + 2 * d[2] * d[8] * d[15] + 2 * d[2] * d[6] * d[17] - 2 * d[0] * d[8] * d[17]; coeffs[19] = -std::pow(d[8], 2) * d[18] + 2 * d[6] * d[8] * d[20] + 2 * d[2] * d[8] * d[24] + 2 * d[2] * d[6] * d[26] - 2 * d[0] * d[8] * d[26]; coeffs[20] = 3 * d[0] * std::pow(d[9], 2) + 2 * d[1] * d[9] * d[10] + d[0] * std::pow(d[10], 2) + 2 * d[3] * d[9] * d[12] + 2 * d[4] * d[10] * d[12] + d[0] * std::pow(d[12], 2) - 2 * d[4] * d[9] * d[13] + 2 * d[3] * d[10] * d[13] + 2 * d[1] * d[12] * d[13] - d[0] * std::pow(d[13], 2); coeffs[21] = 6 * d[0] * d[9] * d[18] + 2 * d[1] * d[10] * d[18] + 2 * d[3] * d[12] * d[18] - 2 * d[4] * d[13] * d[18] + 2 * d[1] * d[9] * d[19] + 2 * d[0] * d[10] * d[19] + 2 * d[4] * d[12] * d[19] + 2 * d[3] * d[13] * d[19] + 2 * d[3] * d[9] * d[21] + 2 * d[4] * d[10] * d[21] + 2 * d[0] * d[12] * d[21] + 2 * d[1] * d[13] * d[21] - 2 * d[4] * d[9] * d[22] + 2 * d[3] * d[10] * d[22] + 2 * d[1] * d[12] * d[22] - 2 * d[0] * d[13] * d[22]; coeffs[22] = 3 * d[0] * std::pow(d[18], 2) + 2 * d[1] * d[18] * d[19] + d[0] * std::pow(d[19], 2) + 2 * d[3] * d[18] * d[21] + 2 * d[4] * d[19] * d[21] + d[0] * std::pow(d[21], 2) - 2 * d[4] * d[18] * d[22] + 2 * d[3] * d[19] * d[22] + 2 * d[1] * d[21] * d[22] - d[0] * std::pow(d[22], 2); coeffs[23] = std::pow(d[2], 2) * d[9] - std::pow(d[5], 2) * d[9] + std::pow(d[6], 2) * d[9] - std::pow(d[7], 2) * d[9] + 2 * d[6] * d[7] * d[10] + 2 * d[0] * d[2] * d[11] + 2 * d[3] * d[5] * d[11] + 2 * d[2] * d[5] * d[12] + 2 * d[2] * d[3] * d[14] - 2 * d[0] * d[5] * d[14] + 2 * d[0] * d[6] * d[15] + 2 * d[1] * d[7] * d[15] + 2 * d[1] * d[6] * d[16] - 2 * d[0] * d[7] * d[16]; coeffs[24] = std::pow(d[2], 2) * d[18] - std::pow(d[5], 2) * d[18] + std::pow(d[6], 2) * d[18] - std::pow(d[7], 2) * d[18] + 2 * d[6] * d[7] * d[19] + 2 * d[0] * d[2] * d[20] + 2 * d[3] * d[5] * d[20] + 2 * d[2] * d[5] * d[21] + 2 * d[2] * d[3] * d[23] - 2 * d[0] * d[5] * d[23] + 2 * d[0] * d[6] * d[24] + 2 * d[1] * d[7] * d[24] + 2 * d[1] * d[6] * d[25] - 2 * d[0] * d[7] * d[25]; coeffs[25] = 2 * d[2] * d[6] * d[8] - d[0] * std::pow(d[8], 2); coeffs[26] = 3 * std::pow(d[0], 2) * d[9] + std::pow(d[1], 2) * d[9] + std::pow(d[3], 2) * d[9] - std::pow(d[4], 2) * d[9] + 2 * d[0] * d[1] * d[10] + 2 * d[3] * d[4] * d[10] + 2 * d[0] * d[3] * d[12] + 2 * d[1] * d[4] * d[12] + 2 * d[1] * d[3] * d[13] - 2 * d[0] * d[4] * d[13]; coeffs[27] = 3 * std::pow(d[0], 2) * d[18] + std::pow(d[1], 2) * d[18] + std::pow(d[3], 2) * d[18] - std::pow(d[4], 2) * d[18] + 2 * d[0] * d[1] * d[19] + 2 * d[3] * d[4] * d[19] + 2 * d[0] * d[3] * d[21] + 2 * d[1] * d[4] * d[21] + 2 * d[1] * d[3] * d[22] - 2 * d[0] * d[4] * d[22]; coeffs[28] = d[0] * std::pow(d[2], 2) + 2 * d[2] * d[3] * d[5] - d[0] * std::pow(d[5], 2) + d[0] * std::pow(d[6], 2) + 2 * d[1] * d[6] * d[7] - d[0] * std::pow(d[7], 2); coeffs[29] = std::pow(d[0], 3) + d[0] * std::pow(d[1], 2) + d[0] * std::pow(d[3], 2) + 2 * d[1] * d[3] * d[4] - d[0] * std::pow(d[4], 2); coeffs[30] = 2 * d[11] * d[16] * d[17] - d[10] * std::pow(d[17], 2); coeffs[31] = -std::pow(d[17], 2) * d[19] + 2 * d[16] * d[17] * d[20] + 2 * d[11] * d[17] * d[25] + 2 * d[11] * d[16] * d[26] - 2 * d[10] * d[17] * d[26]; coeffs[32] = 2 * d[17] * d[20] * d[25] - 2 * d[17] * d[19] * d[26] + 2 * d[16] * d[20] * d[26] + 2 * d[11] * d[25] * d[26] - d[10] * std::pow(d[26], 2); coeffs[33] = 2 * d[20] * d[25] * d[26] - d[19] * std::pow(d[26], 2); coeffs[34] = d[10] * std::pow(d[11], 2) + 2 * d[11] * d[13] * d[14] - d[10] * std::pow(d[14], 2) - d[10] * std::pow(d[15], 2) + 2 * d[9] * d[15] * d[16] + d[10] * std::pow(d[16], 2); coeffs[35] = 2 * d[15] * d[16] * d[18] + std::pow(d[11], 2) * d[19] - std::pow(d[14], 2) * d[19] - std::pow(d[15], 2) * d[19] + std::pow(d[16], 2) * d[19] + 2 * d[10] * d[11] * d[20] + 2 * d[13] * d[14] * d[20] + 2 * d[11] * d[14] * d[22] + 2 * d[11] * d[13] * d[23] - 2 * d[10] * d[14] * d[23] - 2 * d[10] * d[15] * d[24] + 2 * d[9] * d[16] * d[24] + 2 * d[9] * d[15] * d[25] + 2 * d[10] * d[16] * d[25]; coeffs[36] = 2 * d[11] * d[19] * d[20] + d[10] * std::pow(d[20], 2) + 2 * d[14] * d[20] * d[22] - 2 * d[14] * d[19] * d[23] + 2 * d[13] * d[20] * d[23] + 2 * d[11] * d[22] * d[23] - d[10] * std::pow(d[23], 2) + 2 * d[16] * d[18] * d[24] - 2 * d[15] * d[19] * d[24] - d[10] * std::pow(d[24], 2) + 2 * d[15] * d[18] * d[25] + 2 * d[16] * d[19] * d[25] + 2 * d[9] * d[24] * d[25] + d[10] * std::pow(d[25], 2); coeffs[37] = d[19] * std::pow(d[20], 2) + 2 * d[20] * d[22] * d[23] - d[19] * std::pow(d[23], 2) - d[19] * std::pow(d[24], 2) + 2 * d[18] * d[24] * d[25] + d[19] * std::pow(d[25], 2); coeffs[38] = 2 * d[8] * d[11] * d[16] - 2 * d[8] * d[10] * d[17] + 2 * d[7] * d[11] * d[17] + 2 * d[2] * d[16] * d[17] - d[1] * std::pow(d[17], 2); coeffs[39] = -2 * d[8] * d[17] * d[19] + 2 * d[8] * d[16] * d[20] + 2 * d[7] * d[17] * d[20] + 2 * d[8] * d[11] * d[25] + 2 * d[2] * d[17] * d[25] - 2 * d[8] * d[10] * d[26] + 2 * d[7] * d[11] * d[26] + 2 * d[2] * d[16] * d[26] - 2 * d[1] * d[17] * d[26]; coeffs[40] = 2 * d[8] * d[20] * d[25] - 2 * d[8] * d[19] * d[26] + 2 * d[7] * d[20] * d[26] + 2 * d[2] * d[25] * d[26] - d[1] * std::pow(d[26], 2); coeffs[41] = std::pow(d[9], 2) * d[10] + std::pow(d[10], 3) - d[10] * std::pow(d[12], 2) + 2 * d[9] * d[12] * d[13] + d[10] * std::pow(d[13], 2); coeffs[42] = 2 * d[9] * d[10] * d[18] + 2 * d[12] * d[13] * d[18] + std::pow(d[9], 2) * d[19] + 3 * std::pow(d[10], 2) * d[19] - std::pow(d[12], 2) * d[19] + std::pow(d[13], 2) * d[19] - 2 * d[10] * d[12] * d[21] + 2 * d[9] * d[13] * d[21] + 2 * d[9] * d[12] * d[22] + 2 * d[10] * d[13] * d[22]; coeffs[43] = d[10] * std::pow(d[18], 2) + 2 * d[9] * d[18] * d[19] + 3 * d[10] * std::pow(d[19], 2) + 2 * d[13] * d[18] * d[21] - 2 * d[12] * d[19] * d[21] - d[10] * std::pow(d[21], 2) + 2 * d[12] * d[18] * d[22] + 2 * d[13] * d[19] * d[22] + 2 * d[9] * d[21] * d[22] + d[10] * std::pow(d[22], 2); coeffs[44] = std::pow(d[18], 2) * d[19] + std::pow(d[19], 3) - d[19] * std::pow(d[21], 2) + 2 * d[18] * d[21] * d[22] + d[19] * std::pow(d[22], 2); coeffs[45] = 2 * d[2] * d[10] * d[11] + d[1] * std::pow(d[11], 2) + 2 * d[5] * d[11] * d[13] - 2 * d[5] * d[10] * d[14] + 2 * d[4] * d[11] * d[14] + 2 * d[2] * d[13] * d[14] - d[1] * std::pow(d[14], 2) + 2 * d[7] * d[9] * d[15] - 2 * d[6] * d[10] * d[15] - d[1] * std::pow(d[15], 2) + 2 * d[6] * d[9] * d[16] + 2 * d[7] * d[10] * d[16] + 2 * d[0] * d[15] * d[16] + d[1] * std::pow(d[16], 2); coeffs[46] = 2 * d[7] * d[15] * d[18] + 2 * d[6] * d[16] * d[18] + 2 * d[2] * d[11] * d[19] - 2 * d[5] * d[14] * d[19] - 2 * d[6] * d[15] * d[19] + 2 * d[7] * d[16] * d[19] + 2 * d[2] * d[10] * d[20] + 2 * d[1] * d[11] * d[20] + 2 * d[5] * d[13] * d[20] + 2 * d[4] * d[14] * d[20] + 2 * d[5] * d[11] * d[22] + 2 * d[2] * d[14] * d[22] - 2 * d[5] * d[10] * d[23] + 2 * d[4] * d[11] * d[23] + 2 * d[2] * d[13] * d[23] - 2 * d[1] * d[14] * d[23] + 2 * d[7] * d[9] * d[24] - 2 * d[6] * d[10] * d[24] - 2 * d[1] * d[15] * d[24] + 2 * d[0] * d[16] * d[24] + 2 * d[6] * d[9] * d[25] + 2 * d[7] * d[10] * d[25] + 2 * d[0] * d[15] * d[25] + 2 * d[1] * d[16] * d[25]; coeffs[47] = 2 * d[2] * d[19] * d[20] + d[1] * std::pow(d[20], 2) + 2 * d[5] * d[20] * d[22] - 2 * d[5] * d[19] * d[23] + 2 * d[4] * d[20] * d[23] + 2 * d[2] * d[22] * d[23] - d[1] * std::pow(d[23], 2) + 2 * d[7] * d[18] * d[24] - 2 * d[6] * d[19] * d[24] - d[1] * std::pow(d[24], 2) + 2 * d[6] * d[18] * d[25] + 2 * d[7] * d[19] * d[25] + 2 * d[0] * d[24] * d[25] + d[1] * std::pow(d[25], 2); coeffs[48] = -std::pow(d[8], 2) * d[10] + 2 * d[7] * d[8] * d[11] + 2 * d[2] * d[8] * d[16] + 2 * d[2] * d[7] * d[17] - 2 * d[1] * d[8] * d[17]; coeffs[49] = -std::pow(d[8], 2) * d[19] + 2 * d[7] * d[8] * d[20] + 2 * d[2] * d[8] * d[25] + 2 * d[2] * d[7] * d[26] - 2 * d[1] * d[8] * d[26]; coeffs[50] = d[1] * std::pow(d[9], 2) + 2 * d[0] * d[9] * d[10] + 3 * d[1] * std::pow(d[10], 2) + 2 * d[4] * d[9] * d[12] - 2 * d[3] * d[10] * d[12] - d[1] * std::pow(d[12], 2) + 2 * d[3] * d[9] * d[13] + 2 * d[4] * d[10] * d[13] + 2 * d[0] * d[12] * d[13] + d[1] * std::pow(d[13], 2); coeffs[51] = 2 * d[1] * d[9] * d[18] + 2 * d[0] * d[10] * d[18] + 2 * d[4] * d[12] * d[18] + 2 * d[3] * d[13] * d[18] + 2 * d[0] * d[9] * d[19] + 6 * d[1] * d[10] * d[19] - 2 * d[3] * d[12] * d[19] + 2 * d[4] * d[13] * d[19] + 2 * d[4] * d[9] * d[21] - 2 * d[3] * d[10] * d[21] - 2 * d[1] * d[12] * d[21] + 2 * d[0] * d[13] * d[21] + 2 * d[3] * d[9] * d[22] + 2 * d[4] * d[10] * d[22] + 2 * d[0] * d[12] * d[22] + 2 * d[1] * d[13] * d[22]; coeffs[52] = d[1] * std::pow(d[18], 2) + 2 * d[0] * d[18] * d[19] + 3 * d[1] * std::pow(d[19], 2) + 2 * d[4] * d[18] * d[21] - 2 * d[3] * d[19] * d[21] - d[1] * std::pow(d[21], 2) + 2 * d[3] * d[18] * d[22] + 2 * d[4] * d[19] * d[22] + 2 * d[0] * d[21] * d[22] + d[1] * std::pow(d[22], 2); coeffs[53] = 2 * d[6] * d[7] * d[9] + std::pow(d[2], 2) * d[10] - std::pow(d[5], 2) * d[10] - std::pow(d[6], 2) * d[10] + std::pow(d[7], 2) * d[10] + 2 * d[1] * d[2] * d[11] + 2 * d[4] * d[5] * d[11] + 2 * d[2] * d[5] * d[13] + 2 * d[2] * d[4] * d[14] - 2 * d[1] * d[5] * d[14] - 2 * d[1] * d[6] * d[15] + 2 * d[0] * d[7] * d[15] + 2 * d[0] * d[6] * d[16] + 2 * d[1] * d[7] * d[16]; coeffs[54] = 2 * d[6] * d[7] * d[18] + std::pow(d[2], 2) * d[19] - std::pow(d[5], 2) * d[19] - std::pow(d[6], 2) * d[19] + std::pow(d[7], 2) * d[19] + 2 * d[1] * d[2] * d[20] + 2 * d[4] * d[5] * d[20] + 2 * d[2] * d[5] * d[22] + 2 * d[2] * d[4] * d[23] - 2 * d[1] * d[5] * d[23] - 2 * d[1] * d[6] * d[24] + 2 * d[0] * d[7] * d[24] + 2 * d[0] * d[6] * d[25] + 2 * d[1] * d[7] * d[25]; coeffs[55] = 2 * d[2] * d[7] * d[8] - d[1] * std::pow(d[8], 2); coeffs[56] = 2 * d[0] * d[1] * d[9] + 2 * d[3] * d[4] * d[9] + std::pow(d[0], 2) * d[10] + 3 * std::pow(d[1], 2) * d[10] - std::pow(d[3], 2) * d[10] + std::pow(d[4], 2) * d[10] - 2 * d[1] * d[3] * d[12] + 2 * d[0] * d[4] * d[12] + 2 * d[0] * d[3] * d[13] + 2 * d[1] * d[4] * d[13]; coeffs[57] = 2 * d[0] * d[1] * d[18] + 2 * d[3] * d[4] * d[18] + std::pow(d[0], 2) * d[19] + 3 * std::pow(d[1], 2) * d[19] - std::pow(d[3], 2) * d[19] + std::pow(d[4], 2) * d[19] - 2 * d[1] * d[3] * d[21] + 2 * d[0] * d[4] * d[21] + 2 * d[0] * d[3] * d[22] + 2 * d[1] * d[4] * d[22]; coeffs[58] = d[1] * std::pow(d[2], 2) + 2 * d[2] * d[4] * d[5] - d[1] * std::pow(d[5], 2) - d[1] * std::pow(d[6], 2) + 2 * d[0] * d[6] * d[7] + d[1] * std::pow(d[7], 2); coeffs[59] = std::pow(d[0], 2) * d[1] + std::pow(d[1], 3) - d[1] * std::pow(d[3], 2) + 2 * d[0] * d[3] * d[4] + d[1] * std::pow(d[4], 2); coeffs[60] = d[11] * std::pow(d[17], 2); coeffs[61] = std::pow(d[17], 2) * d[20] + 2 * d[11] * d[17] * d[26]; coeffs[62] = 2 * d[17] * d[20] * d[26] + d[11] * std::pow(d[26], 2); coeffs[63] = d[20] * std::pow(d[26], 2); coeffs[64] = std::pow(d[11], 3) + d[11] * std::pow(d[14], 2) - d[11] * std::pow(d[15], 2) - d[11] * std::pow(d[16], 2) + 2 * d[9] * d[15] * d[17] + 2 * d[10] * d[16] * d[17]; coeffs[65] = 2 * d[15] * d[17] * d[18] + 2 * d[16] * d[17] * d[19] + 3 * std::pow(d[11], 2) * d[20] + std::pow(d[14], 2) * d[20] - std::pow(d[15], 2) * d[20] - std::pow(d[16], 2) * d[20] + 2 * d[11] * d[14] * d[23] - 2 * d[11] * d[15] * d[24] + 2 * d[9] * d[17] * d[24] - 2 * d[11] * d[16] * d[25] + 2 * d[10] * d[17] * d[25] + 2 * d[9] * d[15] * d[26] + 2 * d[10] * d[16] * d[26]; coeffs[66] = 3 * d[11] * std::pow(d[20], 2) + 2 * d[14] * d[20] * d[23] + d[11] * std::pow(d[23], 2) + 2 * d[17] * d[18] * d[24] - 2 * d[15] * d[20] * d[24] - d[11] * std::pow(d[24], 2) + 2 * d[17] * d[19] * d[25] - 2 * d[16] * d[20] * d[25] - d[11] * std::pow(d[25], 2) + 2 * d[15] * d[18] * d[26] + 2 * d[16] * d[19] * d[26] + 2 * d[9] * d[24] * d[26] + 2 * d[10] * d[25] * d[26]; coeffs[67] = std::pow(d[20], 3) + d[20] * std::pow(d[23], 2) - d[20] * std::pow(d[24], 2) - d[20] * std::pow(d[25], 2) + 2 * d[18] * d[24] * d[26] + 2 * d[19] * d[25] * d[26]; coeffs[68] = 2 * d[8] * d[11] * d[17] + d[2] * std::pow(d[17], 2); coeffs[69] = 2 * d[8] * d[17] * d[20] + 2 * d[8] * d[11] * d[26] + 2 * d[2] * d[17] * d[26]; coeffs[70] = 2 * d[8] * d[20] * d[26] + d[2] * std::pow(d[26], 2); coeffs[71] = std::pow(d[9], 2) * d[11] + std::pow(d[10], 2) * d[11] - d[11] * std::pow(d[12], 2) - d[11] * std::pow(d[13], 2) + 2 * d[9] * d[12] * d[14] + 2 * d[10] * d[13] * d[14]; coeffs[72] = 2 * d[9] * d[11] * d[18] + 2 * d[12] * d[14] * d[18] + 2 * d[10] * d[11] * d[19] + 2 * d[13] * d[14] * d[19] + std::pow(d[9], 2) * d[20] + std::pow(d[10], 2) * d[20] - std::pow(d[12], 2) * d[20] - std::pow(d[13], 2) * d[20] - 2 * d[11] * d[12] * d[21] + 2 * d[9] * d[14] * d[21] - 2 * d[11] * d[13] * d[22] + 2 * d[10] * d[14] * d[22] + 2 * d[9] * d[12] * d[23] + 2 * d[10] * d[13] * d[23]; coeffs[73] = d[11] * std::pow(d[18], 2) + d[11] * std::pow(d[19], 2) + 2 * d[9] * d[18] * d[20] + 2 * d[10] * d[19] * d[20] + 2 * d[14] * d[18] * d[21] - 2 * d[12] * d[20] * d[21] - d[11] * std::pow(d[21], 2) + 2 * d[14] * d[19] * d[22] - 2 * d[13] * d[20] * d[22] - d[11] * std::pow(d[22], 2) + 2 * d[12] * d[18] * d[23] + 2 * d[13] * d[19] * d[23] + 2 * d[9] * d[21] * d[23] + 2 * d[10] * d[22] * d[23]; coeffs[74] = std::pow(d[18], 2) * d[20] + std::pow(d[19], 2) * d[20] - d[20] * std::pow(d[21], 2) - d[20] * std::pow(d[22], 2) + 2 * d[18] * d[21] * d[23] + 2 * d[19] * d[22] * d[23]; coeffs[75] = 3 * d[2] * std::pow(d[11], 2) + 2 * d[5] * d[11] * d[14] + d[2] * std::pow(d[14], 2) + 2 * d[8] * d[9] * d[15] - 2 * d[6] * d[11] * d[15] - d[2] * std::pow(d[15], 2) + 2 * d[8] * d[10] * d[16] - 2 * d[7] * d[11] * d[16] - d[2] * std::pow(d[16], 2) + 2 * d[6] * d[9] * d[17] + 2 * d[7] * d[10] * d[17] + 2 * d[0] * d[15] * d[17] + 2 * d[1] * d[16] * d[17]; coeffs[76] = 2 * d[8] * d[15] * d[18] + 2 * d[6] * d[17] * d[18] + 2 * d[8] * d[16] * d[19] + 2 * d[7] * d[17] * d[19] + 6 * d[2] * d[11] * d[20] + 2 * d[5] * d[14] * d[20] - 2 * d[6] * d[15] * d[20] - 2 * d[7] * d[16] * d[20] + 2 * d[5] * d[11] * d[23] + 2 * d[2] * d[14] * d[23] + 2 * d[8] * d[9] * d[24] - 2 * d[6] * d[11] * d[24] - 2 * d[2] * d[15] * d[24] + 2 * d[0] * d[17] * d[24] + 2 * d[8] * d[10] * d[25] - 2 * d[7] * d[11] * d[25] - 2 * d[2] * d[16] * d[25] + 2 * d[1] * d[17] * d[25] + 2 * d[6] * d[9] * d[26] + 2 * d[7] * d[10] * d[26] + 2 * d[0] * d[15] * d[26] + 2 * d[1] * d[16] * d[26]; coeffs[77] = 3 * d[2] * std::pow(d[20], 2) + 2 * d[5] * d[20] * d[23] + d[2] * std::pow(d[23], 2) + 2 * d[8] * d[18] * d[24] - 2 * d[6] * d[20] * d[24] - d[2] * std::pow(d[24], 2) + 2 * d[8] * d[19] * d[25] - 2 * d[7] * d[20] * d[25] - d[2] * std::pow(d[25], 2) + 2 * d[6] * d[18] * d[26] + 2 * d[7] * d[19] * d[26] + 2 * d[0] * d[24] * d[26] + 2 * d[1] * d[25] * d[26]; coeffs[78] = std::pow(d[8], 2) * d[11] + 2 * d[2] * d[8] * d[17]; coeffs[79] = std::pow(d[8], 2) * d[20] + 2 * d[2] * d[8] * d[26]; coeffs[80] = d[2] * std::pow(d[9], 2) + d[2] * std::pow(d[10], 2) + 2 * d[0] * d[9] * d[11] + 2 * d[1] * d[10] * d[11] + 2 * d[5] * d[9] * d[12] - 2 * d[3] * d[11] * d[12] - d[2] * std::pow(d[12], 2) + 2 * d[5] * d[10] * d[13] - 2 * d[4] * d[11] * d[13] - d[2] * std::pow(d[13], 2) + 2 * d[3] * d[9] * d[14] + 2 * d[4] * d[10] * d[14] + 2 * d[0] * d[12] * d[14] + 2 * d[1] * d[13] * d[14]; coeffs[81] = 2 * d[2] * d[9] * d[18] + 2 * d[0] * d[11] * d[18] + 2 * d[5] * d[12] * d[18] + 2 * d[3] * d[14] * d[18] + 2 * d[2] * d[10] * d[19] + 2 * d[1] * d[11] * d[19] + 2 * d[5] * d[13] * d[19] + 2 * d[4] * d[14] * d[19] + 2 * d[0] * d[9] * d[20] + 2 * d[1] * d[10] * d[20] - 2 * d[3] * d[12] * d[20] - 2 * d[4] * d[13] * d[20] + 2 * d[5] * d[9] * d[21] - 2 * d[3] * d[11] * d[21] - 2 * d[2] * d[12] * d[21] + 2 * d[0] * d[14] * d[21] + 2 * d[5] * d[10] * d[22] - 2 * d[4] * d[11] * d[22] - 2 * d[2] * d[13] * d[22] + 2 * d[1] * d[14] * d[22] + 2 * d[3] * d[9] * d[23] + 2 * d[4] * d[10] * d[23] + 2 * d[0] * d[12] * d[23] + 2 * d[1] * d[13] * d[23]; coeffs[82] = d[2] * std::pow(d[18], 2) + d[2] * std::pow(d[19], 2) + 2 * d[0] * d[18] * d[20] + 2 * d[1] * d[19] * d[20] + 2 * d[5] * d[18] * d[21] - 2 * d[3] * d[20] * d[21] - d[2] * std::pow(d[21], 2) + 2 * d[5] * d[19] * d[22] - 2 * d[4] * d[20] * d[22] - d[2] * std::pow(d[22], 2) + 2 * d[3] * d[18] * d[23] + 2 * d[4] * d[19] * d[23] + 2 * d[0] * d[21] * d[23] + 2 * d[1] * d[22] * d[23]; coeffs[83] = 2 * d[6] * d[8] * d[9] + 2 * d[7] * d[8] * d[10] + 3 * std::pow(d[2], 2) * d[11] + std::pow(d[5], 2) * d[11] - std::pow(d[6], 2) * d[11] - std::pow(d[7], 2) * d[11] + 2 * d[2] * d[5] * d[14] - 2 * d[2] * d[6] * d[15] + 2 * d[0] * d[8] * d[15] - 2 * d[2] * d[7] * d[16] + 2 * d[1] * d[8] * d[16] + 2 * d[0] * d[6] * d[17] + 2 * d[1] * d[7] * d[17]; coeffs[84] = 2 * d[6] * d[8] * d[18] + 2 * d[7] * d[8] * d[19] + 3 * std::pow(d[2], 2) * d[20] + std::pow(d[5], 2) * d[20] - std::pow(d[6], 2) * d[20] - std::pow(d[7], 2) * d[20] + 2 * d[2] * d[5] * d[23] - 2 * d[2] * d[6] * d[24] + 2 * d[0] * d[8] * d[24] - 2 * d[2] * d[7] * d[25] + 2 * d[1] * d[8] * d[25] + 2 * d[0] * d[6] * d[26] + 2 * d[1] * d[7] * d[26]; coeffs[85] = d[2] * std::pow(d[8], 2); coeffs[86] = 2 * d[0] * d[2] * d[9] + 2 * d[3] * d[5] * d[9] + 2 * d[1] * d[2] * d[10] + 2 * d[4] * d[5] * d[10] + std::pow(d[0], 2) * d[11] + std::pow(d[1], 2) * d[11] - std::pow(d[3], 2) * d[11] - std::pow(d[4], 2) * d[11] - 2 * d[2] * d[3] * d[12] + 2 * d[0] * d[5] * d[12] - 2 * d[2] * d[4] * d[13] + 2 * d[1] * d[5] * d[13] + 2 * d[0] * d[3] * d[14] + 2 * d[1] * d[4] * d[14]; coeffs[87] = 2 * d[0] * d[2] * d[18] + 2 * d[3] * d[5] * d[18] + 2 * d[1] * d[2] * d[19] + 2 * d[4] * d[5] * d[19] + std::pow(d[0], 2) * d[20] + std::pow(d[1], 2) * d[20] - std::pow(d[3], 2) * d[20] - std::pow(d[4], 2) * d[20] - 2 * d[2] * d[3] * d[21] + 2 * d[0] * d[5] * d[21] - 2 * d[2] * d[4] * d[22] + 2 * d[1] * d[5] * d[22] + 2 * d[0] * d[3] * d[23] + 2 * d[1] * d[4] * d[23]; coeffs[88] = std::pow(d[2], 3) + d[2] * std::pow(d[5], 2) - d[2] * std::pow(d[6], 2) - d[2] * std::pow(d[7], 2) + 2 * d[0] * d[6] * d[8] + 2 * d[1] * d[7] * d[8]; coeffs[89] = std::pow(d[0], 2) * d[2] + std::pow(d[1], 2) * d[2] - d[2] * std::pow(d[3], 2) - d[2] * std::pow(d[4], 2) + 2 * d[0] * d[3] * d[5] + 2 * d[1] * d[4] * d[5]; coeffs[90] = 2 * d[14] * d[15] * d[17] - d[12] * std::pow(d[17], 2); coeffs[91] = -std::pow(d[17], 2) * d[21] + 2 * d[15] * d[17] * d[23] + 2 * d[14] * d[17] * d[24] + 2 * d[14] * d[15] * d[26] - 2 * d[12] * d[17] * d[26]; coeffs[92] = 2 * d[17] * d[23] * d[24] - 2 * d[17] * d[21] * d[26] + 2 * d[15] * d[23] * d[26] + 2 * d[14] * d[24] * d[26] - d[12] * std::pow(d[26], 2); coeffs[93] = 2 * d[23] * d[24] * d[26] - d[21] * std::pow(d[26], 2); coeffs[94] = -std::pow(d[11], 2) * d[12] + 2 * d[9] * d[11] * d[14] + d[12] * std::pow(d[14], 2) + d[12] * std::pow(d[15], 2) + 2 * d[13] * d[15] * d[16] - d[12] * std::pow(d[16], 2); coeffs[95] = 2 * d[11] * d[14] * d[18] - 2 * d[11] * d[12] * d[20] + 2 * d[9] * d[14] * d[20] - std::pow(d[11], 2) * d[21] + std::pow(d[14], 2) * d[21] + std::pow(d[15], 2) * d[21] - std::pow(d[16], 2) * d[21] + 2 * d[15] * d[16] * d[22] + 2 * d[9] * d[11] * d[23] + 2 * d[12] * d[14] * d[23] + 2 * d[12] * d[15] * d[24] + 2 * d[13] * d[16] * d[24] + 2 * d[13] * d[15] * d[25] - 2 * d[12] * d[16] * d[25]; coeffs[96] = 2 * d[14] * d[18] * d[20] - d[12] * std::pow(d[20], 2) - 2 * d[11] * d[20] * d[21] + 2 * d[11] * d[18] * d[23] + 2 * d[9] * d[20] * d[23] + 2 * d[14] * d[21] * d[23] + d[12] * std::pow(d[23], 2) + 2 * d[15] * d[21] * d[24] + 2 * d[16] * d[22] * d[24] + d[12] * std::pow(d[24], 2) - 2 * d[16] * d[21] * d[25] + 2 * d[15] * d[22] * d[25] + 2 * d[13] * d[24] * d[25] - d[12] * std::pow(d[25], 2); coeffs[97] = -std::pow(d[20], 2) * d[21] + 2 * d[18] * d[20] * d[23] + d[21] * std::pow(d[23], 2) + d[21] * std::pow(d[24], 2) + 2 * d[22] * d[24] * d[25] - d[21] * std::pow(d[25], 2); coeffs[98] = 2 * d[8] * d[14] * d[15] - 2 * d[8] * d[12] * d[17] + 2 * d[6] * d[14] * d[17] + 2 * d[5] * d[15] * d[17] - d[3] * std::pow(d[17], 2); coeffs[99] = -2 * d[8] * d[17] * d[21] + 2 * d[8] * d[15] * d[23] + 2 * d[6] * d[17] * d[23] + 2 * d[8] * d[14] * d[24] + 2 * d[5] * d[17] * d[24] - 2 * d[8] * d[12] * d[26] + 2 * d[6] * d[14] * d[26] + 2 * d[5] * d[15] * d[26] - 2 * d[3] * d[17] * d[26]; coeffs[100] = 2 * d[8] * d[23] * d[24] - 2 * d[8] * d[21] * d[26] + 2 * d[6] * d[23] * d[26] + 2 * d[5] * d[24] * d[26] - d[3] * std::pow(d[26], 2); coeffs[101] = std::pow(d[9], 2) * d[12] - std::pow(d[10], 2) * d[12] + std::pow(d[12], 3) + 2 * d[9] * d[10] * d[13] + d[12] * std::pow(d[13], 2); coeffs[102] = 2 * d[9] * d[12] * d[18] + 2 * d[10] * d[13] * d[18] - 2 * d[10] * d[12] * d[19] + 2 * d[9] * d[13] * d[19] + std::pow(d[9], 2) * d[21] - std::pow(d[10], 2) * d[21] + 3 * std::pow(d[12], 2) * d[21] + std::pow(d[13], 2) * d[21] + 2 * d[9] * d[10] * d[22] + 2 * d[12] * d[13] * d[22]; coeffs[103] = d[12] * std::pow(d[18], 2) + 2 * d[13] * d[18] * d[19] - d[12] * std::pow(d[19], 2) + 2 * d[9] * d[18] * d[21] - 2 * d[10] * d[19] * d[21] + 3 * d[12] * std::pow(d[21], 2) + 2 * d[10] * d[18] * d[22] + 2 * d[9] * d[19] * d[22] + 2 * d[13] * d[21] * d[22] + d[12] * std::pow(d[22], 2); coeffs[104] = std::pow(d[18], 2) * d[21] - std::pow(d[19], 2) * d[21] + std::pow(d[21], 3) + 2 * d[18] * d[19] * d[22] + d[21] * std::pow(d[22], 2); coeffs[105] = 2 * d[5] * d[9] * d[11] - d[3] * std::pow(d[11], 2) - 2 * d[2] * d[11] * d[12] + 2 * d[2] * d[9] * d[14] + 2 * d[0] * d[11] * d[14] + 2 * d[5] * d[12] * d[14] + d[3] * std::pow(d[14], 2) + 2 * d[6] * d[12] * d[15] + 2 * d[7] * d[13] * d[15] + d[3] * std::pow(d[15], 2) - 2 * d[7] * d[12] * d[16] + 2 * d[6] * d[13] * d[16] + 2 * d[4] * d[15] * d[16] - d[3] * std::pow(d[16], 2); coeffs[106] = 2 * d[5] * d[11] * d[18] + 2 * d[2] * d[14] * d[18] + 2 * d[5] * d[9] * d[20] - 2 * d[3] * d[11] * d[20] - 2 * d[2] * d[12] * d[20] + 2 * d[0] * d[14] * d[20] - 2 * d[2] * d[11] * d[21] + 2 * d[5] * d[14] * d[21] + 2 * d[6] * d[15] * d[21] - 2 * d[7] * d[16] * d[21] + 2 * d[7] * d[15] * d[22] + 2 * d[6] * d[16] * d[22] + 2 * d[2] * d[9] * d[23] + 2 * d[0] * d[11] * d[23] + 2 * d[5] * d[12] * d[23] + 2 * d[3] * d[14] * d[23] + 2 * d[6] * d[12] * d[24] + 2 * d[7] * d[13] * d[24] + 2 * d[3] * d[15] * d[24] + 2 * d[4] * d[16] * d[24] - 2 * d[7] * d[12] * d[25] + 2 * d[6] * d[13] * d[25] + 2 * d[4] * d[15] * d[25] - 2 * d[3] * d[16] * d[25]; coeffs[107] = 2 * d[5] * d[18] * d[20] - d[3] * std::pow(d[20], 2) - 2 * d[2] * d[20] * d[21] + 2 * d[2] * d[18] * d[23] + 2 * d[0] * d[20] * d[23] + 2 * d[5] * d[21] * d[23] + d[3] * std::pow(d[23], 2) + 2 * d[6] * d[21] * d[24] + 2 * d[7] * d[22] * d[24] + d[3] * std::pow(d[24], 2) - 2 * d[7] * d[21] * d[25] + 2 * d[6] * d[22] * d[25] + 2 * d[4] * d[24] * d[25] - d[3] * std::pow(d[25], 2); coeffs[108] = -std::pow(d[8], 2) * d[12] + 2 * d[6] * d[8] * d[14] + 2 * d[5] * d[8] * d[15] + 2 * d[5] * d[6] * d[17] - 2 * d[3] * d[8] * d[17]; coeffs[109] = -std::pow(d[8], 2) * d[21] + 2 * d[6] * d[8] * d[23] + 2 * d[5] * d[8] * d[24] + 2 * d[5] * d[6] * d[26] - 2 * d[3] * d[8] * d[26]; coeffs[110] = d[3] * std::pow(d[9], 2) + 2 * d[4] * d[9] * d[10] - d[3] * std::pow(d[10], 2) + 2 * d[0] * d[9] * d[12] - 2 * d[1] * d[10] * d[12] + 3 * d[3] * std::pow(d[12], 2) + 2 * d[1] * d[9] * d[13] + 2 * d[0] * d[10] * d[13] + 2 * d[4] * d[12] * d[13] + d[3] * std::pow(d[13], 2); coeffs[111] = 2 * d[3] * d[9] * d[18] + 2 * d[4] * d[10] * d[18] + 2 * d[0] * d[12] * d[18] + 2 * d[1] * d[13] * d[18] + 2 * d[4] * d[9] * d[19] - 2 * d[3] * d[10] * d[19] - 2 * d[1] * d[12] * d[19] + 2 * d[0] * d[13] * d[19] + 2 * d[0] * d[9] * d[21] - 2 * d[1] * d[10] * d[21] + 6 * d[3] * d[12] * d[21] + 2 * d[4] * d[13] * d[21] + 2 * d[1] * d[9] * d[22] + 2 * d[0] * d[10] * d[22] + 2 * d[4] * d[12] * d[22] + 2 * d[3] * d[13] * d[22]; coeffs[112] = d[3] * std::pow(d[18], 2) + 2 * d[4] * d[18] * d[19] - d[3] * std::pow(d[19], 2) + 2 * d[0] * d[18] * d[21] - 2 * d[1] * d[19] * d[21] + 3 * d[3] * std::pow(d[21], 2) + 2 * d[1] * d[18] * d[22] + 2 * d[0] * d[19] * d[22] + 2 * d[4] * d[21] * d[22] + d[3] * std::pow(d[22], 2); coeffs[113] = 2 * d[2] * d[5] * d[9] - 2 * d[2] * d[3] * d[11] + 2 * d[0] * d[5] * d[11] - std::pow(d[2], 2) * d[12] + std::pow(d[5], 2) * d[12] + std::pow(d[6], 2) * d[12] - std::pow(d[7], 2) * d[12] + 2 * d[6] * d[7] * d[13] + 2 * d[0] * d[2] * d[14] + 2 * d[3] * d[5] * d[14] + 2 * d[3] * d[6] * d[15] + 2 * d[4] * d[7] * d[15] + 2 * d[4] * d[6] * d[16] - 2 * d[3] * d[7] * d[16]; coeffs[114] = 2 * d[2] * d[5] * d[18] - 2 * d[2] * d[3] * d[20] + 2 * d[0] * d[5] * d[20] - std::pow(d[2], 2) * d[21] + std::pow(d[5], 2) * d[21] + std::pow(d[6], 2) * d[21] - std::pow(d[7], 2) * d[21] + 2 * d[6] * d[7] * d[22] + 2 * d[0] * d[2] * d[23] + 2 * d[3] * d[5] * d[23] + 2 * d[3] * d[6] * d[24] + 2 * d[4] * d[7] * d[24] + 2 * d[4] * d[6] * d[25] - 2 * d[3] * d[7] * d[25]; coeffs[115] = 2 * d[5] * d[6] * d[8] - d[3] * std::pow(d[8], 2); coeffs[116] = 2 * d[0] * d[3] * d[9] + 2 * d[1] * d[4] * d[9] - 2 * d[1] * d[3] * d[10] + 2 * d[0] * d[4] * d[10] + std::pow(d[0], 2) * d[12] - std::pow(d[1], 2) * d[12] + 3 * std::pow(d[3], 2) * d[12] + std::pow(d[4], 2) * d[12] + 2 * d[0] * d[1] * d[13] + 2 * d[3] * d[4] * d[13]; coeffs[117] = 2 * d[0] * d[3] * d[18] + 2 * d[1] * d[4] * d[18] - 2 * d[1] * d[3] * d[19] + 2 * d[0] * d[4] * d[19] + std::pow(d[0], 2) * d[21] - std::pow(d[1], 2) * d[21] + 3 * std::pow(d[3], 2) * d[21] + std::pow(d[4], 2) * d[21] + 2 * d[0] * d[1] * d[22] + 2 * d[3] * d[4] * d[22]; coeffs[118] = -std::pow(d[2], 2) * d[3] + 2 * d[0] * d[2] * d[5] + d[3] * std::pow(d[5], 2) + d[3] * std::pow(d[6], 2) + 2 * d[4] * d[6] * d[7] - d[3] * std::pow(d[7], 2); coeffs[119] = std::pow(d[0], 2) * d[3] - std::pow(d[1], 2) * d[3] + std::pow(d[3], 3) + 2 * d[0] * d[1] * d[4] + d[3] * std::pow(d[4], 2); coeffs[120] = 2 * d[14] * d[16] * d[17] - d[13] * std::pow(d[17], 2); coeffs[121] = -std::pow(d[17], 2) * d[22] + 2 * d[16] * d[17] * d[23] + 2 * d[14] * d[17] * d[25] + 2 * d[14] * d[16] * d[26] - 2 * d[13] * d[17] * d[26]; coeffs[122] = 2 * d[17] * d[23] * d[25] - 2 * d[17] * d[22] * d[26] + 2 * d[16] * d[23] * d[26] + 2 * d[14] * d[25] * d[26] - d[13] * std::pow(d[26], 2); coeffs[123] = 2 * d[23] * d[25] * d[26] - d[22] * std::pow(d[26], 2); coeffs[124] = -std::pow(d[11], 2) * d[13] + 2 * d[10] * d[11] * d[14] + d[13] * std::pow(d[14], 2) - d[13] * std::pow(d[15], 2) + 2 * d[12] * d[15] * d[16] + d[13] * std::pow(d[16], 2); coeffs[125] = 2 * d[11] * d[14] * d[19] - 2 * d[11] * d[13] * d[20] + 2 * d[10] * d[14] * d[20] + 2 * d[15] * d[16] * d[21] - std::pow(d[11], 2) * d[22] + std::pow(d[14], 2) * d[22] - std::pow(d[15], 2) * d[22] + std::pow(d[16], 2) * d[22] + 2 * d[10] * d[11] * d[23] + 2 * d[13] * d[14] * d[23] - 2 * d[13] * d[15] * d[24] + 2 * d[12] * d[16] * d[24] + 2 * d[12] * d[15] * d[25] + 2 * d[13] * d[16] * d[25]; coeffs[126] = 2 * d[14] * d[19] * d[20] - d[13] * std::pow(d[20], 2) - 2 * d[11] * d[20] * d[22] + 2 * d[11] * d[19] * d[23] + 2 * d[10] * d[20] * d[23] + 2 * d[14] * d[22] * d[23] + d[13] * std::pow(d[23], 2) + 2 * d[16] * d[21] * d[24] - 2 * d[15] * d[22] * d[24] - d[13] * std::pow(d[24], 2) + 2 * d[15] * d[21] * d[25] + 2 * d[16] * d[22] * d[25] + 2 * d[12] * d[24] * d[25] + d[13] * std::pow(d[25], 2); coeffs[127] = -std::pow(d[20], 2) * d[22] + 2 * d[19] * d[20] * d[23] + d[22] * std::pow(d[23], 2) - d[22] * std::pow(d[24], 2) + 2 * d[21] * d[24] * d[25] + d[22] * std::pow(d[25], 2); coeffs[128] = 2 * d[8] * d[14] * d[16] - 2 * d[8] * d[13] * d[17] + 2 * d[7] * d[14] * d[17] + 2 * d[5] * d[16] * d[17] - d[4] * std::pow(d[17], 2); coeffs[129] = -2 * d[8] * d[17] * d[22] + 2 * d[8] * d[16] * d[23] + 2 * d[7] * d[17] * d[23] + 2 * d[8] * d[14] * d[25] + 2 * d[5] * d[17] * d[25] - 2 * d[8] * d[13] * d[26] + 2 * d[7] * d[14] * d[26] + 2 * d[5] * d[16] * d[26] - 2 * d[4] * d[17] * d[26]; coeffs[130] = 2 * d[8] * d[23] * d[25] - 2 * d[8] * d[22] * d[26] + 2 * d[7] * d[23] * d[26] + 2 * d[5] * d[25] * d[26] - d[4] * std::pow(d[26], 2); coeffs[131] = 2 * d[9] * d[10] * d[12] - std::pow(d[9], 2) * d[13] + std::pow(d[10], 2) * d[13] + std::pow(d[12], 2) * d[13] + std::pow(d[13], 3); coeffs[132] = 2 * d[10] * d[12] * d[18] - 2 * d[9] * d[13] * d[18] + 2 * d[9] * d[12] * d[19] + 2 * d[10] * d[13] * d[19] + 2 * d[9] * d[10] * d[21] + 2 * d[12] * d[13] * d[21] - std::pow(d[9], 2) * d[22] + std::pow(d[10], 2) * d[22] + std::pow(d[12], 2) * d[22] + 3 * std::pow(d[13], 2) * d[22]; coeffs[133] = -d[13] * std::pow(d[18], 2) + 2 * d[12] * d[18] * d[19] + d[13] * std::pow(d[19], 2) + 2 * d[10] * d[18] * d[21] + 2 * d[9] * d[19] * d[21] + d[13] * std::pow(d[21], 2) - 2 * d[9] * d[18] * d[22] + 2 * d[10] * d[19] * d[22] + 2 * d[12] * d[21] * d[22] + 3 * d[13] * std::pow(d[22], 2); coeffs[134] = 2 * d[18] * d[19] * d[21] - std::pow(d[18], 2) * d[22] + std::pow(d[19], 2) * d[22] + std::pow(d[21], 2) * d[22] + std::pow(d[22], 3); coeffs[135] = 2 * d[5] * d[10] * d[11] - d[4] * std::pow(d[11], 2) - 2 * d[2] * d[11] * d[13] + 2 * d[2] * d[10] * d[14] + 2 * d[1] * d[11] * d[14] + 2 * d[5] * d[13] * d[14] + d[4] * std::pow(d[14], 2) + 2 * d[7] * d[12] * d[15] - 2 * d[6] * d[13] * d[15] - d[4] * std::pow(d[15], 2) + 2 * d[6] * d[12] * d[16] + 2 * d[7] * d[13] * d[16] + 2 * d[3] * d[15] * d[16] + d[4] * std::pow(d[16], 2); coeffs[136] = 2 * d[5] * d[11] * d[19] + 2 * d[2] * d[14] * d[19] + 2 * d[5] * d[10] * d[20] - 2 * d[4] * d[11] * d[20] - 2 * d[2] * d[13] * d[20] + 2 * d[1] * d[14] * d[20] + 2 * d[7] * d[15] * d[21] + 2 * d[6] * d[16] * d[21] - 2 * d[2] * d[11] * d[22] + 2 * d[5] * d[14] * d[22] - 2 * d[6] * d[15] * d[22] + 2 * d[7] * d[16] * d[22] + 2 * d[2] * d[10] * d[23] + 2 * d[1] * d[11] * d[23] + 2 * d[5] * d[13] * d[23] + 2 * d[4] * d[14] * d[23] + 2 * d[7] * d[12] * d[24] - 2 * d[6] * d[13] * d[24] - 2 * d[4] * d[15] * d[24] + 2 * d[3] * d[16] * d[24] + 2 * d[6] * d[12] * d[25] + 2 * d[7] * d[13] * d[25] + 2 * d[3] * d[15] * d[25] + 2 * d[4] * d[16] * d[25]; coeffs[137] = 2 * d[5] * d[19] * d[20] - d[4] * std::pow(d[20], 2) - 2 * d[2] * d[20] * d[22] + 2 * d[2] * d[19] * d[23] + 2 * d[1] * d[20] * d[23] + 2 * d[5] * d[22] * d[23] + d[4] * std::pow(d[23], 2) + 2 * d[7] * d[21] * d[24] - 2 * d[6] * d[22] * d[24] - d[4] * std::pow(d[24], 2) + 2 * d[6] * d[21] * d[25] + 2 * d[7] * d[22] * d[25] + 2 * d[3] * d[24] * d[25] + d[4] * std::pow(d[25], 2); coeffs[138] = -std::pow(d[8], 2) * d[13] + 2 * d[7] * d[8] * d[14] + 2 * d[5] * d[8] * d[16] + 2 * d[5] * d[7] * d[17] - 2 * d[4] * d[8] * d[17]; coeffs[139] = -std::pow(d[8], 2) * d[22] + 2 * d[7] * d[8] * d[23] + 2 * d[5] * d[8] * d[25] + 2 * d[5] * d[7] * d[26] - 2 * d[4] * d[8] * d[26]; coeffs[140] = -d[4] * std::pow(d[9], 2) + 2 * d[3] * d[9] * d[10] + d[4] * std::pow(d[10], 2) + 2 * d[1] * d[9] * d[12] + 2 * d[0] * d[10] * d[12] + d[4] * std::pow(d[12], 2) - 2 * d[0] * d[9] * d[13] + 2 * d[1] * d[10] * d[13] + 2 * d[3] * d[12] * d[13] + 3 * d[4] * std::pow(d[13], 2); coeffs[141] = -2 * d[4] * d[9] * d[18] + 2 * d[3] * d[10] * d[18] + 2 * d[1] * d[12] * d[18] - 2 * d[0] * d[13] * d[18] + 2 * d[3] * d[9] * d[19] + 2 * d[4] * d[10] * d[19] + 2 * d[0] * d[12] * d[19] + 2 * d[1] * d[13] * d[19] + 2 * d[1] * d[9] * d[21] + 2 * d[0] * d[10] * d[21] + 2 * d[4] * d[12] * d[21] + 2 * d[3] * d[13] * d[21] - 2 * d[0] * d[9] * d[22] + 2 * d[1] * d[10] * d[22] + 2 * d[3] * d[12] * d[22] + 6 * d[4] * d[13] * d[22]; coeffs[142] = -d[4] * std::pow(d[18], 2) + 2 * d[3] * d[18] * d[19] + d[4] * std::pow(d[19], 2) + 2 * d[1] * d[18] * d[21] + 2 * d[0] * d[19] * d[21] + d[4] * std::pow(d[21], 2) - 2 * d[0] * d[18] * d[22] + 2 * d[1] * d[19] * d[22] + 2 * d[3] * d[21] * d[22] + 3 * d[4] * std::pow(d[22], 2); coeffs[143] = 2 * d[2] * d[5] * d[10] - 2 * d[2] * d[4] * d[11] + 2 * d[1] * d[5] * d[11] + 2 * d[6] * d[7] * d[12] - std::pow(d[2], 2) * d[13] + std::pow(d[5], 2) * d[13] - std::pow(d[6], 2) * d[13] + std::pow(d[7], 2) * d[13] + 2 * d[1] * d[2] * d[14] + 2 * d[4] * d[5] * d[14] - 2 * d[4] * d[6] * d[15] + 2 * d[3] * d[7] * d[15] + 2 * d[3] * d[6] * d[16] + 2 * d[4] * d[7] * d[16]; coeffs[144] = 2 * d[2] * d[5] * d[19] - 2 * d[2] * d[4] * d[20] + 2 * d[1] * d[5] * d[20] + 2 * d[6] * d[7] * d[21] - std::pow(d[2], 2) * d[22] + std::pow(d[5], 2) * d[22] - std::pow(d[6], 2) * d[22] + std::pow(d[7], 2) * d[22] + 2 * d[1] * d[2] * d[23] + 2 * d[4] * d[5] * d[23] - 2 * d[4] * d[6] * d[24] + 2 * d[3] * d[7] * d[24] + 2 * d[3] * d[6] * d[25] + 2 * d[4] * d[7] * d[25]; coeffs[145] = 2 * d[5] * d[7] * d[8] - d[4] * std::pow(d[8], 2); coeffs[146] = 2 * d[1] * d[3] * d[9] - 2 * d[0] * d[4] * d[9] + 2 * d[0] * d[3] * d[10] + 2 * d[1] * d[4] * d[10] + 2 * d[0] * d[1] * d[12] + 2 * d[3] * d[4] * d[12] - std::pow(d[0], 2) * d[13] + std::pow(d[1], 2) * d[13] + std::pow(d[3], 2) * d[13] + 3 * std::pow(d[4], 2) * d[13]; coeffs[147] = 2 * d[1] * d[3] * d[18] - 2 * d[0] * d[4] * d[18] + 2 * d[0] * d[3] * d[19] + 2 * d[1] * d[4] * d[19] + 2 * d[0] * d[1] * d[21] + 2 * d[3] * d[4] * d[21] - std::pow(d[0], 2) * d[22] + std::pow(d[1], 2) * d[22] + std::pow(d[3], 2) * d[22] + 3 * std::pow(d[4], 2) * d[22]; coeffs[148] = -std::pow(d[2], 2) * d[4] + 2 * d[1] * d[2] * d[5] + d[4] * std::pow(d[5], 2) - d[4] * std::pow(d[6], 2) + 2 * d[3] * d[6] * d[7] + d[4] * std::pow(d[7], 2); coeffs[149] = 2 * d[0] * d[1] * d[3] - std::pow(d[0], 2) * d[4] + std::pow(d[1], 2) * d[4] + std::pow(d[3], 2) * d[4] + std::pow(d[4], 3); coeffs[150] = d[14] * std::pow(d[17], 2); coeffs[151] = std::pow(d[17], 2) * d[23] + 2 * d[14] * d[17] * d[26]; coeffs[152] = 2 * d[17] * d[23] * d[26] + d[14] * std::pow(d[26], 2); coeffs[153] = d[23] * std::pow(d[26], 2); coeffs[154] = std::pow(d[11], 2) * d[14] + std::pow(d[14], 3) - d[14] * std::pow(d[15], 2) - d[14] * std::pow(d[16], 2) + 2 * d[12] * d[15] * d[17] + 2 * d[13] * d[16] * d[17]; coeffs[155] = 2 * d[11] * d[14] * d[20] + 2 * d[15] * d[17] * d[21] + 2 * d[16] * d[17] * d[22] + std::pow(d[11], 2) * d[23] + 3 * std::pow(d[14], 2) * d[23] - std::pow(d[15], 2) * d[23] - std::pow(d[16], 2) * d[23] - 2 * d[14] * d[15] * d[24] + 2 * d[12] * d[17] * d[24] - 2 * d[14] * d[16] * d[25] + 2 * d[13] * d[17] * d[25] + 2 * d[12] * d[15] * d[26] + 2 * d[13] * d[16] * d[26]; coeffs[156] = d[14] * std::pow(d[20], 2) + 2 * d[11] * d[20] * d[23] + 3 * d[14] * std::pow(d[23], 2) + 2 * d[17] * d[21] * d[24] - 2 * d[15] * d[23] * d[24] - d[14] * std::pow(d[24], 2) + 2 * d[17] * d[22] * d[25] - 2 * d[16] * d[23] * d[25] - d[14] * std::pow(d[25], 2) + 2 * d[15] * d[21] * d[26] + 2 * d[16] * d[22] * d[26] + 2 * d[12] * d[24] * d[26] + 2 * d[13] * d[25] * d[26]; coeffs[157] = std::pow(d[20], 2) * d[23] + std::pow(d[23], 3) - d[23] * std::pow(d[24], 2) - d[23] * std::pow(d[25], 2) + 2 * d[21] * d[24] * d[26] + 2 * d[22] * d[25] * d[26]; coeffs[158] = 2 * d[8] * d[14] * d[17] + d[5] * std::pow(d[17], 2); coeffs[159] = 2 * d[8] * d[17] * d[23] + 2 * d[8] * d[14] * d[26] + 2 * d[5] * d[17] * d[26]; coeffs[160] = 2 * d[8] * d[23] * d[26] + d[5] * std::pow(d[26], 2); coeffs[161] = 2 * d[9] * d[11] * d[12] + 2 * d[10] * d[11] * d[13] - std::pow(d[9], 2) * d[14] - std::pow(d[10], 2) * d[14] + std::pow(d[12], 2) * d[14] + std::pow(d[13], 2) * d[14]; coeffs[162] = 2 * d[11] * d[12] * d[18] - 2 * d[9] * d[14] * d[18] + 2 * d[11] * d[13] * d[19] - 2 * d[10] * d[14] * d[19] + 2 * d[9] * d[12] * d[20] + 2 * d[10] * d[13] * d[20] + 2 * d[9] * d[11] * d[21] + 2 * d[12] * d[14] * d[21] + 2 * d[10] * d[11] * d[22] + 2 * d[13] * d[14] * d[22] - std::pow(d[9], 2) * d[23] - std::pow(d[10], 2) * d[23] + std::pow(d[12], 2) * d[23] + std::pow(d[13], 2) * d[23]; coeffs[163] = -d[14] * std::pow(d[18], 2) - d[14] * std::pow(d[19], 2) + 2 * d[12] * d[18] * d[20] + 2 * d[13] * d[19] * d[20] + 2 * d[11] * d[18] * d[21] + 2 * d[9] * d[20] * d[21] + d[14] * std::pow(d[21], 2) + 2 * d[11] * d[19] * d[22] + 2 * d[10] * d[20] * d[22] + d[14] * std::pow(d[22], 2) - 2 * d[9] * d[18] * d[23] - 2 * d[10] * d[19] * d[23] + 2 * d[12] * d[21] * d[23] + 2 * d[13] * d[22] * d[23]; coeffs[164] = 2 * d[18] * d[20] * d[21] + 2 * d[19] * d[20] * d[22] - std::pow(d[18], 2) * d[23] - std::pow(d[19], 2) * d[23] + std::pow(d[21], 2) * d[23] + std::pow(d[22], 2) * d[23]; coeffs[165] = d[5] * std::pow(d[11], 2) + 2 * d[2] * d[11] * d[14] + 3 * d[5] * std::pow(d[14], 2) + 2 * d[8] * d[12] * d[15] - 2 * d[6] * d[14] * d[15] - d[5] * std::pow(d[15], 2) + 2 * d[8] * d[13] * d[16] - 2 * d[7] * d[14] * d[16] - d[5] * std::pow(d[16], 2) + 2 * d[6] * d[12] * d[17] + 2 * d[7] * d[13] * d[17] + 2 * d[3] * d[15] * d[17] + 2 * d[4] * d[16] * d[17]; coeffs[166] = 2 * d[5] * d[11] * d[20] + 2 * d[2] * d[14] * d[20] + 2 * d[8] * d[15] * d[21] + 2 * d[6] * d[17] * d[21] + 2 * d[8] * d[16] * d[22] + 2 * d[7] * d[17] * d[22] + 2 * d[2] * d[11] * d[23] + 6 * d[5] * d[14] * d[23] - 2 * d[6] * d[15] * d[23] - 2 * d[7] * d[16] * d[23] + 2 * d[8] * d[12] * d[24] - 2 * d[6] * d[14] * d[24] - 2 * d[5] * d[15] * d[24] + 2 * d[3] * d[17] * d[24] + 2 * d[8] * d[13] * d[25] - 2 * d[7] * d[14] * d[25] - 2 * d[5] * d[16] * d[25] + 2 * d[4] * d[17] * d[25] + 2 * d[6] * d[12] * d[26] + 2 * d[7] * d[13] * d[26] + 2 * d[3] * d[15] * d[26] + 2 * d[4] * d[16] * d[26]; coeffs[167] = d[5] * std::pow(d[20], 2) + 2 * d[2] * d[20] * d[23] + 3 * d[5] * std::pow(d[23], 2) + 2 * d[8] * d[21] * d[24] - 2 * d[6] * d[23] * d[24] - d[5] * std::pow(d[24], 2) + 2 * d[8] * d[22] * d[25] - 2 * d[7] * d[23] * d[25] - d[5] * std::pow(d[25], 2) + 2 * d[6] * d[21] * d[26] + 2 * d[7] * d[22] * d[26] + 2 * d[3] * d[24] * d[26] + 2 * d[4] * d[25] * d[26]; coeffs[168] = std::pow(d[8], 2) * d[14] + 2 * d[5] * d[8] * d[17]; coeffs[169] = std::pow(d[8], 2) * d[23] + 2 * d[5] * d[8] * d[26]; coeffs[170] = -d[5] * std::pow(d[9], 2) - d[5] * std::pow(d[10], 2) + 2 * d[3] * d[9] * d[11] + 2 * d[4] * d[10] * d[11] + 2 * d[2] * d[9] * d[12] + 2 * d[0] * d[11] * d[12] + d[5] * std::pow(d[12], 2) + 2 * d[2] * d[10] * d[13] + 2 * d[1] * d[11] * d[13] + d[5] * std::pow(d[13], 2) - 2 * d[0] * d[9] * d[14] - 2 * d[1] * d[10] * d[14] + 2 * d[3] * d[12] * d[14] + 2 * d[4] * d[13] * d[14]; coeffs[171] = -2 * d[5] * d[9] * d[18] + 2 * d[3] * d[11] * d[18] + 2 * d[2] * d[12] * d[18] - 2 * d[0] * d[14] * d[18] - 2 * d[5] * d[10] * d[19] + 2 * d[4] * d[11] * d[19] + 2 * d[2] * d[13] * d[19] - 2 * d[1] * d[14] * d[19] + 2 * d[3] * d[9] * d[20] + 2 * d[4] * d[10] * d[20] + 2 * d[0] * d[12] * d[20] + 2 * d[1] * d[13] * d[20] + 2 * d[2] * d[9] * d[21] + 2 * d[0] * d[11] * d[21] + 2 * d[5] * d[12] * d[21] + 2 * d[3] * d[14] * d[21] + 2 * d[2] * d[10] * d[22] + 2 * d[1] * d[11] * d[22] + 2 * d[5] * d[13] * d[22] + 2 * d[4] * d[14] * d[22] - 2 * d[0] * d[9] * d[23] - 2 * d[1] * d[10] * d[23] + 2 * d[3] * d[12] * d[23] + 2 * d[4] * d[13] * d[23]; coeffs[172] = -d[5] * std::pow(d[18], 2) - d[5] * std::pow(d[19], 2) + 2 * d[3] * d[18] * d[20] + 2 * d[4] * d[19] * d[20] + 2 * d[2] * d[18] * d[21] + 2 * d[0] * d[20] * d[21] + d[5] * std::pow(d[21], 2) + 2 * d[2] * d[19] * d[22] + 2 * d[1] * d[20] * d[22] + d[5] * std::pow(d[22], 2) - 2 * d[0] * d[18] * d[23] - 2 * d[1] * d[19] * d[23] + 2 * d[3] * d[21] * d[23] + 2 * d[4] * d[22] * d[23]; coeffs[173] = 2 * d[2] * d[5] * d[11] + 2 * d[6] * d[8] * d[12] + 2 * d[7] * d[8] * d[13] + std::pow(d[2], 2) * d[14] + 3 * std::pow(d[5], 2) * d[14] - std::pow(d[6], 2) * d[14] - std::pow(d[7], 2) * d[14] - 2 * d[5] * d[6] * d[15] + 2 * d[3] * d[8] * d[15] - 2 * d[5] * d[7] * d[16] + 2 * d[4] * d[8] * d[16] + 2 * d[3] * d[6] * d[17] + 2 * d[4] * d[7] * d[17]; coeffs[174] = 2 * d[2] * d[5] * d[20] + 2 * d[6] * d[8] * d[21] + 2 * d[7] * d[8] * d[22] + std::pow(d[2], 2) * d[23] + 3 * std::pow(d[5], 2) * d[23] - std::pow(d[6], 2) * d[23] - std::pow(d[7], 2) * d[23] - 2 * d[5] * d[6] * d[24] + 2 * d[3] * d[8] * d[24] - 2 * d[5] * d[7] * d[25] + 2 * d[4] * d[8] * d[25] + 2 * d[3] * d[6] * d[26] + 2 * d[4] * d[7] * d[26]; coeffs[175] = d[5] * std::pow(d[8], 2); coeffs[176] = 2 * d[2] * d[3] * d[9] - 2 * d[0] * d[5] * d[9] + 2 * d[2] * d[4] * d[10] - 2 * d[1] * d[5] * d[10] + 2 * d[0] * d[3] * d[11] + 2 * d[1] * d[4] * d[11] + 2 * d[0] * d[2] * d[12] + 2 * d[3] * d[5] * d[12] + 2 * d[1] * d[2] * d[13] + 2 * d[4] * d[5] * d[13] - std::pow(d[0], 2) * d[14] - std::pow(d[1], 2) * d[14] + std::pow(d[3], 2) * d[14] + std::pow(d[4], 2) * d[14]; coeffs[177] = 2 * d[2] * d[3] * d[18] - 2 * d[0] * d[5] * d[18] + 2 * d[2] * d[4] * d[19] - 2 * d[1] * d[5] * d[19] + 2 * d[0] * d[3] * d[20] + 2 * d[1] * d[4] * d[20] + 2 * d[0] * d[2] * d[21] + 2 * d[3] * d[5] * d[21] + 2 * d[1] * d[2] * d[22] + 2 * d[4] * d[5] * d[22] - std::pow(d[0], 2) * d[23] - std::pow(d[1], 2) * d[23] + std::pow(d[3], 2) * d[23] + std::pow(d[4], 2) * d[23]; coeffs[178] = std::pow(d[2], 2) * d[5] + std::pow(d[5], 3) - d[5] * std::pow(d[6], 2) - d[5] * std::pow(d[7], 2) + 2 * d[3] * d[6] * d[8] + 2 * d[4] * d[7] * d[8]; coeffs[179] = 2 * d[0] * d[2] * d[3] + 2 * d[1] * d[2] * d[4] - std::pow(d[0], 2) * d[5] - std::pow(d[1], 2) * d[5] + std::pow(d[3], 2) * d[5] + std::pow(d[4], 2) * d[5]; coeffs[180] = d[15] * std::pow(d[17], 2); coeffs[181] = std::pow(d[17], 2) * d[24] + 2 * d[15] * d[17] * d[26]; coeffs[182] = 2 * d[17] * d[24] * d[26] + d[15] * std::pow(d[26], 2); coeffs[183] = d[24] * std::pow(d[26], 2); coeffs[184] = -std::pow(d[11], 2) * d[15] - std::pow(d[14], 2) * d[15] + std::pow(d[15], 3) + d[15] * std::pow(d[16], 2) + 2 * d[9] * d[11] * d[17] + 2 * d[12] * d[14] * d[17]; coeffs[185] = 2 * d[11] * d[17] * d[18] - 2 * d[11] * d[15] * d[20] + 2 * d[9] * d[17] * d[20] + 2 * d[14] * d[17] * d[21] - 2 * d[14] * d[15] * d[23] + 2 * d[12] * d[17] * d[23] - std::pow(d[11], 2) * d[24] - std::pow(d[14], 2) * d[24] + 3 * std::pow(d[15], 2) * d[24] + std::pow(d[16], 2) * d[24] + 2 * d[15] * d[16] * d[25] + 2 * d[9] * d[11] * d[26] + 2 * d[12] * d[14] * d[26]; coeffs[186] = 2 * d[17] * d[18] * d[20] - d[15] * std::pow(d[20], 2) + 2 * d[17] * d[21] * d[23] - d[15] * std::pow(d[23], 2) - 2 * d[11] * d[20] * d[24] - 2 * d[14] * d[23] * d[24] + 3 * d[15] * std::pow(d[24], 2) + 2 * d[16] * d[24] * d[25] + d[15] * std::pow(d[25], 2) + 2 * d[11] * d[18] * d[26] + 2 * d[9] * d[20] * d[26] + 2 * d[14] * d[21] * d[26] + 2 * d[12] * d[23] * d[26]; coeffs[187] = -std::pow(d[20], 2) * d[24] - std::pow(d[23], 2) * d[24] + std::pow(d[24], 3) + d[24] * std::pow(d[25], 2) + 2 * d[18] * d[20] * d[26] + 2 * d[21] * d[23] * d[26]; coeffs[188] = 2 * d[8] * d[15] * d[17] + d[6] * std::pow(d[17], 2); coeffs[189] = 2 * d[8] * d[17] * d[24] + 2 * d[8] * d[15] * d[26] + 2 * d[6] * d[17] * d[26]; coeffs[190] = 2 * d[8] * d[24] * d[26] + d[6] * std::pow(d[26], 2); coeffs[191] = std::pow(d[9], 2) * d[15] - std::pow(d[10], 2) * d[15] + std::pow(d[12], 2) * d[15] - std::pow(d[13], 2) * d[15] + 2 * d[9] * d[10] * d[16] + 2 * d[12] * d[13] * d[16]; coeffs[192] = 2 * d[9] * d[15] * d[18] + 2 * d[10] * d[16] * d[18] - 2 * d[10] * d[15] * d[19] + 2 * d[9] * d[16] * d[19] + 2 * d[12] * d[15] * d[21] + 2 * d[13] * d[16] * d[21] - 2 * d[13] * d[15] * d[22] + 2 * d[12] * d[16] * d[22] + std::pow(d[9], 2) * d[24] - std::pow(d[10], 2) * d[24] + std::pow(d[12], 2) * d[24] - std::pow(d[13], 2) * d[24] + 2 * d[9] * d[10] * d[25] + 2 * d[12] * d[13] * d[25]; coeffs[193] = d[15] * std::pow(d[18], 2) + 2 * d[16] * d[18] * d[19] - d[15] * std::pow(d[19], 2) + d[15] * std::pow(d[21], 2) + 2 * d[16] * d[21] * d[22] - d[15] * std::pow(d[22], 2) + 2 * d[9] * d[18] * d[24] - 2 * d[10] * d[19] * d[24] + 2 * d[12] * d[21] * d[24] - 2 * d[13] * d[22] * d[24] + 2 * d[10] * d[18] * d[25] + 2 * d[9] * d[19] * d[25] + 2 * d[13] * d[21] * d[25] + 2 * d[12] * d[22] * d[25]; coeffs[194] = std::pow(d[18], 2) * d[24] - std::pow(d[19], 2) * d[24] + std::pow(d[21], 2) * d[24] - std::pow(d[22], 2) * d[24] + 2 * d[18] * d[19] * d[25] + 2 * d[21] * d[22] * d[25]; coeffs[195] = 2 * d[8] * d[9] * d[11] - d[6] * std::pow(d[11], 2) + 2 * d[8] * d[12] * d[14] - d[6] * std::pow(d[14], 2) - 2 * d[2] * d[11] * d[15] - 2 * d[5] * d[14] * d[15] + 3 * d[6] * std::pow(d[15], 2) + 2 * d[7] * d[15] * d[16] + d[6] * std::pow(d[16], 2) + 2 * d[2] * d[9] * d[17] + 2 * d[0] * d[11] * d[17] + 2 * d[5] * d[12] * d[17] + 2 * d[3] * d[14] * d[17]; coeffs[196] = 2 * d[8] * d[11] * d[18] + 2 * d[2] * d[17] * d[18] + 2 * d[8] * d[9] * d[20] - 2 * d[6] * d[11] * d[20] - 2 * d[2] * d[15] * d[20] + 2 * d[0] * d[17] * d[20] + 2 * d[8] * d[14] * d[21] + 2 * d[5] * d[17] * d[21] + 2 * d[8] * d[12] * d[23] - 2 * d[6] * d[14] * d[23] - 2 * d[5] * d[15] * d[23] + 2 * d[3] * d[17] * d[23] - 2 * d[2] * d[11] * d[24] - 2 * d[5] * d[14] * d[24] + 6 * d[6] * d[15] * d[24] + 2 * d[7] * d[16] * d[24] + 2 * d[7] * d[15] * d[25] + 2 * d[6] * d[16] * d[25] + 2 * d[2] * d[9] * d[26] + 2 * d[0] * d[11] * d[26] + 2 * d[5] * d[12] * d[26] + 2 * d[3] * d[14] * d[26]; coeffs[197] = 2 * d[8] * d[18] * d[20] - d[6] * std::pow(d[20], 2) + 2 * d[8] * d[21] * d[23] - d[6] * std::pow(d[23], 2) - 2 * d[2] * d[20] * d[24] - 2 * d[5] * d[23] * d[24] + 3 * d[6] * std::pow(d[24], 2) + 2 * d[7] * d[24] * d[25] + d[6] * std::pow(d[25], 2) + 2 * d[2] * d[18] * d[26] + 2 * d[0] * d[20] * d[26] + 2 * d[5] * d[21] * d[26] + 2 * d[3] * d[23] * d[26]; coeffs[198] = std::pow(d[8], 2) * d[15] + 2 * d[6] * d[8] * d[17]; coeffs[199] = std::pow(d[8], 2) * d[24] + 2 * d[6] * d[8] * d[26]; coeffs[200] = d[6] * std::pow(d[9], 2) + 2 * d[7] * d[9] * d[10] - d[6] * std::pow(d[10], 2) + d[6] * std::pow(d[12], 2) + 2 * d[7] * d[12] * d[13] - d[6] * std::pow(d[13], 2) + 2 * d[0] * d[9] * d[15] - 2 * d[1] * d[10] * d[15] + 2 * d[3] * d[12] * d[15] - 2 * d[4] * d[13] * d[15] + 2 * d[1] * d[9] * d[16] + 2 * d[0] * d[10] * d[16] + 2 * d[4] * d[12] * d[16] + 2 * d[3] * d[13] * d[16]; coeffs[201] = 2 * d[6] * d[9] * d[18] + 2 * d[7] * d[10] * d[18] + 2 * d[0] * d[15] * d[18] + 2 * d[1] * d[16] * d[18] + 2 * d[7] * d[9] * d[19] - 2 * d[6] * d[10] * d[19] - 2 * d[1] * d[15] * d[19] + 2 * d[0] * d[16] * d[19] + 2 * d[6] * d[12] * d[21] + 2 * d[7] * d[13] * d[21] + 2 * d[3] * d[15] * d[21] + 2 * d[4] * d[16] * d[21] + 2 * d[7] * d[12] * d[22] - 2 * d[6] * d[13] * d[22] - 2 * d[4] * d[15] * d[22] + 2 * d[3] * d[16] * d[22] + 2 * d[0] * d[9] * d[24] - 2 * d[1] * d[10] * d[24] + 2 * d[3] * d[12] * d[24] - 2 * d[4] * d[13] * d[24] + 2 * d[1] * d[9] * d[25] + 2 * d[0] * d[10] * d[25] + 2 * d[4] * d[12] * d[25] + 2 * d[3] * d[13] * d[25]; coeffs[202] = d[6] * std::pow(d[18], 2) + 2 * d[7] * d[18] * d[19] - d[6] * std::pow(d[19], 2) + d[6] * std::pow(d[21], 2) + 2 * d[7] * d[21] * d[22] - d[6] * std::pow(d[22], 2) + 2 * d[0] * d[18] * d[24] - 2 * d[1] * d[19] * d[24] + 2 * d[3] * d[21] * d[24] - 2 * d[4] * d[22] * d[24] + 2 * d[1] * d[18] * d[25] + 2 * d[0] * d[19] * d[25] + 2 * d[4] * d[21] * d[25] + 2 * d[3] * d[22] * d[25]; coeffs[203] = 2 * d[2] * d[8] * d[9] - 2 * d[2] * d[6] * d[11] + 2 * d[0] * d[8] * d[11] + 2 * d[5] * d[8] * d[12] - 2 * d[5] * d[6] * d[14] + 2 * d[3] * d[8] * d[14] - std::pow(d[2], 2) * d[15] - std::pow(d[5], 2) * d[15] + 3 * std::pow(d[6], 2) * d[15] + std::pow(d[7], 2) * d[15] + 2 * d[6] * d[7] * d[16] + 2 * d[0] * d[2] * d[17] + 2 * d[3] * d[5] * d[17]; coeffs[204] = 2 * d[2] * d[8] * d[18] - 2 * d[2] * d[6] * d[20] + 2 * d[0] * d[8] * d[20] + 2 * d[5] * d[8] * d[21] - 2 * d[5] * d[6] * d[23] + 2 * d[3] * d[8] * d[23] - std::pow(d[2], 2) * d[24] - std::pow(d[5], 2) * d[24] + 3 * std::pow(d[6], 2) * d[24] + std::pow(d[7], 2) * d[24] + 2 * d[6] * d[7] * d[25] + 2 * d[0] * d[2] * d[26] + 2 * d[3] * d[5] * d[26]; coeffs[205] = d[6] * std::pow(d[8], 2); coeffs[206] = 2 * d[0] * d[6] * d[9] + 2 * d[1] * d[7] * d[9] - 2 * d[1] * d[6] * d[10] + 2 * d[0] * d[7] * d[10] + 2 * d[3] * d[6] * d[12] + 2 * d[4] * d[7] * d[12] - 2 * d[4] * d[6] * d[13] + 2 * d[3] * d[7] * d[13] + std::pow(d[0], 2) * d[15] - std::pow(d[1], 2) * d[15] + std::pow(d[3], 2) * d[15] - std::pow(d[4], 2) * d[15] + 2 * d[0] * d[1] * d[16] + 2 * d[3] * d[4] * d[16]; coeffs[207] = 2 * d[0] * d[6] * d[18] + 2 * d[1] * d[7] * d[18] - 2 * d[1] * d[6] * d[19] + 2 * d[0] * d[7] * d[19] + 2 * d[3] * d[6] * d[21] + 2 * d[4] * d[7] * d[21] - 2 * d[4] * d[6] * d[22] + 2 * d[3] * d[7] * d[22] + std::pow(d[0], 2) * d[24] - std::pow(d[1], 2) * d[24] + std::pow(d[3], 2) * d[24] - std::pow(d[4], 2) * d[24] + 2 * d[0] * d[1] * d[25] + 2 * d[3] * d[4] * d[25]; coeffs[208] = -std::pow(d[2], 2) * d[6] - std::pow(d[5], 2) * d[6] + std::pow(d[6], 3) + d[6] * std::pow(d[7], 2) + 2 * d[0] * d[2] * d[8] + 2 * d[3] * d[5] * d[8]; coeffs[209] = std::pow(d[0], 2) * d[6] - std::pow(d[1], 2) * d[6] + std::pow(d[3], 2) * d[6] - std::pow(d[4], 2) * d[6] + 2 * d[0] * d[1] * d[7] + 2 * d[3] * d[4] * d[7]; coeffs[210] = d[16] * std::pow(d[17], 2); coeffs[211] = std::pow(d[17], 2) * d[25] + 2 * d[16] * d[17] * d[26]; coeffs[212] = 2 * d[17] * d[25] * d[26] + d[16] * std::pow(d[26], 2); coeffs[213] = d[25] * std::pow(d[26], 2); coeffs[214] = -std::pow(d[11], 2) * d[16] - std::pow(d[14], 2) * d[16] + std::pow(d[15], 2) * d[16] + std::pow(d[16], 3) + 2 * d[10] * d[11] * d[17] + 2 * d[13] * d[14] * d[17]; coeffs[215] = 2 * d[11] * d[17] * d[19] - 2 * d[11] * d[16] * d[20] + 2 * d[10] * d[17] * d[20] + 2 * d[14] * d[17] * d[22] - 2 * d[14] * d[16] * d[23] + 2 * d[13] * d[17] * d[23] + 2 * d[15] * d[16] * d[24] - std::pow(d[11], 2) * d[25] - std::pow(d[14], 2) * d[25] + std::pow(d[15], 2) * d[25] + 3 * std::pow(d[16], 2) * d[25] + 2 * d[10] * d[11] * d[26] + 2 * d[13] * d[14] * d[26]; coeffs[216] = 2 * d[17] * d[19] * d[20] - d[16] * std::pow(d[20], 2) + 2 * d[17] * d[22] * d[23] - d[16] * std::pow(d[23], 2) + d[16] * std::pow(d[24], 2) - 2 * d[11] * d[20] * d[25] - 2 * d[14] * d[23] * d[25] + 2 * d[15] * d[24] * d[25] + 3 * d[16] * std::pow(d[25], 2) + 2 * d[11] * d[19] * d[26] + 2 * d[10] * d[20] * d[26] + 2 * d[14] * d[22] * d[26] + 2 * d[13] * d[23] * d[26]; coeffs[217] = -std::pow(d[20], 2) * d[25] - std::pow(d[23], 2) * d[25] + std::pow(d[24], 2) * d[25] + std::pow(d[25], 3) + 2 * d[19] * d[20] * d[26] + 2 * d[22] * d[23] * d[26]; coeffs[218] = 2 * d[8] * d[16] * d[17] + d[7] * std::pow(d[17], 2); coeffs[219] = 2 * d[8] * d[17] * d[25] + 2 * d[8] * d[16] * d[26] + 2 * d[7] * d[17] * d[26]; coeffs[220] = 2 * d[8] * d[25] * d[26] + d[7] * std::pow(d[26], 2); coeffs[221] = 2 * d[9] * d[10] * d[15] + 2 * d[12] * d[13] * d[15] - std::pow(d[9], 2) * d[16] + std::pow(d[10], 2) * d[16] - std::pow(d[12], 2) * d[16] + std::pow(d[13], 2) * d[16]; coeffs[222] = 2 * d[10] * d[15] * d[18] - 2 * d[9] * d[16] * d[18] + 2 * d[9] * d[15] * d[19] + 2 * d[10] * d[16] * d[19] + 2 * d[13] * d[15] * d[21] - 2 * d[12] * d[16] * d[21] + 2 * d[12] * d[15] * d[22] + 2 * d[13] * d[16] * d[22] + 2 * d[9] * d[10] * d[24] + 2 * d[12] * d[13] * d[24] - std::pow(d[9], 2) * d[25] + std::pow(d[10], 2) * d[25] - std::pow(d[12], 2) * d[25] + std::pow(d[13], 2) * d[25]; coeffs[223] = -d[16] * std::pow(d[18], 2) + 2 * d[15] * d[18] * d[19] + d[16] * std::pow(d[19], 2) - d[16] * std::pow(d[21], 2) + 2 * d[15] * d[21] * d[22] + d[16] * std::pow(d[22], 2) + 2 * d[10] * d[18] * d[24] + 2 * d[9] * d[19] * d[24] + 2 * d[13] * d[21] * d[24] + 2 * d[12] * d[22] * d[24] - 2 * d[9] * d[18] * d[25] + 2 * d[10] * d[19] * d[25] - 2 * d[12] * d[21] * d[25] + 2 * d[13] * d[22] * d[25]; coeffs[224] = 2 * d[18] * d[19] * d[24] + 2 * d[21] * d[22] * d[24] - std::pow(d[18], 2) * d[25] + std::pow(d[19], 2) * d[25] - std::pow(d[21], 2) * d[25] + std::pow(d[22], 2) * d[25]; coeffs[225] = 2 * d[8] * d[10] * d[11] - d[7] * std::pow(d[11], 2) + 2 * d[8] * d[13] * d[14] - d[7] * std::pow(d[14], 2) + d[7] * std::pow(d[15], 2) - 2 * d[2] * d[11] * d[16] - 2 * d[5] * d[14] * d[16] + 2 * d[6] * d[15] * d[16] + 3 * d[7] * std::pow(d[16], 2) + 2 * d[2] * d[10] * d[17] + 2 * d[1] * d[11] * d[17] + 2 * d[5] * d[13] * d[17] + 2 * d[4] * d[14] * d[17]; coeffs[226] = 2 * d[8] * d[11] * d[19] + 2 * d[2] * d[17] * d[19] + 2 * d[8] * d[10] * d[20] - 2 * d[7] * d[11] * d[20] - 2 * d[2] * d[16] * d[20] + 2 * d[1] * d[17] * d[20] + 2 * d[8] * d[14] * d[22] + 2 * d[5] * d[17] * d[22] + 2 * d[8] * d[13] * d[23] - 2 * d[7] * d[14] * d[23] - 2 * d[5] * d[16] * d[23] + 2 * d[4] * d[17] * d[23] + 2 * d[7] * d[15] * d[24] + 2 * d[6] * d[16] * d[24] - 2 * d[2] * d[11] * d[25] - 2 * d[5] * d[14] * d[25] + 2 * d[6] * d[15] * d[25] + 6 * d[7] * d[16] * d[25] + 2 * d[2] * d[10] * d[26] + 2 * d[1] * d[11] * d[26] + 2 * d[5] * d[13] * d[26] + 2 * d[4] * d[14] * d[26]; coeffs[227] = 2 * d[8] * d[19] * d[20] - d[7] * std::pow(d[20], 2) + 2 * d[8] * d[22] * d[23] - d[7] * std::pow(d[23], 2) + d[7] * std::pow(d[24], 2) - 2 * d[2] * d[20] * d[25] - 2 * d[5] * d[23] * d[25] + 2 * d[6] * d[24] * d[25] + 3 * d[7] * std::pow(d[25], 2) + 2 * d[2] * d[19] * d[26] + 2 * d[1] * d[20] * d[26] + 2 * d[5] * d[22] * d[26] + 2 * d[4] * d[23] * d[26]; coeffs[228] = std::pow(d[8], 2) * d[16] + 2 * d[7] * d[8] * d[17]; coeffs[229] = std::pow(d[8], 2) * d[25] + 2 * d[7] * d[8] * d[26]; coeffs[230] = -d[7] * std::pow(d[9], 2) + 2 * d[6] * d[9] * d[10] + d[7] * std::pow(d[10], 2) - d[7] * std::pow(d[12], 2) + 2 * d[6] * d[12] * d[13] + d[7] * std::pow(d[13], 2) + 2 * d[1] * d[9] * d[15] + 2 * d[0] * d[10] * d[15] + 2 * d[4] * d[12] * d[15] + 2 * d[3] * d[13] * d[15] - 2 * d[0] * d[9] * d[16] + 2 * d[1] * d[10] * d[16] - 2 * d[3] * d[12] * d[16] + 2 * d[4] * d[13] * d[16]; coeffs[231] = -2 * d[7] * d[9] * d[18] + 2 * d[6] * d[10] * d[18] + 2 * d[1] * d[15] * d[18] - 2 * d[0] * d[16] * d[18] + 2 * d[6] * d[9] * d[19] + 2 * d[7] * d[10] * d[19] + 2 * d[0] * d[15] * d[19] + 2 * d[1] * d[16] * d[19] - 2 * d[7] * d[12] * d[21] + 2 * d[6] * d[13] * d[21] + 2 * d[4] * d[15] * d[21] - 2 * d[3] * d[16] * d[21] + 2 * d[6] * d[12] * d[22] + 2 * d[7] * d[13] * d[22] + 2 * d[3] * d[15] * d[22] + 2 * d[4] * d[16] * d[22] + 2 * d[1] * d[9] * d[24] + 2 * d[0] * d[10] * d[24] + 2 * d[4] * d[12] * d[24] + 2 * d[3] * d[13] * d[24] - 2 * d[0] * d[9] * d[25] + 2 * d[1] * d[10] * d[25] - 2 * d[3] * d[12] * d[25] + 2 * d[4] * d[13] * d[25]; coeffs[232] = -d[7] * std::pow(d[18], 2) + 2 * d[6] * d[18] * d[19] + d[7] * std::pow(d[19], 2) - d[7] * std::pow(d[21], 2) + 2 * d[6] * d[21] * d[22] + d[7] * std::pow(d[22], 2) + 2 * d[1] * d[18] * d[24] + 2 * d[0] * d[19] * d[24] + 2 * d[4] * d[21] * d[24] + 2 * d[3] * d[22] * d[24] - 2 * d[0] * d[18] * d[25] + 2 * d[1] * d[19] * d[25] - 2 * d[3] * d[21] * d[25] + 2 * d[4] * d[22] * d[25]; coeffs[233] = 2 * d[2] * d[8] * d[10] - 2 * d[2] * d[7] * d[11] + 2 * d[1] * d[8] * d[11] + 2 * d[5] * d[8] * d[13] - 2 * d[5] * d[7] * d[14] + 2 * d[4] * d[8] * d[14] + 2 * d[6] * d[7] * d[15] - std::pow(d[2], 2) * d[16] - std::pow(d[5], 2) * d[16] + std::pow(d[6], 2) * d[16] + 3 * std::pow(d[7], 2) * d[16] + 2 * d[1] * d[2] * d[17] + 2 * d[4] * d[5] * d[17]; coeffs[234] = 2 * d[2] * d[8] * d[19] - 2 * d[2] * d[7] * d[20] + 2 * d[1] * d[8] * d[20] + 2 * d[5] * d[8] * d[22] - 2 * d[5] * d[7] * d[23] + 2 * d[4] * d[8] * d[23] + 2 * d[6] * d[7] * d[24] - std::pow(d[2], 2) * d[25] - std::pow(d[5], 2) * d[25] + std::pow(d[6], 2) * d[25] + 3 * std::pow(d[7], 2) * d[25] + 2 * d[1] * d[2] * d[26] + 2 * d[4] * d[5] * d[26]; coeffs[235] = d[7] * std::pow(d[8], 2); coeffs[236] = 2 * d[1] * d[6] * d[9] - 2 * d[0] * d[7] * d[9] + 2 * d[0] * d[6] * d[10] + 2 * d[1] * d[7] * d[10] + 2 * d[4] * d[6] * d[12] - 2 * d[3] * d[7] * d[12] + 2 * d[3] * d[6] * d[13] + 2 * d[4] * d[7] * d[13] + 2 * d[0] * d[1] * d[15] + 2 * d[3] * d[4] * d[15] - std::pow(d[0], 2) * d[16] + std::pow(d[1], 2) * d[16] - std::pow(d[3], 2) * d[16] + std::pow(d[4], 2) * d[16]; coeffs[237] = 2 * d[1] * d[6] * d[18] - 2 * d[0] * d[7] * d[18] + 2 * d[0] * d[6] * d[19] + 2 * d[1] * d[7] * d[19] + 2 * d[4] * d[6] * d[21] - 2 * d[3] * d[7] * d[21] + 2 * d[3] * d[6] * d[22] + 2 * d[4] * d[7] * d[22] + 2 * d[0] * d[1] * d[24] + 2 * d[3] * d[4] * d[24] - std::pow(d[0], 2) * d[25] + std::pow(d[1], 2) * d[25] - std::pow(d[3], 2) * d[25] + std::pow(d[4], 2) * d[25]; coeffs[238] = -std::pow(d[2], 2) * d[7] - std::pow(d[5], 2) * d[7] + std::pow(d[6], 2) * d[7] + std::pow(d[7], 3) + 2 * d[1] * d[2] * d[8] + 2 * d[4] * d[5] * d[8]; coeffs[239] = 2 * d[0] * d[1] * d[6] + 2 * d[3] * d[4] * d[6] - std::pow(d[0], 2) * d[7] + std::pow(d[1], 2) * d[7] - std::pow(d[3], 2) * d[7] + std::pow(d[4], 2) * d[7]; coeffs[240] = std::pow(d[17], 3); coeffs[241] = 3 * std::pow(d[17], 2) * d[26]; coeffs[242] = 3 * d[17] * std::pow(d[26], 2); coeffs[243] = std::pow(d[26], 3); coeffs[244] = std::pow(d[11], 2) * d[17] + std::pow(d[14], 2) * d[17] + std::pow(d[15], 2) * d[17] + std::pow(d[16], 2) * d[17]; coeffs[245] = 2 * d[11] * d[17] * d[20] + 2 * d[14] * d[17] * d[23] + 2 * d[15] * d[17] * d[24] + 2 * d[16] * d[17] * d[25] + std::pow(d[11], 2) * d[26] + std::pow(d[14], 2) * d[26] + std::pow(d[15], 2) * d[26] + std::pow(d[16], 2) * d[26]; coeffs[246] = d[17] * std::pow(d[20], 2) + d[17] * std::pow(d[23], 2) + d[17] * std::pow(d[24], 2) + d[17] * std::pow(d[25], 2) + 2 * d[11] * d[20] * d[26] + 2 * d[14] * d[23] * d[26] + 2 * d[15] * d[24] * d[26] + 2 * d[16] * d[25] * d[26]; coeffs[247] = std::pow(d[20], 2) * d[26] + std::pow(d[23], 2) * d[26] + std::pow(d[24], 2) * d[26] + std::pow(d[25], 2) * d[26]; coeffs[248] = 3 * d[8] * std::pow(d[17], 2); coeffs[249] = 6 * d[8] * d[17] * d[26]; coeffs[250] = 3 * d[8] * std::pow(d[26], 2); coeffs[251] = 2 * d[9] * d[11] * d[15] + 2 * d[12] * d[14] * d[15] + 2 * d[10] * d[11] * d[16] + 2 * d[13] * d[14] * d[16] - std::pow(d[9], 2) * d[17] - std::pow(d[10], 2) * d[17] - std::pow(d[12], 2) * d[17] - std::pow(d[13], 2) * d[17]; coeffs[252] = 2 * d[11] * d[15] * d[18] - 2 * d[9] * d[17] * d[18] + 2 * d[11] * d[16] * d[19] - 2 * d[10] * d[17] * d[19] + 2 * d[9] * d[15] * d[20] + 2 * d[10] * d[16] * d[20] + 2 * d[14] * d[15] * d[21] - 2 * d[12] * d[17] * d[21] + 2 * d[14] * d[16] * d[22] - 2 * d[13] * d[17] * d[22] + 2 * d[12] * d[15] * d[23] + 2 * d[13] * d[16] * d[23] + 2 * d[9] * d[11] * d[24] + 2 * d[12] * d[14] * d[24] + 2 * d[10] * d[11] * d[25] + 2 * d[13] * d[14] * d[25] - std::pow(d[9], 2) * d[26] - std::pow(d[10], 2) * d[26] - std::pow(d[12], 2) * d[26] - std::pow(d[13], 2) * d[26]; coeffs[253] = -d[17] * std::pow(d[18], 2) - d[17] * std::pow(d[19], 2) + 2 * d[15] * d[18] * d[20] + 2 * d[16] * d[19] * d[20] - d[17] * std::pow(d[21], 2) - d[17] * std::pow(d[22], 2) + 2 * d[15] * d[21] * d[23] + 2 * d[16] * d[22] * d[23] + 2 * d[11] * d[18] * d[24] + 2 * d[9] * d[20] * d[24] + 2 * d[14] * d[21] * d[24] + 2 * d[12] * d[23] * d[24] + 2 * d[11] * d[19] * d[25] + 2 * d[10] * d[20] * d[25] + 2 * d[14] * d[22] * d[25] + 2 * d[13] * d[23] * d[25] - 2 * d[9] * d[18] * d[26] - 2 * d[10] * d[19] * d[26] - 2 * d[12] * d[21] * d[26] - 2 * d[13] * d[22] * d[26]; coeffs[254] = 2 * d[18] * d[20] * d[24] + 2 * d[21] * d[23] * d[24] + 2 * d[19] * d[20] * d[25] + 2 * d[22] * d[23] * d[25] - std::pow(d[18], 2) * d[26] - std::pow(d[19], 2) * d[26] - std::pow(d[21], 2) * d[26] - std::pow(d[22], 2) * d[26]; coeffs[255] = d[8] * std::pow(d[11], 2) + d[8] * std::pow(d[14], 2) + d[8] * std::pow(d[15], 2) + d[8] * std::pow(d[16], 2) + 2 * d[2] * d[11] * d[17] + 2 * d[5] * d[14] * d[17] + 2 * d[6] * d[15] * d[17] + 2 * d[7] * d[16] * d[17]; coeffs[256] = 2 * d[8] * d[11] * d[20] + 2 * d[2] * d[17] * d[20] + 2 * d[8] * d[14] * d[23] + 2 * d[5] * d[17] * d[23] + 2 * d[8] * d[15] * d[24] + 2 * d[6] * d[17] * d[24] + 2 * d[8] * d[16] * d[25] + 2 * d[7] * d[17] * d[25] + 2 * d[2] * d[11] * d[26] + 2 * d[5] * d[14] * d[26] + 2 * d[6] * d[15] * d[26] + 2 * d[7] * d[16] * d[26]; coeffs[257] = d[8] * std::pow(d[20], 2) + d[8] * std::pow(d[23], 2) + d[8] * std::pow(d[24], 2) + d[8] * std::pow(d[25], 2) + 2 * d[2] * d[20] * d[26] + 2 * d[5] * d[23] * d[26] + 2 * d[6] * d[24] * d[26] + 2 * d[7] * d[25] * d[26]; coeffs[258] = 3 * std::pow(d[8], 2) * d[17]; coeffs[259] = 3 * std::pow(d[8], 2) * d[26]; coeffs[260] = -d[8] * std::pow(d[9], 2) - d[8] * std::pow(d[10], 2) + 2 * d[6] * d[9] * d[11] + 2 * d[7] * d[10] * d[11] - d[8] * std::pow(d[12], 2) - d[8] * std::pow(d[13], 2) + 2 * d[6] * d[12] * d[14] + 2 * d[7] * d[13] * d[14] + 2 * d[2] * d[9] * d[15] + 2 * d[0] * d[11] * d[15] + 2 * d[5] * d[12] * d[15] + 2 * d[3] * d[14] * d[15] + 2 * d[2] * d[10] * d[16] + 2 * d[1] * d[11] * d[16] + 2 * d[5] * d[13] * d[16] + 2 * d[4] * d[14] * d[16] - 2 * d[0] * d[9] * d[17] - 2 * d[1] * d[10] * d[17] - 2 * d[3] * d[12] * d[17] - 2 * d[4] * d[13] * d[17]; coeffs[261] = -2 * d[8] * d[9] * d[18] + 2 * d[6] * d[11] * d[18] + 2 * d[2] * d[15] * d[18] - 2 * d[0] * d[17] * d[18] - 2 * d[8] * d[10] * d[19] + 2 * d[7] * d[11] * d[19] + 2 * d[2] * d[16] * d[19] - 2 * d[1] * d[17] * d[19] + 2 * d[6] * d[9] * d[20] + 2 * d[7] * d[10] * d[20] + 2 * d[0] * d[15] * d[20] + 2 * d[1] * d[16] * d[20] - 2 * d[8] * d[12] * d[21] + 2 * d[6] * d[14] * d[21] + 2 * d[5] * d[15] * d[21] - 2 * d[3] * d[17] * d[21] - 2 * d[8] * d[13] * d[22] + 2 * d[7] * d[14] * d[22] + 2 * d[5] * d[16] * d[22] - 2 * d[4] * d[17] * d[22] + 2 * d[6] * d[12] * d[23] + 2 * d[7] * d[13] * d[23] + 2 * d[3] * d[15] * d[23] + 2 * d[4] * d[16] * d[23] + 2 * d[2] * d[9] * d[24] + 2 * d[0] * d[11] * d[24] + 2 * d[5] * d[12] * d[24] + 2 * d[3] * d[14] * d[24] + 2 * d[2] * d[10] * d[25] + 2 * d[1] * d[11] * d[25] + 2 * d[5] * d[13] * d[25] + 2 * d[4] * d[14] * d[25] - 2 * d[0] * d[9] * d[26] - 2 * d[1] * d[10] * d[26] - 2 * d[3] * d[12] * d[26] - 2 * d[4] * d[13] * d[26]; coeffs[262] = -d[8] * std::pow(d[18], 2) - d[8] * std::pow(d[19], 2) + 2 * d[6] * d[18] * d[20] + 2 * d[7] * d[19] * d[20] - d[8] * std::pow(d[21], 2) - d[8] * std::pow(d[22], 2) + 2 * d[6] * d[21] * d[23] + 2 * d[7] * d[22] * d[23] + 2 * d[2] * d[18] * d[24] + 2 * d[0] * d[20] * d[24] + 2 * d[5] * d[21] * d[24] + 2 * d[3] * d[23] * d[24] + 2 * d[2] * d[19] * d[25] + 2 * d[1] * d[20] * d[25] + 2 * d[5] * d[22] * d[25] + 2 * d[4] * d[23] * d[25] - 2 * d[0] * d[18] * d[26] - 2 * d[1] * d[19] * d[26] - 2 * d[3] * d[21] * d[26] - 2 * d[4] * d[22] * d[26]; coeffs[263] = 2 * d[2] * d[8] * d[11] + 2 * d[5] * d[8] * d[14] + 2 * d[6] * d[8] * d[15] + 2 * d[7] * d[8] * d[16] + std::pow(d[2], 2) * d[17] + std::pow(d[5], 2) * d[17] + std::pow(d[6], 2) * d[17] + std::pow(d[7], 2) * d[17]; coeffs[264] = 2 * d[2] * d[8] * d[20] + 2 * d[5] * d[8] * d[23] + 2 * d[6] * d[8] * d[24] + 2 * d[7] * d[8] * d[25] + std::pow(d[2], 2) * d[26] + std::pow(d[5], 2) * d[26] + std::pow(d[6], 2) * d[26] + std::pow(d[7], 2) * d[26]; coeffs[265] = std::pow(d[8], 3); coeffs[266] = 2 * d[2] * d[6] * d[9] - 2 * d[0] * d[8] * d[9] + 2 * d[2] * d[7] * d[10] - 2 * d[1] * d[8] * d[10] + 2 * d[0] * d[6] * d[11] + 2 * d[1] * d[7] * d[11] + 2 * d[5] * d[6] * d[12] - 2 * d[3] * d[8] * d[12] + 2 * d[5] * d[7] * d[13] - 2 * d[4] * d[8] * d[13] + 2 * d[3] * d[6] * d[14] + 2 * d[4] * d[7] * d[14] + 2 * d[0] * d[2] * d[15] + 2 * d[3] * d[5] * d[15] + 2 * d[1] * d[2] * d[16] + 2 * d[4] * d[5] * d[16] - std::pow(d[0], 2) * d[17] - std::pow(d[1], 2) * d[17] - std::pow(d[3], 2) * d[17] - std::pow(d[4], 2) * d[17]; coeffs[267] = 2 * d[2] * d[6] * d[18] - 2 * d[0] * d[8] * d[18] + 2 * d[2] * d[7] * d[19] - 2 * d[1] * d[8] * d[19] + 2 * d[0] * d[6] * d[20] + 2 * d[1] * d[7] * d[20] + 2 * d[5] * d[6] * d[21] - 2 * d[3] * d[8] * d[21] + 2 * d[5] * d[7] * d[22] - 2 * d[4] * d[8] * d[22] + 2 * d[3] * d[6] * d[23] + 2 * d[4] * d[7] * d[23] + 2 * d[0] * d[2] * d[24] + 2 * d[3] * d[5] * d[24] + 2 * d[1] * d[2] * d[25] + 2 * d[4] * d[5] * d[25] - std::pow(d[0], 2) * d[26] - std::pow(d[1], 2) * d[26] - std::pow(d[3], 2) * d[26] - std::pow(d[4], 2) * d[26]; coeffs[268] = std::pow(d[2], 2) * d[8] + std::pow(d[5], 2) * d[8] + std::pow(d[6], 2) * d[8] + std::pow(d[7], 2) * d[8]; coeffs[269] = 2 * d[0] * d[2] * d[6] + 2 * d[3] * d[5] * d[6] + 2 * d[1] * d[2] * d[7] + 2 * d[4] * d[5] * d[7] - std::pow(d[0], 2) * d[8] - std::pow(d[1], 2) * d[8] - std::pow(d[3], 2) * d[8] - std::pow(d[4], 2) * d[8]; coeffs[270] = -d[11] * d[13] * d[15] + d[10] * d[14] * d[15] + d[11] * d[12] * d[16] - d[9] * d[14] * d[16] - d[10] * d[12] * d[17] + d[9] * d[13] * d[17]; coeffs[271] = -d[14] * d[16] * d[18] + d[13] * d[17] * d[18] + d[14] * d[15] * d[19] - d[12] * d[17] * d[19] - d[13] * d[15] * d[20] + d[12] * d[16] * d[20] + d[11] * d[16] * d[21] - d[10] * d[17] * d[21] - d[11] * d[15] * d[22] + d[9] * d[17] * d[22] + d[10] * d[15] * d[23] - d[9] * d[16] * d[23] - d[11] * d[13] * d[24] + d[10] * d[14] * d[24] + d[11] * d[12] * d[25] - d[9] * d[14] * d[25] - d[10] * d[12] * d[26] + d[9] * d[13] * d[26]; coeffs[272] = -d[17] * d[19] * d[21] + d[16] * d[20] * d[21] + d[17] * d[18] * d[22] - d[15] * d[20] * d[22] - d[16] * d[18] * d[23] + d[15] * d[19] * d[23] + d[14] * d[19] * d[24] - d[13] * d[20] * d[24] - d[11] * d[22] * d[24] + d[10] * d[23] * d[24] - d[14] * d[18] * d[25] + d[12] * d[20] * d[25] + d[11] * d[21] * d[25] - d[9] * d[23] * d[25] + d[13] * d[18] * d[26] - d[12] * d[19] * d[26] - d[10] * d[21] * d[26] + d[9] * d[22] * d[26]; coeffs[273] = -d[20] * d[22] * d[24] + d[19] * d[23] * d[24] + d[20] * d[21] * d[25] - d[18] * d[23] * d[25] - d[19] * d[21] * d[26] + d[18] * d[22] * d[26]; coeffs[274] = -d[8] * d[10] * d[12] + d[7] * d[11] * d[12] + d[8] * d[9] * d[13] - d[6] * d[11] * d[13] - d[7] * d[9] * d[14] + d[6] * d[10] * d[14] + d[5] * d[10] * d[15] - d[4] * d[11] * d[15] - d[2] * d[13] * d[15] + d[1] * d[14] * d[15] - d[5] * d[9] * d[16] + d[3] * d[11] * d[16] + d[2] * d[12] * d[16] - d[0] * d[14] * d[16] + d[4] * d[9] * d[17] - d[3] * d[10] * d[17] - d[1] * d[12] * d[17] + d[0] * d[13] * d[17]; coeffs[275] = d[8] * d[13] * d[18] - d[7] * d[14] * d[18] - d[5] * d[16] * d[18] + d[4] * d[17] * d[18] - d[8] * d[12] * d[19] + d[6] * d[14] * d[19] + d[5] * d[15] * d[19] - d[3] * d[17] * d[19] + d[7] * d[12] * d[20] - d[6] * d[13] * d[20] - d[4] * d[15] * d[20] + d[3] * d[16] * d[20] - d[8] * d[10] * d[21] + d[7] * d[11] * d[21] + d[2] * d[16] * d[21] - d[1] * d[17] * d[21] + d[8] * d[9] * d[22] - d[6] * d[11] * d[22] - d[2] * d[15] * d[22] + d[0] * d[17] * d[22] - d[7] * d[9] * d[23] + d[6] * d[10] * d[23] + d[1] * d[15] * d[23] - d[0] * d[16] * d[23] + d[5] * d[10] * d[24] - d[4] * d[11] * d[24] - d[2] * d[13] * d[24] + d[1] * d[14] * d[24] - d[5] * d[9] * d[25] + d[3] * d[11] * d[25] + d[2] * d[12] * d[25] - d[0] * d[14] * d[25] + d[4] * d[9] * d[26] - d[3] * d[10] * d[26] - d[1] * d[12] * d[26] + d[0] * d[13] * d[26]; coeffs[276] = -d[8] * d[19] * d[21] + d[7] * d[20] * d[21] + d[8] * d[18] * d[22] - d[6] * d[20] * d[22] - d[7] * d[18] * d[23] + d[6] * d[19] * d[23] + d[5] * d[19] * d[24] - d[4] * d[20] * d[24] - d[2] * d[22] * d[24] + d[1] * d[23] * d[24] - d[5] * d[18] * d[25] + d[3] * d[20] * d[25] + d[2] * d[21] * d[25] - d[0] * d[23] * d[25] + d[4] * d[18] * d[26] - d[3] * d[19] * d[26] - d[1] * d[21] * d[26] + d[0] * d[22] * d[26]; coeffs[277] = -d[5] * d[7] * d[9] + d[4] * d[8] * d[9] + d[5] * d[6] * d[10] - d[3] * d[8] * d[10] - d[4] * d[6] * d[11] + d[3] * d[7] * d[11] + d[2] * d[7] * d[12] - d[1] * d[8] * d[12] - d[2] * d[6] * d[13] + d[0] * d[8] * d[13] + d[1] * d[6] * d[14] - d[0] * d[7] * d[14] - d[2] * d[4] * d[15] + d[1] * d[5] * d[15] + d[2] * d[3] * d[16] - d[0] * d[5] * d[16] - d[1] * d[3] * d[17] + d[0] * d[4] * d[17]; coeffs[278] = -d[5] * d[7] * d[18] + d[4] * d[8] * d[18] + d[5] * d[6] * d[19] - d[3] * d[8] * d[19] - d[4] * d[6] * d[20] + d[3] * d[7] * d[20] + d[2] * d[7] * d[21] - d[1] * d[8] * d[21] - d[2] * d[6] * d[22] + d[0] * d[8] * d[22] + d[1] * d[6] * d[23] - d[0] * d[7] * d[23] - d[2] * d[4] * d[24] + d[1] * d[5] * d[24] + d[2] * d[3] * d[25] - d[0] * d[5] * d[25] - d[1] * d[3] * d[26] + d[0] * d[4] * d[26]; coeffs[279] = -d[2] * d[4] * d[6] + d[1] * d[5] * d[6] + d[2] * d[3] * d[7] - d[0] * d[5] * d[7] - d[1] * d[3] * d[8] + d[0] * d[4] * d[8]; // Setup elimination template static const int coeffs0_ind[] = { 0, 30, 60, 90, 120, 150, 180, 210, 240, 1, 31, 61, 91, 121, 151, 181, 211, 241, 2, 32, 62, 92, 122, 152, 182, 212, 242, 4, 34, 64, 30, 0, 60, 94, 124, 154, 90, 120, 150, 184, 214, 180, 210, 240, 244, 270, 5, 35, 65, 31, 1, 61, 95, 125, 155, 91, 121, 151, 185, 215, 181, 211, 241, 245, 271, 6, 36, 66, 32, 2, 62, 96, 126, 156, 92, 122, 152, 186, 216, 182, 212, 242, 246, 272, 7, 37, 67, 33, 3, 63, 97, 127, 157, 93, 123, 153, 187, 217, 183, 213, 243, 247, 273, 8, 38, 68, 98, 128, 158, 188, 218, 248, 9, 39, 69, 99, 129, 159, 189, 219, 249, 11, 41, 71, 34, 4, 64, 101, 131, 161, 90, 94, 60, 120, 124, 154, 191, 221, 0, 180, 184, 150, 210, 214, 30, 240, 244, 251, 270, 12, 42, 72, 35, 5, 65, 102, 132, 162, 91, 95, 61, 121, 125, 155, 192, 222, 1, 181, 185, 151, 211, 215, 31, 241, 245, 252, 271, 13, 43, 73, 36, 6, 66, 103, 133, 163, 92, 96, 62, 122, 126, 156, 193, 223, 2, 182, 186, 152, 212, 216, 32, 242, 246, 253, 272, 14, 44, 74, 37, 7, 67, 104, 134, 164, 93, 97, 63, 123, 127, 157, 194, 224, 3, 183, 187, 153, 213, 217, 33, 243, 247, 254, 273, 15, 45, 75, 38, 8, 68, 105, 135, 165, 98, 128, 158, 195, 225, 188, 218, 248, 255, 274, 16, 46, 76, 39, 9, 69, 106, 136, 166, 99, 129, 159, 196, 226, 189, 219, 249, 256, 275, 17, 47, 77, 40, 10, 70, 107, 137, 167, 100, 130, 160, 197, 227, 190, 220, 250, 257, 276, 18, 48, 78, 108, 138, 168, 198, 228, 258, 41, 11, 71, 94, 101, 64, 124, 131, 161, 4, 184, 191, 154, 214, 221, 34, 244, 251, 270, 42, 12, 72, 95, 102, 65, 125, 132, 162, 5, 185, 192, 155, 215, 222, 35, 245, 252, 271, 20, 50, 80, 45, 15, 75, 110, 140, 170, 98, 105, 68, 128, 135, 165, 200, 230, 8, 188, 195, 158, 218, 225, 38, 248, 255, 260, 274, 23, 53, 83, 48, 18, 78, 113, 143, 173, 108, 138, 168, 203, 233, 198, 228, 258, 263, 277, 101, 71, 131, 11, 191, 161, 221, 41, 251, 270, 50, 20, 80, 105, 110, 75, 135, 140, 170, 15, 195, 200, 165, 225, 230, 45, 255, 260, 274, 102, 72, 132, 12, 192, 162, 222, 42, 252, 271, 103, 73, 133, 13, 193, 163, 223, 43, 253, 272, 43, 13, 73, 96, 103, 66, 126, 133, 163, 6, 186, 193, 156, 216, 223, 36, 246, 253, 272, 21, 51, 81, 46, 16, 76, 111, 141, 171, 99, 106, 69, 129, 136, 166, 201, 231, 9, 189, 196, 159, 219, 226, 39, 249, 256, 261, 275, 104, 74, 134, 14, 194, 164, 224, 44, 254, 273, 44, 14, 74, 97, 104, 67, 127, 134, 164, 7, 187, 194, 157, 217, 224, 37, 247, 254, 273, 22, 52, 82, 47, 17, 77, 112, 142, 172, 100, 107, 70, 130, 137, 167, 202, 232, 10, 190, 197, 160, 220, 227, 40, 250, 257, 262, 276, 24, 54, 84, 49, 19, 79, 114, 144, 174, 109, 139, 169, 204, 234, 199, 229, 259, 264, 278}; static const int coeffs1_ind[] = { 119, 89, 149, 29, 209, 179, 239, 59, 269, 279, 116, 86, 146, 26, 206, 176, 236, 56, 266, 277, 110, 80, 140, 20, 200, 170, 230, 50, 260, 274, 111, 81, 141, 21, 201, 171, 231, 51, 261, 275, 51, 21, 81, 106, 111, 76, 136, 141, 171, 16, 196, 201, 166, 226, 231, 46, 256, 261, 275, 56, 26, 86, 113, 116, 83, 143, 146, 176, 23, 203, 206, 173, 233, 236, 53, 263, 266, 277, 26, 56, 86, 53, 23, 83, 116, 146, 176, 108, 113, 78, 138, 143, 173, 206, 236, 18, 198, 203, 168, 228, 233, 48, 258, 263, 266, 277, 117, 87, 147, 27, 207, 177, 237, 57, 267, 278, 112, 82, 142, 22, 202, 172, 232, 52, 262, 276, 52, 22, 82, 107, 112, 77, 137, 142, 172, 17, 197, 202, 167, 227, 232, 47, 257, 262, 276, 57, 27, 87, 114, 117, 84, 144, 147, 177, 24, 204, 207, 174, 234, 237, 54, 264, 267, 278, 27, 57, 87, 54, 24, 84, 117, 147, 177, 109, 114, 79, 139, 144, 174, 207, 237, 19, 199, 204, 169, 229, 234, 49, 259, 264, 267, 278, 59, 29, 89, 118, 119, 88, 148, 149, 179, 28, 208, 209, 178, 238, 239, 58, 268, 269, 279, 29, 59, 89, 58, 28, 88, 119, 149, 179, 115, 118, 85, 145, 148, 178, 209, 239, 25, 205, 208, 175, 235, 238, 55, 265, 268, 269, 279, 28, 58, 88, 55, 25, 85, 118, 148, 178, 115, 145, 175, 208, 238, 205, 235, 265, 268, 279}; static const int C0_ind[] = { 0, 1, 2, 6, 7, 8, 15, 16, 26, 31, 32, 33, 37, 38, 39, 46, 47, 57, 62, 63, 64, 68, 69, 70, 77, 78, 88, 93, 94, 95, 96, 97, 98, 99, 100, 101, 103, 106, 107, 108, 109, 112, 115, 118, 119, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 134, 137, 138, 139, 140, 143, 146, 149, 150, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 165, 168, 169, 170, 171, 174, 177, 180, 181, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 196, 199, 200, 201, 202, 205, 208, 211, 212, 216, 217, 218, 219, 223, 224, 225, 232, 233, 243, 248, 249, 250, 254, 255, 256, 263, 264, 274, 279, 280, 281, 282, 283, 284, 285, 286, 287, 288, 289, 290, 291, 292, 293, 294, 295, 296, 297, 298, 299, 300, 301, 302, 303, 304, 305, 308, 310, 311, 312, 313, 314, 315, 316, 317, 318, 319, 320, 321, 322, 323, 324, 325, 326, 327, 328, 329, 330, 331, 332, 333, 334, 335, 336, 339, 341, 342, 343, 344, 345, 346, 347, 348, 349, 350, 351, 352, 353, 354, 355, 356, 357, 358, 359, 360, 361, 362, 363, 364, 365, 366, 367, 370, 372, 373, 374, 375, 376, 377, 378, 379, 380, 381, 382, 383, 384, 385, 386, 387, 388, 389, 390, 391, 392, 393, 394, 395, 396, 397, 398, 401, 403, 404, 405, 406, 407, 408, 409, 410, 411, 413, 416, 417, 418, 419, 422, 425, 428, 429, 433, 434, 435, 436, 437, 438, 439, 440, 441, 442, 444, 447, 448, 449, 450, 453, 456, 459, 460, 464, 465, 466, 467, 468, 469, 470, 471, 472, 473, 475, 478, 479, 480, 481, 484, 487, 490, 491, 495, 496, 497, 498, 502, 503, 504, 511, 512, 522, 530, 531, 532, 536, 537, 538, 539, 540, 541, 544, 545, 546, 547, 548, 549, 550, 551, 552, 555, 561, 562, 563, 567, 568, 569, 570, 571, 572, 575, 576, 577, 578, 579, 580, 581, 582, 583, 586, 589, 590, 591, 592, 593, 594, 595, 596, 597, 598, 599, 600, 601, 602, 603, 604, 605, 606, 607, 608, 609, 610, 611, 612, 613, 614, 615, 618, 620, 621, 622, 623, 624, 625, 626, 627, 628, 630, 633, 634, 635, 636, 639, 642, 645, 646, 650, 660, 662, 663, 668, 669, 671, 672, 674, 675, 678, 685, 686, 687, 691, 692, 693, 694, 695, 696, 699, 700, 701, 702, 703, 704, 705, 706, 707, 710, 722, 724, 725, 730, 731, 733, 734, 736, 737, 740, 753, 755, 756, 761, 762, 764, 765, 767, 768, 771, 778, 779, 780, 784, 785, 786, 787, 788, 789, 792, 793, 794, 795, 796, 797, 798, 799, 800, 803, 806, 807, 808, 809, 810, 811, 812, 813, 814, 815, 816, 817, 818, 819, 820, 821, 822, 823, 824, 825, 826, 827, 828, 829, 830, 831, 832, 835, 846, 848, 849, 854, 855, 857, 858, 860, 861, 864, 871, 872, 873, 877, 878, 879, 880, 881, 882, 885, 886, 887, 888, 889, 890, 891, 892, 893, 896, 899, 900, 901, 902, 903, 904, 905, 906, 907, 908, 909, 910, 911, 912, 913, 914, 915, 916, 917, 918, 919, 920, 921, 922, 923, 924, 925, 928, 930, 931, 932, 933, 934, 935, 936, 937, 938, 940, 943, 944, 945, 946, 949, 952, 955, 956, 960}; static const int C1_ind[] = { 9, 11, 12, 17, 18, 20, 21, 23, 24, 27, 40, 42, 43, 48, 49, 51, 52, 54, 55, 58, 71, 73, 74, 79, 80, 82, 83, 85, 86, 89, 102, 104, 105, 110, 111, 113, 114, 116, 117, 120, 127, 128, 129, 133, 134, 135, 136, 137, 138, 141, 142, 143, 144, 145, 146, 147, 148, 149, 152, 158, 159, 160, 164, 165, 166, 167, 168, 169, 172, 173, 174, 175, 176, 177, 178, 179, 180, 183, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 215, 226, 228, 229, 234, 235, 237, 238, 240, 241, 244, 257, 259, 260, 265, 266, 268, 269, 271, 272, 275, 282, 283, 284, 288, 289, 290, 291, 292, 293, 296, 297, 298, 299, 300, 301, 302, 303, 304, 307, 313, 314, 315, 319, 320, 321, 322, 323, 324, 327, 328, 329, 330, 331, 332, 333, 334, 335, 338, 341, 342, 343, 344, 345, 346, 347, 348, 349, 350, 351, 352, 353, 354, 355, 356, 357, 358, 359, 360, 361, 362, 363, 364, 365, 366, 367, 370, 375, 376, 377, 381, 382, 383, 384, 385, 386, 389, 390, 391, 392, 393, 394, 395, 396, 397, 400, 403, 404, 405, 406, 407, 408, 409, 410, 411, 412, 413, 414, 415, 416, 417, 418, 419, 420, 421, 422, 423, 424, 425, 426, 427, 428, 429, 432, 434, 435, 436, 437, 438, 439, 440, 441, 442, 444, 447, 448, 449, 450, 453, 456, 459, 460, 464}; Eigen::Matrix C0; C0.setZero(); Eigen::Matrix C1; C1.setZero(); for (int i = 0; i < 556; i++) { C0(C0_ind[i]) = coeffs(coeffs0_ind[i]); } for (int i = 0; i < 258; i++) { C1(C1_ind[i]) = coeffs(coeffs1_ind[i]); } Eigen::Matrix C12 = C0.partialPivLu().solve(C1); // Setup action matrix Eigen::Matrix RR; RR << -C12.bottomRows(8), Eigen::Matrix::Identity(15, 15); static const int AM_ind[] = {15, 11, 0, 1, 2, 12, 3, 16, 4, 5, 17, 6, 18, 19, 7}; Eigen::Matrix AM; for (int i = 0; i < 15; i++) { AM.row(i) = RR.row(AM_ind[i]); } sols.setZero(); // Solve eigenvalue problem /* Eigen::EigenSolver es(AM, false); Eigen::ArrayXcd D = es.eigenvalues(); int nroots = 0; double eigv[15]; for (int i = 0; i < 15; i++) { if (std::abs(D(i).imag()) < 1e-6) eigv[nroots++] = D(i).real(); }*/ double p[1 + 15]; Eigen::Matrix AMp = AM; sturm::charpoly_danilevsky_piv(AMp, p); double eigv[15]; int nroots; // find_real_roots_sturm(p, 15, eigv, &nroots, 8, 0); nroots = sturm::bisect_sturm<15>(p, eigv, 1e-12); shared_focal_relpose_fast_eigenvector_solver(eigv, nroots, AM, sols); return nroots; } int relpose_6pt_shared_focal(const std::vector &x1, const std::vector &x2, ImagePairVector *out_image_pairs) { // Compute nullspace to epipolar constraints Eigen::Matrix epipolar_constraints; for (size_t i = 0; i < 6; ++i) { epipolar_constraints.col(i) << x1[i](0) * x2[i], x1[i](1) * x2[i], x1[i](2) * x2[i]; } Eigen::Matrix Q = epipolar_constraints.fullPivHouseholderQr().matrixQ(); Eigen::Matrix N = Q.rightCols(3); Eigen::VectorXd B(Eigen::Map(N.data(), N.cols() * N.rows())); Eigen::Matrix, 3, 15> sols; int n_sols = solver_shared_focal_relpose_6pt(B, sols); out_image_pairs->clear(); out_image_pairs->reserve(4 * n_sols); int n_poses = 0; for (int i = 0; i < n_sols; i++) { if (sols(2, i).real() < 1e-8 || sols.col(i).imag().norm() > 1e-8) { continue; } double focal = std::sqrt(1.0 / sols(2, i).real()); Eigen::Matrix F_vector = N.col(0) + sols(0, i).real() * N.col(1) + sols(1, i).real() * N.col(2); F_vector.normalize(); Eigen::Matrix3d F = Eigen::Matrix3d(F_vector.data()); Camera calib = Camera("SIMPLE_PINHOLE", std::vector{focal, 0.0, 0.0}, -1, -1); Eigen::Matrix3d K; K << focal, 0.0, 0.0, 0.0, focal, 0.0, 0.0, 0.0, 1.0; Eigen::Matrix3d E = K * (F * K); CameraPoseVector poses; std::vector x1_u, x2_u; x1_u.clear(); x2_u.clear(); x1_u.reserve(6); x2_u.reserve(6); for (int i = 0; i < 6; i++) { x1_u.push_back(Eigen::Vector3d(x1[i](0) / focal, x1[i](1) / focal, x1[i](2)).normalized()); x2_u.push_back(Eigen::Vector3d(x2[i](0) / focal, x2[i](1) / focal, x2[i](2)).normalized()); } motion_from_essential(E, x1_u, x2_u, &poses); for (CameraPose const &pose : poses) { out_image_pairs->emplace_back(ImagePair(pose, calib, calib)); n_poses++; } } return n_poses; } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/relpose_6pt_focal.h000066400000000000000000000010131504452766400216740ustar00rootroot00000000000000#ifndef POSELIB_RELPOSE_6PT_EQUAL_FOCAL_H #define POSELIB_RELPOSE_6PT_EQUAL_FOCAL_H #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for relative pose with one unknown focal length from 6 correspondences // The solver is created using Larsson et al. CVPR 2017 int relpose_6pt_shared_focal(const std::vector &x1, const std::vector &x2, ImagePairVector *out_image_pairs); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/relpose_7pt.cc000066400000000000000000000057371504452766400207100ustar00rootroot00000000000000#include "relpose_7pt.h" #include "PoseLib/misc/univariate.h" #include namespace poselib { int relpose_7pt(const std::vector &x1, const std::vector &x2, std::vector *fundamental_matrices) { // Compute nullspace to epipolar constraints Eigen::Matrix epipolar_constraints; for (size_t i = 0; i < 7; ++i) { epipolar_constraints.col(i) << x1[i](0) * x2[i], x1[i](1) * x2[i], x1[i](2) * x2[i]; } Eigen::Matrix Q = epipolar_constraints.fullPivHouseholderQr().matrixQ(); Eigen::Matrix N = Q.rightCols(2); // coefficients for det(F(x)) = 0 const double c3 = N(0, 0) * N(4, 0) * N(8, 0) - N(0, 0) * N(5, 0) * N(7, 0) - N(1, 0) * N(3, 0) * N(8, 0) + N(1, 0) * N(5, 0) * N(6, 0) + N(2, 0) * N(3, 0) * N(7, 0) - N(2, 0) * N(4, 0) * N(6, 0); const double c2 = N(0, 0) * N(4, 0) * N(8, 1) + N(0, 0) * N(4, 1) * N(8, 0) - N(0, 0) * N(5, 0) * N(7, 1) - N(0, 0) * N(5, 1) * N(7, 0) + N(0, 1) * N(4, 0) * N(8, 0) - N(0, 1) * N(5, 0) * N(7, 0) - N(1, 0) * N(3, 0) * N(8, 1) - N(1, 0) * N(3, 1) * N(8, 0) + N(1, 0) * N(5, 0) * N(6, 1) + N(1, 0) * N(5, 1) * N(6, 0) - N(1, 1) * N(3, 0) * N(8, 0) + N(1, 1) * N(5, 0) * N(6, 0) + N(2, 0) * N(3, 0) * N(7, 1) + N(2, 0) * N(3, 1) * N(7, 0) - N(2, 0) * N(4, 0) * N(6, 1) - N(2, 0) * N(4, 1) * N(6, 0) + N(2, 1) * N(3, 0) * N(7, 0) - N(2, 1) * N(4, 0) * N(6, 0); const double c1 = N(0, 0) * N(4, 1) * N(8, 1) - N(0, 0) * N(5, 1) * N(7, 1) + N(0, 1) * N(4, 0) * N(8, 1) + N(0, 1) * N(4, 1) * N(8, 0) - N(0, 1) * N(5, 0) * N(7, 1) - N(0, 1) * N(5, 1) * N(7, 0) - N(1, 0) * N(3, 1) * N(8, 1) + N(1, 0) * N(5, 1) * N(6, 1) - N(1, 1) * N(3, 0) * N(8, 1) - N(1, 1) * N(3, 1) * N(8, 0) + N(1, 1) * N(5, 0) * N(6, 1) + N(1, 1) * N(5, 1) * N(6, 0) + N(2, 0) * N(3, 1) * N(7, 1) - N(2, 0) * N(4, 1) * N(6, 1) + N(2, 1) * N(3, 0) * N(7, 1) + N(2, 1) * N(3, 1) * N(7, 0) - N(2, 1) * N(4, 0) * N(6, 1) - N(2, 1) * N(4, 1) * N(6, 0); const double c0 = N(0, 1) * N(4, 1) * N(8, 1) - N(0, 1) * N(5, 1) * N(7, 1) - N(1, 1) * N(3, 1) * N(8, 1) + N(1, 1) * N(5, 1) * N(6, 1) + N(2, 1) * N(3, 1) * N(7, 1) - N(2, 1) * N(4, 1) * N(6, 1); // Solve the cubic double inv_c3 = 1.0 / c3; double roots[3]; int n_roots = univariate::solve_cubic_real(c2 * inv_c3, c1 * inv_c3, c0 * inv_c3, roots); // Reshape back into 3x3 matrices fundamental_matrices->clear(); fundamental_matrices->reserve(n_roots); for (int i = 0; i < n_roots; ++i) { Eigen::Matrix f = N.col(0) * roots[i] + N.col(1); f.normalize(); fundamental_matrices->push_back(Eigen::Map(f.data())); } return n_roots; } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/relpose_7pt.h000066400000000000000000000037721504452766400205470ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RELPOSE_7PT_H_ #define POSELIB_RELPOSE_7PT_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Computes the fundamental matrix from seven point correspondences. Returning up to three solutions. int relpose_7pt(const std::vector &x1, const std::vector &x2, std::vector *fundamental_matrices); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/relpose_8pt.cc000066400000000000000000000104211504452766400206730ustar00rootroot00000000000000// Copyright (c) 2020, Pierre Moulon // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "relpose_8pt.h" #include "PoseLib/misc/essential.h" #include #include /** * Build a 9 x n matrix from bearing vector matches, where each row is equivalent to the * equation x'T*F*x = 0 for a single correspondence pair (x', x). * * Note that this does not resize the matrix A; it is expected to have the * appropriate size already (n x 9). */ void encode_epipolar_equation(const std::vector &x1, const std::vector &x2, Eigen::Matrix *A) { assert(x1.size() == x2.size()); assert(A->cols() == 9); assert(static_cast(A->rows()) == x1.size()); for (size_t i = 0; i < x1.size(); ++i) { A->row(i) << x2[i].x() * x1[i].transpose(), x2[i].y() * x1[i].transpose(), x2[i].z() * x1[i].transpose(); } } void poselib::essential_matrix_8pt(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *essential_matrix) { assert(8 <= x1.size()); using MatX9 = Eigen::Matrix; MatX9 epipolar_constraint(x1.size(), 9); encode_epipolar_equation(x1, x2, &epipolar_constraint); using RMat3 = Eigen::Matrix; Eigen::Matrix3d E; if (x1.size() == 8) { // In the case where we have exactly 8 correspondences, there is no need to compute the SVD Eigen::Matrix Q = epipolar_constraint.transpose().householderQr().householderQ(); Eigen::Matrix e = Q.col(8); E = Eigen::Map(e.data()); } else { Eigen::SelfAdjointEigenSolver> solver(epipolar_constraint.transpose() * epipolar_constraint); E = Eigen::Map(solver.eigenvectors().leftCols<1>().data()); } // Find the closest essential matrix to E in frobenius norm // E = UD'VT Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Vector3d d = USV.singularValues(); const double a = d[0]; const double b = d[1]; d << (a + b) / 2., (a + b) / 2., 0.0; E = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose(); (*essential_matrix) = E; } int poselib::relpose_8pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output) { Eigen::Matrix3d essential_matrix; essential_matrix_8pt(x1, x2, &essential_matrix); // Generate plausible relative motion from E output->clear(); poselib::motion_from_essential(essential_matrix, x1, x2, output); return output->size(); } PoseLib-2.0.5/PoseLib/solvers/relpose_8pt.h000066400000000000000000000046121504452766400205420ustar00rootroot00000000000000// Copyright (c) 2020, Pierre Moulon // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RELPOSE_8PT_H_ #define POSELIB_RELPOSE_8PT_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { // Relative pose from eight to n bearing vector correspondences. // Port from OpenMVG (Essential_matrix computation then decomposition in 4 pose [R|t]). int relpose_8pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output); // Computation of essential matrix from eight to n bearing vector correspondences. // See page 294 in [HZ] Result 11.1. // [HZ] Multiple View Geometry - Richard Hartley, Andrew Zisserman - second edition // Port from OpenMVG void essential_matrix_8pt(const std::vector &x1, const std::vector &x2, Eigen::Matrix3d *essential_matrix); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/relpose_upright_3pt.cc000066400000000000000000000173321504452766400224400ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Author: Yaqing Ding // Mark Shachkov #include "relpose_upright_3pt.h" #include "PoseLib/misc/univariate.h" #include "p3p_common.h" namespace poselib { int relpose_upright_3pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output) { Eigen::Vector3d u1 = x1[0].cross(x1[1]); Eigen::Vector3d v1 = x2[1].cross(x2[0]); Eigen::Vector3d u2 = x1[0].cross(x1[2]); Eigen::Vector3d v2 = x2[2].cross(x2[0]); double a[12]; a[0] = u1(0); a[1] = u1(1); a[2] = u1(2); a[3] = x2[1](0); a[4] = x2[1](1); a[5] = x2[1](2); a[6] = v1(0); a[7] = v1(1); a[8] = v1(2); a[9] = x1[1](0); a[10] = x1[1](1); a[11] = x1[1](2); double b[12]; b[0] = u2(0); b[1] = u2(1); b[2] = u2(2); b[3] = x2[2](0); b[4] = x2[2](1); b[5] = x2[2](2); b[6] = v2(0); b[7] = v2(1); b[8] = v2(2); b[9] = x1[2](0); b[10] = x1[2](1); b[11] = x1[2](2); double m[12]; m[0] = a[1] * a[4]; m[1] = a[0] * a[3] + a[2] * a[5]; m[2] = a[2] * a[3] - a[0] * a[5]; m[5] = a[8] * a[9] - a[6] * a[11]; m[4] = -a[6] * a[9] - a[8] * a[11]; m[3] = -a[7] * a[10]; m[8] = b[2] * b[3] - b[0] * b[5]; m[7] = b[0] * b[3] + b[2] * b[5]; m[6] = b[1] * b[4]; m[11] = b[8] * b[9] - b[6] * b[11]; m[10] = -b[6] * b[9] - b[8] * b[11]; m[9] = -b[7] * b[10]; Eigen::Matrix3d D1, D2; // first conic D1 << m[0] * m[9] - m[3] * m[6], (m[0] * m[10] + m[1] * m[9] - m[3] * m[7] - m[4] * m[6]) * 0.5, (m[0] * m[11] + m[2] * m[9] - m[3] * m[8] - m[5] * m[6]) * 0.5, (m[0] * m[10] + m[1] * m[9] - m[3] * m[7] - m[4] * m[6]) * 0.5, m[1] * m[10] - m[4] * m[7], (m[1] * m[11] + m[2] * m[10] - m[4] * m[8] - m[5] * m[7]) * 0.5, (m[0] * m[11] + m[2] * m[9] - m[3] * m[8] - m[5] * m[6]) * 0.5, (m[1] * m[11] + m[2] * m[10] - m[4] * m[8] - m[5] * m[7]) * 0.5, m[2] * m[11] - m[5] * m[8]; // circle D2 << -1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0; Eigen::Matrix3d DX1, DX2; DX1 << D1.col(1).cross(D1.col(2)), D1.col(2).cross(D1.col(0)), D1.col(0).cross(D1.col(1)); DX2 << D2.col(1).cross(D2.col(2)), D2.col(2).cross(D2.col(0)), D2.col(0).cross(D2.col(1)); double k3 = D2.col(0).dot(DX2.col(0)); double k2 = (D1.array() * DX2.array()).sum(); double k1 = (D2.array() * DX1.array()).sum(); double k0 = D1.col(0).dot(DX1.col(0)); double k3_inv = 1.0 / k3; k2 *= k3_inv; k1 *= k3_inv; k0 *= k3_inv; double s; bool G = univariate::solve_cubic_single_real(k2, k1, k0, s); Eigen::Matrix3d C = D1 + s * D2; std::array pq = compute_pq(C); int n_sols = 0; for (int i = 0; i < 2; ++i) { // [p1 p2 p3] * [1; cos; sin] = 0 double p1 = pq[i](0); double p2 = pq[i](1); double p3 = pq[i](2); bool switch_23 = std::abs(p3) <= std::abs(p2); if (switch_23) { double w0 = -p1 / p2; double w1 = -p3 / p2; // find intersections between line [p1 p2 p3] * [1; cos; sin] = 0 and circle sin^2+cos^2=1 double ca = 1.0 / (w1 * w1 + 1.0); double cb = 2.0 * w0 * w1 * ca; double cc = (w0 * w0 - 1.0) * ca; double taus[2]; if (!root2real(cb, cc, taus[0], taus[1])) continue; for (double sq : taus) { double cq = w0 + w1 * sq; double lambda = -(m[0] + m[1] * cq + m[2] * sq) / (m[3] + m[4] * cq + m[5] * sq); if (lambda < 0) continue; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d trans; trans = lambda * x2[0] - R * x1[0]; trans.normalize(); CameraPose pose(R, trans); output->push_back(pose); ++n_sols; } } else { double w0 = -p1 / p3; double w1 = -p2 / p3; double ca = 1.0 / (w1 * w1 + 1); double cb = 2.0 * w0 * w1 * ca; double cc = (w0 * w0 - 1.0) * ca; double taus[2]; if (!root2real(cb, cc, taus[0], taus[1])) continue; for (double cq : taus) { double sq = w0 + w1 * cq; double lambda = -(m[0] + m[1] * cq + m[2] * sq) / (m[3] + m[4] * cq + m[5] * sq); if (lambda < 0) continue; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d trans; trans = lambda * x2[0] - R * x1[0]; trans.normalize(); CameraPose pose(R, trans); output->push_back(pose); ++n_sols; } } if (n_sols > 0 && G) break; } return output->size(); } int relpose_upright_3pt(const std::vector &x1, const std::vector &x2, const Eigen::Vector3d &g_cam1, const Eigen::Vector3d &g_cam2, CameraPoseVector *output) { // Rotate camera world coordinate system Eigen::Matrix3d Rc1 = Eigen::Quaterniond::FromTwoVectors(g_cam1, Eigen::Vector3d::UnitY()).toRotationMatrix(); Eigen::Matrix3d Rc2 = Eigen::Quaterniond::FromTwoVectors(g_cam2, Eigen::Vector3d::UnitY()).toRotationMatrix(); std::vector x1_upright = x1; std::vector x2_upright = x2; for (int i = 0; i < 3; ++i) { x1_upright[i] = Rc1 * x1[i]; x2_upright[i] = Rc2 * x2[i]; } int n_sols = relpose_upright_3pt(x1_upright, x2_upright, output); // De-rotate coordinate systems for (int i = 0; i < n_sols; ++i) { Eigen::Matrix3d R = (*output)[i].R(); Eigen::Vector3d t = (*output)[i].t; t = Rc2.transpose() * t; R = Rc2.transpose() * R * Rc1; (*output)[i] = CameraPose(R, t); } return n_sols; } } // namespace poselib PoseLib-2.0.5/PoseLib/solvers/relpose_upright_3pt.h000066400000000000000000000047561504452766400223100ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RELPOSE_UPRIGHT_3PT_H_ #define POSELIB_RELPOSE_UPRIGHT_3PT_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { // Upright relative pose from three point correspondences, i.e. // R * (p1 + lambda1 * x1) + t = p2 + lambda2 * x2 // The implementation is based on finding intersections of two conics: // Y. Ding, J. Yang, V. Larsson, C. Olsson, K. Ã…ström, Revisiting the P3P Problem, CVPR 2023 int relpose_upright_3pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output); // Wrapper for non-upright gravity (g_cam = R*g_world), where g_world is cancelled out and thus not needed. int relpose_upright_3pt(const std::vector &x1, const std::vector &x2, const Eigen::Vector3d &g_cam1, const Eigen::Vector3d &g_cam2, CameraPoseVector *output); }; // namespace poselib #endif PoseLib-2.0.5/PoseLib/solvers/relpose_upright_planar_2pt.cc000066400000000000000000000100111504452766400237570ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "relpose_upright_planar_2pt.h" #include "PoseLib/misc/essential.h" inline bool recover_a_b(const Eigen::Matrix &C, double cos2phi, double sin2phi, Eigen::Vector2d &a, Eigen::Vector2d &b) { if (std::abs(cos2phi) >= 1.0) return false; const double inv_sq2 = 1.0 / std::sqrt(2.0); a << std::sqrt(1 + cos2phi) * inv_sq2, std::sqrt(1 - cos2phi) * inv_sq2; if (sin2phi < 0) a(1) = -a(1); b = C * a; return true; } int poselib::relpose_upright_planar_2pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output) { Eigen::Matrix A, B, C; Eigen::Vector2d a, b; A << x2[0](1) * x1[0](0), -x2[0](1) * x1[0](2), x2[1](1) * x1[1](0), -x2[1](1) * x1[1](2); B << x2[0](0) * x1[0](1), x2[0](2) * x1[0](1), x2[1](0) * x1[1](1), x2[1](2) * x1[1](1); C = B.inverse() * A; // There is a bug in the paper here where the factor 2 is missing from beta; const double alpha = C.col(0).dot(C.col(0)); const double beta = 2.0 * C.col(0).dot(C.col(1)); const double gamma = C.col(1).dot(C.col(1)); const double alphap = alpha - gamma; const double gammap = alpha + gamma - 2.0; double inv_norm = 1.0 / (alphap * alphap + beta * beta); const double disc2 = alphap * alphap + beta * beta - gammap * gammap; output->clear(); if (disc2 < 0) { // Degenerate situation. In this case we return the closest non-degen solution // See equation (27) in the paper inv_norm = std::sqrt(inv_norm); if (gammap < 0) inv_norm = -inv_norm; if (recover_a_b(C, -alphap * inv_norm, -beta * inv_norm, a, b)) { b.normalize(); motion_from_essential_planar(b(0), b(1), -a(0), a(1), x1, x2, output); } return output->size(); } const double disc = std::sqrt(disc2); // First set of solutions if (recover_a_b(C, (-alphap * gammap + beta * disc) * inv_norm, (-beta * gammap - alphap * disc) * inv_norm, a, b)) { motion_from_essential_planar(b(0), b(1), -a(0), a(1), x1, x2, output); } // Second set of solutions if (recover_a_b(C, (-alphap * gammap - beta * disc) * inv_norm, (-beta * gammap + alphap * disc) * inv_norm, a, b)) { motion_from_essential_planar(b(0), b(1), -a(0), a(1), x1, x2, output); } return output->size(); } PoseLib-2.0.5/PoseLib/solvers/relpose_upright_planar_2pt.h000066400000000000000000000043521504452766400236340ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RELPOSE_UPRIGHT_PLANAR_2PT_H_ #define POSELIB_RELPOSE_UPRIGHT_PLANAR_2PT_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { /** * Two-point algorithm for solving for the essential matrix from bearing * vector correspondences assuming upright images. * Implementation of [1] section 3.4. 2-point Algorithm (Line version) * * [1] "Fast and Reliable Minimal Relative Pose Estimation under Planar Motion" * Sunglok Choi, Jong-Hwan Kim, 2018 * */ int relpose_upright_planar_2pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/relpose_upright_planar_3pt.cc000066400000000000000000000047171504452766400240000ustar00rootroot00000000000000// Copyright (c) 2020, Pierre Moulon // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "relpose_upright_planar_3pt.h" #include "PoseLib/misc/essential.h" int poselib::relpose_upright_planar_3pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output) { // Build the action matrix -> see (6,7) in the paper Eigen::Matrix A; for (const int i : {0, 1, 2}) { const auto &bearing_a_i = x1[i]; const auto &bearing_b_i = x2[i]; A.col(i) << bearing_a_i.x() * bearing_b_i.y(), -bearing_a_i.z() * bearing_b_i.y(), -bearing_b_i.x() * bearing_a_i.y(), -bearing_b_i.z() * bearing_a_i.y(); } const Eigen::Matrix4d Q = A.householderQr().householderQ(); const Eigen::Vector4d nullspace = Q.col(3); output->clear(); motion_from_essential_planar(nullspace(2), nullspace(3), -nullspace(0), nullspace(1), x1, x2, output); return output->size(); } PoseLib-2.0.5/PoseLib/solvers/relpose_upright_planar_3pt.h000066400000000000000000000055601504452766400236370ustar00rootroot00000000000000// Copyright (c) 2020, Pierre Moulon // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_RELPOSE_UPRIGHT_PLANAR_3PT_H_ #define POSELIB_RELPOSE_UPRIGHT_PLANAR_3PT_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { /** * Three-point algorithm for solving for the essential matrix from bearing * vector correspondences assuming upright images. * Implementation of [1] section 3.3. Linear 3-point Algorithm * Note: this is an approximate solver, not a minimal solver * * [1] "Fast and Reliable Minimal Relative Pose Estimation under Planar Motion" * Sunglok Choi, Jong-Hwan Kim, 2018 * * [2] Street View Goes Indoors: Automatic Pose Estimation From Uncalibrated Unordered Spherical Panoramas. * Mohamed Aly and Jean-Yves Bouguet. * IEEE Workshop on Applications of Computer Vision (WACV), Colorado, January 2012. * * Comment [2] and [1] propose both a Direct Linear Method using 3 correspondences. * Note that they are using gravity axis and that [1] provides more details about the fact that * the linear formulation is not a minimal solver since it cannot enforce the * "Pythagorean" identity on sin^2(t) + cos^2(t) = 1 * * Reimplementation from OpenMVG to PoseLib */ int relpose_upright_planar_3pt(const std::vector &x1, const std::vector &x2, CameraPoseVector *output); }; // namespace poselib #endif PoseLib-2.0.5/PoseLib/solvers/ugp2p.cc000066400000000000000000000067121504452766400174740ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ugp2p.h" #include "PoseLib/misc/univariate.h" int poselib::ugp2p(const std::vector &p, const std::vector &x, const std::vector &X, poselib::CameraPoseVector *output) { Eigen::Matrix A; Eigen::Matrix b; A << -x[0](2), 0, x[0](0), x[0](2) * (X[0](0) + p[0](0)) - x[0](0) * (X[0](2) + p[0](2)), 0, -x[0](2), x[0](1), -x[0](2) * (X[0](1) - p[0](1)) - x[0](1) * (X[0](2) + p[0](2)), -x[1](2), 0, x[1](0), x[1](2) * (X[1](0) + p[1](0)) - x[1](0) * (X[1](2) + p[1](2)), 0, -x[1](2), x[1](1), -x[1](2) * (X[1](1) - p[1](1)) - x[1](1) * (X[1](2) + p[1](2)); b << -2 * X[0](0) * x[0](0) - 2 * X[0](2) * x[0](2), x[0](0) * (X[0](2) - p[0](2)) - x[0](2) * (X[0](0) - p[0](0)), -2 * X[0](0) * x[0](1), x[0](1) * (X[0](2) - p[0](2)) - x[0](2) * (X[0](1) - p[0](1)), -2 * X[1](0) * x[1](0) - 2 * X[1](2) * x[1](2), x[1](0) * (X[1](2) - p[1](2)) - x[1](2) * (X[1](0) - p[1](0)), -2 * X[1](0) * x[1](1), x[1](1) * (X[1](2) - p[1](2)) - x[1](2) * (X[1](1) - p[1](1)); // b = A.partialPivLu().solve(b); b = A.inverse() * b; const double c2 = b(3, 0); const double c3 = b(3, 1); double qq[2]; const int sols = univariate::solve_quadratic_real(1.0, c2, c3, qq); output->clear(); for (int i = 0; i < sols; ++i) { CameraPose pose; const double q = qq[i]; const double q2 = q * q; const double inv_norm = 1.0 / (1 + q2); const double cq = (1 - q2) * inv_norm; const double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d t; t = b.block<3, 1>(0, 0) * q + b.block<3, 1>(0, 1); t *= -inv_norm; output->emplace_back(R, t); } return sols; } PoseLib-2.0.5/PoseLib/solvers/ugp2p.h000066400000000000000000000035611504452766400173350ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UGP2P_H_ #define POSELIB_UGP2P_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { int ugp2p(const std::vector &p, const std::vector &x, const std::vector &X, CameraPoseVector *output); }; #endifPoseLib-2.0.5/PoseLib/solvers/ugp3ps.cc000066400000000000000000000105051504452766400176530ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ugp3ps.h" #include "PoseLib/misc/univariate.h" namespace poselib { int ugp3ps(const std::vector &p, const std::vector &x, const std::vector &X, poselib::CameraPoseVector *output, std::vector *output_scale, bool filter_solutions) { Eigen::Matrix A; Eigen::Matrix b; A << -x[0](2), 0, x[0](0), p[0](0) * x[0](2) - p[0](2) * x[0](0), X[0](0) * x[0](2) - X[0](2) * x[0](0), 0, -x[0](2), x[0](1), p[0](1) * x[0](2) - p[0](2) * x[0](1), -X[0](1) * x[0](2) - X[0](2) * x[0](1), -x[1](2), 0, x[1](0), p[1](0) * x[1](2) - p[1](2) * x[1](0), X[1](0) * x[1](2) - X[1](2) * x[1](0), 0, -x[1](2), x[1](1), p[1](1) * x[1](2) - p[1](2) * x[1](1), -X[1](1) * x[1](2) - X[1](2) * x[1](1), -x[2](2), 0, x[2](0), p[2](0) * x[2](2) - p[2](2) * x[2](0), X[2](0) * x[2](2) - X[2](2) * x[2](0); b << -2 * X[0](0) * x[0](0) - 2 * X[0](2) * x[0](2), X[0](2) * x[0](0) - X[0](0) * x[0](2), -2 * X[0](0) * x[0](1), X[0](2) * x[0](1) - X[0](1) * x[0](2), -2 * X[1](0) * x[1](0) - 2 * X[1](2) * x[1](2), X[1](2) * x[1](0) - X[1](0) * x[1](2), -2 * X[1](0) * x[1](1), X[1](2) * x[1](1) - X[1](1) * x[1](2), -2 * X[2](0) * x[2](0) - 2 * X[2](2) * x[2](2), X[2](2) * x[2](0) - X[2](0) * x[2](2); b = A.partialPivLu().solve(b); // b = A.inverse()*b; double c2 = b(4, 0); double c3 = b(4, 1); double qq[2]; int n_sols = univariate::solve_quadratic_real(1.0, c2, c3, qq); output->clear(); output_scale->clear(); double best_res = 0.0; double best_scale = 1.0; CameraPose best_pose; for (int i = 0; i < n_sols; ++i) { double q = qq[i]; double q2 = q * q; double inv_norm = 1.0 / (1 + q2); double cq = (1 - q2) * inv_norm; double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d t; t = b.block<3, 1>(0, 0) * q + b.block<3, 1>(0, 1); t *= -inv_norm; CameraPose pose(R, t); double scale = b(3, 0) * q + b(3, 1); scale *= -inv_norm; if (filter_solutions) { double res = std::abs(x[2].dot((R * X[2] + t - scale * p[2]).normalized())); if (res > best_res) { best_pose = pose; best_scale = scale; best_res = res; } } else { output->push_back(pose); output_scale->push_back(scale); } } if (filter_solutions && best_res > 0.0) { output->push_back(best_pose); output_scale->push_back(best_scale); } return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/ugp3ps.h000066400000000000000000000043211504452766400175140ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UGP3PS_H_ #define POSELIB_UGP3PS_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Solves for camera pose such that: scale*p+lambda*x = R*X+t // This is similar to the gp4ps problem but for upright cameras. // Note: this impl. assumes that x has been normalized. // If filter_solutions is true, only the best solution is returned. int ugp3ps(const std::vector &p, const std::vector &x, const std::vector &X, CameraPoseVector *output, std::vector *output_scale, bool filter_solutions = true); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/ugp4pl.cc000066400000000000000000000173111504452766400176470ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ugp4pl.h" #include "PoseLib/misc/qep.h" int poselib::ugp4pl(const std::vector &p, const std::vector &x, const std::vector &X, const std::vector &V, CameraPoseVector *output) { Eigen::Matrix M, C, K; Eigen::Matrix VX; for (int i = 0; i < 4; ++i) VX.col(i) = V[i].cross(X[i]); M(0, 0) = -V[0](1) * x[0](2) - V[0](2) * x[0](1); M(0, 1) = V[0](2) * x[0](0) - V[0](0) * x[0](2); M(0, 2) = V[0](0) * x[0](1) + V[0](1) * x[0](0); M(0, 3) = VX(1, 0) * x[0](1) - VX(0, 0) * x[0](0) - VX(2, 0) * x[0](2) + V[0](0) * p[0](1) * x[0](2) - V[0](0) * p[0](2) * x[0](1) + V[0](1) * p[0](0) * x[0](2) - V[0](1) * p[0](2) * x[0](0) + V[0](2) * p[0](0) * x[0](1) - V[0](2) * p[0](1) * x[0](0); M(1, 0) = -V[1](1) * x[1](2) - V[1](2) * x[1](1); M(1, 1) = V[1](2) * x[1](0) - V[1](0) * x[1](2); M(1, 2) = V[1](0) * x[1](1) + V[1](1) * x[1](0); M(1, 3) = VX(1, 1) * x[1](1) - VX(0, 1) * x[1](0) - VX(2, 1) * x[1](2) + V[1](0) * p[1](1) * x[1](2) - V[1](0) * p[1](2) * x[1](1) + V[1](1) * p[1](0) * x[1](2) - V[1](1) * p[1](2) * x[1](0) + V[1](2) * p[1](0) * x[1](1) - V[1](2) * p[1](1) * x[1](0); M(2, 0) = -V[2](1) * x[2](2) - V[2](2) * x[2](1); M(2, 1) = V[2](2) * x[2](0) - V[2](0) * x[2](2); M(2, 2) = V[2](0) * x[2](1) + V[2](1) * x[2](0); M(2, 3) = VX(1, 2) * x[2](1) - VX(0, 2) * x[2](0) - VX(2, 2) * x[2](2) + V[2](0) * p[2](1) * x[2](2) - V[2](0) * p[2](2) * x[2](1) + V[2](1) * p[2](0) * x[2](2) - V[2](1) * p[2](2) * x[2](0) + V[2](2) * p[2](0) * x[2](1) - V[2](2) * p[2](1) * x[2](0); M(3, 0) = -V[3](1) * x[3](2) - V[3](2) * x[3](1); M(3, 1) = V[3](2) * x[3](0) - V[3](0) * x[3](2); M(3, 2) = V[3](0) * x[3](1) + V[3](1) * x[3](0); M(3, 3) = VX(1, 3) * x[3](1) - VX(0, 3) * x[3](0) - VX(2, 3) * x[3](2) + V[3](0) * p[3](1) * x[3](2) - V[3](0) * p[3](2) * x[3](1) + V[3](1) * p[3](0) * x[3](2) - V[3](1) * p[3](2) * x[3](0) + V[3](2) * p[3](0) * x[3](1) - V[3](2) * p[3](1) * x[3](0); C(0, 0) = -2 * V[0](0) * x[0](1); C(0, 1) = 2 * V[0](0) * x[0](0) + 2 * V[0](2) * x[0](2); C(0, 2) = -2 * V[0](2) * x[0](1); C(0, 3) = 2 * VX(2, 0) * x[0](0) - 2 * VX(0, 0) * x[0](2) + 2 * V[0](0) * p[0](0) * x[0](1) - 2 * V[0](0) * p[0](1) * x[0](0) - 2 * V[0](2) * p[0](1) * x[0](2) + 2 * V[0](2) * p[0](2) * x[0](1); C(1, 0) = -2 * V[1](0) * x[1](1); C(1, 1) = 2 * V[1](0) * x[1](0) + 2 * V[1](2) * x[1](2); C(1, 2) = -2 * V[1](2) * x[1](1); C(1, 3) = 2 * VX(2, 1) * x[1](0) - 2 * VX(0, 1) * x[1](2) + 2 * V[1](0) * p[1](0) * x[1](1) - 2 * V[1](0) * p[1](1) * x[1](0) - 2 * V[1](2) * p[1](1) * x[1](2) + 2 * V[1](2) * p[1](2) * x[1](1); C(2, 0) = -2 * V[2](0) * x[2](1); C(2, 1) = 2 * V[2](0) * x[2](0) + 2 * V[2](2) * x[2](2); C(2, 2) = -2 * V[2](2) * x[2](1); C(2, 3) = 2 * VX(2, 2) * x[2](0) - 2 * VX(0, 2) * x[2](2) + 2 * V[2](0) * p[2](0) * x[2](1) - 2 * V[2](0) * p[2](1) * x[2](0) - 2 * V[2](2) * p[2](1) * x[2](2) + 2 * V[2](2) * p[2](2) * x[2](1); C(3, 0) = -2 * V[3](0) * x[3](1); C(3, 1) = 2 * V[3](0) * x[3](0) + 2 * V[3](2) * x[3](2); C(3, 2) = -2 * V[3](2) * x[3](1); C(3, 3) = 2 * VX(2, 3) * x[3](0) - 2 * VX(0, 3) * x[3](2) + 2 * V[3](0) * p[3](0) * x[3](1) - 2 * V[3](0) * p[3](1) * x[3](0) - 2 * V[3](2) * p[3](1) * x[3](2) + 2 * V[3](2) * p[3](2) * x[3](1); K(0, 0) = V[0](2) * x[0](1) - V[0](1) * x[0](2); K(0, 1) = V[0](0) * x[0](2) - V[0](2) * x[0](0); K(0, 2) = V[0](1) * x[0](0) - V[0](0) * x[0](1); K(0, 3) = VX(0, 0) * x[0](0) + VX(1, 0) * x[0](1) + VX(2, 0) * x[0](2) - V[0](0) * p[0](1) * x[0](2) + V[0](0) * p[0](2) * x[0](1) + V[0](1) * p[0](0) * x[0](2) - V[0](1) * p[0](2) * x[0](0) - V[0](2) * p[0](0) * x[0](1) + V[0](2) * p[0](1) * x[0](0); K(1, 0) = V[1](2) * x[1](1) - V[1](1) * x[1](2); K(1, 1) = V[1](0) * x[1](2) - V[1](2) * x[1](0); K(1, 2) = V[1](1) * x[1](0) - V[1](0) * x[1](1); K(1, 3) = VX(0, 1) * x[1](0) + VX(1, 1) * x[1](1) + VX(2, 1) * x[1](2) - V[1](0) * p[1](1) * x[1](2) + V[1](0) * p[1](2) * x[1](1) + V[1](1) * p[1](0) * x[1](2) - V[1](1) * p[1](2) * x[1](0) - V[1](2) * p[1](0) * x[1](1) + V[1](2) * p[1](1) * x[1](0); K(2, 0) = V[2](2) * x[2](1) - V[2](1) * x[2](2); K(2, 1) = V[2](0) * x[2](2) - V[2](2) * x[2](0); K(2, 2) = V[2](1) * x[2](0) - V[2](0) * x[2](1); K(2, 3) = VX(0, 2) * x[2](0) + VX(1, 2) * x[2](1) + VX(2, 2) * x[2](2) - V[2](0) * p[2](1) * x[2](2) + V[2](0) * p[2](2) * x[2](1) + V[2](1) * p[2](0) * x[2](2) - V[2](1) * p[2](2) * x[2](0) - V[2](2) * p[2](0) * x[2](1) + V[2](2) * p[2](1) * x[2](0); K(3, 0) = V[3](2) * x[3](1) - V[3](1) * x[3](2); K(3, 1) = V[3](0) * x[3](2) - V[3](2) * x[3](0); K(3, 2) = V[3](1) * x[3](0) - V[3](0) * x[3](1); K(3, 3) = VX(0, 3) * x[3](0) + VX(1, 3) * x[3](1) + VX(2, 3) * x[3](2) - V[3](0) * p[3](1) * x[3](2) + V[3](0) * p[3](2) * x[3](1) + V[3](1) * p[3](0) * x[3](2) - V[3](1) * p[3](2) * x[3](0) - V[3](2) * p[3](0) * x[3](1) + V[3](2) * p[3](1) * x[3](0); /* Eigen::Matrix eig_vecs; double eig_vals[8]; const int n_roots = qep::qep_sturm(M, C, K, eig_vals, &eig_vecs); */ // We know that (1+q^2) is a factor. Dividing by this gives degree 6 poly. Eigen::Matrix eig_vecs; double eig_vals[6]; const int n_roots = qep::qep_sturm_div_1_q2(M, C, K, eig_vals, &eig_vecs); output->clear(); for (int i = 0; i < n_roots; ++i) { poselib::CameraPose pose; const double q = eig_vals[i]; const double q2 = q * q; const double inv_norm = 1.0 / (1 + q2); const double cq = (1 - q2) * inv_norm; const double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; output->emplace_back(R, eig_vecs.col(i)); } return n_roots; } PoseLib-2.0.5/PoseLib/solvers/ugp4pl.h000066400000000000000000000043321504452766400175100ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UGP4PL_H_ #define POSELIB_UGP4PL_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { // Upright absolute pose from four point-line constraints, i.e. // p + lambda * x = R * (X + mu * V) + t // This problem is equivalent to upright generalized relative pose estimation // Sweeney et al., Solving for Relative Pose with a Partially Known Rotation is a Quadratic Eigenvalue Problem, 3DV // 2014 int ugp4pl(const std::vector &p, const std::vector &x, const std::vector &X, const std::vector &V, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/up1p1ll.cc000066400000000000000000000100361504452766400177270ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "up1p1ll.h" #include "PoseLib/misc/univariate.h" int poselib::up1p1ll(const Eigen::Vector3d &xp, const Eigen::Vector3d &Xp, const Eigen::Vector3d &l, const Eigen::Vector3d &X, const Eigen::Vector3d &V, CameraPoseVector *output) { const double c2 = V[1] * l[1] - V[0] * l[0] - V[2] * l[2]; const double c1 = 2 * V[2] * l[0] - 2 * V[0] * l[2]; const double c0 = V[0] * l[0] + V[1] * l[1] + V[2] * l[2]; double qq[2]; const int sols = univariate::solve_quadratic_real(c2, c1, c0, qq); Eigen::Matrix3d A; A.row(0) << xp(2), 0.0, -xp(0); A.row(1) << 0.0, xp(2), -xp(1); A.row(2) << l(0), l(1), l(2); Eigen::Matrix3d Ainv = A.inverse(); output->clear(); for (int i = 0; i < sols; ++i) { const double q = qq[i]; const double q2 = q * q; const double inv_norm = 1.0 / (1 + q2); const double cq = (1 - q2) * inv_norm; const double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d RXp = R * Xp; Eigen::Vector3d RX = R * X; Eigen::Vector3d b; b << A.row(0).dot(RXp), A.row(1).dot(RXp), A.row(2).dot(RX); Eigen::Vector3d t = -Ainv * b; output->emplace_back(R, t); } return sols; } int poselib::up1p1ll(const Eigen::Vector3d &xp, const Eigen::Vector3d &Xp, const Eigen::Vector3d &l, const Eigen::Vector3d &X, const Eigen::Vector3d &V, const Eigen::Vector3d &g_cam, const Eigen::Vector3d &g_world, CameraPoseVector *output) { // Rotate camera world coordinate system Eigen::Matrix3d Rc = Eigen::Quaterniond::FromTwoVectors(g_cam, Eigen::Vector3d::UnitY()).toRotationMatrix(); Eigen::Matrix3d Rw = Eigen::Quaterniond::FromTwoVectors(g_world, Eigen::Vector3d::UnitY()).toRotationMatrix(); Eigen::Vector3d xp_upright = Rc * xp; Eigen::Vector3d Xp_upright = Rw * Xp; Eigen::Vector3d l_upright = Rc * l; Eigen::Vector3d X_upright = Rw * X; Eigen::Vector3d V_upright = Rw * V; int n_sols = up1p1ll(xp_upright, Xp_upright, l_upright, X_upright, V_upright, output); // De-rotate coordinate systems for (int i = 0; i < n_sols; ++i) { Eigen::Matrix3d R = (*output)[i].R(); Eigen::Vector3d t = (*output)[i].t; t = Rc.transpose() * t; R = Rc.transpose() * R * Rw; (*output)[i] = CameraPose(R, t); } return n_sols; } PoseLib-2.0.5/PoseLib/solvers/up1p1ll.h000066400000000000000000000045351504452766400176000ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UP1P1LL_H_ #define POSELIB_UP1P1LL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // Point correspondence: // lambda * xp = R * Xp + t // Line correspondence: // l^T * (R * X + t) = 0 // l^T * R * V = 0 int up1p1ll(const Eigen::Vector3d &xp, const Eigen::Vector3d &Xp, const Eigen::Vector3d &l, const Eigen::Vector3d &X, const Eigen::Vector3d &V, CameraPoseVector *output); // Wrapper for non-upright gravity (g_cam = R*g_world) int up1p1ll(const Eigen::Vector3d &xp, const Eigen::Vector3d &Xp, const Eigen::Vector3d &l, const Eigen::Vector3d &X, const Eigen::Vector3d &V, const Eigen::Vector3d &g_cam, const Eigen::Vector3d &g_world, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/up1p2pl.cc000066400000000000000000000141511504452766400177360ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Author: Yaqing Ding #include "up1p2pl.h" #include "PoseLib/misc/univariate.h" #include "p3p_common.h" namespace poselib { int up1p2pl(const std::vector &xp, const std::vector &Xp, const std::vector &x, const std::vector &X0, const std::vector &V, CameraPoseVector *output) { Eigen::Matrix X; X << X0[0] - Xp[0], X0[1] - Xp[0]; Eigen::Vector3d z1 = xp[0].cross(x[0]); Eigen::Vector3d z2 = xp[0].cross(x[1]); Eigen::Vector3d z3 = V[0].cross(X.col(0)); Eigen::Vector3d z4 = V[1].cross(X.col(1)); double a[12]; a[0] = V[0](0) * z1(0) + V[0](2) * z1(2); a[1] = V[0](2) * z1(0) - V[0](0) * z1(2); a[2] = V[0](1) * z1(1); a[3] = x[1](0) * z4(0) + x[1](2) * z4(2); a[4] = x[1](0) * z4(2) - x[1](2) * z4(0); a[5] = x[1](1) * z4(1); a[6] = V[1](0) * z2(0) + V[1](2) * z2(2); a[7] = V[1](2) * z2(0) - V[1](0) * z2(2); a[8] = V[1](1) * z2(1); a[9] = x[0](0) * z3(0) + x[0](2) * z3(2); a[10] = -x[0](2) * z3(0) + x[0](0) * z3(2); a[11] = x[0](1) * z3(1); double b[6]; b[0] = a[0] * a[3] - a[6] * a[9]; b[1] = a[0] * a[4] + a[1] * a[3] - a[6] * a[10] - a[7] * a[9]; b[2] = a[0] * a[5] + a[2] * a[3] - a[6] * a[11] - a[8] * a[9]; b[3] = a[1] * a[4] - a[7] * a[10]; b[4] = a[1] * a[5] + a[2] * a[4] - a[7] * a[11] - a[8] * a[10]; b[5] = a[2] * a[5] - a[8] * a[11]; Eigen::Matrix3d D1, D2; // first conic D1 << b[5], b[2] / 2, b[4] / 2, b[2] / 2, b[0], b[1] / 2, b[4] / 2, b[1] / 2, b[3]; // circle D2 << -1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0; Eigen::Matrix3d DX1, DX2; DX1 << D1.col(1).cross(D1.col(2)), D1.col(2).cross(D1.col(0)), D1.col(0).cross(D1.col(1)); DX2 << D2.col(1).cross(D2.col(2)), D2.col(2).cross(D2.col(0)), D2.col(0).cross(D2.col(1)); double k3 = D2.col(0).dot(DX2.col(0)); double k2 = (D1.array() * DX2.array()).sum(); double k1 = (D2.array() * DX1.array()).sum(); double k0 = D1.col(0).dot(DX1.col(0)); double k3_inv = 1.0 / k3; k2 *= k3_inv; k1 *= k3_inv; k0 *= k3_inv; double s; bool G = univariate::solve_cubic_single_real(k2, k1, k0, s); Eigen::Matrix3d C = D1 + s * D2; std::array pq = compute_pq(C); output->clear(); int n_sols = 0; for (int i = 0; i < 2; ++i) { // [p1 p2 p3] * [1; cos; sin] = 0 double p1 = pq[i](0); double p2 = pq[i](1); double p3 = pq[i](2); bool switch_23 = std::abs(p3) <= std::abs(p2); if (switch_23) { double w0 = -p1 / p2; double w1 = -p3 / p2; // find intersections between line [p1 p2 p3] * [1; cos; sin] = 0 and circle sin^2+cos^2=1 double ca = 1.0 / (w1 * w1 + 1.0); double cb = 2.0 * w0 * w1 * ca; double cc = (w0 * w0 - 1.0) * ca; double taus[2]; if (!root2real(cb, cc, taus[0], taus[1])) continue; for (double sq : taus) { double cq = w0 + w1 * sq; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d a = x[0].cross(R * V[0]); Eigen::Vector3d b = R * X.col(0); double alpha = -a.dot(b) / a.dot(xp[0]); Eigen::Vector3d t = alpha * xp[0] - R * Xp[0]; output->emplace_back(R, t); ++n_sols; } } else { double w0 = -p1 / p3; double w1 = -p2 / p3; double ca = 1.0 / (w1 * w1 + 1); double cb = 2.0 * w0 * w1 * ca; double cc = (w0 * w0 - 1.0) * ca; double taus[2]; if (!root2real(cb, cc, taus[0], taus[1])) continue; for (double cq : taus) { double sq = w0 + w1 * cq; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d a = x[0].cross(R * V[0]); Eigen::Vector3d b = R * X.col(0); double alpha = -a.dot(b) / a.dot(xp[0]); Eigen::Vector3d t = alpha * xp[0] - R * Xp[0]; output->emplace_back(R, t); ++n_sols; } } if (G && n_sols > 0) break; } return output->size(); } } // namespace poselibPoseLib-2.0.5/PoseLib/solvers/up1p2pl.h000066400000000000000000000042251504452766400176010ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UP1P2PL_H_ #define POSELIB_UP1P2PL_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { // The implementation is based on finding intersections of two conics: // Y. Ding, J. Yang, V. Larsson, C. Olsson, K. Ã…ström, Revisiting the P3P Problem, CVPR 2023 int up1p2pl(const std::vector &xp, const std::vector &Xp, const std::vector &x, const std::vector &X, const std::vector &V, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/up2p.cc000066400000000000000000000103101504452766400173120ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "up2p.h" #include "PoseLib/misc/univariate.h" int poselib::up2p(const std::vector &x, const std::vector &X, poselib::CameraPoseVector *output) { Eigen::Matrix A; Eigen::Matrix b; A << -x[0](2), 0, x[0](0), X[0](0) * x[0](2) - X[0](2) * x[0](0), 0, -x[0](2), x[0](1), -X[0](1) * x[0](2) - X[0](2) * x[0](1), -x[1](2), 0, x[1](0), X[1](0) * x[1](2) - X[1](2) * x[1](0), 0, -x[1](2), x[1](1), -X[1](1) * x[1](2) - X[1](2) * x[1](1); b << -2 * X[0](0) * x[0](0) - 2 * X[0](2) * x[0](2), X[0](2) * x[0](0) - X[0](0) * x[0](2), -2 * X[0](0) * x[0](1), X[0](2) * x[0](1) - X[0](1) * x[0](2), -2 * X[1](0) * x[1](0) - 2 * X[1](2) * x[1](2), X[1](2) * x[1](0) - X[1](0) * x[1](2), -2 * X[1](0) * x[1](1), X[1](2) * x[1](1) - X[1](1) * x[1](2); // b = A.partialPivLu().solve(b); b = A.inverse() * b; const double c2 = b(3, 0); const double c3 = b(3, 1); double qq[2]; const int sols = univariate::solve_quadratic_real(1.0, c2, c3, qq); output->clear(); for (int i = 0; i < sols; ++i) { const double q = qq[i]; const double q2 = q * q; const double inv_norm = 1.0 / (1 + q2); const double cq = (1 - q2) * inv_norm; const double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; Eigen::Vector3d t; t = b.block<3, 1>(0, 0) * q + b.block<3, 1>(0, 1); t *= -inv_norm; output->emplace_back(R, t); } return sols; } int poselib::up2p(const std::vector &x, const std::vector &X, const Eigen::Vector3d &g_cam, const Eigen::Vector3d &g_world, CameraPoseVector *output) { // Rotate camera world coordinate system Eigen::Matrix3d Rc = Eigen::Quaterniond::FromTwoVectors(g_cam, Eigen::Vector3d::UnitY()).toRotationMatrix(); Eigen::Matrix3d Rw = Eigen::Quaterniond::FromTwoVectors(g_world, Eigen::Vector3d::UnitY()).toRotationMatrix(); std::vector x_upright = x; std::vector X_upright = X; for (int i = 0; i < 2; ++i) { x_upright[i] = Rc * x[i]; X_upright[i] = Rw * X[i]; } int n_sols = up2p(x_upright, X_upright, output); // De-rotate coordinate systems for (int i = 0; i < n_sols; ++i) { Eigen::Matrix3d R = (*output)[i].R(); Eigen::Vector3d t = (*output)[i].t; t = Rc.transpose() * t; R = Rc.transpose() * R * Rw; (*output)[i] = CameraPose(R, t); } return n_sols; } PoseLib-2.0.5/PoseLib/solvers/up2p.h000066400000000000000000000041041504452766400171600ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UP2P_H_ #define POSELIB_UP2P_H_ #include "PoseLib/camera_pose.h" #include #include namespace poselib { int up2p(const std::vector &x, const std::vector &X, CameraPoseVector *output); // Wrapper for non-upright gravity (g_cam = R*g_world) int up2p(const std::vector &x, const std::vector &X, const Eigen::Vector3d &g_cam, const Eigen::Vector3d &g_world, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/solvers/up4pl.cc000066400000000000000000000126211504452766400174770ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "up4pl.h" #include "PoseLib/misc/qep.h" int poselib::up4pl(const std::vector &x, const std::vector &X, const std::vector &V, CameraPoseVector *output) { Eigen::Matrix M, C, K; Eigen::Matrix VX; for (int i = 0; i < 4; ++i) VX.col(i) = V[i].cross(X[i]); M(0, 0) = -V[0](1) * x[0](2) - V[0](2) * x[0](1); M(0, 1) = V[0](2) * x[0](0) - V[0](0) * x[0](2); M(0, 2) = V[0](0) * x[0](1) + V[0](1) * x[0](0); M(0, 3) = VX(1, 0) * x[0](1) - VX(0, 0) * x[0](0) - VX(2, 0) * x[0](2); M(1, 0) = -V[1](1) * x[1](2) - V[1](2) * x[1](1); M(1, 1) = V[1](2) * x[1](0) - V[1](0) * x[1](2); M(1, 2) = V[1](0) * x[1](1) + V[1](1) * x[1](0); M(1, 3) = VX(1, 1) * x[1](1) - VX(0, 1) * x[1](0) - VX(2, 1) * x[1](2); M(2, 0) = -V[2](1) * x[2](2) - V[2](2) * x[2](1); M(2, 1) = V[2](2) * x[2](0) - V[2](0) * x[2](2); M(2, 2) = V[2](0) * x[2](1) + V[2](1) * x[2](0); M(2, 3) = VX(1, 2) * x[2](1) - VX(0, 2) * x[2](0) - VX(2, 2) * x[2](2); M(3, 0) = -V[3](1) * x[3](2) - V[3](2) * x[3](1); M(3, 1) = V[3](2) * x[3](0) - V[3](0) * x[3](2); M(3, 2) = V[3](0) * x[3](1) + V[3](1) * x[3](0); M(3, 3) = VX(1, 3) * x[3](1) - VX(0, 3) * x[3](0) - VX(2, 3) * x[3](2); C(0, 0) = -2 * V[0](0) * x[0](1); C(0, 1) = 2 * V[0](0) * x[0](0) + 2 * V[0](2) * x[0](2); C(0, 2) = -2 * V[0](2) * x[0](1); C(0, 3) = 2 * VX(2, 0) * x[0](0) - 2 * VX(0, 0) * x[0](2); C(1, 0) = -2 * V[1](0) * x[1](1); C(1, 1) = 2 * V[1](0) * x[1](0) + 2 * V[1](2) * x[1](2); C(1, 2) = -2 * V[1](2) * x[1](1); C(1, 3) = 2 * VX(2, 1) * x[1](0) - 2 * VX(0, 1) * x[1](2); C(2, 0) = -2 * V[2](0) * x[2](1); C(2, 1) = 2 * V[2](0) * x[2](0) + 2 * V[2](2) * x[2](2); C(2, 2) = -2 * V[2](2) * x[2](1); C(2, 3) = 2 * VX(2, 2) * x[2](0) - 2 * VX(0, 2) * x[2](2); C(3, 0) = -2 * V[3](0) * x[3](1); C(3, 1) = 2 * V[3](0) * x[3](0) + 2 * V[3](2) * x[3](2); C(3, 2) = -2 * V[3](2) * x[3](1); C(3, 3) = 2 * VX(2, 3) * x[3](0) - 2 * VX(0, 3) * x[3](2); K(0, 0) = V[0](2) * x[0](1) - V[0](1) * x[0](2); K(0, 1) = V[0](0) * x[0](2) - V[0](2) * x[0](0); K(0, 2) = V[0](1) * x[0](0) - V[0](0) * x[0](1); K(0, 3) = VX(0, 0) * x[0](0) + VX(1, 0) * x[0](1) + VX(2, 0) * x[0](2); K(1, 0) = V[1](2) * x[1](1) - V[1](1) * x[1](2); K(1, 1) = V[1](0) * x[1](2) - V[1](2) * x[1](0); K(1, 2) = V[1](1) * x[1](0) - V[1](0) * x[1](1); K(1, 3) = VX(0, 1) * x[1](0) + VX(1, 1) * x[1](1) + VX(2, 1) * x[1](2); K(2, 0) = V[2](2) * x[2](1) - V[2](1) * x[2](2); K(2, 1) = V[2](0) * x[2](2) - V[2](2) * x[2](0); K(2, 2) = V[2](1) * x[2](0) - V[2](0) * x[2](1); K(2, 3) = VX(0, 2) * x[2](0) + VX(1, 2) * x[2](1) + VX(2, 2) * x[2](2); K(3, 0) = V[3](2) * x[3](1) - V[3](1) * x[3](2); K(3, 1) = V[3](0) * x[3](2) - V[3](2) * x[3](0); K(3, 2) = V[3](1) * x[3](0) - V[3](0) * x[3](1); K(3, 3) = VX(0, 3) * x[3](0) + VX(1, 3) * x[3](1) + VX(2, 3) * x[3](2); /* Eigen::Matrix eig_vecs; double eig_vals[8]; const int n_roots = qep::qep_sturm(M, C, K, eig_vals, &eig_vecs); */ // We know that (1+q^2) is a factor. Dividing by this gives degree 6 poly. Eigen::Matrix eig_vecs; double eig_vals[6]; const int n_roots = qep::qep_sturm_div_1_q2(M, C, K, eig_vals, &eig_vecs); output->clear(); for (int i = 0; i < n_roots; ++i) { const double q = eig_vals[i]; const double q2 = q * q; const double inv_norm = 1.0 / (1 + q2); const double cq = (1 - q2) * inv_norm; const double sq = 2 * q * inv_norm; Eigen::Matrix3d R; R.setIdentity(); R(0, 0) = cq; R(0, 2) = sq; R(2, 0) = -sq; R(2, 2) = cq; output->emplace_back(R, eig_vecs.col(i)); } return n_roots; } PoseLib-2.0.5/PoseLib/solvers/up4pl.h000066400000000000000000000043251504452766400173430ustar00rootroot00000000000000// Copyright (c) 2020, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_UP4PL_H_ #define POSELIB_UP4PL_H_ #include "PoseLib/camera_pose.h" #include namespace poselib { // Upright absolute pose from four point-line constraints, i.e. // lambda * x = R * (X + mu * V) + t // This problem is equivalent to upright generalized relative pose estimation // (where only one camera is generalized) // Sweeney et al., Solving for Relative Pose with a Partially Known Rotation is a Quadratic Eigenvalue Problem, 3DV // 2014 int up4pl(const std::vector &x, const std::vector &X, const std::vector &V, CameraPoseVector *output); }; // namespace poselib #endifPoseLib-2.0.5/PoseLib/types.h000066400000000000000000000077731504452766400157600ustar00rootroot00000000000000// Copyright (c) 2021, Viktor Larsson // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the copyright holder nor the // names of its contributors may be used to endorse or promote products // derived from this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef POSELIB_ROBUST_TYPES_H_ #define POSELIB_ROBUST_TYPES_H_ #include "alignment.h" #include #include namespace poselib { struct RansacOptions { size_t max_iterations = 100000; size_t min_iterations = 1000; double dyn_num_trials_mult = 3.0; double success_prob = 0.9999; double max_reproj_error = 12.0; // used for 2D-3D matches double max_epipolar_error = 1.0; // used for 2D-2D matches unsigned long seed = 0; // If we should use PROSAC sampling. Assumes data is sorted bool progressive_sampling = false; size_t max_prosac_iterations = 100000; // Whether we should use real focal length checking: https://arxiv.org/abs/2311.16304 // Assumes that principal points of both cameras are at origin. bool real_focal_check = false; // Whether to treat the input 'best_model' as an initial model and score it before running the main RANSAC loop bool score_initial_model = false; }; struct RansacStats { size_t refinements = 0; size_t iterations = 0; size_t num_inliers = 0; double inlier_ratio = 0; double model_score = std::numeric_limits::max(); }; struct BundleOptions { size_t max_iterations = 100; enum LossType { TRIVIAL, TRUNCATED, HUBER, CAUCHY, // This is the TR-IRLS scheme from Le and Zach, 3DV 2021 TRUNCATED_LE_ZACH } loss_type = LossType::CAUCHY; double loss_scale = 1.0; double gradient_tol = 1e-10; double step_tol = 1e-8; double initial_lambda = 1e-3; double min_lambda = 1e-10; double max_lambda = 1e10; bool verbose = false; }; struct BundleStats { size_t iterations = 0; double initial_cost; double cost; double lambda; size_t invalid_steps; double step_norm; double grad_norm; }; typedef Eigen::Vector2d Point2D; typedef Eigen::Vector3d Point3D; // Used to store pairwise matches for generalized pose estimation struct PairwiseMatches { EIGEN_MAKE_ALIGNED_OPERATOR_NEW size_t cam_id1, cam_id2; std::vector x1, x2; }; struct Line2D { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Line2D() {} Line2D(const Eigen::Vector2d &e1, const Eigen::Vector2d &e2) : x1(e1), x2(e2) {} Eigen::Vector2d x1, x2; }; struct Line3D { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Line3D() {} Line3D(const Eigen::Vector3d &e1, const Eigen::Vector3d &e2) : X1(e1), X2(e2) {} Eigen::Vector3d X1, X2; }; } // namespace poselib #endifPoseLib-2.0.5/PoseLib/version.h.in000066400000000000000000000006031504452766400166670ustar00rootroot00000000000000#ifndef @PROJECT_NAME_UPPERCASE@_VERSION_ #define @PROJECT_NAME_UPPERCASE@_VERSION_ #define @PROJECT_NAME_UPPERCASE@_MAJOR_VERSION (@MAJOR_VERSION@) #define @PROJECT_NAME_UPPERCASE@_MINOR_VERSION (@MINOR_VERSION@) #define @PROJECT_NAME_UPPERCASE@_PATCH_VERSION (@PATCH_VERSION@) #define @PROJECT_NAME_UPPERCASE@_VERSION "@PROJECT_VERSION@" #endif // @PROJECT_NAME_UPPERCASE@_VERSION_ PoseLib-2.0.5/README.md000066400000000000000000000501221504452766400143470ustar00rootroot00000000000000![GitHub release (latest by date)](https://img.shields.io/github/v/release/PoseLib/PoseLib) [![Conan Center](https://img.shields.io/conan/v/poselib)](https://conan.io/center/recipes/poselib) ![PyPI](https://img.shields.io/pypi/v/poselib) # PoseLib This library provides a collection of minimal solvers for camera pose estimation. The focus is on calibrated absolute pose estimation problems from different types of correspondences (e.g. point-point, point-line, line-point, line-line). The goals of this project are to provide * Fast and robust implementation of the current state-of-the-art solvers. * Consistent calling interface between different solvers. * Minimize dependencies, both external (currently only [Eigen](http://eigen.tuxfamily.org/)) and internal. Each solver is (mostly) stand-alone, making it easy to extract only a specific solver to integrate into other frameworks. * Robust estimators (based on LO-RANSAC) that just works out-of-the-box for most cases. # Robust Estimation and Non-linear Refinement We provide robust estimators for the most common problems * Absolute pose from points (and lines) * Essential / Fundamental matrix * Homography * Generalized relative pose It is fairly straight-forward to implement robust estimators for other problems. See for example [absolute_pose.h](PoseLib/robust/estimators/absolute_pose.h). If you implement estimators for other problems, please consider submitting a pull-request. In [robust.h](PoseLib/robust.h) we provide interfaces which normalizes the data, calls the RANSAC and runs a post-RANSAC non-linear refinement. It is also possible to directly call the individual components as well (see e.g. [ransac.h](PoseLib/robust/ransac.h), [bundle.h](PoseLib/robust/bundle.h), etc.). The RANSAC is straight-forward implementation of LO-RANSAC which generate hypothesis with minimal solvers and relies on non-linear refinement for refitting. The robust estimator takes the following options ```c++ struct RansacOptions { size_t max_iterations = 100000; size_t min_iterations = 1000; double dyn_num_trials_mult = 3.0; double success_prob = 0.9999; double max_reproj_error = 12.0; // used for 2D-3D matches double max_epipolar_error = 1.0; // used for 2D-2D matches unsigned long seed = 0; // If we should use PROSAC sampling. Assumes data is sorted bool progressive_sampling = false; size_t max_prosac_iterations = 100000; // Whether to use real focal length checking for F estimation: https://arxiv.org/abs/2311.16304 // Assumes that principal points of both cameras are at origin. bool real_focal_check = false; // Whether to treat the input 'best_model' as an initial model and score it before running the main RANSAC loop bool score_initial_model = false; }; ``` and the non-linear refinement ```c++ struct BundleOptions { size_t max_iterations = 100; enum LossType { TRIVIAL, TRUNCATED, HUBER, CAUCHY, TRUNCATED_LE_ZACH } loss_type = LossType::CAUCHY; double loss_scale = 1.0; double gradient_tol = 1e-8; double step_tol = 1e-8; double initial_lambda = 1e-3; double min_lambda = 1e-10; double max_lambda = 1e10; bool verbose = false; }; ``` Note that in [robust.h](PoseLib/robust.h) this is only used for the post-RANSAC refinement. In [bundle.h](PoseLib/robust/bundle.h) we provide non-linear refinement for different problems. Mainly minimizing reprojection error and Sampson error as these performed best in our internal evaluations. These are used in the LO-RANSAC to perform non-linear refitting. Most estimators directly minimize the MSAC score (using `loss_type = TRUNCATED` and `loss_scale = threshold`) over all input correspondences. In practice we found that this works quite well and avoids recursive LO where inliers are added in steps. ## Camera models PoseLib use [COLMAP](https://colmap.github.io/cameras.html)-compatible camera models. These are defined in [colmap_models.h](PoseLib/misc/colmap_models.h). Currently we only support * SIMPLE_PINHOLE * PINHOLE * SIMPLE_RADIAL * RADIAL * OPENCV * OPENCV_FISHEYE but it is relatively straight-forward to add other models. If you do so please consider opening a pull-request. In contrast to COLMAP, we require analytical jacobians for the distortion mappings which make it a bit more work to port them. The `Camera` struct currently contains `width`/`height` fields, however these are not used anywhere in the code-base and are provided simply to be consistent with COLMAP. The `Camera` class also provides the helper function `initialize_from_txt(str)` which initializes the camera from a line given by the `cameras.txt` file of a COLMAP reconstruction. The python bindings also expose the `poselib.Camera` class with `focal(), focal_x(), focal_y(), model_name(), prinicipal_point()` read-only methods and a read-write `params` property, but currently this is only used as a return type for some methods. To supply camera information to robust estimators you should use python `dicts` as shown below. ## Python bindings The python bindings can be installed by running `pip install .`. The python bindings expose all minimal solvers, e.g. `poselib.p3p(x,X)`, as well as all robust estimators from [robust.h](PoseLib/robust.h). Examples of how the robust estimators can be called are ```python camera = {'model': 'SIMPLE_PINHOLE', 'width': 1200, 'height': 800, 'params': [960, 600, 400]} pose, info = poselib.estimate_absolute_pose(p2d, p3d, camera, {'max_reproj_error': 16.0}, {}) ``` or ```python F, info = poselib.estimate_fundamental_matrix(p2d_1, p2d_2, {'max_epipolar_error': 0.75, 'progressive_sampling': True}, {}) ``` The return value `info` is a dict containing information about the robust estimation (inliers, iterations, etc). The last two options are dicts which describe the `RansacOptions` and `BundleOptions`. Ommited values are set to their default (see above), except for the `loss_scale` used for the Cauchy loss which is set to half of the threshold used in RANSAC (which seems to be a good heuristic). Dicts with the default options can be obtained as `opt = poselib.RansacOptions()` or `poselib.BundleOptions()`. Some of the available estimators are listed below, check [pyposelib.cpp](pybind/pyposelib.cc) and [robust.h](PoseLib/robust.h) for more details. The table also shows which error threshold is used in the estimation (`RansacOptions.max_reproj_error` or `RansacOptions.max_epipolar_error`). All thresholds are given in pixels. | Method | Arguments | (RansacOptions) Threshold | | --- | --- | --- | | `estimate_absolute_pose` | `(p2d, p3d, camera, ransac_opt, bundle_opt, initial_pose=None)` | `max_reproj_error` | | `estimate_absolute_pose_pnpl` | `(p2d, p3d, l2d_1, l2d_2, l3d_1, l3d_2, camera, ransac_opt, bundle_opt, initial_pose=None)` | `max_reproj_error` (points), `max_epipolar_error` (lines) | | `estimate_generalized_absolute_pose` | `(p2ds, p3ds, camera_ext, cameras, ransac_opt, bundle_opt, initial_pose=None)` | `max_reproj_error` | | `estimate_relative_pose` | `(x1, x2, camera1, camera2, ransac_opt, bundle_opt, initial_pose=None)` | `max_epipolar_error` | | `estimate_shared_focal_relative_pose` | `(x1, x2, pp, ransac_opt, bundle_opt, initial_image_pair=None)` | `max_epipolar_error` | | `estimate_fundamental` | `(x1, x2, ransac_opt, bundle_opt, initial_F=None)` | `max_epipolar_error` | | `estimate_homography` | `(x1, x2, ransac_opt, bundle_opt, initial_H=None)` | `max_reproj_error` | | `estimate_generalized_relative_pose` | `(matches, camera1_ext, cameras1, camera2_ext, cameras2, ransac_opt, bundle_opt, initial_pose=None)` | `max_epipolar_error` | ### Storing poses and estimated camera parameters To handle poses and cameras we provide the following classes: - `CameraPose`: This class is the return type for the most of the methods. While the class internally represent the pose with `q` and `t`, it also exposes `R` (3x3) and `Rt` (3x4) which are read/write, i.e. you can do `pose.R = Rnew` and it will update the underlying quaternion `q`. - `Image`: Following COLMAP, this class stores information about the camera (`image.camera`) and its pose (`image.pose`) used to take an image. - `ImagePair`: This class holds information about two cameras (`image_pair.camera1`, `image_pair.camera2`) and their relative pose (`image_pair.pose`). This class is used as the return type for the `estimate_shared_focal_relative_pose` robust estimator. All of these are also exposed via python bindings as: `poselib.CameraPose, poselib.Image, poselib.ImagePair`. ### Benchmarking the robust estimators To sanity-check the robust estimators we benchmark against the LO-RANSAC implementation from [pycolmap](https://github.com/colmap/pycolmap). For all of the metrics higher is better (except for runtime). # Minimal Solvers ## Naming convention For the solver names we use a slightly non-standard notation where we denote the solver as 
pXpYplZlpWll
where the number of correspondences required is given by * Xp - 2D point to 3D point, * Ypl - 2D point to 3D line, * Zlp - 2D line to 3D point, * Wll - 2D line to 3D line. The prefix with `u` is for upright solvers and `g` for generalized camera solvers. Solvers that estimate focal length have the postfix with `f` and similarly `s` for solvers that estimate scale. ## Calling conventions All solvers return their solutions as a vector of `CameraPose` structs, which defined as ```c++ struct CameraPose { Eigen::Vector4d q; Eigen::Vector3d t; }; ``` where the rotation is representation as a quaternion `q` and the convention is that `[R t]` maps from the world coordinate system into the camera coordinate system. For 2D point to 3D point correspondences, the image points are represented as unit-length bearings vectors. The returned camera poses `(R,t)` then satisfies (for some `lambda`) ```c++ lambda * x[i] = R * X[i] + t ``` where `x[i]` is the 2D point and `X[i]` is the 3D point. Note that only the P3P solver filters solutions with negative `lambda`. Solvers that use point-to-point constraints take one vector with bearing vectors `x` and one vector with the corresponding 3D points `X`, e.g. for the P3P solver the function declaration is ```c++ int p3p(const std::vector &x, const std::vector &X, std::vector *output); ``` Each solver returns the number of real solutions found. For constraints with 2D lines, the lines are represented in homogeneous coordinates. In the case of 2D line to 3D point constraints, the returned camera poses then satisfies ```c++ l[i].transpose() * (R * X[i] + t) = 0 ``` where `l[i]` is the line and `X[i]` is the 3D point. For constraints with 3D lines, the lines are represented by a 3D point `X` and a bearing vector `V`. In the case of 2D point to 3D point constraints ```c++ lambda * x[i] = R * (X[i] + mu * V[i]) + t ``` for some values of `lambda` and `mu`. Similarly, for line to line constraints we have ```c++ l[i].transpose() * (R * (X[i] + mu * V[i]) + t) = 0 ``` ### Generalized Cameras For generalized cameras we represent the image rays similarly to the 3D lines above, with an offset `p` and a bearing vector `x`. For example, in the case of point-to-point correspondences we have ```c++ p[i] + lambda * x[i] = R * X[i] + t ``` In the case of unknown scale we also estimate `alpha` such that ```c++ alpha * p[i] + lambda * x[i] = R * X[i] + t ``` For example, the generalized pose and scale solver (from four points) has the following signature ```c++ int gp4ps(const std::vector &p, const std::vector &x, const std::vector &X, std::vector *output); ``` ### Upright Solvers For the upright solvers it assumed that the rotation is around the y-axis, i.e. ```c++ R = [a 0 -b; 0 1 0; b 0 a] ``` To use these solvers it necessary to pre-rotate the input such that this is satisfied. ## Implemented solvers The following solvers are currently implemented. ### Absolute Pose | Solver | Point-Point | Point-Line | Line-Point | Line-Line | Upright | Generalized | Approx. runtime | Max. solutions | Comment | | --- | :---: | :---: | :---: | :---: | :---: | :---: | :---: | :---: | --- | | `p3p` | 3 | 0 | 0| 0| | | 250 ns | 4 | Ding et al., (CVPR23) | | `gp3p` | 3 | 0 | 0| 0| | :heavy_check_mark: | 1.6 us | 8 | Kukelova et al., E3Q3 (CVPR16) | | `gp4ps` | 4 | 0 | 0| 0| | :heavy_check_mark: | 1.8 us | 8 | Unknown scale.
Kukelova et al., E3Q3 (CVPR16)
Camposeco et al.(ECCV16) | | `p4pf` | 4 | 0 | 0| 0| | | 2.3 us | 8 | Unknown focal length.
Kukelova et al., E3Q3 (CVPR16) | | `p2p2pl` | 2 | 2 | 0| 0| | | 30 us | 16 | Josephson et al. (CVPR07) | | `p6lp` | 0 | 0 | 6| 0| | | 1.8 us | 8 | Kukelova et al., E3Q3 (CVPR16) | | `p5lp_radial` | 0 | 0 | 5| 0| | | 1 us | 4 | Kukelova et al., (ICCV13) | | `p2p1ll` | 2 | 0 | 0 | 1| | | 1.6 us | 8 | Kukelova et al., E3Q3 (CVPR16), Zhou et al. (ACCV18) | | `p1p2ll` | 1 | 0 | 0 | 2| | | 1.7 us | 8 | Kukelova et al., E3Q3 (CVPR16), Zhou et al. (ACCV18) | | `p3ll` | 0 | 0 | 0 | 3| | | 1.8 us | 8 | Kukelova et al., E3Q3 (CVPR16), Zhou et al. (ACCV18) | | `up2p` | 2 | 0 | 0| 0| :heavy_check_mark: | | 65 ns | 2 | Kukelova et al. (ACCV10) | | `ugp2p` | 2 | 0 | 0| 0| :heavy_check_mark: | :heavy_check_mark: | 65 ns | 2 | Adapted from Kukelova et al. (ACCV10) | | `ugp3ps` | 3 | 0 | 0| 0| :heavy_check_mark: | :heavy_check_mark: | 390 ns | 2 | Unknown scale. Adapted from Kukelova et al. (ACCV10) | | `up1p2pl` | 1 | 2 | 0| 0| :heavy_check_mark: | | 370 ns | 4 | | | `up4pl` | 0 | 4 | 0| 0| :heavy_check_mark: | | 1.4 us | 8 | Sweeney et al. (3DV14) | | `ugp4pl` | 0 | 4 | 0| 0| :heavy_check_mark: | :heavy_check_mark: | 1.4 us | 8 | Sweeney et al. (3DV14) | ### Relative Pose | Solver | Point-Point | Upright | Planar | Generalized | Approx. runtime | Max. solutions | Comment | | --- | :---: | :---: | :---: | :---: | :---: | :---: | --- | | `relpose_5pt` | 5 | | | | 5.5 us | 10 | Nister (PAMI 2004) | | `relpose_8pt` | 8+ | | | | 2.2+ us | 1 | | | `relpose_upright_3pt` | 3 | :heavy_check_mark: | | | 210 ns | 4 | Ding et al., (CVPR23) | | `gen_relpose_upright_4pt` | 4 | :heavy_check_mark: | | :heavy_check_mark: | 1.2 us | 6 | Sweeney et al. (3DV14) | | `relpose_upright_planar_2pt` | 2 | :heavy_check_mark: | :heavy_check_mark: | | 120 ns | 2 | Choi and Kim (IVC 2018) | | `relpose_upright_planar_3pt` | 3 | :heavy_check_mark: | :heavy_check_mark: | | 300 ns | 1 | Choi and Kim (IVC 2018) | | `gen_relpose_5p1pt` | 5+1 | | | :heavy_check_mark: | 5.5 us | 10 | E + 1pt to fix scale | | `relpose_6pt_shared_focal` | 6 | | | | 33 us | 15 | Stewénius et al. (IVC 2008) | ## Decompositions Poselib also provides methods and python bindings for decomposing fundamental matrices to obtain the focal lengths of the cameras and a method for decomposition of homography to poses and plane normals. | Method | Arguments | Output | Comment | |---|:---:|:---:|:---:| | `focals_from_fundamental` | `(F, pp1, pp2)` | `(cam1, cam2)` | Bougnoux (ICCV 1998) | | `focals_from_fundamental_iterative` | `(F, cam1_prior, cam2_prior, max_iters = 50, weights = {5e-4, 1.0, 5e-4, 1.0})` | `(cam1, cam2, iters)` | Kocur et al. (CVPR 2024) | | `motion_from_homography` | `(H)` | `(poses, normals)` | Adapted from Ma et al. (Springer 2004) | To obtain the focal lengths from the camera object you can use `focal = cam.focal()`. Note that both focal length methods can produce very inaccurate results and fail often such that the output focal lengths can be NaNs or negative numbers. If you need to estimate a focal length shared by both cameras (e.g. the same camera in both views) you should use `estimate_shared_focal_relative_pose`. If you use H obtained using correspondences in image coordinates from two cameras you need to use `K2_inv * H * K1` as input to `motion_from_homography`. ## How to compile? Getting the code: > git clone --recursive https://github.com/vlarsson/PoseLib.git > cd PoseLib Example of a local installation: > mkdir _build && cd _build > cmake -DCMAKE_INSTALL_PREFIX=../_install .. > cmake --build . --target install -j 8 (equivalent to 'make install -j8' in linux) Installed files: > tree ../_install . ├── bin │   └── benchmark ├── include │   └── PoseLib │   ├── solvers/gp3p.h │   ├── ... │   ├── poselib.h <== Library header (includes all the rest) │   ├── ... │   └── version.h └── lib ├── cmake │   └── PoseLib │   ├── PoseLibConfig.cmake │   ├── PoseLibConfigVersion.cmake │   ├── PoseLibTargets.cmake │   └── PoseLibTargets-release.cmake └── libPoseLib.a Uninstall library: > make uninstall ## Installation ### Installing PoseLib using Conan You can install pre-built binaries for PoseLib or build it from source using [Conan](https://conan.io/). Use the following command: ```bash conan install --requires="poselib/[*]" --build=missing ``` The PoseLib Conan recipe is kept up to date by Conan maintainers and community contributors. If the version is out of date, please [create an issue or pull request](https://github.com/conan-io/conan-center-index) on the ConanCenterIndex repository. ## Benchmark Conditional compilation of `benchmark` binary is controlled by `WITH_BENCHMARK` option. Default if OFF (without benchmark). Add `-DWITH_BENCHMARK=ON` to cmake to activate. > cmake -DWITH_BENCHMARK=ON .. ## Use library (as dependency) in an external project. cmake_minimum_required(VERSION 3.13) project(Foo) find_package(PoseLib REQUIRED) add_executable(foo foo.cpp) target_link_libraries(foo PRIVATE PoseLib::PoseLib) ## Citing If you are using the library for (scientific) publications, please cite the following source: ``` @misc{PoseLib, title = {{PoseLib - Minimal Solvers for Camera Pose Estimation}}, author = {Viktor Larsson and contributors}, URL = {https://github.com/vlarsson/PoseLib}, year = {2020} } ``` Please cite also the original publications of the different methods (see table above). ## Changelog 2.0.4 - Aug 5th 2024 * Added implementation of OpenCVFisheye camera model * Bumped pybind11 version which seems to fix some crashes * Added cmake option to disable -march=native 2.0.3 - Jul. 2024 * Added decomposition methods for estimation of focal lengths from fundamental matrices 2.0.2 - Apr. 2024 * Added solver and robust estimator for 6p relative pose with unknown shared focal length * Added Image, ImagePair classes with python bindings * Exposed Camera via python bindings 2.0.1 - Sep. 2023 * Refactor pybind such that `pip install .` works. Moved pybind11 to submodule. * C++ alignment fixes. Should now work with Eigen 3.3 and the header should be COLMAP compatible. 2.0 - Jan. 2022 * Added robust estimators (LO-RANSAC) and non-linear refinement * Refactored CameraPose to use quaternion instead 3x3 matrix. Removed alpha. * Implemented TR-IRLS method from Le and Zach (3DV 2021) * Restructured pybind11 interface * Added support for PROSAC sampling * Many minor fixes and improvements.... 1.0 - Jan. 2020 * Initial release ## License PoseLib is licensed under the BSD 3-Clause license. Please see [License](https://github.com/vlarsson/PoseLib/blob/master/LICENSE) for details. ## Acknowledgements The RANSAC implementation is heavily inspired by [RansacLib](https://github.com/tsattler/RansacLib) from Torsten Sattler. PoseLib-2.0.5/benchmark/000077500000000000000000000000001504452766400150225ustar00rootroot00000000000000PoseLib-2.0.5/benchmark/CMakeLists.txt000066400000000000000000000004061504452766400175620ustar00rootroot00000000000000add_executable(benchmark benchmark.cc problem_generator.cc ) target_link_libraries(benchmark PRIVATE PoseLib::PoseLib Eigen3::Eigen) set_compile_flags(benchmark) install( TARGETS benchmark RUNTIME DESTINATION bin COMPONENT "${INSTALL_BIN_DIR}" ) PoseLib-2.0.5/benchmark/benchmark.cc000066400000000000000000000450531504452766400172720ustar00rootroot00000000000000#include "benchmark.h" #include "problem_generator.h" #include #include #include namespace poselib { template BenchmarkResult benchmark(int n_problems, const ProblemOptions &options, double tol = 1e-6) { std::vector problem_instances; generate_abspose_problems(n_problems, &problem_instances, options); BenchmarkResult result; result.instances_ = n_problems; result.name_ = Solver::name(); if (options.additional_name_ != "") { result.name_ += options.additional_name_; } result.options_ = options; std::cout << "Running benchmark: " << result.name_ << std::flush; // Run benchmark where we check solution quality for (const AbsolutePoseProblemInstance &instance : problem_instances) { CameraPoseVector solutions; int sols = Solver::solve(instance, &solutions); double pose_error = std::numeric_limits::max(); result.solutions_ += sols; // std::cout << "\nGt: " << instance.pose_gt.R() << "\n"<< instance.pose_gt.t << "\n"; // std::cout << "gt valid = " << Solver::validator::is_valid(instance, instance.pose_gt, 1.0, tol) << "\n"; for (const CameraPose &pose : solutions) { if (Solver::validator::is_valid(instance, pose, 1.0, tol)) result.valid_solutions_++; // std::cout << "Pose: " << pose.R() << "\n" << pose.t << "\n"; pose_error = std::min(pose_error, Solver::validator::compute_pose_error(instance, pose, 1.0)); } if (pose_error < tol) result.found_gt_pose_++; } std::vector runtimes; CameraPoseVector solutions; for (int iter = 0; iter < 10; ++iter) { auto start_time = std::chrono::high_resolution_clock::now(); for (const AbsolutePoseProblemInstance &instance : problem_instances) { solutions.clear(); Solver::solve(instance, &solutions); } auto end_time = std::chrono::high_resolution_clock::now(); runtimes.push_back(std::chrono::duration_cast(end_time - start_time).count()); } std::sort(runtimes.begin(), runtimes.end()); result.runtime_ns_ = runtimes[runtimes.size() / 2]; std::cout << "\r \r"; return result; } template BenchmarkResult benchmark_w_extra(int n_problems, const ProblemOptions &options, double tol = 1e-6) { std::vector problem_instances; generate_abspose_problems(n_problems, &problem_instances, options); BenchmarkResult result; result.instances_ = n_problems; result.name_ = Solver::name(); if (options.additional_name_ != "") { result.name_ += options.additional_name_; } result.options_ = options; std::cout << "Running benchmark: " << result.name_ << std::flush; // Run benchmark where we check solution quality for (const AbsolutePoseProblemInstance &instance : problem_instances) { CameraPoseVector solutions; std::vector extra; int sols = Solver::solve(instance, &solutions, &extra); double pose_error = std::numeric_limits::max(); result.solutions_ += sols; for (size_t k = 0; k < solutions.size(); ++k) { if (Solver::validator::is_valid(instance, solutions[k], extra[k], tol)) result.valid_solutions_++; pose_error = std::min(pose_error, Solver::validator::compute_pose_error(instance, solutions[k], extra[k])); } if (pose_error < tol) result.found_gt_pose_++; } std::vector runtimes; CameraPoseVector solutions; std::vector extra; for (int iter = 0; iter < 10; ++iter) { auto start_time = std::chrono::high_resolution_clock::now(); for (const AbsolutePoseProblemInstance &instance : problem_instances) { solutions.clear(); extra.clear(); Solver::solve(instance, &solutions, &extra); } auto end_time = std::chrono::high_resolution_clock::now(); runtimes.push_back(std::chrono::duration_cast(end_time - start_time).count()); } std::sort(runtimes.begin(), runtimes.end()); result.runtime_ns_ = runtimes[runtimes.size() / 2]; std::cout << "\r \r"; return result; } template BenchmarkResult benchmark_relative(int n_problems, const ProblemOptions &options, double tol = 1e-6) { std::vector problem_instances; generate_relpose_problems(n_problems, &problem_instances, options); BenchmarkResult result; result.instances_ = n_problems; result.name_ = Solver::name(); if (options.additional_name_ != "") { result.name_ += options.additional_name_; } result.options_ = options; std::cout << "Running benchmark: " << result.name_ << std::flush; // Run benchmark where we check solution quality for (const RelativePoseProblemInstance &instance : problem_instances) { // CameraPoseVector solutions; std::vector solutions; int sols = Solver::solve(instance, &solutions); double pose_error = std::numeric_limits::max(); result.solutions_ += sols; // std::cout << "Gt: " << instance.pose_gt.R << "\n"<< instance.pose_gt.t << "\n"; for (const typename Solver::Solution &pose : solutions) { if (Solver::validator::is_valid(instance, pose, tol)) result.valid_solutions_++; // std::cout << "Pose: " << pose.R << "\n" << pose.t << "\n"; pose_error = std::min(pose_error, Solver::validator::compute_pose_error(instance, pose)); } if (pose_error < tol) result.found_gt_pose_++; } std::vector runtimes; std::vector solutions; for (int iter = 0; iter < 10; ++iter) { auto start_time = std::chrono::high_resolution_clock::now(); for (const RelativePoseProblemInstance &instance : problem_instances) { solutions.clear(); Solver::solve(instance, &solutions); } auto end_time = std::chrono::high_resolution_clock::now(); runtimes.push_back(std::chrono::duration_cast(end_time - start_time).count()); } std::sort(runtimes.begin(), runtimes.end()); result.runtime_ns_ = runtimes[runtimes.size() / 2]; std::cout << "\r \r"; return result; } template BenchmarkResult benchmark_homography(int n_problems, const ProblemOptions &options, double tol = 1e-6) { std::vector problem_instances; generate_homography_problems(n_problems, &problem_instances, options); BenchmarkResult result; result.instances_ = n_problems; result.name_ = Solver::name(); if (options.additional_name_ != "") { result.name_ += options.additional_name_; } result.options_ = options; std::cout << "Running benchmark: " << result.name_ << std::flush; // Run benchmark where we check solution quality for (const RelativePoseProblemInstance &instance : problem_instances) { std::vector solutions; int sols = Solver::solve(instance, &solutions); double hom_error = std::numeric_limits::max(); result.solutions_ += sols; // std::cout << "Gt: " << instance.pose_gt.R << "\n"<< instance.pose_gt.t << "\n"; for (const Eigen::Matrix3d &H : solutions) { if (Solver::validator::is_valid(instance, H, tol)) result.valid_solutions_++; // std::cout << "Pose: " << pose.R << "\n" << pose.t << "\n"; hom_error = std::min(hom_error, Solver::validator::compute_pose_error(instance, H)); } if (hom_error < tol) result.found_gt_pose_++; } std::vector runtimes; std::vector solutions; for (int iter = 0; iter < 10; ++iter) { auto start_time = std::chrono::high_resolution_clock::now(); for (const RelativePoseProblemInstance &instance : problem_instances) { solutions.clear(); Solver::solve(instance, &solutions); } auto end_time = std::chrono::high_resolution_clock::now(); runtimes.push_back(std::chrono::duration_cast(end_time - start_time).count()); } std::sort(runtimes.begin(), runtimes.end()); result.runtime_ns_ = runtimes[runtimes.size() / 2]; std::cout << "\r \r"; return result; } } // namespace poselib void print_runtime(double runtime_ns) { if (runtime_ns < 1e3) { std::cout << runtime_ns << " ns"; } else if (runtime_ns < 1e6) { std::cout << runtime_ns / 1e3 << " us"; } else if (runtime_ns < 1e9) { std::cout << runtime_ns / 1e6 << " ms"; } else { std::cout << runtime_ns / 1e9 << " s"; } } void display_result(const std::vector &results) { // Print PoseLib version and buidling type std::cout << "\n" << poselib_info() << "\n\n"; int w = 13; // display header std::cout << std::setw(2 * w) << "Solver"; std::cout << std::setw(w) << "Solutions"; std::cout << std::setw(w) << "Valid"; std::cout << std::setw(w) << "GT found"; std::cout << std::setw(w) << "Runtime\n"; for (int i = 0; i < w * 6; ++i) std::cout << "-"; std::cout << "\n"; int prec = 6; for (const poselib::BenchmarkResult &result : results) { double num_tests = static_cast(result.instances_); double solutions = result.solutions_ / num_tests; double valid_sols = result.valid_solutions_ / static_cast(result.solutions_) * 100.0; double gt_found = result.found_gt_pose_ / num_tests * 100.0; double runtime_ns = result.runtime_ns_ / num_tests; std::cout << std::setprecision(prec) << std::setw(2 * w) << result.name_; std::cout << std::setprecision(prec) << std::setw(w) << solutions; std::cout << std::setprecision(prec) << std::setw(w) << valid_sols; std::cout << std::setprecision(prec) << std::setw(w) << gt_found; std::cout << std::setprecision(prec) << std::setw(w - 3); print_runtime(runtime_ns); std::cout << "\n"; } } int main() { std::vector results; poselib::ProblemOptions options; // options.camera_fov_ = 45; // Narrow options.camera_fov_ = 75; // Medium // options.camera_fov_ = 120; // Wide double tol = 1e-6; // P3P poselib::ProblemOptions p3p_opt = options; p3p_opt.n_point_point_ = 3; p3p_opt.n_point_line_ = 0; results.push_back(poselib::benchmark(1e5, p3p_opt, tol)); results.push_back(poselib::benchmark(1e5, p3p_opt, tol)); // gP3P poselib::ProblemOptions gp3p_opt = options; gp3p_opt.n_point_point_ = 3; gp3p_opt.n_point_line_ = 0; gp3p_opt.generalized_ = true; results.push_back(poselib::benchmark(1e4, gp3p_opt, tol)); // gP4Ps poselib::ProblemOptions gp4p_opt = options; gp4p_opt.n_point_point_ = 4; gp4p_opt.n_point_line_ = 0; gp4p_opt.generalized_ = true; gp4p_opt.unknown_scale_ = true; results.push_back(poselib::benchmark_w_extra(1e4, gp4p_opt, tol)); // gP4Ps Quasi-degenerate case (same 3D point observed twice) gp4p_opt.generalized_duplicate_obs_ = true; gp4p_opt.additional_name_ = "(Deg.)"; results.push_back(poselib::benchmark_w_extra(1e4, gp4p_opt, tol)); // P4Pf poselib::ProblemOptions p4pf_opt = options; p4pf_opt.n_point_point_ = 4; p4pf_opt.n_point_line_ = 0; p4pf_opt.unknown_focal_ = true; results.push_back(poselib::benchmark_w_extra(1e4, p4pf_opt, tol)); // P2P2PL poselib::ProblemOptions p2p2pl_opt = options; p2p2pl_opt.n_point_point_ = 2; p2p2pl_opt.n_point_line_ = 2; results.push_back(poselib::benchmark(1e3, p2p2pl_opt, tol)); // P6LP poselib::ProblemOptions p6lp_opt = options; p6lp_opt.n_line_point_ = 6; results.push_back(poselib::benchmark(1e4, p6lp_opt, tol)); // P5LP Radial poselib::ProblemOptions p5lp_radial_opt = options; p5lp_radial_opt.n_line_point_ = 5; p5lp_radial_opt.radial_lines_ = true; results.push_back(poselib::benchmark(1e5, p5lp_radial_opt, tol)); // P2P1LL poselib::ProblemOptions p2p1ll_opt = options; p2p1ll_opt.n_point_point_ = 2; p2p1ll_opt.n_line_line_ = 1; results.push_back(poselib::benchmark(1e4, p2p1ll_opt, tol)); // P1P2LL poselib::ProblemOptions p1p2ll_opt = options; p1p2ll_opt.n_point_point_ = 1; p1p2ll_opt.n_line_line_ = 2; results.push_back(poselib::benchmark(1e4, p1p2ll_opt, tol)); // P3LL poselib::ProblemOptions p3ll_opt = options; p3ll_opt.n_line_line_ = 3; results.push_back(poselib::benchmark(1e4, p3ll_opt, tol)); // uP2P poselib::ProblemOptions up2p_opt = options; up2p_opt.n_point_point_ = 2; up2p_opt.n_point_line_ = 0; up2p_opt.upright_ = true; results.push_back(poselib::benchmark(1e6, up2p_opt, tol)); // uP1P1LL poselib::ProblemOptions up1p1ll_opt = options; up1p1ll_opt.n_point_point_ = 1; up1p1ll_opt.n_point_line_ = 0; up1p1ll_opt.n_line_line_ = 1; up1p1ll_opt.upright_ = true; results.push_back(poselib::benchmark(1e6, up1p1ll_opt, tol)); // uGP2P poselib::ProblemOptions ugp2p_opt = options; ugp2p_opt.n_point_point_ = 2; ugp2p_opt.n_point_line_ = 0; ugp2p_opt.upright_ = true; ugp2p_opt.generalized_ = true; results.push_back(poselib::benchmark(1e6, ugp2p_opt, tol)); // uGP3Ps poselib::ProblemOptions ugp3ps_opt = options; ugp3ps_opt.n_point_point_ = 3; ugp3ps_opt.n_point_line_ = 0; ugp3ps_opt.upright_ = true; ugp3ps_opt.generalized_ = true; ugp3ps_opt.unknown_scale_ = true; results.push_back(poselib::benchmark_w_extra(1e5, ugp3ps_opt, tol)); // uP1P2PL poselib::ProblemOptions up1p2pl_opt = options; up1p2pl_opt.n_point_point_ = 1; up1p2pl_opt.n_point_line_ = 2; up1p2pl_opt.upright_ = true; results.push_back(poselib::benchmark(1e4, up1p2pl_opt, tol)); // uP4PL poselib::ProblemOptions up4pl_opt = options; up4pl_opt.n_point_point_ = 0; up4pl_opt.n_point_line_ = 4; up4pl_opt.upright_ = true; results.push_back(poselib::benchmark(1e4, up4pl_opt, tol)); // ugP4PL poselib::ProblemOptions ugp4pl_opt = options; ugp4pl_opt.n_point_point_ = 0; ugp4pl_opt.n_point_line_ = 4; ugp4pl_opt.upright_ = true; ugp4pl_opt.generalized_ = true; results.push_back(poselib::benchmark(1e4, ugp4pl_opt, tol)); // Relative Pose Upright poselib::ProblemOptions relupright3pt_opt = options; relupright3pt_opt.n_point_point_ = 3; relupright3pt_opt.upright_ = true; results.push_back(poselib::benchmark_relative(1e4, relupright3pt_opt, tol)); // Generalized Relative Pose Upright poselib::ProblemOptions genrelupright4pt_opt = options; genrelupright4pt_opt.n_point_point_ = 4; genrelupright4pt_opt.upright_ = true; genrelupright4pt_opt.generalized_ = true; results.push_back(poselib::benchmark_relative(1e4, genrelupright4pt_opt, tol)); // Relative Pose 8pt poselib::ProblemOptions rel8pt_opt = options; rel8pt_opt.n_point_point_ = 8; results.push_back(poselib::benchmark_relative(1e4, rel8pt_opt, tol)); rel8pt_opt.additional_name_ = "(100 pts)"; rel8pt_opt.n_point_point_ = 100; results.push_back(poselib::benchmark_relative(1e4, rel8pt_opt, tol)); // Relative Pose 5pt poselib::ProblemOptions rel5pt_opt = options; rel5pt_opt.n_point_point_ = 5; results.push_back(poselib::benchmark_relative(1e4, rel5pt_opt, tol)); // Relative Pose With Single Unknown Focal 6pt poselib::ProblemOptions rel_focal_6pt_opt = options; rel_focal_6pt_opt.n_point_point_ = 6; rel_focal_6pt_opt.min_focal_ = 0.1; rel_focal_6pt_opt.max_focal_ = 5.0; rel_focal_6pt_opt.unknown_focal_ = true; results.push_back(poselib::benchmark_relative(1e4, rel_focal_6pt_opt, tol)); // Relative Pose Upright Planar 2pt poselib::ProblemOptions reluprightplanar2pt_opt = options; reluprightplanar2pt_opt.n_point_point_ = 2; reluprightplanar2pt_opt.upright_ = true; reluprightplanar2pt_opt.planar_ = true; results.push_back( poselib::benchmark_relative(1e4, reluprightplanar2pt_opt, tol)); // Relative Pose Upright Planar 3pt poselib::ProblemOptions reluprightplanar3pt_opt = options; reluprightplanar3pt_opt.n_point_point_ = 3; reluprightplanar3pt_opt.upright_ = true; reluprightplanar3pt_opt.planar_ = true; results.push_back( poselib::benchmark_relative(1e4, reluprightplanar3pt_opt, tol)); // Generalized Relative Pose (5+1pt) poselib::ProblemOptions genrel5p1pt_opt = options; genrel5p1pt_opt.n_point_point_ = 6; genrel5p1pt_opt.generalized_ = true; genrel5p1pt_opt.generalized_first_cam_obs_ = 5; results.push_back(poselib::benchmark_relative(1e4, genrel5p1pt_opt, tol)); // Generalized Relative Pose (6pt) poselib::ProblemOptions genrel6pt_opt = options; genrel6pt_opt.n_point_point_ = 6; genrel6pt_opt.generalized_ = true; results.push_back(poselib::benchmark_relative(1e3, genrel6pt_opt, tol)); // Homograpy (4pt) poselib::ProblemOptions homo4pt_opt = options; homo4pt_opt.n_point_point_ = 4; results.push_back(poselib::benchmark_homography>(1e5, homo4pt_opt, tol)); results.push_back(poselib::benchmark_homography>(1e5, homo4pt_opt, tol)); display_result(results); return 0; } PoseLib-2.0.5/benchmark/benchmark.h000066400000000000000000000253531504452766400171350ustar00rootroot00000000000000#pragma once #include "PoseLib/poselib.h" #include "problem_generator.h" #include #include #include #include #include namespace poselib { struct BenchmarkResult { std::string name_; ProblemOptions options_; int instances_ = 0; int solutions_ = 0; int valid_solutions_ = 0; int found_gt_pose_ = 0; int runtime_ns_ = 0; }; // Wrappers for the Benchmarking code struct SolverP3P { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p3p(instance.x_point_, instance.X_point_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p3p"; } }; struct SolverP3P_lambdatwist { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p3p_lambdatwist(instance.x_point_, instance.X_point_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p3p_lambdatwist"; } }; struct SolverP4PF { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions, std::vector *focals) { std::vector p2d(4, Eigen::Vector2d::Zero()); for (int i = 0; i < 4; ++i) { p2d[i] = instance.x_point_[i].hnormalized(); } return p4pf(p2d, instance.X_point_, solutions, focals, true); } typedef UnknownFocalValidator validator; static std::string name() { return "p4pf"; } }; struct SolverGP3P { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return gp3p(instance.p_point_, instance.x_point_, instance.X_point_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "gp3p"; } }; struct SolverGP4PS { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions, std::vector *scales) { return gp4ps(instance.p_point_, instance.x_point_, instance.X_point_, solutions, scales); } typedef CalibPoseValidator validator; static std::string name() { return "gp4ps"; } }; struct SolverP2P2PL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p2p2pl(instance.x_point_, instance.X_point_, instance.x_line_, instance.X_line_, instance.V_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p2p2pl"; } }; struct SolverP6LP { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p6lp(instance.l_line_point_, instance.X_line_point_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p6lp"; } }; struct SolverP5LP_Radial { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p5lp_radial(instance.l_line_point_, instance.X_line_point_, solutions); } typedef RadialPoseValidator validator; static std::string name() { return "p5lp_radial"; } }; struct SolverP2P1LL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p2p1ll(instance.x_point_, instance.X_point_, instance.l_line_line_, instance.X_line_line_, instance.V_line_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p2p1ll"; } }; struct SolverP1P2LL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p1p2ll(instance.x_point_, instance.X_point_, instance.l_line_line_, instance.X_line_line_, instance.V_line_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p1p2ll"; } }; struct SolverP3LL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return p3ll(instance.l_line_line_, instance.X_line_line_, instance.V_line_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "p3ll"; } }; struct SolverUP2P { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return up2p(instance.x_point_, instance.X_point_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "up2p"; } }; struct SolverUP1P1LL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return up1p1ll(instance.x_point_[0], instance.X_point_[0], instance.l_line_line_[0], instance.X_line_line_[0], instance.V_line_line_[0], solutions); } typedef CalibPoseValidator validator; static std::string name() { return "up1p1ll"; } }; struct SolverUGP2P { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return ugp2p(instance.p_point_, instance.x_point_, instance.X_point_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "ugp2p"; } }; struct SolverUGP3PS { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions, std::vector *scales) { return ugp3ps(instance.p_point_, instance.x_point_, instance.X_point_, solutions, scales); } typedef CalibPoseValidator validator; static std::string name() { return "ugp3ps"; } }; struct SolverUP1P2PL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return up1p2pl(instance.x_point_, instance.X_point_, instance.x_line_, instance.X_line_, instance.V_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "up1p2pl"; } }; struct SolverUP4PL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return up4pl(instance.x_line_, instance.X_line_, instance.V_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "up4pl"; } }; struct SolverUGP4PL { static inline int solve(const AbsolutePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return ugp4pl(instance.p_line_, instance.x_line_, instance.X_line_, instance.V_line_, solutions); } typedef CalibPoseValidator validator; static std::string name() { return "ugp4pl"; } }; struct SolverRelUpright3pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return relpose_upright_3pt(instance.x1_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "RelUpright3pt"; } }; struct SolverGenRelUpright4pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return gen_relpose_upright_4pt(instance.p1_, instance.x1_, instance.p2_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "GenRelUpright4pt"; } }; struct SolverRel8pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return relpose_8pt(instance.x1_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "Rel8pt"; } }; struct SolverSharedFocalRel6pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::ImagePairVector *solutions) { return relpose_6pt_shared_focal(instance.x1_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef ImagePair Solution; static std::string name() { return "SharedFocalRel6pt"; } }; struct SolverRel5pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return relpose_5pt(instance.x1_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "Rel5pt"; } }; struct SolverGenRel5p1pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return gen_relpose_5p1pt(instance.p1_, instance.x1_, instance.p2_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "GenRel5p1pt"; } }; struct SolverGenRel6pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return gen_relpose_6pt(instance.p1_, instance.x1_, instance.p2_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "GenRel6pt"; } }; struct SolverRelUprightPlanar2pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return relpose_upright_planar_2pt(instance.x1_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "RelUprightPlanar2pt"; } }; struct SolverRelUprightPlanar3pt { static inline int solve(const RelativePoseProblemInstance &instance, poselib::CameraPoseVector *solutions) { return relpose_upright_planar_3pt(instance.x1_, instance.x2_, solutions); } typedef CalibPoseValidator validator; typedef CameraPose Solution; static std::string name() { return "RelUprightPlanar3pt"; } }; template struct SolverHomography4pt { static inline int solve(const RelativePoseProblemInstance &instance, std::vector *solutions) { Eigen::Matrix3d H; int sols = homography_4pt(instance.x1_, instance.x2_, &H, CheiralCheck); solutions->clear(); if (sols == 1) { solutions->push_back(H); } return sols; } typedef HomographyValidator validator; static std::string name() { if (CheiralCheck) { return "Homography4pt(C)"; } else { return "Homography4pt"; } } }; } // namespace poselib PoseLib-2.0.5/benchmark/problem_generator.cc000066400000000000000000000570151504452766400210470ustar00rootroot00000000000000#include "problem_generator.h" #include #include #include #include #include namespace poselib { static const double kPI = 3.14159265358979323846; double CalibPoseValidator::compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale) { return (instance.pose_gt.R() - pose.R()).norm() + (instance.pose_gt.t - pose.t).norm() + std::abs(instance.scale_gt - scale); } double CalibPoseValidator::compute_pose_error(const RelativePoseProblemInstance &instance, const CameraPose &pose) { return (instance.pose_gt.R() - pose.R()).norm() + (instance.pose_gt.t - pose.t).norm(); } double CalibPoseValidator::compute_pose_error(const RelativePoseProblemInstance &instance, const ImagePair &image_pair) { return (instance.pose_gt.R() - image_pair.pose.R()).norm() + (instance.pose_gt.t - image_pair.pose.t).norm() + std::abs(instance.focal_gt - image_pair.camera1.focal()) / instance.focal_gt + std::abs(instance.focal_gt - image_pair.camera2.focal()) / instance.focal_gt; } bool CalibPoseValidator::is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale, double tol) { // Point to point correspondences // alpha * p + lambda*x = R*X + t for (int i = 0; i < instance.x_point_.size(); ++i) { double err = 1.0 - std::abs(instance.x_point_[i].dot( (pose.R() * instance.X_point_[i] + pose.t - scale * instance.p_point_[i]).normalized())); if (err > tol) return false; } // Point to Line correspondences // alpha * p + lambda * x = R*(X + mu*V) + t for (int i = 0; i < instance.x_line_.size(); ++i) { // lambda * x - mu * R*V = R*X + t - alpha * p // x.cross(R*V).dot(R*X+t-alpha.p) = 0 Eigen::Vector3d X = pose.R() * instance.X_line_[i] + pose.t - scale * instance.p_line_[i]; double err = instance.x_line_[i].cross(pose.R() * instance.V_line_[i]).normalized().dot(X); if (err > tol) return false; } // Line to point correspondences // l'*(R*X + t - alpha*p) = 0 for (int i = 0; i < instance.l_line_point_.size(); ++i) { Eigen::Vector3d X = pose.R() * instance.X_line_point_[i] + pose.t - scale * instance.p_line_point_[i]; double err = std::abs(instance.l_line_point_[i].dot(X.normalized())); if (err > tol) return false; } // Line to line correspondences // l'*(R*(X + mu*V) + t - alpha*p) = 0 for (int i = 0; i < instance.l_line_line_.size(); ++i) { Eigen::Vector3d X = pose.R() * instance.X_line_line_[i] + pose.t - scale * instance.p_line_line_[i]; Eigen::Vector3d V = pose.R() * instance.V_line_line_[i]; double err = std::abs(instance.l_line_line_[i].dot(X.normalized())) + std::abs(instance.l_line_line_[i].dot(V.normalized())); if (err > tol) return false; } return true; } bool CalibPoseValidator::is_valid(const RelativePoseProblemInstance &instance, const CameraPose &pose, double tol) { if ((pose.R().transpose() * pose.R() - Eigen::Matrix3d::Identity()).norm() > tol) return false; // Point to point correspondences // R * (alpha * p1 + lambda1 * x1) + t = alpha * p2 + lambda2 * x2 // // cross(R*x1, x2)' * (alpha * p2 - t - alpha * R*p1) = 0 for (int i = 0; i < instance.x1_.size(); ++i) { double err = std::abs(instance.x2_[i] .cross(pose.R() * instance.x1_[i]) .normalized() .dot(pose.R() * instance.p1_[i] + pose.t - instance.p2_[i])); if (err > tol) return false; } return true; } bool CalibPoseValidator::is_valid(const RelativePoseProblemInstance &instance, const ImagePair &image_pair, double tol) { if ((image_pair.pose.R().transpose() * image_pair.pose.R() - Eigen::Matrix3d::Identity()).norm() > tol) return false; Eigen::Matrix3d K_1_inv, K_2_inv; K_1_inv << 1.0 / image_pair.camera1.focal(), 0.0, 0.0, 0.0, 1.0 / image_pair.camera1.focal(), 0.0, 0.0, 0.0, 1.0; K_2_inv << 1.0 / image_pair.camera2.focal(), 0.0, 0.0, 0.0, 1.0 / image_pair.camera2.focal(), 0.0, 0.0, 0.0, 1.0; // Point to point correspondences // cross(R*x1, x2)' * - t = 0 // This currently works only for focal information from calib for (int i = 0; i < instance.x1_.size(); ++i) { Eigen::Vector3d x1_u = K_1_inv * instance.x1_[i]; Eigen::Vector3d x2_u = K_2_inv * instance.x2_[i]; double err = std::abs((x2_u.cross(image_pair.pose.R() * x1_u).dot(-image_pair.pose.t))); if (err > tol) return false; } // return is_valid(instance, image_pair.pose, tol) && (std::fabs(image_pair.camera.focal() - instance.focal_gt) < // tol); return true; } double HomographyValidator::compute_pose_error(const RelativePoseProblemInstance &instance, const Eigen::Matrix3d &H) { double err1 = (H.normalized() - instance.H_gt.normalized()).norm(); double err2 = (H.normalized() + instance.H_gt.normalized()).norm(); return std::min(err1, err2); } bool HomographyValidator::is_valid(const RelativePoseProblemInstance &instance, const Eigen::Matrix3d &H, double tol) { for (int i = 0; i < instance.x1_.size(); ++i) { Eigen::Vector3d z = H * instance.x1_[i]; double err = 1.0 - std::abs(z.normalized().dot(instance.x2_[i].normalized())); if (err > tol) return false; } return true; } double UnknownFocalValidator::compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double focal) { return (instance.pose_gt.R() - pose.R()).norm() + (instance.pose_gt.t - pose.t).norm() + std::abs(instance.focal_gt - focal); } bool UnknownFocalValidator::is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double focal, double tol) { if ((pose.R().transpose() * pose.R() - Eigen::Matrix3d::Identity()).norm() > tol) return false; if (focal < 0) return false; Eigen::Matrix3d Kinv; Kinv.setIdentity(); Kinv(2, 2) = focal; // lambda*diag(1,1,alpha)*x = R*X + t for (int i = 0; i < instance.x_point_.size(); ++i) { double err = 1.0 - std::abs((Kinv * instance.x_point_[i]) .normalized() .dot((pose.R() * instance.X_point_[i] + pose.t).normalized())); if (err > tol) return false; } return true; } double RadialPoseValidator::compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale) { // Only compute up to sign for radial cameras double err1 = (instance.pose_gt.R().topRows(2) - pose.R().topRows(2)).norm() + (instance.pose_gt.t.topRows(2) - pose.t.topRows(2)).norm(); double err2 = (instance.pose_gt.R().topRows(2) + pose.R().topRows(2)).norm() + (instance.pose_gt.t.topRows(2) + pose.t.topRows(2)).norm(); return std::min(err1, err2); } bool RadialPoseValidator::is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale, double tol) { if ((pose.R().transpose() * pose.R() - Eigen::Matrix3d::Identity()).norm() > tol) return false; // Point to point correspondences -- Convert these to line correspondences // alpha * p + lambda*x = R*X + t for (int i = 0; i < instance.x_point_.size(); ++i) { Eigen::Vector3d radial_line{-instance.x_point_[i](1), instance.x_point_[i](0), 0.0}; Eigen::Vector3d X = pose.R() * instance.X_point_[i] + pose.t; double err = std::abs(radial_line.dot(X.normalized())); if (err > tol) return false; } // Line to point correspondences // l'*(R*X + t) = 0 for (int i = 0; i < instance.l_line_point_.size(); ++i) { Eigen::Vector3d X = pose.R() * instance.X_line_point_[i] + pose.t; double err = std::abs(instance.l_line_point_[i].dot(X.normalized())); if (err > tol) return false; } return true; } void set_random_pose(CameraPose &pose, bool upright, bool planar) { if (upright) { Eigen::Vector2d r; r.setRandom().normalize(); Eigen::Matrix3d R; R << r(0), 0.0, r(1), 0.0, 1.0, 0.0, -r(1), 0.0, r(0); // y-gravity // pose.R << r(0), r(1), 0.0, -r(1), r(0), 0.0, 0.0, 0.0, 1.0; // z-gravity pose.q = rotmat_to_quat(R); } else { pose.q = Eigen::Quaternion::UnitRandom().coeffs(); } pose.t.setRandom(); if (planar) { pose.t.y() = 0; } } void generate_abspose_problems(int n_problems, std::vector *problem_instances, const ProblemOptions &options) { problem_instances->clear(); problem_instances->reserve(n_problems); double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0); // Random generators std::default_random_engine random_engine; std::uniform_real_distribution depth_gen(options.min_depth_, options.max_depth_); std::uniform_real_distribution coord_gen(-fov_scale, fov_scale); std::uniform_real_distribution scale_gen(options.min_scale_, options.max_scale_); std::uniform_real_distribution focal_gen(options.min_focal_, options.max_focal_); std::normal_distribution direction_gen(0.0, 1.0); std::normal_distribution offset_gen(0.0, 1.0); for (int i = 0; i < n_problems; ++i) { AbsolutePoseProblemInstance instance; set_random_pose(instance.pose_gt, options.upright_, options.planar_); if (options.unknown_scale_) { instance.scale_gt = scale_gen(random_engine); } if (options.unknown_focal_) { instance.focal_gt = focal_gen(random_engine); } // Point to point correspondences instance.x_point_.reserve(options.n_point_point_); instance.X_point_.reserve(options.n_point_point_); instance.p_point_.reserve(options.n_point_point_); for (int j = 0; j < options.n_point_point_; ++j) { Eigen::Vector3d p{0.0, 0.0, 0.0}; Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0}; x.normalize(); Eigen::Vector3d X; if (options.generalized_) { p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine); } X = instance.scale_gt * p + x * depth_gen(random_engine); X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t); if (options.unknown_focal_) { x.block<2, 1>(0, 0) *= instance.focal_gt; x.normalize(); } instance.x_point_.push_back(x); instance.X_point_.push_back(X); instance.p_point_.push_back(p); } // This generates instances where the same 3D point is observed twice in a generalized camera // This is degenerate case for the 3Q3 based gp3p/gp4ps solver unless specifically handled. if (options.generalized_ && options.generalized_duplicate_obs_) { std::vector ind = {0, 1, 2, 3}; assert(options.n_point_point_ >= 4); std::random_device rd; std::mt19937 g(rd()); std::shuffle(ind.begin(), ind.end(), g); instance.X_point_[ind[1]] = instance.X_point_[ind[0]]; instance.x_point_[ind[1]] = (instance.pose_gt.R() * instance.X_point_[ind[0]] + instance.pose_gt.t - instance.scale_gt * instance.p_point_[ind[1]]) .normalized(); } // Point to line correspondences instance.x_line_.reserve(options.n_point_line_); instance.X_line_.reserve(options.n_point_line_); instance.V_line_.reserve(options.n_point_line_); instance.p_line_.reserve(options.n_point_line_); for (int j = 0; j < options.n_point_line_; ++j) { Eigen::Vector3d p{0.0, 0.0, 0.0}; Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0}; x.normalize(); Eigen::Vector3d X; if (options.generalized_) { p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine); } X = instance.scale_gt * p + x * depth_gen(random_engine); X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t); Eigen::Vector3d V{direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)}; V.normalize(); // Translate X such that X.dot(V) = 0 X = X - V.dot(X) * V; if (options.unknown_focal_) { // TODO implement this. } instance.x_line_.push_back(x); instance.X_line_.push_back(X); instance.V_line_.push_back(V); instance.p_line_.push_back(p); } // Line to point correspondences instance.l_line_point_.reserve(options.n_line_point_); instance.X_line_point_.reserve(options.n_line_point_); instance.p_line_point_.reserve(options.n_line_point_); for (int j = 0; j < options.n_line_point_; ++j) { Eigen::Vector3d p{0.0, 0.0, 0.0}; Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0}; x.normalize(); Eigen::Vector3d X; if (options.generalized_) { p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine); } X = instance.scale_gt * p + x * depth_gen(random_engine); X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t); // Cross product with random vector to generate line Eigen::Vector3d l; if (options.radial_lines_) { // Line passing through image center l = x.cross(Eigen::Vector3d{0.0, 0.0, 1.0}); } else { // Random line l = x.cross(Eigen::Vector3d(direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine))); } l.normalize(); if (options.unknown_focal_) { // TODO implement this. } instance.l_line_point_.push_back(l); instance.X_line_point_.push_back(X); instance.p_line_point_.push_back(p); } // Line to line correspondences instance.l_line_line_.reserve(options.n_line_line_); instance.X_line_line_.reserve(options.n_line_line_); instance.V_line_line_.reserve(options.n_line_line_); instance.p_line_line_.reserve(options.n_line_line_); for (int j = 0; j < options.n_line_line_; ++j) { Eigen::Vector3d p{0.0, 0.0, 0.0}; Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0}; x.normalize(); Eigen::Vector3d X; if (options.generalized_) { p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine); } X = instance.scale_gt * p + x * depth_gen(random_engine); X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t); Eigen::Vector3d V{direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)}; V.normalize(); // Translate X such that X.dot(V) = 0 X = X - V.dot(X) * V; Eigen::Vector3d l = x.cross(instance.pose_gt.R() * V); l.normalize(); if (options.unknown_focal_) { // TODO implement this. } instance.l_line_line_.push_back(l); instance.X_line_line_.push_back(X); instance.V_line_line_.push_back(V); instance.p_line_line_.push_back(p); } problem_instances->push_back(instance); } } void generate_relpose_problems(int n_problems, std::vector *problem_instances, const ProblemOptions &options) { problem_instances->clear(); problem_instances->reserve(n_problems); double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0); // Random generators std::default_random_engine random_engine; std::uniform_real_distribution depth_gen(options.min_depth_, options.max_depth_); std::uniform_real_distribution coord_gen(-fov_scale, fov_scale); std::uniform_real_distribution scale_gen(options.min_scale_, options.max_scale_); std::uniform_real_distribution focal_gen(options.min_focal_, options.max_focal_); std::normal_distribution direction_gen(0.0, 1.0); std::normal_distribution offset_gen(0.0, 1.0); while (problem_instances->size() < n_problems) { RelativePoseProblemInstance instance; set_random_pose(instance.pose_gt, options.upright_, options.planar_); if (options.unknown_scale_) { instance.scale_gt = scale_gen(random_engine); } if (options.unknown_focal_) { instance.focal_gt = focal_gen(random_engine); } if (!options.generalized_) { instance.pose_gt.t.normalize(); } // Point to point correspondences instance.p1_.reserve(options.n_point_point_); instance.x1_.reserve(options.n_point_point_); instance.p2_.reserve(options.n_point_point_); instance.x2_.reserve(options.n_point_point_); for (int j = 0; j < options.n_point_point_; ++j) { Eigen::Vector3d p1{0.0, 0.0, 0.0}; Eigen::Vector3d p2{0.0, 0.0, 0.0}; Eigen::Vector3d x1{coord_gen(random_engine), coord_gen(random_engine), 1.0}; x1.normalize(); Eigen::Vector3d X; if (options.generalized_) { p1 << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine); p2 << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine); if (j > 0 && j < options.generalized_first_cam_obs_) { p1 = instance.p1_[0]; p2 = instance.p2_[0]; } } X = instance.scale_gt * p1 + x1 * depth_gen(random_engine); // Map into second image X = instance.pose_gt.R() * X + instance.pose_gt.t; Eigen::Vector3d x2 = (X - instance.scale_gt * p2).normalized(); if (options.unknown_focal_) { // We check whether all pts are in front of both cameras if (x2[2] < 0.0 || x1[2] < 0.0) break; x1[0] *= instance.focal_gt / x1[2]; x1[1] *= instance.focal_gt / x1[2]; x1[2] = 1.0; x2[0] *= instance.focal_gt / x2[2]; x2[1] *= instance.focal_gt / x2[2]; x2[2] = 1.0; } // TODO: ensure FoV of second cameras as well... instance.p1_.push_back(p1); instance.x1_.push_back(x1); instance.p2_.push_back(p2); instance.x2_.push_back(x2); } // we do not add instance if not all points were valid if (instance.x1_.size() < options.n_point_point_) continue; problem_instances->push_back(instance); } } void generate_homography_problems(int n_problems, std::vector *problem_instances, const ProblemOptions &options) { problem_instances->clear(); problem_instances->reserve(n_problems); double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0); // Random generators std::default_random_engine random_engine; std::uniform_real_distribution depth_gen(options.min_depth_, options.max_depth_); std::uniform_real_distribution coord_gen(-fov_scale, fov_scale); std::uniform_real_distribution scale_gen(options.min_scale_, options.max_scale_); std::uniform_real_distribution focal_gen(options.min_focal_, options.max_focal_); std::normal_distribution direction_gen(0.0, 1.0); std::normal_distribution offset_gen(0.0, 1.0); while (problem_instances->size() < n_problems) { RelativePoseProblemInstance instance; set_random_pose(instance.pose_gt, options.upright_, options.planar_); if (options.unknown_scale_) { instance.scale_gt = scale_gen(random_engine); } if (options.unknown_focal_) { instance.focal_gt = focal_gen(random_engine); } if (!options.generalized_) { instance.pose_gt.t.normalize(); } // Point to point correspondences instance.x1_.reserve(options.n_point_point_); instance.x2_.reserve(options.n_point_point_); // Generate plane Eigen::Vector3d n; n << direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine); n.normalize(); // Choose depth of plane such that center point of image 1 is at depth d double d_center = depth_gen(random_engine); double alpha = d_center / n(2); // plane is n'*X = alpha // ground truth homography instance.H_gt = alpha * instance.pose_gt.R() + instance.pose_gt.t * n.transpose(); bool failed_instance = false; for (int j = 0; j < options.n_point_point_; ++j) { bool point_okay = false; for (int trials = 0; trials < 10; ++trials) { Eigen::Vector3d x1{coord_gen(random_engine), coord_gen(random_engine), 1.0}; x1.normalize(); Eigen::Vector3d X; // compute depth double lambda = alpha / n.dot(x1); X = x1 * lambda; // Map into second image X = instance.pose_gt.R() * X + instance.pose_gt.t; Eigen::Vector3d x2 = X.normalized(); // Check cheirality if (x2(2) < 0 || lambda < 0) { // try to generate another point continue; } // Check FoV of second camera Eigen::Vector2d x2h = x2.hnormalized(); if (x2h(0) < -fov_scale || x2h(0) > fov_scale || x2h(1) < -fov_scale || x2h(1) > fov_scale) { // try to generate another point continue; } if (options.generalized_) { // NYI assert(false); } if (options.unknown_focal_) { // NYI assert(false); } instance.x1_.push_back(x1); instance.x2_.push_back(x2); point_okay = true; break; } if (!point_okay) { failed_instance = true; break; } } if (failed_instance) { continue; } problem_instances->push_back(instance); } } }; // namespace poselib PoseLib-2.0.5/benchmark/problem_generator.h000066400000000000000000000116271504452766400207100ustar00rootroot00000000000000#pragma once #include #include #include namespace poselib { struct AbsolutePoseProblemInstance { public: // Ground truth camera pose CameraPose pose_gt; double scale_gt = 1.0; double focal_gt = 1.0; // Point-to-point correspondences std::vector x_point_; std::vector X_point_; // Point-to-line correspondences std::vector x_line_; std::vector X_line_; std::vector V_line_; // Line-to-point correspondences std::vector l_line_point_; std::vector X_line_point_; // Line-to-line correspondences std::vector l_line_line_; std::vector X_line_line_; std::vector V_line_line_; // For generalized cameras we have an offset in the camera coordinate system std::vector p_point_; std::vector p_line_; std::vector p_line_point_; std::vector p_line_line_; }; struct RelativePoseProblemInstance { public: // Ground truth camera pose CameraPose pose_gt; Eigen::Matrix3d H_gt; // for homography problems double scale_gt = 1.0; double focal_gt = 1.0; // Point-to-point correspondences std::vector p1_; std::vector x1_; std::vector p2_; std::vector x2_; }; struct CalibPoseValidator { // Computes the distance to the ground truth pose static double compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale); static double compute_pose_error(const RelativePoseProblemInstance &instance, const CameraPose &pose); static double compute_pose_error(const RelativePoseProblemInstance &instance, const ImagePair &image_pair); // Checks if the solution is valid (i.e. is rotation matrix and satisfies projection constraints) static bool is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale, double tol); static bool is_valid(const RelativePoseProblemInstance &instance, const CameraPose &pose, double tol); static bool is_valid(const RelativePoseProblemInstance &instance, const ImagePair &image_pair, double tol); }; struct HomographyValidator { // Computes the distance to the ground truth pose static double compute_pose_error(const RelativePoseProblemInstance &instance, const Eigen::Matrix3d &H); // Checks if the solution is valid (i.e. is rotation matrix and satisfies projection constraints) static bool is_valid(const RelativePoseProblemInstance &instance, const Eigen::Matrix3d &H, double tol); }; struct UnknownFocalValidator { // Computes the distance to the ground truth pose static double compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double focal); // Checks if the solution is valid (i.e. is rotation matrix and satisfies projection constraints) static bool is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double focal, double tol); }; struct RadialPoseValidator { // Computes the distance to the ground truth pose static double compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale); // Checks if the solution is valid (i.e. is rotation matrix and satisfies projection constraints) static bool is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale, double tol); }; struct ProblemOptions { double min_depth_ = 0.1; double max_depth_ = 10.0; double camera_fov_ = 70.0; int n_point_point_ = 0; int n_point_line_ = 0; int n_line_point_ = 0; int n_line_line_ = 0; bool upright_ = false; bool planar_ = false; bool generalized_ = false; bool generalized_duplicate_obs_ = false; int generalized_first_cam_obs_ = 0; // how many of the points should from the first camera (relpose only) bool unknown_scale_ = false; bool unknown_focal_ = false; bool radial_lines_ = false; double min_scale_ = 0.1; double max_scale_ = 10.0; double min_focal_ = 100.0; double max_focal_ = 1000.0; std::string additional_name_ = ""; }; void set_random_pose(CameraPose &pose, bool upright, bool planar); void generate_abspose_problems(int n_problems, std::vector *problem_instances, const ProblemOptions &options); void generate_relpose_problems(int n_problems, std::vector *problem_instances, const ProblemOptions &options); void generate_homography_problems(int n_problems, std::vector *problem_instances, const ProblemOptions &options); }; // namespace poselib PoseLib-2.0.5/cmake/000077500000000000000000000000001504452766400141505ustar00rootroot00000000000000PoseLib-2.0.5/cmake/CMakeRegistry.cmake000066400000000000000000000052051504452766400176650ustar00rootroot00000000000000# # CMake Registry # # Export package to CMake registry such that it can be easily found by # CMake even if it has not been installed to a standard directory. # # Note: this feature is disabled by default. Possible options: # -DCMAKE_REGISTRY_FOLDER="OFF" (disable CMake registry [default]) # -DCMAKE_REGISTRY_FOLDER="INSTALL_FOLDER" # -DCMAKE_REGISTRY_FOLDER="BUILD_FOLDER" # if( NOT (DEFINED CMAKE_REGISTRY_FOLDER) ) set(CMAKE_REGISTRY_FOLDER "OFF" CACHE STRING "Choose CMake registry folder." FORCE) endif() # Set possible values for cmake-gui set_property(CACHE CMAKE_REGISTRY_FOLDER PROPERTY STRINGS "BUILD_FOLDER" "INSTALL_FOLDER" "OFF") message(STATUS "CMAKE_REGISTRY_FOLDER: ${CMAKE_REGISTRY_FOLDER}") # CMake Register (build directory) if(CMAKE_REGISTRY_FOLDER STREQUAL "BUILD_FOLDER") export(PACKAGE ${PROJECT_NAME}) endif() # # Register installed package with CMake # # This function adds an entry to the CMake registry for packages with the # path of the directory where the package configuration file of the installed # package is located in order to help CMake find the package in a custom # installation prefix. This differs from CMake's export(PACKAGE) command # which registers the build directory instead. # function (register_package PACKAGE_FOLDER) if (NOT IS_ABSOLUTE "${PACKAGE_FOLDER}") set (PACKAGE_FOLDER "${CMAKE_INSTALL_PREFIX}/${PACKAGE_FOLDER}") endif () string (MD5 REGISTRY_ENTRY "${PACKAGE_FOLDER}") if (WIN32) install (CODE "execute_process ( COMMAND reg add \"HKCU\\\\Software\\\\Kitware\\\\CMake\\\\Packages\\\\${PROJECT_NAME}\" /v \"${REGISTRY_ENTRY}\" /d \"${PACKAGE_FOLDER}\" /t REG_SZ /f RESULT_VARIABLE RT ERROR_VARIABLE ERR OUTPUT_QUIET ) if (RT EQUAL 0) message (STATUS \"Register: Added HKEY_CURRENT_USER\\\\Software\\\\Kitware\\\\CMake\\\\Packages\\\\${PROJECT_NAME}\\\\${REGISTRY_ENTRY}\") else () string (STRIP \"\${ERR}\" ERR) message (STATUS \"Register: Failed to add registry entry: \${ERR}\") endif ()" ) elseif (IS_DIRECTORY "$ENV{HOME}") file (WRITE "${PROJECT_BINARY_DIR}/${PROJECT_NAME}-registry-entry" "${PACKAGE_FOLDER}") install ( FILES "${PROJECT_BINARY_DIR}/${PROJECT_NAME}-registry-entry" DESTINATION "$ENV{HOME}/.cmake/packages/${PROJECT_NAME}" RENAME "${REGISTRY_ENTRY}" ) message (STATUS "CMake registry: $ENV{HOME}/.cmake/packages/${PROJECT_NAME}") endif () endfunction () # CMake Register (install directory) if(CMAKE_REGISTRY_FOLDER STREQUAL "INSTALL_FOLDER") register_package(${CONFIG_INSTALL_DIR}) endif () PoseLib-2.0.5/cmake/Config.cmake.in000066400000000000000000000004021504452766400167600ustar00rootroot00000000000000@PACKAGE_INIT@ if(NOT TARGET ${PROJECT_NAME}::${LIBRARY_NAME}) include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") endif() set(@PROJECT_NAME_UPPERCASE@_LIBRARIES @PROJECT_NAME@::@LIBRARY_NAME@) check_required_components("@PROJECT_NAME@") PoseLib-2.0.5/cmake/LibraryConfig.cmake000066400000000000000000000060171504452766400177100ustar00rootroot00000000000000# Target add_library(${LIBRARY_NAME} ${SOURCES} ${HEADERS_PUBLIC} ${HEADERS_PRIVATE} ) # Alias: # - PoseLib::PoseLib alias of PoseLib add_library(${PROJECT_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME}) # Add definitions for targets # Values: # - Debug : -DPOSELIB_DEBUG=1 # - Release: -DPOSELIB_DEBUG=0 # - others : -DPOSELIB_DEBUG=0 target_compile_definitions(${LIBRARY_NAME} PUBLIC "${PROJECT_NAME_UPPERCASE}_DEBUG=$") # Global includes. Used by all targets # Note: # - allow includes relative to source root directory: #include "type.h". # - headers can also be included with: #include # - add headers location: ${CMAKE_CURRENT_BINARY_DIR}/generated_headers target_include_directories( ${LIBRARY_NAME} PUBLIC "$" "$" "$" ) # Targets: # - /lib/libPoseLib.a # - header location after install: /PoseLib/poselib.h # - headers can be included by C++ code `#include ` install( TARGETS "${LIBRARY_NAME}" EXPORT "${TARGETS_EXPORT_NAME}" LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" INCLUDES DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" ) # Generate 'PoseLib/poselib.h' automatically from public headers foreach(file ${HEADERS_PUBLIC}) # get basename of each header file get_filename_component(basename ${file} NAME) # append '#include <...>' to the 'poselib.h' file # ToDo: set(...) creates a list separated with ';'. Find a different way. set(LIB_INCLUDES_STRING ${LIB_INCLUDES_STRING} "#include \n") endforeach(file) string(REPLACE ";" "" LIB_INCLUDES_STRING "${LIB_INCLUDES_STRING}") configure_file(${PROJECT_NAME_LOWERCASE}.h.in "${GENERATED_HEADERS_DIR}/PoseLib/poselib.h" @ONLY) # Headers: # - PoseLib/*.h -> /include/PoseLib/*.h foreach ( file ${HEADERS_PUBLIC} ) get_filename_component( dir ${file} DIRECTORY ) install( FILES ${file} DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/${LIBRARY_FOLDER}/${dir}" ) endforeach() install( FILES "${GENERATED_HEADERS_DIR}/PoseLib/poselib.h" DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/${LIBRARY_FOLDER}") # Headers: # - generated_headers/PoseLib/version.h -> /include/PoseLib/version.h install( FILES "${GENERATED_HEADERS_DIR}/${LIBRARY_FOLDER}/version.h" DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/${LIBRARY_FOLDER}" ) # Config # - /lib/cmake/PoseLib/PoseLibConfig.cmake # - /lib/cmake/PoseLib/PoseLibConfigVersion.cmake install( FILES "${PROJECT_CONFIG_FILE}" "${VERSION_CONFIG_FILE}" DESTINATION "${CONFIG_INSTALL_DIR}" ) # Config # - /lib/cmake/PoseLib/PoseLibTargets.cmake install( EXPORT "${TARGETS_EXPORT_NAME}" FILE "${PROJECT_NAME}Targets.cmake" DESTINATION "${CONFIG_INSTALL_DIR}" NAMESPACE "${PROJECT_NAME}::" ) PoseLib-2.0.5/cmake/SetEnv.cmake000066400000000000000000000106511504452766400163610ustar00rootroot00000000000000# Set PROJECT_NAME_UPPERCASE and PROJECT_NAME_LOWERCASE variables string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPERCASE) string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWERCASE) # Library name (by default is the project name) if(NOT LIBRARY_NAME) set(LIBRARY_NAME ${PROJECT_NAME_LOWERCASE}) endif() # Library folder name (by default is the project name in lowercase) # Example: #include if(NOT LIBRARY_FOLDER) set(LIBRARY_FOLDER ${PROJECT_NAME_LOWERCASE}) endif() # Make sure different configurations don't collide set(CMAKE_DEBUG_POSTFIX "d") # Select library type (default: STATIC) option(BUILD_SHARED_LIBS "Build ${LIBRARY_NAME} as a shared library." OFF) message(STATUS "BUILD_SHARED_LIBS: ${BUILD_SHARED_LIBS}") # Build type (default: RELEASE) # # No reason to set CMAKE_CONFIGURATION_TYPES if it's not a multiconfig generator # Also no reason of using CMAKE_BUILD_TYPE if it's a multiconfig generator. # get_property(isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) if(isMultiConfig) set(CMAKE_CONFIGURATION_TYPES "Release;Debug;MinSizeRel;RelWithDebInfo" CACHE STRING "" FORCE) message(STATUS "CMAKE_CONFIGURATION_TYPES: ${CMAKE_CONFIGURATION_TYPES}") message(STATUS "CMAKE_GENERATOR: Multi-config") else() # Set a default build type if none was specified if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) endif() # Set the possible values of build type for cmake-gui set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "MinSizeRel" "RelWithDebInfo") message(STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}") message(STATUS "CMAKE_GENERATOR: Single-config") endif() message(STATUS "CMAKE_GENERATOR: ${CMAKE_GENERATOR}") # Generated headers folder set(GENERATED_HEADERS_DIR "${CMAKE_CURRENT_BINARY_DIR}/generated_headers" ) # Create 'version.h' configure_file( "${PROJECT_SOURCE_DIR}/${LIBRARY_FOLDER}/version.h.in" "${GENERATED_HEADERS_DIR}/${LIBRARY_FOLDER}/version.h" @ONLY ) # Introduce variables: # - CMAKE_INSTALL_LIBDIR # - CMAKE_INSTALL_BINDIR # - CMAKE_INSTALL_INCLUDEDIR include(GNUInstallDirs) # Layout. This works for all platforms: # - /lib*/cmake/ # - /lib*/ # - /include/ set(CONFIG_INSTALL_DIR "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}") # Configuration set(GENERATED_DIR "${CMAKE_CURRENT_BINARY_DIR}/generated") set(VERSION_CONFIG_FILE "${GENERATED_DIR}/${PROJECT_NAME}ConfigVersion.cmake") set(PROJECT_CONFIG_FILE "${GENERATED_DIR}/${PROJECT_NAME}Config.cmake") set(TARGETS_EXPORT_NAME "${PROJECT_NAME}Targets") # Include module with functions: # - write_basic_package_version_file(...) # - configure_package_config_file(...) include(CMakePackageConfigHelpers) # Configure 'ConfigVersion.cmake' # Use: # - PROJECT_VERSION write_basic_package_version_file( "${VERSION_CONFIG_FILE}" VERSION "${${PROJECT_NAME}_VERSION}" COMPATIBILITY SameMajorVersion ) # Configure 'Config.cmake' # Use variables: # - TARGETS_EXPORT_NAME # - PROJECT_NAME configure_package_config_file( "${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in" "${PROJECT_CONFIG_FILE}" INSTALL_DESTINATION "${CONFIG_INSTALL_DIR}" ) # Uninstall targets configure_file("${PROJECT_SOURCE_DIR}/cmake/Uninstall.cmake.in" "${GENERATED_DIR}/Uninstall.cmake" IMMEDIATE @ONLY) add_custom_target(uninstall_poselib COMMAND ${CMAKE_COMMAND} -P ${GENERATED_DIR}/Uninstall.cmake) # Always full RPATH (for shared libraries) # https://gitlab.kitware.com/cmake/community/-/wikis/doc/cmake/RPATH-handling if(BUILD_SHARED_LIBS) # use, i.e. don't skip the full RPATH for the build tree set(CMAKE_SKIP_BUILD_RPATH FALSE) # when building, don't use the install RPATH already # (but later on when installing) set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") # add the automatically determined parts of the RPATH # which point to directories outside the build tree to the install RPATH set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) # the RPATH to be used when installing, but only if it's not a system directory list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) if("${isSystemDir}" STREQUAL "-1") set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") endif() endif() # CMake Registry include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/CMakeRegistry.cmake) PoseLib-2.0.5/cmake/Uninstall.cmake.in000066400000000000000000000042061504452766400175320ustar00rootroot00000000000000# # Based on: # http://www.cmake.org/Wiki/CMake_FAQ#Can_I_do_.22make_uninstall.22_with_CMake.3F # # 'delete_empty_folder' function function(delete_empty_folder DIR) if(NOT EXISTS ${DIR}) return() endif() # check if folder is empty file(GLOB RESULT "${DIR}/*") list(LENGTH RESULT RES_LEN) if(RES_LEN EQUAL 0) message(STATUS "Delete empty folder ${DIR}") file(REMOVE_RECURSE ${DIR}) else() # message(STATUS "${DIR} is not empty! It won't be removed.") # message(STATUS "FILES = ${RESULT}") endif() endfunction(delete_empty_folder) # find 'install_manifest.txt' if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") endif() # remove files from 'install_manifest.txt' message(STATUS "") message(STATUS "Removing files from 'install_manifest.txt'") file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) string(REGEX REPLACE "\n" ";" files "${files}") foreach(file ${files}) message(STATUS "Uninstalling $ENV{DESTDIR}${file}") if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) if(NOT "${rm_retval}" STREQUAL 0) message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") endif() else() message(STATUS "File $ENV{DESTDIR}${file} does not exist.") endif() # create list of folders get_filename_component(FOLDER ${file} DIRECTORY) set(FOLDERS ${FOLDERS} ${FOLDER}) endforeach(file) # remove empty folders message(STATUS "") message(STATUS "Removing empty folders") list(REMOVE_DUPLICATES FOLDERS) foreach(dir ${FOLDERS}) # message(STATUS ${dir}) delete_empty_folder(${dir}) # message(STATUS "") endforeach(dir) delete_empty_folder("@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@") delete_empty_folder("@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_BINDIR@") delete_empty_folder("@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@/cmake") delete_empty_folder("@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@") message(STATUS "") PoseLib-2.0.5/pybind/000077500000000000000000000000001504452766400143555ustar00rootroot00000000000000PoseLib-2.0.5/pybind/CMakeLists.txt000066400000000000000000000032161504452766400171170ustar00rootroot00000000000000# setup PYTHON_EXECUTABLE find_package (Python COMPONENTS Interpreter Development) message(STATUS "Python_EXECUTABLE: " ${Python_EXECUTABLE}) option(GENERATE_STUBS "Whether to generate stubs" ON) add_subdirectory(pybind11) pybind11_add_module(pyposelib MODULE pyposelib.cc) target_link_libraries(pyposelib PUBLIC PoseLib::PoseLib Eigen3::Eigen) set_target_properties(pyposelib PROPERTIES OUTPUT_NAME _core) set_compile_flags(pyposelib) # Install the compiled module install(TARGETS pyposelib LIBRARY DESTINATION . COMPONENT python) if(GENERATE_STUBS) message(STATUS "Enabling stubs generation") set(STUBGEN_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/_core") add_custom_command( TARGET pyposelib POST_BUILD COMMAND "${CMAKE_COMMAND}" -E env "PYTHONPATH=$$$ENV{PYTHONPATH}" "${Python_EXECUTABLE}" "${PROJECT_SOURCE_DIR}/pybind/generate_stubs.py" "${Python_EXECUTABLE}" "${CMAKE_CURRENT_BINARY_DIR}" WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMENT "Generating pybind11 stubs" VERBATIM ) # Install the generated stub file (handle both single file and directory cases) # Most pybind11_stubgen versions create a single .pyi file install(FILES "${CMAKE_CURRENT_BINARY_DIR}/_core.pyi" DESTINATION . COMPONENT python OPTIONAL) # OPTIONAL in case it doesn't exist # Some versions might create a directory instead install(DIRECTORY "${STUBGEN_OUTPUT_DIR}/" DESTINATION . COMPONENT python OPTIONAL) # OPTIONAL in case it doesn't exist endif() PoseLib-2.0.5/pybind/generate_stubs.py000066400000000000000000000050151504452766400177420ustar00rootroot00000000000000#!/usr/bin/env python3 """ Cross-platform stub generation script for pybind11 modules. Adapted from generate_stubs.sh to work on Windows, macOS, and Linux. """ import sys import os import subprocess import re import shutil from pathlib import Path def main(): if len(sys.argv) != 3: print("Usage: python generate_stubs.py ") sys.exit(1) python_exec = sys.argv[1] output_dir = Path(sys.argv[2]) package_name = "_core" print(f"Building stubs with {python_exec} to {output_dir}") # Run pybind11_stubgen cmd = [ python_exec, "-m", "pybind11_stubgen", package_name, "-o", str(output_dir), "--numpy-array-use-type-var", f"--enum-class-locations=.+:{package_name}", "--ignore-invalid-expressions", "poselib::*", "--print-invalid-expressions-as-is", "--print-safe-value-reprs", "[a-zA-Z]+Options\\(\\)" ] try: subprocess.run(cmd, check=True) except subprocess.CalledProcessError as e: print(f"Error running pybind11_stubgen: {e}") sys.exit(1) # Find the generated stub files stub_file = output_dir / f"{package_name}.pyi" stub_dir = output_dir / package_name files_to_process = [] if stub_file.exists(): files_to_process = [stub_file] elif stub_dir.exists(): files_to_process = list(stub_dir.glob("**/*.pyi")) else: print(f"Error: Neither stub file {stub_file} nor directory {stub_dir} exists") print(f"Available files in {output_dir}:") for item in output_dir.iterdir(): print(f" {item}") sys.exit(1) # Process each stub file for file_path in files_to_process: process_stub_file(file_path) # Format with ruff if available if shutil.which("ruff"): print("Formatting stubs with ruff...") try: subprocess.run(["ruff", "format"] + [str(f) for f in files_to_process], check=False) except Exception as e: print(f"Warning: ruff formatting failed: {e}") else: print("ruff not found, skipping formatting") def process_stub_file(file_path): """Apply regex replacements to clean up the stub file.""" print(f"Processing {file_path}") with open(file_path, 'r', encoding='utf-8') as f: content = f.read() content = re.sub(r'\b_core.\b', '', content) with open(file_path, 'w', encoding='utf-8') as f: f.write(content) if __name__ == "__main__": main() PoseLib-2.0.5/pybind/helpers.h000066400000000000000000000144321504452766400161740ustar00rootroot00000000000000#ifndef POSELIB_PYBIND_HELPERS_H_ #define POSELIB_PYBIND_HELPERS_H_ #include "pybind11_extension.h" #include #include #include #include #include namespace py = pybind11; static std::string toString(const Eigen::MatrixXd &mat) { std::stringstream ss; ss << mat; return ss.str(); } namespace poselib { template void update(const py::dict &input, const std::string &name, T &value) { if (input.contains(name)) { value = input[name.c_str()].cast(); } } template <> void update(const py::dict &input, const std::string &name, bool &value) { if (input.contains(name)) { py::object input_value = input[name.c_str()]; value = (py::str(input_value).is(py::str(Py_True))); } } void update_ransac_options(const py::dict &input, RansacOptions &ransac_opt) { update(input, "max_iterations", ransac_opt.max_iterations); update(input, "min_iterations", ransac_opt.min_iterations); update(input, "dyn_num_trials_mult", ransac_opt.dyn_num_trials_mult); update(input, "success_prob", ransac_opt.success_prob); update(input, "max_reproj_error", ransac_opt.max_reproj_error); update(input, "max_epipolar_error", ransac_opt.max_epipolar_error); update(input, "seed", ransac_opt.seed); update(input, "progressive_sampling", ransac_opt.progressive_sampling); update(input, "max_prosac_iterations", ransac_opt.max_prosac_iterations); update(input, "real_focal_check", ransac_opt.real_focal_check); // "score_initial_model" purposely omitted } void update_bundle_options(const py::dict &input, BundleOptions &bundle_opt) { update(input, "max_iterations", bundle_opt.max_iterations); update(input, "loss_scale", bundle_opt.loss_scale); update(input, "gradient_tol", bundle_opt.gradient_tol); update(input, "step_tol", bundle_opt.step_tol); update(input, "initial_lambda", bundle_opt.initial_lambda); update(input, "min_lambda", bundle_opt.min_lambda); update(input, "max_lambda", bundle_opt.max_lambda); update(input, "verbose", bundle_opt.verbose); if (input.contains("loss_type")) { std::string loss_type = input["loss_type"].cast(); for (char &c : loss_type) c = std::toupper(c); if (loss_type == "TRIVIAL") { bundle_opt.loss_type = BundleOptions::LossType::TRIVIAL; } else if (loss_type == "TRUNCATED") { bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED; } else if (loss_type == "HUBER") { bundle_opt.loss_type = BundleOptions::LossType::HUBER; } else if (loss_type == "CAUCHY") { bundle_opt.loss_type = BundleOptions::LossType::CAUCHY; } else if (loss_type == "TRUNCATED_LE_ZACH") { bundle_opt.loss_type = BundleOptions::LossType::TRUNCATED_LE_ZACH; } } } void write_to_dict(const RansacOptions &ransac_opt, py::dict &dict) { dict["max_iterations"] = ransac_opt.max_iterations; dict["min_iterations"] = ransac_opt.min_iterations; dict["dyn_num_trials_mult"] = ransac_opt.dyn_num_trials_mult; dict["success_prob"] = ransac_opt.success_prob; dict["max_reproj_error"] = ransac_opt.max_reproj_error; dict["max_epipolar_error"] = ransac_opt.max_epipolar_error; dict["seed"] = ransac_opt.seed; dict["progressive_sampling"] = ransac_opt.progressive_sampling; dict["max_prosac_iterations"] = ransac_opt.max_prosac_iterations; dict["real_focal_check"] = ransac_opt.real_focal_check; } void write_to_dict(const BundleOptions &bundle_opt, py::dict &dict) { dict["max_iterations"] = bundle_opt.max_iterations; dict["loss_scale"] = bundle_opt.loss_scale; switch (bundle_opt.loss_type) { default: case BundleOptions::LossType::TRIVIAL: dict["loss_type"] = "TRIVIAL"; break; case BundleOptions::LossType::TRUNCATED: dict["loss_type"] = "TRUNCATED"; break; case BundleOptions::LossType::HUBER: dict["loss_type"] = "HUBER"; break; case BundleOptions::LossType::CAUCHY: dict["loss_type"] = "CAUCHY"; break; case BundleOptions::LossType::TRUNCATED_LE_ZACH: dict["loss_type"] = "TRUNCATED_LE_ZACH"; break; } dict["gradient_tol"] = bundle_opt.gradient_tol; dict["step_tol"] = bundle_opt.step_tol; dict["initial_lambda"] = bundle_opt.initial_lambda; dict["min_lambda"] = bundle_opt.min_lambda; dict["max_lambda"] = bundle_opt.max_lambda; dict["verbose"] = bundle_opt.verbose; ; } void write_to_dict(const BundleStats &stats, py::dict &dict) { dict["iterations"] = stats.iterations; dict["cost"] = stats.cost; dict["initial_cost"] = stats.initial_cost; dict["invalid_steps"] = stats.invalid_steps; dict["grad_norm"] = stats.grad_norm; dict["step_norm"] = stats.step_norm; dict["lambda"] = stats.lambda; } void write_to_dict(const RansacStats &stats, py::dict &dict) { dict["refinements"] = stats.refinements; dict["iterations"] = stats.iterations; dict["num_inliers"] = stats.num_inliers; dict["inlier_ratio"] = stats.inlier_ratio; dict["model_score"] = stats.model_score; } Camera camera_from_dict(const py::dict &camera_dict) { Camera camera; camera.model_id = Camera::id_from_string(camera_dict["model"].cast()); update(camera_dict, "width", camera.width); update(camera_dict, "height", camera.height); camera.params = camera_dict["params"].cast>(); return camera; } std::vector convert_inlier_vector(const std::vector &inliers) { std::vector inliers_bool(inliers.size()); for (size_t k = 0; k < inliers.size(); ++k) { inliers_bool[k] = static_cast(inliers[k]); } return inliers_bool; } std::vector> convert_inlier_vectors(const std::vector> &inliers) { std::vector> inliers_bool(inliers.size()); for (size_t cam_k = 0; cam_k < inliers.size(); ++cam_k) { inliers_bool[cam_k].resize(inliers[cam_k].size()); for (size_t pt_k = 0; pt_k < inliers[cam_k].size(); ++pt_k) { inliers_bool[cam_k][pt_k] = static_cast(inliers[cam_k][pt_k]); } } return inliers_bool; } } // namespace poselib #endifPoseLib-2.0.5/pybind/pybind11/000077500000000000000000000000001504452766400160045ustar00rootroot00000000000000PoseLib-2.0.5/pybind/pybind11_extension.h000066400000000000000000000036431504452766400202570ustar00rootroot00000000000000#ifndef POSELIB_PYBIND_EXTENSIONS_H_ #define POSELIB_PYBIND_EXTENSIONS_H_ #include #include #include #include #include #include #include #include /* Code for quickly mapping numpy arrays to std::vector. Copied from COLMAP project: https://github.com/colmap/colmap/blob/main/src/pycolmap/pybind11_extension.h originally written by Paul-Edouard Sarlin. */ namespace PYBIND11_NAMESPACE { namespace detail { // Autocast from numpy.ndarray to std::vector template struct type_caster>> { public: using MatrixType = typename Eigen::Matrix; using VectorType = typename Eigen::Matrix; using props = EigenProps; PYBIND11_TYPE_CASTER(std::vector, props::descriptor); bool load(handle src, bool) { const auto buf = array::ensure(src); if (!buf) { return false; } const buffer_info info = buf.request(); if (info.ndim != 2 || info.shape[1] != Size) { return false; } const size_t num_elements = info.shape[0]; value.resize(num_elements); const auto &mat = src.cast>(); Eigen::Map(reinterpret_cast(value.data()), num_elements, Size) = mat; return true; } static handle cast(const std::vector &vec, return_value_policy /* policy */, handle h) { Eigen::Map mat(reinterpret_cast(vec.data()), vec.size(), Size); return type_caster>::cast(mat, return_value_policy::copy, h); } }; } // namespace detail } // namespace PYBIND11_NAMESPACE #endif PoseLib-2.0.5/pybind/pyposelib.cc000066400000000000000000002156041504452766400167020ustar00rootroot00000000000000#include "helpers.h" #include "pybind11_extension.h" #include #include #include #include #include #include #include namespace py = pybind11; namespace poselib { py::dict RansacOptions_wrapper(py::dict overwrite) { RansacOptions opt; update_ransac_options(overwrite, opt); py::dict result; write_to_dict(opt, result); return result; } py::dict BundleOptions_wrapper(py::dict overwrite) { BundleOptions opt; update_bundle_options(overwrite, opt); py::dict result; write_to_dict(opt, result); return result; } std::vector p3p_wrapper(const std::vector &x, const std::vector &X) { std::vector output; p3p(x, X, &output); return output; } std::vector gp3p_wrapper(const std::vector &p, const std::vector &x, const std::vector &X) { std::vector output; gp3p(p, x, X, &output); return output; } std::pair, std::vector> gp4ps_wrapper(const std::vector &p, const std::vector &x, const std::vector &X, bool filter_solutions = true) { std::vector output; std::vector output_scales; gp4ps(p, x, X, &output, &output_scales, filter_solutions); return std::make_pair(output, output_scales); } std::pair, std::vector> gp4ps_kukelova_wrapper(const std::vector &p, const std::vector &x, const std::vector &X, bool filter_solutions = true) { std::vector output; std::vector output_scales; gp4ps_kukelova(p, x, X, &output, &output_scales, filter_solutions); return std::make_pair(output, output_scales); } std::pair, std::vector> gp4ps_camposeco_wrapper(const std::vector &p, const std::vector &x, const std::vector &X) { std::vector output; std::vector output_scales; gp4ps_camposeco(p, x, X, &output, &output_scales); return std::make_pair(output, output_scales); } std::pair, std::vector> p4pf_wrapper(const std::vector &x, const std::vector &X, bool filter_solutions = true) { std::vector output; std::vector output_focal; p4pf(x, X, &output, &output_focal, filter_solutions); return std::make_pair(output, output_focal); } std::vector p2p2pl_wrapper(const std::vector &xp, const std::vector &Xp, const std::vector &x, const std::vector &X, const std::vector &V) { std::vector output; p2p2pl(xp, Xp, x, X, V, &output); return output; } std::vector p6lp_wrapper(const std::vector &l, const std::vector &X) { std::vector output; p6lp(l, X, &output); return output; } std::vector p5lp_radial_wrapper(const std::vector &l, const std::vector &X) { std::vector output; p5lp_radial(l, X, &output); return output; } std::vector p2p1ll_wrapper(const std::vector &xp, const std::vector &Xp, const std::vector &l, const std::vector &X, const std::vector &V) { std::vector output; p2p1ll(xp, Xp, l, X, V, &output); return output; } std::vector p1p2ll_wrapper(const std::vector &xp, const std::vector &Xp, const std::vector &l, const std::vector &X, const std::vector &V) { std::vector output; p1p2ll(xp, Xp, l, X, V, &output); return output; } std::vector p3ll_wrapper(const std::vector &l, const std::vector &X, const std::vector &V) { std::vector output; p3ll(l, X, V, &output); return output; } std::vector up2p_wrapper(const std::vector &x, const std::vector &X) { std::vector output; up2p(x, X, &output); return output; } std::vector ugp2p_wrapper(const std::vector &p, const std::vector &x, const std::vector &X) { std::vector output; ugp2p(p, x, X, &output); return output; } std::pair, std::vector> ugp3ps_wrapper(const std::vector &p, const std::vector &x, const std::vector &X, bool filter_solutions = true) { std::vector output; std::vector output_scales; ugp3ps(p, x, X, &output, &output_scales, filter_solutions); return std::make_pair(output, output_scales); } std::vector up1p2pl_wrapper(const std::vector &xp, const std::vector &Xp, const std::vector &x, const std::vector &X, const std::vector &V) { std::vector output; up1p2pl(xp, Xp, x, X, V, &output); return output; } std::vector up4pl_wrapper(const std::vector &x, const std::vector &X, const std::vector &V) { std::vector output; up4pl(x, X, V, &output); return output; } std::vector ugp4pl_wrapper(const std::vector &p, const std::vector &x, const std::vector &X, const std::vector &V) { std::vector output; ugp4pl(p, x, X, V, &output); return output; } std::vector essential_matrix_relpose_5pt_wrapper(const std::vector &x1, const std::vector &x2) { std::vector essential_matrices; relpose_5pt(x1, x2, &essential_matrices); return essential_matrices; } std::vector relpose_5pt_wrapper(const std::vector &x1, const std::vector &x2) { std::vector output; relpose_5pt(x1, x2, &output); return output; } ImagePairVector shared_focal_relpose_6pt_wrapper(const std::vector &x1, const std::vector &x2) { ImagePairVector output; relpose_6pt_shared_focal(x1, x2, &output); return output; } std::vector relpose_8pt_wrapper(const std::vector &x1, const std::vector &x2) { std::vector output; relpose_8pt(x1, x2, &output); return output; } Eigen::Matrix3d essential_matrix_8pt_wrapper(const std::vector &x1, const std::vector &x2) { Eigen::Matrix3d essential_matrix; essential_matrix_8pt(x1, x2, &essential_matrix); return essential_matrix; } std::vector relpose_upright_3pt_wrapper(const std::vector &x1, const std::vector &x2) { std::vector output; relpose_upright_3pt(x1, x2, &output); return output; } std::vector gen_relpose_upright_4pt_wrapper(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2) { std::vector output; gen_relpose_upright_4pt(p1, x1, p2, x2, &output); return output; } std::vector gen_relpose_6pt_wrapper(const std::vector &p1, const std::vector &x1, const std::vector &p2, const std::vector &x2) { std::vector output; gen_relpose_6pt(p1, x1, p2, x2, &output); return output; } std::vector relpose_upright_planar_2pt_wrapper(const std::vector &x1, const std::vector &x2) { std::vector output; relpose_upright_planar_2pt(x1, x2, &output); return output; } std::vector relpose_upright_planar_3pt_wrapper(const std::vector &x1, const std::vector &x2) { std::vector output; relpose_upright_planar_3pt(x1, x2, &output); return output; } std::pair estimate_absolute_pose_wrapper(const std::vector &points2D, const std::vector &points3D, const Camera &camera, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_reproj_error; update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_absolute_pose(points2D, points3D, camera, ransac_opt, bundle_opt, &pose, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_mask); return std::make_pair(pose, output_dict); } std::pair estimate_absolute_pose_wrapper(const std::vector &points2D, const std::vector &points3D, const py::dict &camera_dict, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { Camera camera = camera_from_dict(camera_dict); return estimate_absolute_pose_wrapper(points2D, points3D, camera, ransac_opt_dict, bundle_opt_dict, initial_pose); } std::pair refine_absolute_pose_wrapper(const std::vector &points2D, const std::vector &points3D, const CameraPose &initial_pose, const Camera &camera, const py::dict &bundle_opt_dict) { // We normalize to improve numerics in the optimization const double scale = 1.0 / camera.focal(); Camera norm_camera = camera; norm_camera.rescale(scale); std::vector points2D_scaled = points2D; for (size_t k = 0; k < points2D_scaled.size(); ++k) { points2D_scaled[k] *= scale; } BundleOptions bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); bundle_opt.loss_scale *= scale; CameraPose refined_pose = initial_pose; py::gil_scoped_release release; BundleStats stats = bundle_adjust(points2D_scaled, points3D, norm_camera, &refined_pose, bundle_opt); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_pose, output_dict); } std::pair refine_absolute_pose_wrapper(const std::vector &points2D, const std::vector &points3D, const CameraPose &initial_pose, const py::dict &camera_dict, const py::dict &bundle_opt_dict) { Camera camera = camera_from_dict(camera_dict); return refine_absolute_pose_wrapper(points2D, points3D, initial_pose, camera, bundle_opt_dict); } std::pair estimate_absolute_pose_pnpl_wrapper( const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D_1, const std::vector &lines2D_2, const std::vector &lines3D_1, const std::vector &lines3D_2, const Camera &camera, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_reproj_error; update_bundle_options(bundle_opt_dict, bundle_opt); py::gil_scoped_release release; std::vector lines2D; std::vector lines3D; lines2D.reserve(lines2D_1.size()); lines3D.reserve(lines3D_1.size()); for (size_t k = 0; k < lines2D_1.size(); ++k) { lines2D.emplace_back(lines2D_1[k], lines2D_2[k]); lines3D.emplace_back(lines3D_1[k], lines3D_2[k]); } CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector inlier_points_mask; std::vector inlier_lines_mask; RansacStats stats = estimate_absolute_pose_pnpl(points2D, points3D, lines2D, lines3D, camera, ransac_opt, bundle_opt, &pose, &inlier_points_mask, &inlier_lines_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_points_mask); output_dict["inliers_lines"] = convert_inlier_vector(inlier_lines_mask); return std::make_pair(pose, output_dict); } std::pair estimate_absolute_pose_pnpl_wrapper( const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D_1, const std::vector &lines2D_2, const std::vector &lines3D_1, const std::vector &lines3D_2, const py::dict &camera_dict, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { Camera camera = camera_from_dict(camera_dict); return estimate_absolute_pose_pnpl_wrapper(points2D, points3D, lines2D_1, lines2D_2, lines3D_1, lines3D_2, camera, ransac_opt_dict, bundle_opt_dict, initial_pose); } std::pair refine_absolute_pose_pnpl_wrapper( const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D_1, const std::vector &lines2D_2, const std::vector &lines3D_1, const std::vector &lines3D_2, const CameraPose &initial_pose, const Camera &camera, const py::dict &bundle_opt_dict, const py::dict &line_bundle_opt_dict) { BundleOptions bundle_opt, line_bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); bundle_opt.loss_scale /= camera.focal(); if (line_bundle_opt_dict.empty()) { line_bundle_opt = bundle_opt; } else { update_bundle_options(line_bundle_opt_dict, line_bundle_opt); line_bundle_opt.loss_scale /= camera.focal(); } py::gil_scoped_release release; // Setup line objects std::vector lines2D; std::vector lines3D; lines2D.reserve(lines2D_1.size()); lines3D.reserve(lines3D_1.size()); for (size_t k = 0; k < lines2D_1.size(); ++k) { lines2D.emplace_back(lines2D_1[k], lines2D_2[k]); lines3D.emplace_back(lines3D_1[k], lines3D_2[k]); } // Calibrate points std::vector points2D_calib(points2D.size()); for (size_t k = 0; k < points2D.size(); ++k) { camera.unproject(points2D[k], &points2D_calib[k]); } // Calibrate 2D line segments std::vector lines2D_calib(lines2D.size()); for (size_t k = 0; k < lines2D.size(); ++k) { camera.unproject(lines2D[k].x1, &lines2D_calib[k].x1); camera.unproject(lines2D[k].x2, &lines2D_calib[k].x2); } CameraPose refined_pose = initial_pose; BundleStats stats = bundle_adjust(points2D_calib, points3D, lines2D_calib, lines3D, &refined_pose, bundle_opt, line_bundle_opt); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_pose, output_dict); } std::pair refine_absolute_pose_pnpl_wrapper( const std::vector &points2D, const std::vector &points3D, const std::vector &lines2D_1, const std::vector &lines2D_2, const std::vector &lines3D_1, const std::vector &lines3D_2, const CameraPose &initial_pose, const py::dict &camera_dict, const py::dict &bundle_opt_dict, const py::dict &line_bundle_opt_dict) { Camera camera = camera_from_dict(camera_dict); return refine_absolute_pose_pnpl_wrapper(points2D, points3D, lines2D_1, lines2D_2, lines3D_1, lines3D_2, initial_pose, camera, bundle_opt_dict, line_bundle_opt_dict); } std::pair estimate_generalized_absolute_pose_wrapper( const std::vector> &points2D, const std::vector> &points3D, const std::vector &camera_ext, const std::vector &cameras, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_reproj_error; update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector> inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_generalized_absolute_pose(points2D, points3D, camera_ext, cameras, ransac_opt, bundle_opt, &pose, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vectors(inlier_mask); return std::make_pair(pose, output_dict); } std::pair estimate_generalized_absolute_pose_wrapper( const std::vector> &points2D, const std::vector> &points3D, const std::vector &camera_ext, const std::vector &camera_dicts, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { std::vector cameras; for (const py::dict &camera_dict : camera_dicts) { cameras.push_back(camera_from_dict(camera_dict)); } return estimate_generalized_absolute_pose_wrapper(points2D, points3D, camera_ext, cameras, ransac_opt_dict, bundle_opt_dict, initial_pose); } std::pair refine_generalized_absolute_pose_wrapper(const std::vector> &points2D, const std::vector> &points3D, const CameraPose &initial_pose, const std::vector &camera_ext, const std::vector &cameras, const py::dict &bundle_opt_dict) { BundleOptions bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose refined_pose = initial_pose; py::gil_scoped_release release; BundleStats stats = generalized_bundle_adjust(points2D, points3D, camera_ext, cameras, &refined_pose, bundle_opt); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_pose, output_dict); } std::pair refine_generalized_absolute_pose_wrapper(const std::vector> &points2D, const std::vector> &points3D, const CameraPose &initial_pose, const std::vector &camera_ext, const std::vector &camera_dicts, const py::dict &bundle_opt_dict) { std::vector cameras; for (const py::dict &camera_dict : camera_dicts) { cameras.push_back(camera_from_dict(camera_dict)); } return refine_generalized_absolute_pose_wrapper(points2D, points3D, initial_pose, camera_ext, cameras, bundle_opt_dict); } std::pair estimate_relative_pose_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const Camera &camera1, const Camera &camera2, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_epipolar_error; update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_relative_pose(points2D_1, points2D_2, camera1, camera2, ransac_opt, bundle_opt, &pose, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_mask); return std::make_pair(pose, output_dict); } std::pair estimate_relative_pose_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const py::dict &camera1_dict, const py::dict &camera2_dict, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { Camera camera1 = camera_from_dict(camera1_dict); Camera camera2 = camera_from_dict(camera2_dict); return estimate_relative_pose_wrapper(points2D_1, points2D_2, camera1, camera2, ransac_opt_dict, bundle_opt_dict, initial_pose); } std::pair estimate_shared_focal_relative_pose_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const Eigen::Vector2d &pp, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_image_pair) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_epipolar_error; update_bundle_options(bundle_opt_dict, bundle_opt); ImagePair image_pair; if (initial_image_pair.has_value()) { image_pair = initial_image_pair.value(); ransac_opt.score_initial_model = true; } std::vector inlier_mask; std::vector output; py::gil_scoped_release release; RansacStats stats = estimate_shared_focal_relative_pose(points2D_1, points2D_2, pp, ransac_opt, bundle_opt, &image_pair, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_mask); return std::make_pair(image_pair, output_dict); } std::pair refine_relative_pose_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const CameraPose &initial_pose, const Camera &camera1, const Camera &camera2, const py::dict &bundle_opt_dict) { BundleOptions bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); py::gil_scoped_release release; // Normalize image points std::vector x1_calib = points2D_1; std::vector x2_calib = points2D_2; for (size_t i = 0; i < x1_calib.size(); ++i) { camera1.unproject(points2D_1[i], &x1_calib[i]); camera2.unproject(points2D_2[i], &x2_calib[i]); } bundle_opt.loss_scale *= (1.0 / camera1.focal() + 1.0 / camera2.focal()) * 0.5; CameraPose refined_pose = initial_pose; BundleStats stats = refine_relpose(x1_calib, x2_calib, &refined_pose, bundle_opt); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_pose, output_dict); } std::pair refine_relative_pose_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const CameraPose &initial_pose, const py::dict &camera1_dict, const py::dict &camera2_dict, const py::dict &bundle_opt_dict) { Camera camera1 = camera_from_dict(camera1_dict); Camera camera2 = camera_from_dict(camera2_dict); return refine_relative_pose_wrapper(points2D_1, points2D_2, initial_pose, camera1, camera2, bundle_opt_dict); } std::pair estimate_fundamental_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_F) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_epipolar_error; update_bundle_options(bundle_opt_dict, bundle_opt); Eigen::Matrix3d F; if (initial_F.has_value()) { F = initial_F.value(); ransac_opt.score_initial_model = true; } std::vector inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_fundamental(points2D_1, points2D_2, ransac_opt, bundle_opt, &F, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_mask); return std::make_pair(F, output_dict); } std::pair refine_fundamental_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const Eigen::Matrix3d &initial_F, const py::dict &bundle_opt_dict) { BundleOptions bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); py::gil_scoped_release release; // Normalize image points std::vector x1_norm = points2D_1; std::vector x2_norm = points2D_2; Eigen::Matrix3d T1, T2; double scale = normalize_points(x1_norm, x2_norm, T1, T2, true, true, true); BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale /= scale; Eigen::Matrix3d refined_F = T2.transpose().inverse() * initial_F * T1.inverse(); BundleStats stats = refine_fundamental(x1_norm, x2_norm, &refined_F, bundle_opt_scaled); refined_F = T2.transpose() * refined_F * T1; refined_F /= refined_F.norm(); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_F, output_dict); } std::pair estimate_homography_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_H) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_reproj_error; update_bundle_options(bundle_opt_dict, bundle_opt); Eigen::Matrix3d H; if (initial_H.has_value()) { H = initial_H.value(); ransac_opt.score_initial_model = true; } std::vector inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_homography(points2D_1, points2D_2, ransac_opt, bundle_opt, &H, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_mask); return std::make_pair(H, output_dict); } std::pair refine_homography_wrapper(const std::vector &points2D_1, const std::vector &points2D_2, const Eigen::Matrix3d initial_H, const py::dict &bundle_opt_dict) { BundleOptions bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); py::gil_scoped_release release; // Normalize image points std::vector x1_norm = points2D_1; std::vector x2_norm = points2D_2; Eigen::Matrix3d T1, T2; double scale = normalize_points(x1_norm, x2_norm, T1, T2, true, true, true); BundleOptions bundle_opt_scaled = bundle_opt; bundle_opt_scaled.loss_scale /= scale; Eigen::Matrix3d refined_H = T2 * initial_H * T1.inverse(); BundleStats stats = refine_homography(x1_norm, x2_norm, &refined_H, bundle_opt_scaled); refined_H = T2.inverse() * refined_H * T1; refined_H /= refined_H.norm(); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_H, output_dict); } std::pair estimate_generalized_relative_pose_wrapper( const std::vector &matches, const std::vector &camera1_ext, const std::vector &cameras1, const std::vector &camera2_ext, const std::vector &cameras2, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_epipolar_error; update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector> inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_generalized_relative_pose(matches, camera1_ext, cameras1, camera2_ext, cameras2, ransac_opt, bundle_opt, &pose, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vectors(inlier_mask); return std::make_pair(pose, output_dict); } std::pair estimate_generalized_relative_pose_wrapper( const std::vector &matches, const std::vector &camera1_ext, const std::vector &cameras1_dict, const std::vector &camera2_ext, const std::vector &cameras2_dict, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { std::vector cameras1, cameras2; for (const py::dict &camera_dict : cameras1_dict) { cameras1.push_back(camera_from_dict(camera_dict)); } for (const py::dict &camera_dict : cameras2_dict) { cameras2.push_back(camera_from_dict(camera_dict)); } return estimate_generalized_relative_pose_wrapper(matches, camera1_ext, cameras1, camera2_ext, cameras2, ransac_opt_dict, bundle_opt_dict, initial_pose); } std::pair refine_generalized_relative_pose_wrapper( const std::vector &matches, const CameraPose &initial_pose, const std::vector &camera1_ext, const std::vector &cameras1, const std::vector &camera2_ext, const std::vector &cameras2, const py::dict &bundle_opt_dict) { BundleOptions bundle_opt; update_bundle_options(bundle_opt_dict, bundle_opt); py::gil_scoped_release release; // Compute normalized matches std::vector calib_matches = matches; for (PairwiseMatches &m : calib_matches) { for (size_t k = 0; k < m.x1.size(); ++k) { cameras1[m.cam_id1].unproject(m.x1[k], &m.x1[k]); cameras2[m.cam_id2].unproject(m.x2[k], &m.x2[k]); } } double scaling_factor = 0; for (size_t k = 0; k < cameras1.size(); ++k) { scaling_factor += 1.0 / cameras1[k].focal(); } for (size_t k = 0; k < cameras2.size(); ++k) { scaling_factor += 1.0 / cameras2[k].focal(); } scaling_factor /= cameras1.size() + cameras2.size(); bundle_opt.loss_scale *= scaling_factor; CameraPose refined_pose = initial_pose; BundleStats stats = refine_generalized_relpose(calib_matches, camera1_ext, camera2_ext, &refined_pose, bundle_opt); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); return std::make_pair(refined_pose, output_dict); } std::pair refine_generalized_relative_pose_wrapper(const std::vector &matches, const CameraPose &initial_pose, const std::vector &camera1_ext, const std::vector &cameras1_dict, const std::vector &camera2_ext, const std::vector &cameras2_dict, const py::dict &bundle_opt_dict) { std::vector cameras1, cameras2; for (const py::dict &camera_dict : cameras1_dict) { cameras1.push_back(camera_from_dict(camera_dict)); } for (const py::dict &camera_dict : cameras2_dict) { cameras2.push_back(camera_from_dict(camera_dict)); } return refine_generalized_relative_pose_wrapper(matches, initial_pose, camera1_ext, cameras1, camera2_ext, cameras2, bundle_opt_dict); } std::pair estimate_hybrid_pose_wrapper(const std::vector &points2D, const std::vector &points3D, const std::vector &matches_2D_2D, const Camera &camera, const std::vector &map_ext, const std::vector &map_cameras, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; // TODO: figure out what do to here... bundle_opt.loss_scale = 0.25 * (ransac_opt.max_reproj_error + ransac_opt.max_epipolar_error); update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector inliers_mask_2d3d; std::vector> inliers_mask_2d2d; py::gil_scoped_release release; RansacStats stats = estimate_hybrid_pose(points2D, points3D, matches_2D_2D, camera, map_ext, map_cameras, ransac_opt, bundle_opt, &pose, &inliers_mask_2d3d, &inliers_mask_2d2d); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inliers_mask_2d3d); output_dict["inliers_2D"] = convert_inlier_vectors(inliers_mask_2d2d); return std::make_pair(pose, output_dict); } std::pair estimate_hybrid_pose_wrapper(const std::vector &points2D, const std::vector &points3D, const std::vector &matches_2D_2D, const py::dict &camera_dict, const std::vector &map_ext, const std::vector &map_camera_dicts, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { Camera camera = camera_from_dict(camera_dict); std::vector map_cameras; for (const py::dict &camera_dict : map_camera_dicts) { map_cameras.push_back(camera_from_dict(camera_dict)); } return estimate_hybrid_pose_wrapper(points2D, points3D, matches_2D_2D, camera, map_ext, map_cameras, ransac_opt_dict, bundle_opt_dict, initial_pose); } std::pair estimate_1D_radial_absolute_pose_wrapper( const std::vector &points2D, const std::vector &points3D, const py::dict &ransac_opt_dict, const py::dict &bundle_opt_dict, const std::optional &initial_pose) { RansacOptions ransac_opt; update_ransac_options(ransac_opt_dict, ransac_opt); BundleOptions bundle_opt; bundle_opt.loss_scale = 0.5 * ransac_opt.max_reproj_error; update_bundle_options(bundle_opt_dict, bundle_opt); CameraPose pose; if (initial_pose.has_value()) { pose = initial_pose.value(); ransac_opt.score_initial_model = true; } std::vector inlier_mask; py::gil_scoped_release release; RansacStats stats = estimate_1D_radial_absolute_pose(points2D, points3D, ransac_opt, bundle_opt, &pose, &inlier_mask); py::gil_scoped_acquire acquire; py::dict output_dict; write_to_dict(stats, output_dict); output_dict["inliers"] = convert_inlier_vector(inlier_mask); return std::make_pair(pose, output_dict); } std::pair, std::vector> motion_from_homography_wrapper(Eigen::Matrix3d &H) { std::vector poses; std::vector normals; motion_from_homography(H, &poses, &normals); return std::make_pair(poses, normals); } std::tuple focals_from_fundamental_iterative_wrapper(const Eigen::Matrix3d F, const py::dict &camera1_dict, const py::dict &camera2_dict, const int max_iters, const Eigen::Vector4d &weights) { Camera camera1 = camera_from_dict(camera1_dict); Camera camera2 = camera_from_dict(camera2_dict); py::gil_scoped_release release; return focals_from_fundamental_iterative(F, camera1, camera2, max_iters, weights); } } // namespace poselib PYBIND11_MODULE(_core, m) { py::class_(m, "CameraPose") .def(py::init<>()) .def_readwrite("q", &poselib::CameraPose::q) .def_readwrite("t", &poselib::CameraPose::t) .def_property("R", &poselib::CameraPose::R, [](poselib::CameraPose &self, Eigen::Matrix3d R_new) { self.q = poselib::rotmat_to_quat(R_new); }) .def_property("Rt", &poselib::CameraPose::Rt, [](poselib::CameraPose &self, Eigen::Matrix Rt_new) { self.q = poselib::rotmat_to_quat(Rt_new.leftCols<3>()); self.t = Rt_new.col(3); }) .def("center", &poselib::CameraPose::center, "Returns the camera center (c=-R^T*t).") .def("__repr__", [](const poselib::CameraPose &a) { return "[q: " + toString(a.q.transpose()) + ", " + "t: " + toString(a.t.transpose()) + "]"; }); py::class_(m, "Camera") .def(py::init<>()) .def(py::init &, int, int>()) .def_readwrite("model_id", &poselib::Camera::model_id) .def_readwrite("width", &poselib::Camera::width) .def_readwrite("height", &poselib::Camera::height) .def_readwrite("params", &poselib::Camera::params) .def("focal", &poselib::Camera::focal, "Returns the camera focal length.") .def("focal_x", &poselib::Camera::focal_x, "Returns the camera focal_x.") .def("focal_y", &poselib::Camera::focal_y, "Returns the camera focal_y.") .def("model_name", &poselib::Camera::model_name, "Returns the camera model name.") .def("principal_point", &poselib::Camera::principal_point, "Returns the camera principal point.") .def("initialize_from_txt", &poselib::Camera::initialize_from_txt, "Initialize camera from a cameras.txt line") .def("project", [](poselib::Camera &self, std::vector &xp) { std::vector x; self.project(xp, &x); return x; }) .def("project_with_jac", [](poselib::Camera &self, std::vector &xp) { std::vector x; std::vector jac; self.project_with_jac(xp, &x, &jac); return std::make_pair(x, jac); }) .def("unproject", [](poselib::Camera &self, std::vector &x) { std::vector xp; self.unproject(x, &xp); return xp; }) .def("__repr__", [](const poselib::Camera &a) { return a.to_cameras_txt(); }); py::class_(m, "Image") .def(py::init<>()) .def_readwrite("camera", &poselib::Image::camera) .def_readwrite("pose", &poselib::Image::pose) .def("__repr__", [](const poselib::Image &a) { return "[pose q: " + toString(a.pose.q.transpose()) + ", t: " + toString(a.pose.t.transpose()) + ", camera: " + a.camera.to_cameras_txt() + "]"; }); py::class_(m, "ImagePair") .def(py::init<>()) .def_readwrite("pose", &poselib::ImagePair::pose) .def_readwrite("camera1", &poselib::ImagePair::camera1) .def_readwrite("camera2", &poselib::ImagePair::camera2) .def("__repr__", [](const poselib::ImagePair &a) { return "[pose q: " + toString(a.pose.q.transpose()) + ", t: " + toString(a.pose.t.transpose()) + ", camera1: " + a.camera1.to_cameras_txt() + ", camera2: " + a.camera2.to_cameras_txt() + "]"; }); py::class_(m, "PairwiseMatches") .def(py::init<>()) .def_readwrite("cam_id1", &poselib::PairwiseMatches::cam_id1) .def_readwrite("cam_id2", &poselib::PairwiseMatches::cam_id2) .def_readwrite("x1", &poselib::PairwiseMatches::x1) .def_readwrite("x2", &poselib::PairwiseMatches::x2) .def("__repr__", [](const poselib::PairwiseMatches &a) { return "[cam_id1: " + std::to_string(a.cam_id1) + "\n" + "cam_id2: " + std::to_string(a.cam_id2) + "\n" + "x1: [2x" + std::to_string(a.x1.size()) + "]\n" + "x2: [2x" + std::to_string(a.x2.size()) + "]]\n"; }); m.doc() = "This library provides a collection of minimal solvers for camera pose estimation."; // Minimal solvers m.def("p3p", &poselib::p3p_wrapper, py::arg("x"), py::arg("X"), py::call_guard()); m.def("gp3p", &poselib::gp3p_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::call_guard()); m.def("gp4ps", &poselib::gp4ps_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::arg("filter_solutions"), py::call_guard()); m.def("gp4ps_kukelova", &poselib::gp4ps_kukelova_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::arg("filter_solutions"), py::call_guard()); m.def("gp4ps_camposeco", &poselib::gp4ps_camposeco_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::call_guard()); m.def("p4pf", &poselib::p4pf_wrapper, py::arg("x"), py::arg("X"), py::arg("filter_solutions"), py::call_guard()); m.def("p2p2pl", &poselib::p2p2pl_wrapper, py::arg("xp"), py::arg("Xp"), py::arg("x"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("p6lp", &poselib::p6lp_wrapper, py::arg("l"), py::arg("X"), py::call_guard()); m.def("p5lp_radial", &poselib::p5lp_radial_wrapper, py::arg("l"), py::arg("X"), py::call_guard()); m.def("p2p1ll", &poselib::p2p1ll_wrapper, py::arg("xp"), py::arg("Xp"), py::arg("l"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("p1p2ll", &poselib::p1p2ll_wrapper, py::arg("xp"), py::arg("Xp"), py::arg("l"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("p3ll", &poselib::p3ll_wrapper, py::arg("l"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("up2p", &poselib::up2p_wrapper, py::arg("x"), py::arg("X"), py::call_guard()); m.def("ugp2p", &poselib::ugp2p_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::call_guard()); m.def("ugp3ps", &poselib::ugp3ps_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::arg("filter_solutions"), py::call_guard()); m.def("up1p2pl", &poselib::up1p2pl_wrapper, py::arg("xp"), py::arg("Xp"), py::arg("x"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("up4pl", &poselib::up4pl_wrapper, py::arg("x"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("ugp4pl", &poselib::ugp4pl_wrapper, py::arg("p"), py::arg("x"), py::arg("X"), py::arg("V"), py::call_guard()); m.def("essential_matrix_5pt", &poselib::essential_matrix_relpose_5pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("shared_focal_relpose_6pt", &poselib::shared_focal_relpose_6pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("relpose_5pt", &poselib::relpose_5pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("relpose_8pt", &poselib::relpose_8pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("essential_matrix_8pt", &poselib::essential_matrix_8pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("relpose_upright_3pt", &poselib::relpose_upright_3pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("gen_relpose_upright_4pt", &poselib::gen_relpose_upright_4pt_wrapper, py::arg("p1"), py::arg("x1"), py::arg("p2"), py::arg("x2"), py::call_guard()); m.def("gen_relpose_6pt", &poselib::gen_relpose_6pt_wrapper, py::arg("p1"), py::arg("x1"), py::arg("p2"), py::arg("x2"), py::call_guard()); m.def("relpose_upright_planar_2pt", &poselib::relpose_upright_planar_2pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); m.def("relpose_upright_planar_3pt", &poselib::relpose_upright_planar_3pt_wrapper, py::arg("x1"), py::arg("x2"), py::call_guard()); // Robust estimators m.def("estimate_absolute_pose", py::overload_cast &, const std::vector &, const poselib::Camera &, const py::dict &, const py::dict &, const std::optional &>(&poselib::estimate_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("camera"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Absolute pose estimation with non-linear refinement."); m.def( "estimate_absolute_pose", py::overload_cast &, const std::vector &, const py::dict &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("camera_dict"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Absolute pose estimation with non-linear refinement."); m.def("estimate_absolute_pose_pnpl", py::overload_cast &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const poselib::Camera &, const py::dict &, const py::dict &, const std::optional &>(&poselib::estimate_absolute_pose_pnpl_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("lines2D_1"), py::arg("lines2D_2"), py::arg("lines3D_1"), py::arg("lines3D_2"), py::arg("camera"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Absolute pose estimation with non-linear refinement from points and lines."); m.def( "estimate_absolute_pose_pnpl", py::overload_cast &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_absolute_pose_pnpl_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("lines2D_1"), py::arg("lines2D_2"), py::arg("lines3D_1"), py::arg("lines3D_2"), py::arg("camera_dict"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Absolute pose estimation with non-linear refinement from points and lines."); m.def("estimate_generalized_absolute_pose", py::overload_cast> &, const std::vector> &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_generalized_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("camera_ext"), py::arg("cameras"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Generalized absolute pose estimation with non-linear refinement."); m.def("estimate_generalized_absolute_pose", py::overload_cast> &, const std::vector> &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_generalized_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("camera_ext"), py::arg("camera_dicts"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Generalized absolute pose estimation with non-linear refinement."); m.def("estimate_relative_pose", py::overload_cast &, const std::vector &, const poselib::Camera &, const poselib::Camera &, const py::dict &, const py::dict &, const std::optional &>(&poselib::estimate_relative_pose_wrapper), py::arg("points2D_1"), py::arg("points2D_2"), py::arg("camera1"), py::arg("camera2"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Relative pose estimation with non-linear refinement."); m.def("estimate_relative_pose", py::overload_cast &, const std::vector &, const py::dict &, const py::dict &, const py::dict &, const py::dict &, const std::optional &>(&poselib::estimate_relative_pose_wrapper), py::arg("points2D_1"), py::arg("points2D_2"), py::arg("camera1_dict"), py::arg("camera2_dict"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Relative pose estimation with non-linear refinement."); m.def("estimate_shared_focal_relative_pose", &poselib::estimate_shared_focal_relative_pose_wrapper, py::arg("points2D_1"), py::arg("points2D_2"), py::arg("pp") = Eigen::Vector2d::Zero(), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_image_pair") = py::none(), "Relative pose estimation with unknown equal focal lengths with non-linear refinement."); m.def("estimate_fundamental", &poselib::estimate_fundamental_wrapper, py::arg("points2D_1"), py::arg("points2D_2"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_F") = py::none(), "Fundamental matrix estimation with non-linear refinement. Note: if you have known intrinsics you should use " "estimate_relative_pose instead!"); m.def("estimate_homography", &poselib::estimate_homography_wrapper, py::arg("points2D_1"), py::arg("points2D_2"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_H") = py::none(), "Homography matrix estimation with non-linear refinement."); m.def("estimate_generalized_relative_pose", py::overload_cast &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_generalized_relative_pose_wrapper), py::arg("matches"), py::arg("camera1_ext"), py::arg("cameras1"), py::arg("camera2_ext"), py::arg("cameras2"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Generalized relative pose estimation with non-linear refinement."); m.def("estimate_generalized_relative_pose", py::overload_cast &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_generalized_relative_pose_wrapper), py::arg("matches"), py::arg("camera1_ext"), py::arg("camera1_dict"), py::arg("camera2_ext"), py::arg("camera2_dict"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Generalized relative pose estimation with non-linear refinement."); m.def( "estimate_hybrid_pose", py::overload_cast &, const std::vector &, const std::vector &, const poselib::Camera &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_hybrid_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("matches_2D_2D"), py::arg("camera"), py::arg("map_ext"), py::arg("map_cameras"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Hybrid camera pose estimation (both 2D-3D and 2D-2D correspondences to the map) with non-linear refinement."); m.def( "estimate_hybrid_pose", py::overload_cast &, const std::vector &, const std::vector &, const py::dict &, const std::vector &, const std::vector &, const py::dict &, const py::dict &, const std::optional &>( &poselib::estimate_hybrid_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("matches_2D_2D"), py::arg("camera_dict"), py::arg("map_ext"), py::arg("map_camera_dicts"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Hybrid camera pose estimation (both 2D-3D and 2D-2D correspondences to the map) with non-linear refinement."); m.def("estimate_1D_radial_absolute_pose", &poselib::estimate_1D_radial_absolute_pose_wrapper, py::arg("points2D"), py::arg("points3D"), py::arg("ransac_opt") = py::dict(), py::arg("bundle_opt") = py::dict(), py::arg("initial_pose") = py::none(), "Absolute pose estimation for the 1D radial camera model with non-linear refinement."); m.def("motion_from_homography", &poselib::motion_from_homography_wrapper, py::arg("H")); m.def("focals_from_fundamental", &poselib::focals_from_fundamental, py::arg("F"), py::arg("pp1"), py::arg("pp2")); m.def("focals_from_fundamental_iterative", &poselib::focals_from_fundamental_iterative, py::arg("F"), py::arg("camera1"), py::arg("camera2"), py::arg("max_iters") = 50, py::arg("weights") = Eigen::Vector4d(5.0e-4, 1.0, 5.0e-4, 1.0)); m.def("focals_from_fundamental_iterative", &poselib::focals_from_fundamental_iterative_wrapper, py::arg("F"), py::arg("camera1_dict"), py::arg("camera2_dict"), py::arg("max_iters") = 50, py::arg("weights") = Eigen::Vector4d(5.0e-4, 1.0, 5.0e-4, 1.0)); // Stand-alone non-linear refinement m.def("refine_absolute_pose", py::overload_cast &, const std::vector &, const poselib::CameraPose &, const poselib::Camera &, const py::dict &>( &poselib::refine_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("initial_pose"), py::arg("camera"), py::arg("bundle_options") = py::dict(), "Absolute pose non-linear refinement."); m.def("refine_absolute_pose", py::overload_cast &, const std::vector &, const poselib::CameraPose &, const py::dict &, const py::dict &>( &poselib::refine_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("initial_pose"), py::arg("camera_dict"), py::arg("bundle_options") = py::dict(), "Absolute pose non-linear refinement."); m.def("refine_absolute_pose_pnpl", py::overload_cast &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const poselib::CameraPose &, const poselib::Camera &, const py::dict &, const py::dict &>( &poselib::refine_absolute_pose_pnpl_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("lines2D_1"), py::arg("lines2D_2"), py::arg("lines3D_1"), py::arg("lines3D_2"), py::arg("initial_pose"), py::arg("camera"), py::arg("bundle_opt") = py::dict(), py::arg("line_bundle_opt") = py::dict(), "Absolute pose non-linear refinement from points and lines."); m.def("refine_absolute_pose_pnpl", py::overload_cast &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const poselib::CameraPose &, const py::dict &, const py::dict &, const py::dict &>( &poselib::refine_absolute_pose_pnpl_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("lines2D_1"), py::arg("lines2D_2"), py::arg("lines3D_1"), py::arg("lines3D_2"), py::arg("initial_pose"), py::arg("camera_dict"), py::arg("bundle_opt") = py::dict(), py::arg("line_bundle_opt") = py::dict(), "Absolute pose non-linear refinement from points and lines."); m.def("refine_generalized_absolute_pose", py::overload_cast> &, const std::vector> &, const poselib::CameraPose &, const std::vector &, const std::vector &, const py::dict &>(&poselib::refine_generalized_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("initial_pose"), py::arg("camera_ext"), py::arg("cameras"), py::arg("bundle_opt") = py::dict(), "Generalized absolute pose non-linear refinement."); m.def("refine_generalized_absolute_pose", py::overload_cast> &, const std::vector> &, const poselib::CameraPose &, const std::vector &, const std::vector &, const py::dict &>( &poselib::refine_generalized_absolute_pose_wrapper), py::arg("points2D"), py::arg("points3D"), py::arg("initial_pose"), py::arg("camera_ext"), py::arg("camera_dicts"), py::arg("bundle_opt") = py::dict(), "Generalized absolute pose non-linear refinement."); m.def("refine_relative_pose", py::overload_cast &, const std::vector &, const poselib::CameraPose &, const poselib::Camera &, const poselib::Camera &, const py::dict &>(&poselib::refine_relative_pose_wrapper), py::arg("points2D_1"), py::arg("points2D_2"), py::arg("initial_pose"), py::arg("camera1"), py::arg("camera2"), py::arg("bundle_options") = py::dict(), "Relative pose non-linear refinement."); m.def("refine_relative_pose", py::overload_cast &, const std::vector &, const poselib::CameraPose &, const py::dict &, const py::dict &, const py::dict &>( &poselib::refine_relative_pose_wrapper), py::arg("points2D_1"), py::arg("points2D_2"), py::arg("initial_pose"), py::arg("camera1_dict"), py::arg("camera2_dict"), py::arg("bundle_options") = py::dict(), "Relative pose non-linear refinement."); m.def("refine_homography", &poselib::refine_homography_wrapper, py::arg("points2D_1"), py::arg("points2D_2"), py::arg("initial_H"), py::arg("bundle_options") = py::dict(), "Homography non-linear refinement."); m.def("refine_fundamental", &poselib::refine_fundamental_wrapper, py::arg("points2D_1"), py::arg("points2D_2"), py::arg("initial_F"), py::arg("bundle_options") = py::dict(), "Fundamental matrix non-linear refinement."); m.def("refine_generalized_relative_pose", py::overload_cast &, const poselib::CameraPose &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const py::dict &>(&poselib::refine_generalized_relative_pose_wrapper), py::arg("matches"), py::arg("initial_pose"), py::arg("camera1_ext"), py::arg("cameras1"), py::arg("camera2_ext"), py::arg("cameras2"), py::arg("bundle_opt") = py::dict(), "Generalized relative pose non-linear refinement."); m.def("refine_generalized_relative_pose", py::overload_cast &, const poselib::CameraPose &, const std::vector &, const std::vector &, const std::vector &, const std::vector &, const py::dict &>( &poselib::refine_generalized_relative_pose_wrapper), py::arg("matches"), py::arg("initial_pose"), py::arg("camera1_ext"), py::arg("camera1_dict"), py::arg("camera2_ext"), py::arg("camera2_dict"), py::arg("bundle_opt") = py::dict(), "Generalized relative pose non-linear refinement."); m.def("RansacOptions", &poselib::RansacOptions_wrapper, py::arg("opt") = py::dict(), "Options for RANSAC."); m.def("BundleOptions", &poselib::BundleOptions_wrapper, py::arg("opt") = py::dict(), "Options for non-linear refinement."); m.attr("__version__") = std::string(POSELIB_VERSION); } PoseLib-2.0.5/pyposelib/000077500000000000000000000000001504452766400150765ustar00rootroot00000000000000PoseLib-2.0.5/pyposelib/__init__.py000066400000000000000000000015641504452766400172150ustar00rootroot00000000000000# Adapted from https://github.com/colmap/colmap/blob/1ec758f2eb028049129b852d4d020d7542c866c9/python/pycolmap/__init__.py import textwrap from typing import TYPE_CHECKING from .utils import import_module_symbols try: from . import _core except ImportError as e: raise RuntimeError( textwrap.dedent(""" Cannot import the C++ backend poselib._core. Make sure that you successfully install the package with $ python -m pip install . or build it with $ python setup.py build_ext --inplace """) ) from e # Type checkers cannot deal with dynamic manipulation of globals. # Instead, we use the same workaround as PyTorch. if TYPE_CHECKING: from ._core import * # noqa F403 __all__ = import_module_symbols( globals(), _core, exclude=set() ) __all__.extend(["__version__"]) __version__ = _core.__version__ PoseLib-2.0.5/pyposelib/utils.py000066400000000000000000000006121504452766400166070ustar00rootroot00000000000000def import_module_symbols(globals_dict, module, exclude=None): """Import all public symbols from a module into globals.""" if exclude is None: exclude = set() symbols = [] for name in dir(module): if not name.startswith('_') and name not in exclude: globals_dict[name] = getattr(module, name) symbols.append(name) return symbols PoseLib-2.0.5/pyproject.toml000066400000000000000000000013161504452766400160050ustar00rootroot00000000000000[build-system] requires = ["setuptools>=40.8.0", "wheel", "pybind11-stubgen", "ruff", "numpy"] build-backend = "setuptools.build_meta" [project] name = "poselib" dynamic = ["version"] description = "RANSAC + collection of minimal solvers for camera pose estimation." authors = [ {name = "Viktor Larsson and contributors", email = "viktor.larsson@math.lth.se"} ] license = {file = "LICENSE"} readme = "README.md" requires-python = ">=3.8" dependencies = [ "numpy", ] [project.optional-dependencies] test = [ "posebench@git+https://github.com/PoseLib/posebench.git ; python_version < '3.14'", "pytest>=8.3.5", ] [tool.cibuildwheel] test-extras = ["test"] test-command = "pytest -s -v {project}/tests" PoseLib-2.0.5/scripts/000077500000000000000000000000001504452766400145575ustar00rootroot00000000000000PoseLib-2.0.5/scripts/build_linux.sh000077500000000000000000000022211504452766400174310ustar00rootroot00000000000000#!/bin/bash # This script can be called from anywhere and allows to build out of source. # Determine script absolute path SCRIPT_ABS_PATH=$(readlink -f ${BASH_SOURCE[0]}) SCRIPT_ABS_PATH=$(dirname ${SCRIPT_ABS_PATH}) # root folder where top-level CMakeLists.txt lives ROOT="${SCRIPT_ABS_PATH}/../" # Build type BUILD_TYPE=Release # BUILD_TYPE=Debug # Build folder BUILD_DIR=_build_linux # Installation folder INSTALL_DIR=_install_linux # Library type BUILD_SHARED_LIBS=OFF # Static # BUILD_SHARED_LIBS=ON # Shared # Number of cores NUM_CORES=$(nproc) # Options summary echo "" echo "BUILD_TYPE =" ${BUILD_TYPE} echo "BUILD_DIR =" ${SCRIPT_ABS_PATH}/${BUILD_DIR}/ echo "INSTALL_DIR =" ${SCRIPT_ABS_PATH}/${INSTALL_DIR}/ echo "BUILD_SHARED_LIBS =" ${BUILD_SHARED_LIBS} echo "" # switch to ROOT path cd ${ROOT} # clean # rm -fr ${BUILD_DIR} ${INSTALL_DIR} # cmake cmake \ -S . \ -B ${BUILD_DIR} \ -DCMAKE_BUILD_TYPE=${BUILD_TYPE} \ -DBUILD_SHARED_LIBS=${BUILD_SHARED_LIBS} \ -DCMAKE_INSTALL_PREFIX="${INSTALL_DIR}" # compile & install cmake \ --build ${BUILD_DIR} \ --target install \ -j $NUM_CORES PoseLib-2.0.5/scripts/build_ninja.sh000077500000000000000000000024371504452766400174020ustar00rootroot00000000000000#!/bin/bash # This script can be called from anywhere and allows to build out of source. # Determine script absolute path SCRIPT_ABS_PATH=$(readlink -f ${BASH_SOURCE[0]}) SCRIPT_ABS_PATH=$(dirname ${SCRIPT_ABS_PATH}) # root folder where top-level CMakeLists.txt lives ROOT="${SCRIPT_ABS_PATH}/../" # Build type BUILD_TYPE=Release # BUILD_TYPE=Debug # Build folder BUILD_DIR=_build_ninja # Installation folder INSTALL_DIR=_install_ninja # Library type BUILD_SHARED_LIBS=OFF # Static # BUILD_SHARED_LIBS=ON # Shared # Number of cores NUM_CORES=$(nproc) # Options summary echo "" echo "BUILD_TYPE =" ${BUILD_TYPE} echo "BUILD_DIR =" ${SCRIPT_ABS_PATH}/${BUILD_DIR}/ echo "INSTALL_DIR =" ${SCRIPT_ABS_PATH}/${INSTALL_DIR}/ echo "BUILD_SHARED_LIBS =" ${BUILD_SHARED_LIBS} echo "" # switch to ROOT path cd ${ROOT} # clean # rm -fr ${BUILD_DIR} ${INSTALL_DIR} # cmake cmake \ -S . \ -B ${BUILD_DIR} \ -G"Ninja" \ -DCMAKE_BUILD_TYPE=${BUILD_TYPE} \ -DBUILD_SHARED_LIBS=${BUILD_SHARED_LIBS} \ -DCMAKE_INSTALL_PREFIX="${INSTALL_DIR}" # For multi-config generator: # -G"Ninja Multi-Config" \ # -DCMAKE_BUILD_TYPE=${BUILD_TYPE} \ (and remove this line) # compile & install cmake \ --build ${BUILD_DIR} \ --target install \ -j $NUM_CORES PoseLib-2.0.5/scripts/build_win.sh000077500000000000000000000023211504452766400170700ustar00rootroot00000000000000#!/bin/bash # This script can be called from anywhere and allows to build out of source. # Note: it is assumed Git for Windows is installed (https://gitforwindows.org/). # Determine script absolute path SCRIPT_ABS_PATH=$(readlink -f ${BASH_SOURCE[0]}) SCRIPT_ABS_PATH=$(dirname ${SCRIPT_ABS_PATH}) # root folder where top-level CMakeLists.txt lives ROOT="${SCRIPT_ABS_PATH}/../" # Build type BUILD_TYPE=Release # BUILD_TYPE=Debug # Build folder BUILD_DIR="_build_win" # Installation folder INSTALL_DIR="_install_win" # Library type BUILD_SHARED_LIBS=OFF # Static # BUILD_SHARED_LIBS=ON # Shared # Number of cores NUM_CORES=16 # Options summary echo "" echo "BUILD_TYPE =" ${BUILD_TYPE} echo "BUILD_DIR =" ${SCRIPT_ABS_PATH}/${BUILD_DIR}/ echo "INSTALL_DIR =" ${SCRIPT_ABS_PATH}/${INSTALL_DIR}/ echo "BUILD_SHARED_LIBS =" ${BUILD_SHARED_LIBS} echo "" # switch to ROOT path cd ${ROOT} # clean # rm -fr ${BUILD_DIR} ${INSTALL_DIR} # cmake cmake \ -S . \ -B ${BUILD_DIR} \ -DBUILD_SHARED_LIBS=${BUILD_SHARED_LIBS} \ -DCMAKE_INSTALL_PREFIX="${INSTALL_DIR}" # compile & install cmake \ --build ${BUILD_DIR} \ --config ${BUILD_TYPE} \ --target install \ -j $NUM_CORES PoseLib-2.0.5/scripts/clang_format.sh000077500000000000000000000011421504452766400175500ustar00rootroot00000000000000#!/bin/bash # Determine script absolute path SCRIPT_ABS_PATH=$(readlink -f ${BASH_SOURCE[0]}) SCRIPT_ABS_PATH=$(dirname ${SCRIPT_ABS_PATH}) # root folder where top-level CMakeLists.txt lives ROOT="$(readlink -f ${SCRIPT_ABS_PATH}/..)" find ${ROOT}/PoseLib -iname "*.h" -o -iname "*.cc" | xargs clang-format -i --verbose find ${ROOT}/benchmark -iname "*.h" -o -iname "*.cc" | xargs clang-format -i --verbose clang-format -i --verbose ${ROOT}/pybind/pyposelib.cc clang-format -i --verbose ${ROOT}/pybind/pybind11_extension.h clang-format -i --verbose ${ROOT}/pybind/helpers.h echo $(clang-format --version) PoseLib-2.0.5/setup.py000066400000000000000000000074611504452766400146120ustar00rootroot00000000000000import os import re import sys import platform import subprocess from setuptools import setup, Extension from setuptools.command.build_ext import build_ext from distutils.version import LooseVersion class CMakeExtension(Extension): def __init__(self, name, sourcedir=''): Extension.__init__(self, name, sources=[]) self.sourcedir = os.path.abspath(sourcedir) class CMakeBuild(build_ext): def run(self): try: out = subprocess.check_output(['cmake', '--version']) except OSError: raise RuntimeError("CMake must be installed to build the following extensions: " + ", ".join(e.name for e in self.extensions)) if platform.system() == "Windows": cmake_version = LooseVersion(re.search(r'version\s*([\d.]+)', out.decode()).group(1)) if cmake_version < '3.1.0': raise RuntimeError("CMake >= 3.1.0 is required on Windows") for ext in self.extensions: self.build_extension(ext) def build_extension(self, ext): extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) cmake_args = ['-DPYTHON_EXECUTABLE=' + sys.executable, '-DPython_EXECUTABLE=' + sys.executable, '-DPYTHON_PACKAGE=ON', '-DBUILD_SHARED_LIBS=OFF',] if os.environ.get('CMAKE_INSTALL_PREFIX') is not None: cmake_args += [f"-DCMAKE_INSTALL_PREFIX={os.environ.get('CMAKE_INSTALL_PREFIX')}"] else: # Set the install prefix to the extension directory so install() commands work cmake_args += [f"-DCMAKE_INSTALL_PREFIX={extdir}"] cfg = 'Debug' if self.debug else 'Release' build_args = ['--config', cfg] if platform.system() == "Windows": if os.environ.get('CMAKE_TOOLCHAIN_FILE') is not None: cmake_toolchain_file = os.environ.get('CMAKE_TOOLCHAIN_FILE') cmake_args += [f'-DCMAKE_TOOLCHAIN_FILE={cmake_toolchain_file}'] cmake_args += ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}'.format(cfg.upper(), extdir)] if sys.maxsize > 2**32: if os.environ.get('CMAKE_TOOLCHAIN_FILE') is not None: cmake_args += ['-DVCPKG_TARGET_TRIPLET=x64-windows'] cmake_args += ['-A', 'x64'] build_args += ['--', '/m'] else: cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg] build_args += ['--', '-j2'] env = os.environ.copy() env['CXXFLAGS'] = '{} -DVERSION_INFO=\\"{}\\"'.format( env.get('CXXFLAGS', ''), self.distribution.get_version() ) if not os.path.exists(self.build_temp): os.makedirs(self.build_temp) subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp, env=env) subprocess.check_call(['cmake', '--build', '.'] + build_args, cwd=self.build_temp) # Run cmake --install but only install the python component to avoid library headers/libs install_args = ['--config', cfg] if platform.system() == "Windows" else [] install_args += ['--component', 'python'] subprocess.check_call(['cmake', '--install', '.'] + install_args, cwd=self.build_temp) # The information here can also be placed in setup.cfg - better separation of # logic and declaration, and simpler if you include description/version in a file. setup( name="poselib", version="2.0.4", author="Viktor Larsson and contributors", author_email="viktor.larsson@math.lth.se", description="", long_description="", ext_modules=[CMakeExtension("poselib._core")], packages=["poselib"], package_dir={"poselib": "pyposelib"}, cmdclass={"build_ext": CMakeBuild}, zip_safe=False, ) PoseLib-2.0.5/tests/000077500000000000000000000000001504452766400142325ustar00rootroot00000000000000PoseLib-2.0.5/tests/test_poselib.py000066400000000000000000000007671504452766400173120ustar00rootroot00000000000000import sys import poselib def test_poselib(): print(f'Python version: {sys.version}') print(f'PoseLib version: {poselib.__version__}') if sys.version_info < (3, 14): from posebench import run_benchmark print(f'Running posebench...') result = run_benchmark( subsample=10, subset=True, ) print(result) print('Posebench done.') else: print('Skipping tests for Python >= 3.14 due to deps not ready yet (h5py)')PoseLib-2.0.5/uv.lock000066400000000000000000003173101504452766400144010ustar00rootroot00000000000000version = 1 revision = 1 requires-python = ">=3.8" resolution-markers = [ "python_full_version >= '3.11'", "python_full_version == '3.10.*'", "python_full_version == '3.9.*'", "python_full_version < '3.9'", ] [[package]] name = "certifi" version = "2025.7.14" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/b3/76/52c535bcebe74590f296d6c77c86dabf761c41980e1347a2422e4aa2ae41/certifi-2025.7.14.tar.gz", hash = "sha256:8ea99dbdfaaf2ba2f9bac77b9249ef62ec5218e7c2b2e903378ed5fccf765995", size = 163981 } wheels = [ { url = "https://files.pythonhosted.org/packages/4f/52/34c6cf5bb9285074dc3531c437b3919e825d976fde097a7a73f79e726d03/certifi-2025.7.14-py3-none-any.whl", hash = "sha256:6b31f564a415d79ee77df69d757bb49a5bb53bd9f756cbbe24394ffd6fc1f4b2", size = 162722 }, ] [[package]] name = "charset-normalizer" version = "3.4.2" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/e4/33/89c2ced2b67d1c2a61c19c6751aa8902d46ce3dacb23600a283619f5a12d/charset_normalizer-3.4.2.tar.gz", hash = "sha256:5baececa9ecba31eff645232d59845c07aa030f0c81ee70184a90d35099a0e63", size = 126367 } wheels = [ { url = "https://files.pythonhosted.org/packages/95/28/9901804da60055b406e1a1c5ba7aac1276fb77f1dde635aabfc7fd84b8ab/charset_normalizer-3.4.2-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:7c48ed483eb946e6c04ccbe02c6b4d1d48e51944b6db70f697e089c193404941", size = 201818 }, { url = "https://files.pythonhosted.org/packages/d9/9b/892a8c8af9110935e5adcbb06d9c6fe741b6bb02608c6513983048ba1a18/charset_normalizer-3.4.2-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:b2d318c11350e10662026ad0eb71bb51c7812fc8590825304ae0bdd4ac283acd", size = 144649 }, { url = "https://files.pythonhosted.org/packages/7b/a5/4179abd063ff6414223575e008593861d62abfc22455b5d1a44995b7c101/charset_normalizer-3.4.2-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:9cbfacf36cb0ec2897ce0ebc5d08ca44213af24265bd56eca54bee7923c48fd6", size = 155045 }, { url = "https://files.pythonhosted.org/packages/3b/95/bc08c7dfeddd26b4be8c8287b9bb055716f31077c8b0ea1cd09553794665/charset_normalizer-3.4.2-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:18dd2e350387c87dabe711b86f83c9c78af772c748904d372ade190b5c7c9d4d", size = 147356 }, { url = "https://files.pythonhosted.org/packages/a8/2d/7a5b635aa65284bf3eab7653e8b4151ab420ecbae918d3e359d1947b4d61/charset_normalizer-3.4.2-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8075c35cd58273fee266c58c0c9b670947c19df5fb98e7b66710e04ad4e9ff86", size = 149471 }, { url = "https://files.pythonhosted.org/packages/ae/38/51fc6ac74251fd331a8cfdb7ec57beba8c23fd5493f1050f71c87ef77ed0/charset_normalizer-3.4.2-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:5bf4545e3b962767e5c06fe1738f951f77d27967cb2caa64c28be7c4563e162c", size = 151317 }, { url = "https://files.pythonhosted.org/packages/b7/17/edee1e32215ee6e9e46c3e482645b46575a44a2d72c7dfd49e49f60ce6bf/charset_normalizer-3.4.2-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:7a6ab32f7210554a96cd9e33abe3ddd86732beeafc7a28e9955cdf22ffadbab0", size = 146368 }, { url = "https://files.pythonhosted.org/packages/26/2c/ea3e66f2b5f21fd00b2825c94cafb8c326ea6240cd80a91eb09e4a285830/charset_normalizer-3.4.2-cp310-cp310-musllinux_1_2_i686.whl", hash = "sha256:b33de11b92e9f75a2b545d6e9b6f37e398d86c3e9e9653c4864eb7e89c5773ef", size = 154491 }, { url = "https://files.pythonhosted.org/packages/52/47/7be7fa972422ad062e909fd62460d45c3ef4c141805b7078dbab15904ff7/charset_normalizer-3.4.2-cp310-cp310-musllinux_1_2_ppc64le.whl", hash = "sha256:8755483f3c00d6c9a77f490c17e6ab0c8729e39e6390328e42521ef175380ae6", size = 157695 }, { url = "https://files.pythonhosted.org/packages/2f/42/9f02c194da282b2b340f28e5fb60762de1151387a36842a92b533685c61e/charset_normalizer-3.4.2-cp310-cp310-musllinux_1_2_s390x.whl", hash = "sha256:68a328e5f55ec37c57f19ebb1fdc56a248db2e3e9ad769919a58672958e8f366", size = 154849 }, { url = "https://files.pythonhosted.org/packages/67/44/89cacd6628f31fb0b63201a618049be4be2a7435a31b55b5eb1c3674547a/charset_normalizer-3.4.2-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:21b2899062867b0e1fde9b724f8aecb1af14f2778d69aacd1a5a1853a597a5db", size = 150091 }, { url = "https://files.pythonhosted.org/packages/1f/79/4b8da9f712bc079c0f16b6d67b099b0b8d808c2292c937f267d816ec5ecc/charset_normalizer-3.4.2-cp310-cp310-win32.whl", hash = "sha256:e8082b26888e2f8b36a042a58307d5b917ef2b1cacab921ad3323ef91901c71a", size = 98445 }, { url = "https://files.pythonhosted.org/packages/7d/d7/96970afb4fb66497a40761cdf7bd4f6fca0fc7bafde3a84f836c1f57a926/charset_normalizer-3.4.2-cp310-cp310-win_amd64.whl", hash = "sha256:f69a27e45c43520f5487f27627059b64aaf160415589230992cec34c5e18a509", size = 105782 }, { url = "https://files.pythonhosted.org/packages/05/85/4c40d00dcc6284a1c1ad5de5e0996b06f39d8232f1031cd23c2f5c07ee86/charset_normalizer-3.4.2-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:be1e352acbe3c78727a16a455126d9ff83ea2dfdcbc83148d2982305a04714c2", size = 198794 }, { url = "https://files.pythonhosted.org/packages/41/d9/7a6c0b9db952598e97e93cbdfcb91bacd89b9b88c7c983250a77c008703c/charset_normalizer-3.4.2-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:aa88ca0b1932e93f2d961bf3addbb2db902198dca337d88c89e1559e066e7645", size = 142846 }, { url = "https://files.pythonhosted.org/packages/66/82/a37989cda2ace7e37f36c1a8ed16c58cf48965a79c2142713244bf945c89/charset_normalizer-3.4.2-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:d524ba3f1581b35c03cb42beebab4a13e6cdad7b36246bd22541fa585a56cccd", size = 153350 }, { url = "https://files.pythonhosted.org/packages/df/68/a576b31b694d07b53807269d05ec3f6f1093e9545e8607121995ba7a8313/charset_normalizer-3.4.2-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:28a1005facc94196e1fb3e82a3d442a9d9110b8434fc1ded7a24a2983c9888d8", size = 145657 }, { url = "https://files.pythonhosted.org/packages/92/9b/ad67f03d74554bed3aefd56fe836e1623a50780f7c998d00ca128924a499/charset_normalizer-3.4.2-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fdb20a30fe1175ecabed17cbf7812f7b804b8a315a25f24678bcdf120a90077f", size = 147260 }, { url = "https://files.pythonhosted.org/packages/a6/e6/8aebae25e328160b20e31a7e9929b1578bbdc7f42e66f46595a432f8539e/charset_normalizer-3.4.2-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:0f5d9ed7f254402c9e7d35d2f5972c9bbea9040e99cd2861bd77dc68263277c7", size = 149164 }, { url = "https://files.pythonhosted.org/packages/8b/f2/b3c2f07dbcc248805f10e67a0262c93308cfa149a4cd3d1fe01f593e5fd2/charset_normalizer-3.4.2-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:efd387a49825780ff861998cd959767800d54f8308936b21025326de4b5a42b9", size = 144571 }, { url = "https://files.pythonhosted.org/packages/60/5b/c3f3a94bc345bc211622ea59b4bed9ae63c00920e2e8f11824aa5708e8b7/charset_normalizer-3.4.2-cp311-cp311-musllinux_1_2_i686.whl", hash = "sha256:f0aa37f3c979cf2546b73e8222bbfa3dc07a641585340179d768068e3455e544", size = 151952 }, { url = "https://files.pythonhosted.org/packages/e2/4d/ff460c8b474122334c2fa394a3f99a04cf11c646da895f81402ae54f5c42/charset_normalizer-3.4.2-cp311-cp311-musllinux_1_2_ppc64le.whl", hash = "sha256:e70e990b2137b29dc5564715de1e12701815dacc1d056308e2b17e9095372a82", size = 155959 }, { url = "https://files.pythonhosted.org/packages/a2/2b/b964c6a2fda88611a1fe3d4c400d39c66a42d6c169c924818c848f922415/charset_normalizer-3.4.2-cp311-cp311-musllinux_1_2_s390x.whl", hash = "sha256:0c8c57f84ccfc871a48a47321cfa49ae1df56cd1d965a09abe84066f6853b9c0", size = 153030 }, { url = "https://files.pythonhosted.org/packages/59/2e/d3b9811db26a5ebf444bc0fa4f4be5aa6d76fc6e1c0fd537b16c14e849b6/charset_normalizer-3.4.2-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:6b66f92b17849b85cad91259efc341dce9c1af48e2173bf38a85c6329f1033e5", size = 148015 }, { url = "https://files.pythonhosted.org/packages/90/07/c5fd7c11eafd561bb51220d600a788f1c8d77c5eef37ee49454cc5c35575/charset_normalizer-3.4.2-cp311-cp311-win32.whl", hash = "sha256:daac4765328a919a805fa5e2720f3e94767abd632ae410a9062dff5412bae65a", size = 98106 }, { url = "https://files.pythonhosted.org/packages/a8/05/5e33dbef7e2f773d672b6d79f10ec633d4a71cd96db6673625838a4fd532/charset_normalizer-3.4.2-cp311-cp311-win_amd64.whl", hash = "sha256:e53efc7c7cee4c1e70661e2e112ca46a575f90ed9ae3fef200f2a25e954f4b28", size = 105402 }, { url = "https://files.pythonhosted.org/packages/d7/a4/37f4d6035c89cac7930395a35cc0f1b872e652eaafb76a6075943754f095/charset_normalizer-3.4.2-cp312-cp312-macosx_10_13_universal2.whl", hash = "sha256:0c29de6a1a95f24b9a1aa7aefd27d2487263f00dfd55a77719b530788f75cff7", size = 199936 }, { url = "https://files.pythonhosted.org/packages/ee/8a/1a5e33b73e0d9287274f899d967907cd0bf9c343e651755d9307e0dbf2b3/charset_normalizer-3.4.2-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:cddf7bd982eaa998934a91f69d182aec997c6c468898efe6679af88283b498d3", size = 143790 }, { url = "https://files.pythonhosted.org/packages/66/52/59521f1d8e6ab1482164fa21409c5ef44da3e9f653c13ba71becdd98dec3/charset_normalizer-3.4.2-cp312-cp312-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:fcbe676a55d7445b22c10967bceaaf0ee69407fbe0ece4d032b6eb8d4565982a", size = 153924 }, { url = "https://files.pythonhosted.org/packages/86/2d/fb55fdf41964ec782febbf33cb64be480a6b8f16ded2dbe8db27a405c09f/charset_normalizer-3.4.2-cp312-cp312-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:d41c4d287cfc69060fa91cae9683eacffad989f1a10811995fa309df656ec214", size = 146626 }, { url = "https://files.pythonhosted.org/packages/8c/73/6ede2ec59bce19b3edf4209d70004253ec5f4e319f9a2e3f2f15601ed5f7/charset_normalizer-3.4.2-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4e594135de17ab3866138f496755f302b72157d115086d100c3f19370839dd3a", size = 148567 }, { url = "https://files.pythonhosted.org/packages/09/14/957d03c6dc343c04904530b6bef4e5efae5ec7d7990a7cbb868e4595ee30/charset_normalizer-3.4.2-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:cf713fe9a71ef6fd5adf7a79670135081cd4431c2943864757f0fa3a65b1fafd", size = 150957 }, { url = "https://files.pythonhosted.org/packages/0d/c8/8174d0e5c10ccebdcb1b53cc959591c4c722a3ad92461a273e86b9f5a302/charset_normalizer-3.4.2-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:a370b3e078e418187da8c3674eddb9d983ec09445c99a3a263c2011993522981", size = 145408 }, { url = "https://files.pythonhosted.org/packages/58/aa/8904b84bc8084ac19dc52feb4f5952c6df03ffb460a887b42615ee1382e8/charset_normalizer-3.4.2-cp312-cp312-musllinux_1_2_i686.whl", hash = "sha256:a955b438e62efdf7e0b7b52a64dc5c3396e2634baa62471768a64bc2adb73d5c", size = 153399 }, { url = "https://files.pythonhosted.org/packages/c2/26/89ee1f0e264d201cb65cf054aca6038c03b1a0c6b4ae998070392a3ce605/charset_normalizer-3.4.2-cp312-cp312-musllinux_1_2_ppc64le.whl", hash = "sha256:7222ffd5e4de8e57e03ce2cef95a4c43c98fcb72ad86909abdfc2c17d227fc1b", size = 156815 }, { url = "https://files.pythonhosted.org/packages/fd/07/68e95b4b345bad3dbbd3a8681737b4338ff2c9df29856a6d6d23ac4c73cb/charset_normalizer-3.4.2-cp312-cp312-musllinux_1_2_s390x.whl", hash = "sha256:bee093bf902e1d8fc0ac143c88902c3dfc8941f7ea1d6a8dd2bcb786d33db03d", size = 154537 }, { url = "https://files.pythonhosted.org/packages/77/1a/5eefc0ce04affb98af07bc05f3bac9094513c0e23b0562d64af46a06aae4/charset_normalizer-3.4.2-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:dedb8adb91d11846ee08bec4c8236c8549ac721c245678282dcb06b221aab59f", size = 149565 }, { url = "https://files.pythonhosted.org/packages/37/a0/2410e5e6032a174c95e0806b1a6585eb21e12f445ebe239fac441995226a/charset_normalizer-3.4.2-cp312-cp312-win32.whl", hash = "sha256:db4c7bf0e07fc3b7d89ac2a5880a6a8062056801b83ff56d8464b70f65482b6c", size = 98357 }, { url = "https://files.pythonhosted.org/packages/6c/4f/c02d5c493967af3eda9c771ad4d2bbc8df6f99ddbeb37ceea6e8716a32bc/charset_normalizer-3.4.2-cp312-cp312-win_amd64.whl", hash = "sha256:5a9979887252a82fefd3d3ed2a8e3b937a7a809f65dcb1e068b090e165bbe99e", size = 105776 }, { url = "https://files.pythonhosted.org/packages/ea/12/a93df3366ed32db1d907d7593a94f1fe6293903e3e92967bebd6950ed12c/charset_normalizer-3.4.2-cp313-cp313-macosx_10_13_universal2.whl", hash = "sha256:926ca93accd5d36ccdabd803392ddc3e03e6d4cd1cf17deff3b989ab8e9dbcf0", size = 199622 }, { url = "https://files.pythonhosted.org/packages/04/93/bf204e6f344c39d9937d3c13c8cd5bbfc266472e51fc8c07cb7f64fcd2de/charset_normalizer-3.4.2-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:eba9904b0f38a143592d9fc0e19e2df0fa2e41c3c3745554761c5f6447eedabf", size = 143435 }, { url = "https://files.pythonhosted.org/packages/22/2a/ea8a2095b0bafa6c5b5a55ffdc2f924455233ee7b91c69b7edfcc9e02284/charset_normalizer-3.4.2-cp313-cp313-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:3fddb7e2c84ac87ac3a947cb4e66d143ca5863ef48e4a5ecb83bd48619e4634e", size = 153653 }, { url = "https://files.pythonhosted.org/packages/b6/57/1b090ff183d13cef485dfbe272e2fe57622a76694061353c59da52c9a659/charset_normalizer-3.4.2-cp313-cp313-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:98f862da73774290f251b9df8d11161b6cf25b599a66baf087c1ffe340e9bfd1", size = 146231 }, { url = "https://files.pythonhosted.org/packages/e2/28/ffc026b26f441fc67bd21ab7f03b313ab3fe46714a14b516f931abe1a2d8/charset_normalizer-3.4.2-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6c9379d65defcab82d07b2a9dfbfc2e95bc8fe0ebb1b176a3190230a3ef0e07c", size = 148243 }, { url = "https://files.pythonhosted.org/packages/c0/0f/9abe9bd191629c33e69e47c6ef45ef99773320e9ad8e9cb08b8ab4a8d4cb/charset_normalizer-3.4.2-cp313-cp313-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:e635b87f01ebc977342e2697d05b56632f5f879a4f15955dfe8cef2448b51691", size = 150442 }, { url = "https://files.pythonhosted.org/packages/67/7c/a123bbcedca91d5916c056407f89a7f5e8fdfce12ba825d7d6b9954a1a3c/charset_normalizer-3.4.2-cp313-cp313-musllinux_1_2_aarch64.whl", hash = "sha256:1c95a1e2902a8b722868587c0e1184ad5c55631de5afc0eb96bc4b0d738092c0", size = 145147 }, { url = "https://files.pythonhosted.org/packages/ec/fe/1ac556fa4899d967b83e9893788e86b6af4d83e4726511eaaad035e36595/charset_normalizer-3.4.2-cp313-cp313-musllinux_1_2_i686.whl", hash = "sha256:ef8de666d6179b009dce7bcb2ad4c4a779f113f12caf8dc77f0162c29d20490b", size = 153057 }, { url = "https://files.pythonhosted.org/packages/2b/ff/acfc0b0a70b19e3e54febdd5301a98b72fa07635e56f24f60502e954c461/charset_normalizer-3.4.2-cp313-cp313-musllinux_1_2_ppc64le.whl", hash = "sha256:32fc0341d72e0f73f80acb0a2c94216bd704f4f0bce10aedea38f30502b271ff", size = 156454 }, { url = "https://files.pythonhosted.org/packages/92/08/95b458ce9c740d0645feb0e96cea1f5ec946ea9c580a94adfe0b617f3573/charset_normalizer-3.4.2-cp313-cp313-musllinux_1_2_s390x.whl", hash = "sha256:289200a18fa698949d2b39c671c2cc7a24d44096784e76614899a7ccf2574b7b", size = 154174 }, { url = "https://files.pythonhosted.org/packages/78/be/8392efc43487ac051eee6c36d5fbd63032d78f7728cb37aebcc98191f1ff/charset_normalizer-3.4.2-cp313-cp313-musllinux_1_2_x86_64.whl", hash = "sha256:4a476b06fbcf359ad25d34a057b7219281286ae2477cc5ff5e3f70a246971148", size = 149166 }, { url = "https://files.pythonhosted.org/packages/44/96/392abd49b094d30b91d9fbda6a69519e95802250b777841cf3bda8fe136c/charset_normalizer-3.4.2-cp313-cp313-win32.whl", hash = "sha256:aaeeb6a479c7667fbe1099af9617c83aaca22182d6cf8c53966491a0f1b7ffb7", size = 98064 }, { url = "https://files.pythonhosted.org/packages/e9/b0/0200da600134e001d91851ddc797809e2fe0ea72de90e09bec5a2fbdaccb/charset_normalizer-3.4.2-cp313-cp313-win_amd64.whl", hash = "sha256:aa6af9e7d59f9c12b33ae4e9450619cf2488e2bbe9b44030905877f0b2324980", size = 105641 }, { url = "https://files.pythonhosted.org/packages/4c/fd/f700cfd4ad876def96d2c769d8a32d808b12d1010b6003dc6639157f99ee/charset_normalizer-3.4.2-cp38-cp38-macosx_10_9_universal2.whl", hash = "sha256:76af085e67e56c8816c3ccf256ebd136def2ed9654525348cfa744b6802b69eb", size = 198257 }, { url = "https://files.pythonhosted.org/packages/3a/95/6eec4cbbbd119e6a402e3bfd16246785cc52ce64cf21af2ecdf7b3a08e91/charset_normalizer-3.4.2-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e45ba65510e2647721e35323d6ef54c7974959f6081b58d4ef5d87c60c84919a", size = 143453 }, { url = "https://files.pythonhosted.org/packages/b6/b3/d4f913660383b3d93dbe6f687a312ea9f7e89879ae883c4e8942048174d4/charset_normalizer-3.4.2-cp38-cp38-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:046595208aae0120559a67693ecc65dd75d46f7bf687f159127046628178dc45", size = 153130 }, { url = "https://files.pythonhosted.org/packages/e5/69/7540141529eabc55bf19cc05cd9b61c2078bebfcdbd3e799af99b777fc28/charset_normalizer-3.4.2-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:75d10d37a47afee94919c4fab4c22b9bc2a8bf7d4f46f87363bcf0573f3ff4f5", size = 145688 }, { url = "https://files.pythonhosted.org/packages/2e/bb/d76d3d6e340fb0967c43c564101e28a78c9a363ea62f736a68af59ee3683/charset_normalizer-3.4.2-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6333b3aa5a12c26b2a4d4e7335a28f1475e0e5e17d69d55141ee3cab736f66d1", size = 147418 }, { url = "https://files.pythonhosted.org/packages/3e/ef/b7c1f39c0dc3808160c8b72e0209c2479393966313bfebc833533cfff9cc/charset_normalizer-3.4.2-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:e8323a9b031aa0393768b87f04b4164a40037fb2a3c11ac06a03ffecd3618027", size = 150066 }, { url = "https://files.pythonhosted.org/packages/20/26/4e47cc23d2a4a5eb6ed7d6f0f8cda87d753e2f8abc936d5cf5ad2aae8518/charset_normalizer-3.4.2-cp38-cp38-musllinux_1_2_aarch64.whl", hash = "sha256:24498ba8ed6c2e0b56d4acbf83f2d989720a93b41d712ebd4f4979660db4417b", size = 144499 }, { url = "https://files.pythonhosted.org/packages/d7/9c/efdf59dd46593cecad0548d36a702683a0bdc056793398a9cd1e1546ad21/charset_normalizer-3.4.2-cp38-cp38-musllinux_1_2_i686.whl", hash = "sha256:844da2b5728b5ce0e32d863af26f32b5ce61bc4273a9c720a9f3aa9df73b1455", size = 152954 }, { url = "https://files.pythonhosted.org/packages/59/b3/4e8b73f7299d9aaabd7cd26db4a765f741b8e57df97b034bb8de15609002/charset_normalizer-3.4.2-cp38-cp38-musllinux_1_2_ppc64le.whl", hash = "sha256:65c981bdbd3f57670af8b59777cbfae75364b483fa8a9f420f08094531d54a01", size = 155876 }, { url = "https://files.pythonhosted.org/packages/53/cb/6fa0ccf941a069adce3edb8a1e430bc80e4929f4d43b5140fdf8628bdf7d/charset_normalizer-3.4.2-cp38-cp38-musllinux_1_2_s390x.whl", hash = "sha256:3c21d4fca343c805a52c0c78edc01e3477f6dd1ad7c47653241cf2a206d4fc58", size = 153186 }, { url = "https://files.pythonhosted.org/packages/ac/c6/80b93fabc626b75b1665ffe405e28c3cef0aae9237c5c05f15955af4edd8/charset_normalizer-3.4.2-cp38-cp38-musllinux_1_2_x86_64.whl", hash = "sha256:dc7039885fa1baf9be153a0626e337aa7ec8bf96b0128605fb0d77788ddc1681", size = 148007 }, { url = "https://files.pythonhosted.org/packages/41/eb/c7367ac326a2628e4f05b5c737c86fe4a8eb3ecc597a4243fc65720b3eeb/charset_normalizer-3.4.2-cp38-cp38-win32.whl", hash = "sha256:8272b73e1c5603666618805fe821edba66892e2870058c94c53147602eab29c7", size = 97923 }, { url = "https://files.pythonhosted.org/packages/7c/02/1c82646582ccf2c757fa6af69b1a3ea88744b8d2b4ab93b7686b2533e023/charset_normalizer-3.4.2-cp38-cp38-win_amd64.whl", hash = "sha256:70f7172939fdf8790425ba31915bfbe8335030f05b9913d7ae00a87d4395620a", size = 105020 }, { url = "https://files.pythonhosted.org/packages/28/f8/dfb01ff6cc9af38552c69c9027501ff5a5117c4cc18dcd27cb5259fa1888/charset_normalizer-3.4.2-cp39-cp39-macosx_10_9_universal2.whl", hash = "sha256:005fa3432484527f9732ebd315da8da8001593e2cf46a3d817669f062c3d9ed4", size = 201671 }, { url = "https://files.pythonhosted.org/packages/32/fb/74e26ee556a9dbfe3bd264289b67be1e6d616329403036f6507bb9f3f29c/charset_normalizer-3.4.2-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e92fca20c46e9f5e1bb485887d074918b13543b1c2a1185e69bb8d17ab6236a7", size = 144744 }, { url = "https://files.pythonhosted.org/packages/ad/06/8499ee5aa7addc6f6d72e068691826ff093329fe59891e83b092ae4c851c/charset_normalizer-3.4.2-cp39-cp39-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:50bf98d5e563b83cc29471fa114366e6806bc06bc7a25fd59641e41445327836", size = 154993 }, { url = "https://files.pythonhosted.org/packages/f1/a2/5e4c187680728219254ef107a6949c60ee0e9a916a5dadb148c7ae82459c/charset_normalizer-3.4.2-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:721c76e84fe669be19c5791da68232ca2e05ba5185575086e384352e2c309597", size = 147382 }, { url = "https://files.pythonhosted.org/packages/4c/fe/56aca740dda674f0cc1ba1418c4d84534be51f639b5f98f538b332dc9a95/charset_normalizer-3.4.2-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:82d8fd25b7f4675d0c47cf95b594d4e7b158aca33b76aa63d07186e13c0e0ab7", size = 149536 }, { url = "https://files.pythonhosted.org/packages/53/13/db2e7779f892386b589173dd689c1b1e304621c5792046edd8a978cbf9e0/charset_normalizer-3.4.2-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:b3daeac64d5b371dea99714f08ffc2c208522ec6b06fbc7866a450dd446f5c0f", size = 151349 }, { url = "https://files.pythonhosted.org/packages/69/35/e52ab9a276186f729bce7a0638585d2982f50402046e4b0faa5d2c3ef2da/charset_normalizer-3.4.2-cp39-cp39-musllinux_1_2_aarch64.whl", hash = "sha256:dccab8d5fa1ef9bfba0590ecf4d46df048d18ffe3eec01eeb73a42e0d9e7a8ba", size = 146365 }, { url = "https://files.pythonhosted.org/packages/a6/d8/af7333f732fc2e7635867d56cb7c349c28c7094910c72267586947561b4b/charset_normalizer-3.4.2-cp39-cp39-musllinux_1_2_i686.whl", hash = "sha256:aaf27faa992bfee0264dc1f03f4c75e9fcdda66a519db6b957a3f826e285cf12", size = 154499 }, { url = "https://files.pythonhosted.org/packages/7a/3d/a5b2e48acef264d71e036ff30bcc49e51bde80219bb628ba3e00cf59baac/charset_normalizer-3.4.2-cp39-cp39-musllinux_1_2_ppc64le.whl", hash = "sha256:eb30abc20df9ab0814b5a2524f23d75dcf83cde762c161917a2b4b7b55b1e518", size = 157735 }, { url = "https://files.pythonhosted.org/packages/85/d8/23e2c112532a29f3eef374375a8684a4f3b8e784f62b01da931186f43494/charset_normalizer-3.4.2-cp39-cp39-musllinux_1_2_s390x.whl", hash = "sha256:c72fbbe68c6f32f251bdc08b8611c7b3060612236e960ef848e0a517ddbe76c5", size = 154786 }, { url = "https://files.pythonhosted.org/packages/c7/57/93e0169f08ecc20fe82d12254a200dfaceddc1c12a4077bf454ecc597e33/charset_normalizer-3.4.2-cp39-cp39-musllinux_1_2_x86_64.whl", hash = "sha256:982bb1e8b4ffda883b3d0a521e23abcd6fd17418f6d2c4118d257a10199c0ce3", size = 150203 }, { url = "https://files.pythonhosted.org/packages/2c/9d/9bf2b005138e7e060d7ebdec7503d0ef3240141587651f4b445bdf7286c2/charset_normalizer-3.4.2-cp39-cp39-win32.whl", hash = "sha256:43e0933a0eff183ee85833f341ec567c0980dae57c464d8a508e1b2ceb336471", size = 98436 }, { url = "https://files.pythonhosted.org/packages/6d/24/5849d46cf4311bbf21b424c443b09b459f5b436b1558c04e45dbb7cc478b/charset_normalizer-3.4.2-cp39-cp39-win_amd64.whl", hash = "sha256:d11b54acf878eef558599658b0ffca78138c8c3655cf4f3a4a673c437e67732e", size = 105772 }, { url = "https://files.pythonhosted.org/packages/20/94/c5790835a017658cbfabd07f3bfb549140c3ac458cfc196323996b10095a/charset_normalizer-3.4.2-py3-none-any.whl", hash = "sha256:7f56930ab0abd1c45cd15be65cc741c28b1c9a34876ce8c17a2fa107810c0af0", size = 52626 }, ] [[package]] name = "colorama" version = "0.4.6" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/d8/53/6f443c9a4a8358a93a6792e2acffb9d9d5cb0a5cfd8802644b7b1c9a02e4/colorama-0.4.6.tar.gz", hash = "sha256:08695f5cb7ed6e0531a20572697297273c47b8cae5a63ffc6d6ed5c201be6e44", size = 27697 } wheels = [ { url = "https://files.pythonhosted.org/packages/d1/d6/3965ed04c63042e047cb6a3e6ed1a63a35087b6a609aa3a15ed8ac56c221/colorama-0.4.6-py2.py3-none-any.whl", hash = "sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6", size = 25335 }, ] [[package]] name = "exceptiongroup" version = "1.3.0" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "typing-extensions", version = "4.13.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "typing-extensions", version = "4.14.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.9' and python_full_version < '3.11'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/0b/9f/a65090624ecf468cdca03533906e7c69ed7588582240cfe7cc9e770b50eb/exceptiongroup-1.3.0.tar.gz", hash = "sha256:b241f5885f560bc56a59ee63ca4c6a8bfa46ae4ad651af316d4e81817bb9fd88", size = 29749 } wheels = [ { url = "https://files.pythonhosted.org/packages/36/f4/c6e662dade71f56cd2f3735141b265c3c79293c109549c1e6933b0651ffc/exceptiongroup-1.3.0-py3-none-any.whl", hash = "sha256:4d111e6e0c13d0644cad6ddaa7ed0261a0b36971f6d23e7ec9b4b9097da78a10", size = 16674 }, ] [[package]] name = "h5py" version = "3.11.0" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version < '3.9'", ] dependencies = [ { name = "numpy", version = "1.24.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/52/8f/e557819155a282da36fb21f8de4730cfd10a964b52b3ae8d20157ac1c668/h5py-3.11.0.tar.gz", hash = "sha256:7b7e8f78072a2edec87c9836f25f34203fd492a4475709a18b417a33cfb21fa9", size = 406519 } wheels = [ { url = "https://files.pythonhosted.org/packages/ac/25/a1cc81b3a742b73f9409bafe4762c9de0940cce0955d4b6754698fd5ce44/h5py-3.11.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:1625fd24ad6cfc9c1ccd44a66dac2396e7ee74940776792772819fc69f3a3731", size = 3477113 }, { url = "https://files.pythonhosted.org/packages/d4/03/bbb9a992fb43d3ce46687b7c14107f0fa56e6c8704c9ca945a9392cbc8ce/h5py-3.11.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:c072655ad1d5fe9ef462445d3e77a8166cbfa5e599045f8aa3c19b75315f10e5", size = 2939879 }, { url = "https://files.pythonhosted.org/packages/94/00/94bf8573e7487b7c37f2b613fc381880d48ec2311f2e859b8a5817deb4df/h5py-3.11.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:77b19a40788e3e362b54af4dcf9e6fde59ca016db2c61360aa30b47c7b7cef00", size = 5306122 }, { url = "https://files.pythonhosted.org/packages/bb/0d/fbadb9c69e2a31f641bc24e8d21671129ef3b73f0c61bb16b094fadf1385/h5py-3.11.0-cp310-cp310-win_amd64.whl", hash = "sha256:ef4e2f338fc763f50a8113890f455e1a70acd42a4d083370ceb80c463d803972", size = 2968816 }, { url = "https://files.pythonhosted.org/packages/a0/52/38bb74cc4362738cc7ef819503fc54d70f0c3a7378519ccb0ac309389122/h5py-3.11.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:bbd732a08187a9e2a6ecf9e8af713f1d68256ee0f7c8b652a32795670fb481ba", size = 3489913 }, { url = "https://files.pythonhosted.org/packages/f0/af/dfbea0c69fe725e9e77259d42f4e14eb582eb094200aaf697feb36f513d8/h5py-3.11.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:75bd7b3d93fbeee40860fd70cdc88df4464e06b70a5ad9ce1446f5f32eb84007", size = 2946912 }, { url = "https://files.pythonhosted.org/packages/af/26/f231ee425c8df93c1abbead3d90ea4a5ff3d6aa49e0edfd3b4c017e74844/h5py-3.11.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:52c416f8eb0daae39dabe71415cb531f95dce2d81e1f61a74537a50c63b28ab3", size = 5420165 }, { url = "https://files.pythonhosted.org/packages/d8/5e/b7b83cfe60504cc4d24746aed04353af7ea8ec104e597e5ae71b8d0390cb/h5py-3.11.0-cp311-cp311-win_amd64.whl", hash = "sha256:083e0329ae534a264940d6513f47f5ada617da536d8dccbafc3026aefc33c90e", size = 2979079 }, { url = "https://files.pythonhosted.org/packages/58/a9/2655d4b8355d0ee783dc89dd40b5f0780e6f54a4c9b60721dc235fd6c457/h5py-3.11.0-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:a76cae64080210389a571c7d13c94a1a6cf8cb75153044fd1f822a962c97aeab", size = 3466468 }, { url = "https://files.pythonhosted.org/packages/9d/3f/cf80ef55e0a9b18aae96c763fbd275c54d0723e0f2cc54f954f87cc5c69a/h5py-3.11.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:f3736fe21da2b7d8a13fe8fe415f1272d2a1ccdeff4849c1421d2fb30fd533bc", size = 2943214 }, { url = "https://files.pythonhosted.org/packages/db/7e/fedac8bb8c4729409e2dec5e4136a289116d701d54f69ce73c5617afc5f0/h5py-3.11.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:aa6ae84a14103e8dc19266ef4c3e5d7c00b68f21d07f2966f0ca7bdb6c2761fb", size = 5378375 }, { url = "https://files.pythonhosted.org/packages/2b/b2/0ee327933ffa37af1fc7915df7fc067e6009adcd8445d55ad07a9bec11b5/h5py-3.11.0-cp312-cp312-win_amd64.whl", hash = "sha256:21dbdc5343f53b2e25404673c4f00a3335aef25521bd5fa8c707ec3833934892", size = 2970991 }, { url = "https://files.pythonhosted.org/packages/33/97/c1a8f28329ad794d18fc61bf251268ac03959bf93b82fdd7701ac6931fed/h5py-3.11.0-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:754c0c2e373d13d6309f408325343b642eb0f40f1a6ad21779cfa9502209e150", size = 3470228 }, { url = "https://files.pythonhosted.org/packages/a4/1d/fd0b88c51c37bc8aeedecc4f4b48397f7ce13c87073aaf6912faec06e9f6/h5py-3.11.0-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:731839240c59ba219d4cb3bc5880d438248533366f102402cfa0621b71796b62", size = 2935809 }, { url = "https://files.pythonhosted.org/packages/86/43/fd0bd74462b3c3fb35d98568935d3e5a435c8ec24d45ef408ac8869166af/h5py-3.11.0-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8ec9df3dd2018904c4cc06331951e274f3f3fd091e6d6cc350aaa90fa9b42a76", size = 5309045 }, { url = "https://files.pythonhosted.org/packages/15/9a/b5456e1acc4abb382938d4a730600823bfe77a4bbfd29140ccbf01ba5596/h5py-3.11.0-cp38-cp38-win_amd64.whl", hash = "sha256:55106b04e2c83dfb73dc8732e9abad69d83a436b5b82b773481d95d17b9685e1", size = 2989172 }, { url = "https://files.pythonhosted.org/packages/c2/1f/36a84945616881bd47e6c40dcdca7e929bc811725d78d001eddba6864185/h5py-3.11.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:f4e025e852754ca833401777c25888acb96889ee2c27e7e629a19aee288833f0", size = 3490090 }, { url = "https://files.pythonhosted.org/packages/3c/fb/e213586de5ea56f1747a843e725c62eef350512be57452186996ba660d52/h5py-3.11.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:6c4b760082626120031d7902cd983d8c1f424cdba2809f1067511ef283629d4b", size = 2951710 }, { url = "https://files.pythonhosted.org/packages/71/28/69a881e01f198ccdb65c36f7adcfef22bfe85e38ffbfdf833af24f58eb5e/h5py-3.11.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:67462d0669f8f5459529de179f7771bd697389fcb3faab54d63bf788599a48ea", size = 5326481 }, { url = "https://files.pythonhosted.org/packages/c3/61/0b35ad9aac0ab0a33365879556fdb824fc83013df69b247386690db59015/h5py-3.11.0-cp39-cp39-win_amd64.whl", hash = "sha256:d9c944d364688f827dc889cf83f1fca311caf4fa50b19f009d1f2b525edd33a3", size = 2978689 }, ] [[package]] name = "h5py" version = "3.14.0" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version >= '3.11'", "python_full_version == '3.10.*'", "python_full_version == '3.9.*'", ] dependencies = [ { name = "numpy", version = "2.0.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version == '3.9.*'" }, { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.10'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/5d/57/dfb3c5c3f1bf5f5ef2e59a22dec4ff1f3d7408b55bfcefcfb0ea69ef21c6/h5py-3.14.0.tar.gz", hash = "sha256:2372116b2e0d5d3e5e705b7f663f7c8d96fa79a4052d250484ef91d24d6a08f4", size = 424323 } wheels = [ { url = "https://files.pythonhosted.org/packages/52/89/06cbb421e01dea2e338b3154326523c05d9698f89a01f9d9b65e1ec3fb18/h5py-3.14.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:24df6b2622f426857bda88683b16630014588a0e4155cba44e872eb011c4eaed", size = 3332522 }, { url = "https://files.pythonhosted.org/packages/c3/e7/6c860b002329e408348735bfd0459e7b12f712c83d357abeef3ef404eaa9/h5py-3.14.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:6ff2389961ee5872de697054dd5a033b04284afc3fb52dc51d94561ece2c10c6", size = 2831051 }, { url = "https://files.pythonhosted.org/packages/fa/cd/3dd38cdb7cc9266dc4d85f27f0261680cb62f553f1523167ad7454e32b11/h5py-3.14.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:016e89d3be4c44f8d5e115fab60548e518ecd9efe9fa5c5324505a90773e6f03", size = 4324677 }, { url = "https://files.pythonhosted.org/packages/b1/45/e1a754dc7cd465ba35e438e28557119221ac89b20aaebef48282654e3dc7/h5py-3.14.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1223b902ef0b5d90bcc8a4778218d6d6cd0f5561861611eda59fa6c52b922f4d", size = 4557272 }, { url = "https://files.pythonhosted.org/packages/5c/06/f9506c1531645829d302c420851b78bb717af808dde11212c113585fae42/h5py-3.14.0-cp310-cp310-win_amd64.whl", hash = "sha256:852b81f71df4bb9e27d407b43071d1da330d6a7094a588efa50ef02553fa7ce4", size = 2866734 }, { url = "https://files.pythonhosted.org/packages/61/1b/ad24a8ce846cf0519695c10491e99969d9d203b9632c4fcd5004b1641c2e/h5py-3.14.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:f30dbc58f2a0efeec6c8836c97f6c94afd769023f44e2bb0ed7b17a16ec46088", size = 3352382 }, { url = "https://files.pythonhosted.org/packages/36/5b/a066e459ca48b47cc73a5c668e9924d9619da9e3c500d9fb9c29c03858ec/h5py-3.14.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:543877d7f3d8f8a9828ed5df6a0b78ca3d8846244b9702e99ed0d53610b583a8", size = 2852492 }, { url = "https://files.pythonhosted.org/packages/08/0c/5e6aaf221557314bc15ba0e0da92e40b24af97ab162076c8ae009320a42b/h5py-3.14.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:8c497600c0496548810047257e36360ff551df8b59156d3a4181072eed47d8ad", size = 4298002 }, { url = "https://files.pythonhosted.org/packages/21/d4/d461649cafd5137088fb7f8e78fdc6621bb0c4ff2c090a389f68e8edc136/h5py-3.14.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:723a40ee6505bd354bfd26385f2dae7bbfa87655f4e61bab175a49d72ebfc06b", size = 4516618 }, { url = "https://files.pythonhosted.org/packages/db/0c/6c3f879a0f8e891625817637fad902da6e764e36919ed091dc77529004ac/h5py-3.14.0-cp311-cp311-win_amd64.whl", hash = "sha256:d2744b520440a996f2dae97f901caa8a953afc055db4673a993f2d87d7f38713", size = 2874888 }, { url = "https://files.pythonhosted.org/packages/3e/77/8f651053c1843391e38a189ccf50df7e261ef8cd8bfd8baba0cbe694f7c3/h5py-3.14.0-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:e0045115d83272090b0717c555a31398c2c089b87d212ceba800d3dc5d952e23", size = 3312740 }, { url = "https://files.pythonhosted.org/packages/ff/10/20436a6cf419b31124e59fefc78d74cb061ccb22213226a583928a65d715/h5py-3.14.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:6da62509b7e1d71a7d110478aa25d245dd32c8d9a1daee9d2a42dba8717b047a", size = 2829207 }, { url = "https://files.pythonhosted.org/packages/3f/19/c8bfe8543bfdd7ccfafd46d8cfd96fce53d6c33e9c7921f375530ee1d39a/h5py-3.14.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:554ef0ced3571366d4d383427c00c966c360e178b5fb5ee5bb31a435c424db0c", size = 4708455 }, { url = "https://files.pythonhosted.org/packages/86/f9/f00de11c82c88bfc1ef22633557bfba9e271e0cb3189ad704183fc4a2644/h5py-3.14.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0cbd41f4e3761f150aa5b662df991868ca533872c95467216f2bec5fcad84882", size = 4929422 }, { url = "https://files.pythonhosted.org/packages/7a/6d/6426d5d456f593c94b96fa942a9b3988ce4d65ebaf57d7273e452a7222e8/h5py-3.14.0-cp312-cp312-win_amd64.whl", hash = "sha256:bf4897d67e613ecf5bdfbdab39a1158a64df105827da70ea1d90243d796d367f", size = 2862845 }, { url = "https://files.pythonhosted.org/packages/6c/c2/7efe82d09ca10afd77cd7c286e42342d520c049a8c43650194928bcc635c/h5py-3.14.0-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:aa4b7bbce683379b7bf80aaba68e17e23396100336a8d500206520052be2f812", size = 3289245 }, { url = "https://files.pythonhosted.org/packages/4f/31/f570fab1239b0d9441024b92b6ad03bb414ffa69101a985e4c83d37608bd/h5py-3.14.0-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:ef9603a501a04fcd0ba28dd8f0995303d26a77a980a1f9474b3417543d4c6174", size = 2807335 }, { url = "https://files.pythonhosted.org/packages/0d/ce/3a21d87896bc7e3e9255e0ad5583ae31ae9e6b4b00e0bcb2a67e2b6acdbc/h5py-3.14.0-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e8cbaf6910fa3983c46172666b0b8da7b7bd90d764399ca983236f2400436eeb", size = 4700675 }, { url = "https://files.pythonhosted.org/packages/e7/ec/86f59025306dcc6deee5fda54d980d077075b8d9889aac80f158bd585f1b/h5py-3.14.0-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d90e6445ab7c146d7f7981b11895d70bc1dd91278a4f9f9028bc0c95e4a53f13", size = 4921632 }, { url = "https://files.pythonhosted.org/packages/3f/6d/0084ed0b78d4fd3e7530c32491f2884140d9b06365dac8a08de726421d4a/h5py-3.14.0-cp313-cp313-win_amd64.whl", hash = "sha256:ae18e3de237a7a830adb76aaa68ad438d85fe6e19e0d99944a3ce46b772c69b3", size = 2852929 }, { url = "https://files.pythonhosted.org/packages/ec/ac/9ea82488c8790ee5b6ad1a807cd7dc3b9dadfece1cd0e0e369f68a7a8937/h5py-3.14.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:f5cc1601e78027cedfec6dd50efb4802f018551754191aeb58d948bd3ec3bd7a", size = 3345097 }, { url = "https://files.pythonhosted.org/packages/6c/bc/a172ecaaf287e3af2f837f23b470b0a2229c79555a0da9ac8b5cc5bed078/h5py-3.14.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:5e59d2136a8b302afd25acdf7a89b634e0eb7c66b1a211ef2d0457853768a2ef", size = 2843320 }, { url = "https://files.pythonhosted.org/packages/66/40/b423b57696514e05aa7bb06150ef96667d0e0006cc6de7ab52c71734ab51/h5py-3.14.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:573c33ad056ac7c1ab6d567b6db9df3ffc401045e3f605736218f96c1e0490c6", size = 4326368 }, { url = "https://files.pythonhosted.org/packages/f7/07/e088f89f04fdbe57ddf9de377f857158d3daa38cf5d0fb20ef9bd489e313/h5py-3.14.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ccbe17dc187c0c64178f1a10aa274ed3a57d055117588942b8a08793cc448216", size = 4559686 }, { url = "https://files.pythonhosted.org/packages/b4/e4/fb8032d0e5480b1db9b419b5b50737b61bb3c7187c49d809975d62129fb0/h5py-3.14.0-cp39-cp39-win_amd64.whl", hash = "sha256:4f025cf30ae738c4c4e38c7439a761a71ccfcce04c2b87b2a2ac64e8c5171d43", size = 2877166 }, ] [[package]] name = "idna" version = "3.10" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/f1/70/7703c29685631f5a7590aa73f1f1d3fa9a380e654b86af429e0934a32f7d/idna-3.10.tar.gz", hash = "sha256:12f65c9b470abda6dc35cf8e63cc574b1c52b11df2c86030af0ac09b01b13ea9", size = 190490 } wheels = [ { url = "https://files.pythonhosted.org/packages/76/c6/c88e154df9c4e1a2a66ccf0005a88dfb2650c1dffb6f5ce603dfbd452ce3/idna-3.10-py3-none-any.whl", hash = "sha256:946d195a0d259cbba61165e88e65941f16e9b36ea6ddb97f00452bae8b1287d3", size = 70442 }, ] [[package]] name = "iniconfig" version = "2.1.0" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/f2/97/ebf4da567aa6827c909642694d71c9fcf53e5b504f2d96afea02718862f3/iniconfig-2.1.0.tar.gz", hash = "sha256:3abbd2e30b36733fee78f9c7f7308f2d0050e88f0087fd25c2645f63c773e1c7", size = 4793 } wheels = [ { url = "https://files.pythonhosted.org/packages/2c/e1/e6716421ea10d38022b952c159d5161ca1193197fb744506875fbb87ea7b/iniconfig-2.1.0-py3-none-any.whl", hash = "sha256:9deba5723312380e77435581c6bf4935c94cbfab9b1ed33ef8d238ea168eb760", size = 6050 }, ] [[package]] name = "numpy" version = "1.24.4" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version < '3.9'", ] sdist = { url = "https://files.pythonhosted.org/packages/a4/9b/027bec52c633f6556dba6b722d9a0befb40498b9ceddd29cbe67a45a127c/numpy-1.24.4.tar.gz", hash = "sha256:80f5e3a4e498641401868df4208b74581206afbee7cf7b8329daae82676d9463", size = 10911229 } wheels = [ { url = "https://files.pythonhosted.org/packages/6b/80/6cdfb3e275d95155a34659163b83c09e3a3ff9f1456880bec6cc63d71083/numpy-1.24.4-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:c0bfb52d2169d58c1cdb8cc1f16989101639b34c7d3ce60ed70b19c63eba0b64", size = 19789140 }, { url = "https://files.pythonhosted.org/packages/64/5f/3f01d753e2175cfade1013eea08db99ba1ee4bdb147ebcf3623b75d12aa7/numpy-1.24.4-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:ed094d4f0c177b1b8e7aa9cba7d6ceed51c0e569a5318ac0ca9a090680a6a1b1", size = 13854297 }, { url = "https://files.pythonhosted.org/packages/5a/b3/2f9c21d799fa07053ffa151faccdceeb69beec5a010576b8991f614021f7/numpy-1.24.4-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:79fc682a374c4a8ed08b331bef9c5f582585d1048fa6d80bc6c35bc384eee9b4", size = 13995611 }, { url = "https://files.pythonhosted.org/packages/10/be/ae5bf4737cb79ba437879915791f6f26d92583c738d7d960ad94e5c36adf/numpy-1.24.4-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7ffe43c74893dbf38c2b0a1f5428760a1a9c98285553c89e12d70a96a7f3a4d6", size = 17282357 }, { url = "https://files.pythonhosted.org/packages/c0/64/908c1087be6285f40e4b3e79454552a701664a079321cff519d8c7051d06/numpy-1.24.4-cp310-cp310-win32.whl", hash = "sha256:4c21decb6ea94057331e111a5bed9a79d335658c27ce2adb580fb4d54f2ad9bc", size = 12429222 }, { url = "https://files.pythonhosted.org/packages/22/55/3d5a7c1142e0d9329ad27cece17933b0e2ab4e54ddc5c1861fbfeb3f7693/numpy-1.24.4-cp310-cp310-win_amd64.whl", hash = "sha256:b4bea75e47d9586d31e892a7401f76e909712a0fd510f58f5337bea9572c571e", size = 14841514 }, { url = "https://files.pythonhosted.org/packages/a9/cc/5ed2280a27e5dab12994c884f1f4d8c3bd4d885d02ae9e52a9d213a6a5e2/numpy-1.24.4-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:f136bab9c2cfd8da131132c2cf6cc27331dd6fae65f95f69dcd4ae3c3639c810", size = 19775508 }, { url = "https://files.pythonhosted.org/packages/c0/bc/77635c657a3668cf652806210b8662e1aff84b818a55ba88257abf6637a8/numpy-1.24.4-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:e2926dac25b313635e4d6cf4dc4e51c8c0ebfed60b801c799ffc4c32bf3d1254", size = 13840033 }, { url = "https://files.pythonhosted.org/packages/a7/4c/96cdaa34f54c05e97c1c50f39f98d608f96f0677a6589e64e53104e22904/numpy-1.24.4-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:222e40d0e2548690405b0b3c7b21d1169117391c2e82c378467ef9ab4c8f0da7", size = 13991951 }, { url = "https://files.pythonhosted.org/packages/22/97/dfb1a31bb46686f09e68ea6ac5c63fdee0d22d7b23b8f3f7ea07712869ef/numpy-1.24.4-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7215847ce88a85ce39baf9e89070cb860c98fdddacbaa6c0da3ffb31b3350bd5", size = 17278923 }, { url = "https://files.pythonhosted.org/packages/35/e2/76a11e54139654a324d107da1d98f99e7aa2a7ef97cfd7c631fba7dbde71/numpy-1.24.4-cp311-cp311-win32.whl", hash = "sha256:4979217d7de511a8d57f4b4b5b2b965f707768440c17cb70fbf254c4b225238d", size = 12422446 }, { url = "https://files.pythonhosted.org/packages/d8/ec/ebef2f7d7c28503f958f0f8b992e7ce606fb74f9e891199329d5f5f87404/numpy-1.24.4-cp311-cp311-win_amd64.whl", hash = "sha256:b7b1fc9864d7d39e28f41d089bfd6353cb5f27ecd9905348c24187a768c79694", size = 14834466 }, { url = "https://files.pythonhosted.org/packages/11/10/943cfb579f1a02909ff96464c69893b1d25be3731b5d3652c2e0cf1281ea/numpy-1.24.4-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:1452241c290f3e2a312c137a9999cdbf63f78864d63c79039bda65ee86943f61", size = 19780722 }, { url = "https://files.pythonhosted.org/packages/a7/ae/f53b7b265fdc701e663fbb322a8e9d4b14d9cb7b2385f45ddfabfc4327e4/numpy-1.24.4-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:04640dab83f7c6c85abf9cd729c5b65f1ebd0ccf9de90b270cd61935eef0197f", size = 13843102 }, { url = "https://files.pythonhosted.org/packages/25/6f/2586a50ad72e8dbb1d8381f837008a0321a3516dfd7cb57fc8cf7e4bb06b/numpy-1.24.4-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a5425b114831d1e77e4b5d812b69d11d962e104095a5b9c3b641a218abcc050e", size = 14039616 }, { url = "https://files.pythonhosted.org/packages/98/5d/5738903efe0ecb73e51eb44feafba32bdba2081263d40c5043568ff60faf/numpy-1.24.4-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:dd80e219fd4c71fc3699fc1dadac5dcf4fd882bfc6f7ec53d30fa197b8ee22dc", size = 17316263 }, { url = "https://files.pythonhosted.org/packages/d1/57/8d328f0b91c733aa9aa7ee540dbc49b58796c862b4fbcb1146c701e888da/numpy-1.24.4-cp38-cp38-win32.whl", hash = "sha256:4602244f345453db537be5314d3983dbf5834a9701b7723ec28923e2889e0bb2", size = 12455660 }, { url = "https://files.pythonhosted.org/packages/69/65/0d47953afa0ad569d12de5f65d964321c208492064c38fe3b0b9744f8d44/numpy-1.24.4-cp38-cp38-win_amd64.whl", hash = "sha256:692f2e0f55794943c5bfff12b3f56f99af76f902fc47487bdfe97856de51a706", size = 14868112 }, { url = "https://files.pythonhosted.org/packages/9a/cd/d5b0402b801c8a8b56b04c1e85c6165efab298d2f0ab741c2406516ede3a/numpy-1.24.4-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:2541312fbf09977f3b3ad449c4e5f4bb55d0dbf79226d7724211acc905049400", size = 19816549 }, { url = "https://files.pythonhosted.org/packages/14/27/638aaa446f39113a3ed38b37a66243e21b38110d021bfcb940c383e120f2/numpy-1.24.4-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:9667575fb6d13c95f1b36aca12c5ee3356bf001b714fc354eb5465ce1609e62f", size = 13879950 }, { url = "https://files.pythonhosted.org/packages/8f/27/91894916e50627476cff1a4e4363ab6179d01077d71b9afed41d9e1f18bf/numpy-1.24.4-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:f3a86ed21e4f87050382c7bc96571755193c4c1392490744ac73d660e8f564a9", size = 14030228 }, { url = "https://files.pythonhosted.org/packages/7a/7c/d7b2a0417af6428440c0ad7cb9799073e507b1a465f827d058b826236964/numpy-1.24.4-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d11efb4dbecbdf22508d55e48d9c8384db795e1b7b51ea735289ff96613ff74d", size = 17311170 }, { url = "https://files.pythonhosted.org/packages/18/9d/e02ace5d7dfccee796c37b995c63322674daf88ae2f4a4724c5dd0afcc91/numpy-1.24.4-cp39-cp39-win32.whl", hash = "sha256:6620c0acd41dbcb368610bb2f4d83145674040025e5536954782467100aa8835", size = 12454918 }, { url = "https://files.pythonhosted.org/packages/63/38/6cc19d6b8bfa1d1a459daf2b3fe325453153ca7019976274b6f33d8b5663/numpy-1.24.4-cp39-cp39-win_amd64.whl", hash = "sha256:befe2bf740fd8373cf56149a5c23a0f601e82869598d41f8e188a0e9869926f8", size = 14867441 }, { url = "https://files.pythonhosted.org/packages/a4/fd/8dff40e25e937c94257455c237b9b6bf5a30d42dd1cc11555533be099492/numpy-1.24.4-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:31f13e25b4e304632a4619d0e0777662c2ffea99fcae2029556b17d8ff958aef", size = 19156590 }, { url = "https://files.pythonhosted.org/packages/42/e7/4bf953c6e05df90c6d351af69966384fed8e988d0e8c54dad7103b59f3ba/numpy-1.24.4-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:95f7ac6540e95bc440ad77f56e520da5bf877f87dca58bd095288dce8940532a", size = 16705744 }, { url = "https://files.pythonhosted.org/packages/fc/dd/9106005eb477d022b60b3817ed5937a43dad8fd1f20b0610ea8a32fcb407/numpy-1.24.4-pp38-pypy38_pp73-win_amd64.whl", hash = "sha256:e98f220aa76ca2a977fe435f5b04d7b3470c0a2e6312907b37ba6068f26787f2", size = 14734290 }, ] [[package]] name = "numpy" version = "2.0.2" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version == '3.9.*'", ] sdist = { url = "https://files.pythonhosted.org/packages/a9/75/10dd1f8116a8b796cb2c737b674e02d02e80454bda953fa7e65d8c12b016/numpy-2.0.2.tar.gz", hash = "sha256:883c987dee1880e2a864ab0dc9892292582510604156762362d9326444636e78", size = 18902015 } wheels = [ { url = "https://files.pythonhosted.org/packages/21/91/3495b3237510f79f5d81f2508f9f13fea78ebfdf07538fc7444badda173d/numpy-2.0.2-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:51129a29dbe56f9ca83438b706e2e69a39892b5eda6cedcb6b0c9fdc9b0d3ece", size = 21165245 }, { url = "https://files.pythonhosted.org/packages/05/33/26178c7d437a87082d11019292dce6d3fe6f0e9026b7b2309cbf3e489b1d/numpy-2.0.2-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:f15975dfec0cf2239224d80e32c3170b1d168335eaedee69da84fbe9f1f9cd04", size = 13738540 }, { url = "https://files.pythonhosted.org/packages/ec/31/cc46e13bf07644efc7a4bf68df2df5fb2a1a88d0cd0da9ddc84dc0033e51/numpy-2.0.2-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:8c5713284ce4e282544c68d1c3b2c7161d38c256d2eefc93c1d683cf47683e66", size = 5300623 }, { url = "https://files.pythonhosted.org/packages/6e/16/7bfcebf27bb4f9d7ec67332ffebee4d1bf085c84246552d52dbb548600e7/numpy-2.0.2-cp310-cp310-macosx_14_0_x86_64.whl", hash = "sha256:becfae3ddd30736fe1889a37f1f580e245ba79a5855bff5f2a29cb3ccc22dd7b", size = 6901774 }, { url = "https://files.pythonhosted.org/packages/f9/a3/561c531c0e8bf082c5bef509d00d56f82e0ea7e1e3e3a7fc8fa78742a6e5/numpy-2.0.2-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:2da5960c3cf0df7eafefd806d4e612c5e19358de82cb3c343631188991566ccd", size = 13907081 }, { url = "https://files.pythonhosted.org/packages/fa/66/f7177ab331876200ac7563a580140643d1179c8b4b6a6b0fc9838de2a9b8/numpy-2.0.2-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:496f71341824ed9f3d2fd36cf3ac57ae2e0165c143b55c3a035ee219413f3318", size = 19523451 }, { url = "https://files.pythonhosted.org/packages/25/7f/0b209498009ad6453e4efc2c65bcdf0ae08a182b2b7877d7ab38a92dc542/numpy-2.0.2-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:a61ec659f68ae254e4d237816e33171497e978140353c0c2038d46e63282d0c8", size = 19927572 }, { url = "https://files.pythonhosted.org/packages/3e/df/2619393b1e1b565cd2d4c4403bdd979621e2c4dea1f8532754b2598ed63b/numpy-2.0.2-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:d731a1c6116ba289c1e9ee714b08a8ff882944d4ad631fd411106a30f083c326", size = 14400722 }, { url = "https://files.pythonhosted.org/packages/22/ad/77e921b9f256d5da36424ffb711ae79ca3f451ff8489eeca544d0701d74a/numpy-2.0.2-cp310-cp310-win32.whl", hash = "sha256:984d96121c9f9616cd33fbd0618b7f08e0cfc9600a7ee1d6fd9b239186d19d97", size = 6472170 }, { url = "https://files.pythonhosted.org/packages/10/05/3442317535028bc29cf0c0dd4c191a4481e8376e9f0db6bcf29703cadae6/numpy-2.0.2-cp310-cp310-win_amd64.whl", hash = "sha256:c7b0be4ef08607dd04da4092faee0b86607f111d5ae68036f16cc787e250a131", size = 15905558 }, { url = "https://files.pythonhosted.org/packages/8b/cf/034500fb83041aa0286e0fb16e7c76e5c8b67c0711bb6e9e9737a717d5fe/numpy-2.0.2-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:49ca4decb342d66018b01932139c0961a8f9ddc7589611158cb3c27cbcf76448", size = 21169137 }, { url = "https://files.pythonhosted.org/packages/4a/d9/32de45561811a4b87fbdee23b5797394e3d1504b4a7cf40c10199848893e/numpy-2.0.2-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:11a76c372d1d37437857280aa142086476136a8c0f373b2e648ab2c8f18fb195", size = 13703552 }, { url = "https://files.pythonhosted.org/packages/c1/ca/2f384720020c7b244d22508cb7ab23d95f179fcfff33c31a6eeba8d6c512/numpy-2.0.2-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:807ec44583fd708a21d4a11d94aedf2f4f3c3719035c76a2bbe1fe8e217bdc57", size = 5298957 }, { url = "https://files.pythonhosted.org/packages/0e/78/a3e4f9fb6aa4e6fdca0c5428e8ba039408514388cf62d89651aade838269/numpy-2.0.2-cp311-cp311-macosx_14_0_x86_64.whl", hash = "sha256:8cafab480740e22f8d833acefed5cc87ce276f4ece12fdaa2e8903db2f82897a", size = 6905573 }, { url = "https://files.pythonhosted.org/packages/a0/72/cfc3a1beb2caf4efc9d0b38a15fe34025230da27e1c08cc2eb9bfb1c7231/numpy-2.0.2-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a15f476a45e6e5a3a79d8a14e62161d27ad897381fecfa4a09ed5322f2085669", size = 13914330 }, { url = "https://files.pythonhosted.org/packages/ba/a8/c17acf65a931ce551fee11b72e8de63bf7e8a6f0e21add4c937c83563538/numpy-2.0.2-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:13e689d772146140a252c3a28501da66dfecd77490b498b168b501835041f951", size = 19534895 }, { url = "https://files.pythonhosted.org/packages/ba/86/8767f3d54f6ae0165749f84648da9dcc8cd78ab65d415494962c86fac80f/numpy-2.0.2-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:9ea91dfb7c3d1c56a0e55657c0afb38cf1eeae4544c208dc465c3c9f3a7c09f9", size = 19937253 }, { url = "https://files.pythonhosted.org/packages/df/87/f76450e6e1c14e5bb1eae6836478b1028e096fd02e85c1c37674606ab752/numpy-2.0.2-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:c1c9307701fec8f3f7a1e6711f9089c06e6284b3afbbcd259f7791282d660a15", size = 14414074 }, { url = "https://files.pythonhosted.org/packages/5c/ca/0f0f328e1e59f73754f06e1adfb909de43726d4f24c6a3f8805f34f2b0fa/numpy-2.0.2-cp311-cp311-win32.whl", hash = "sha256:a392a68bd329eafac5817e5aefeb39038c48b671afd242710b451e76090e81f4", size = 6470640 }, { url = "https://files.pythonhosted.org/packages/eb/57/3a3f14d3a759dcf9bf6e9eda905794726b758819df4663f217d658a58695/numpy-2.0.2-cp311-cp311-win_amd64.whl", hash = "sha256:286cd40ce2b7d652a6f22efdfc6d1edf879440e53e76a75955bc0c826c7e64dc", size = 15910230 }, { url = "https://files.pythonhosted.org/packages/45/40/2e117be60ec50d98fa08c2f8c48e09b3edea93cfcabd5a9ff6925d54b1c2/numpy-2.0.2-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:df55d490dea7934f330006d0f81e8551ba6010a5bf035a249ef61a94f21c500b", size = 20895803 }, { url = "https://files.pythonhosted.org/packages/46/92/1b8b8dee833f53cef3e0a3f69b2374467789e0bb7399689582314df02651/numpy-2.0.2-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:8df823f570d9adf0978347d1f926b2a867d5608f434a7cff7f7908c6570dcf5e", size = 13471835 }, { url = "https://files.pythonhosted.org/packages/7f/19/e2793bde475f1edaea6945be141aef6c8b4c669b90c90a300a8954d08f0a/numpy-2.0.2-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:9a92ae5c14811e390f3767053ff54eaee3bf84576d99a2456391401323f4ec2c", size = 5038499 }, { url = "https://files.pythonhosted.org/packages/e3/ff/ddf6dac2ff0dd50a7327bcdba45cb0264d0e96bb44d33324853f781a8f3c/numpy-2.0.2-cp312-cp312-macosx_14_0_x86_64.whl", hash = "sha256:a842d573724391493a97a62ebbb8e731f8a5dcc5d285dfc99141ca15a3302d0c", size = 6633497 }, { url = "https://files.pythonhosted.org/packages/72/21/67f36eac8e2d2cd652a2e69595a54128297cdcb1ff3931cfc87838874bd4/numpy-2.0.2-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:c05e238064fc0610c840d1cf6a13bf63d7e391717d247f1bf0318172e759e692", size = 13621158 }, { url = "https://files.pythonhosted.org/packages/39/68/e9f1126d757653496dbc096cb429014347a36b228f5a991dae2c6b6cfd40/numpy-2.0.2-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0123ffdaa88fa4ab64835dcbde75dcdf89c453c922f18dced6e27c90d1d0ec5a", size = 19236173 }, { url = "https://files.pythonhosted.org/packages/d1/e9/1f5333281e4ebf483ba1c888b1d61ba7e78d7e910fdd8e6499667041cc35/numpy-2.0.2-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:96a55f64139912d61de9137f11bf39a55ec8faec288c75a54f93dfd39f7eb40c", size = 19634174 }, { url = "https://files.pythonhosted.org/packages/71/af/a469674070c8d8408384e3012e064299f7a2de540738a8e414dcfd639996/numpy-2.0.2-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:ec9852fb39354b5a45a80bdab5ac02dd02b15f44b3804e9f00c556bf24b4bded", size = 14099701 }, { url = "https://files.pythonhosted.org/packages/d0/3d/08ea9f239d0e0e939b6ca52ad403c84a2bce1bde301a8eb4888c1c1543f1/numpy-2.0.2-cp312-cp312-win32.whl", hash = "sha256:671bec6496f83202ed2d3c8fdc486a8fc86942f2e69ff0e986140339a63bcbe5", size = 6174313 }, { url = "https://files.pythonhosted.org/packages/b2/b5/4ac39baebf1fdb2e72585c8352c56d063b6126be9fc95bd2bb5ef5770c20/numpy-2.0.2-cp312-cp312-win_amd64.whl", hash = "sha256:cfd41e13fdc257aa5778496b8caa5e856dc4896d4ccf01841daee1d96465467a", size = 15606179 }, { url = "https://files.pythonhosted.org/packages/43/c1/41c8f6df3162b0c6ffd4437d729115704bd43363de0090c7f913cfbc2d89/numpy-2.0.2-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:9059e10581ce4093f735ed23f3b9d283b9d517ff46009ddd485f1747eb22653c", size = 21169942 }, { url = "https://files.pythonhosted.org/packages/39/bc/fd298f308dcd232b56a4031fd6ddf11c43f9917fbc937e53762f7b5a3bb1/numpy-2.0.2-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:423e89b23490805d2a5a96fe40ec507407b8ee786d66f7328be214f9679df6dd", size = 13711512 }, { url = "https://files.pythonhosted.org/packages/96/ff/06d1aa3eeb1c614eda245c1ba4fb88c483bee6520d361641331872ac4b82/numpy-2.0.2-cp39-cp39-macosx_14_0_arm64.whl", hash = "sha256:2b2955fa6f11907cf7a70dab0d0755159bca87755e831e47932367fc8f2f2d0b", size = 5306976 }, { url = "https://files.pythonhosted.org/packages/2d/98/121996dcfb10a6087a05e54453e28e58694a7db62c5a5a29cee14c6e047b/numpy-2.0.2-cp39-cp39-macosx_14_0_x86_64.whl", hash = "sha256:97032a27bd9d8988b9a97a8c4d2c9f2c15a81f61e2f21404d7e8ef00cb5be729", size = 6906494 }, { url = "https://files.pythonhosted.org/packages/15/31/9dffc70da6b9bbf7968f6551967fc21156207366272c2a40b4ed6008dc9b/numpy-2.0.2-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1e795a8be3ddbac43274f18588329c72939870a16cae810c2b73461c40718ab1", size = 13912596 }, { url = "https://files.pythonhosted.org/packages/b9/14/78635daab4b07c0930c919d451b8bf8c164774e6a3413aed04a6d95758ce/numpy-2.0.2-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f26b258c385842546006213344c50655ff1555a9338e2e5e02a0756dc3e803dd", size = 19526099 }, { url = "https://files.pythonhosted.org/packages/26/4c/0eeca4614003077f68bfe7aac8b7496f04221865b3a5e7cb230c9d055afd/numpy-2.0.2-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:5fec9451a7789926bcf7c2b8d187292c9f93ea30284802a0ab3f5be8ab36865d", size = 19932823 }, { url = "https://files.pythonhosted.org/packages/f1/46/ea25b98b13dccaebddf1a803f8c748680d972e00507cd9bc6dcdb5aa2ac1/numpy-2.0.2-cp39-cp39-musllinux_1_2_aarch64.whl", hash = "sha256:9189427407d88ff25ecf8f12469d4d39d35bee1db5d39fc5c168c6f088a6956d", size = 14404424 }, { url = "https://files.pythonhosted.org/packages/c8/a6/177dd88d95ecf07e722d21008b1b40e681a929eb9e329684d449c36586b2/numpy-2.0.2-cp39-cp39-win32.whl", hash = "sha256:905d16e0c60200656500c95b6b8dca5d109e23cb24abc701d41c02d74c6b3afa", size = 6476809 }, { url = "https://files.pythonhosted.org/packages/ea/2b/7fc9f4e7ae5b507c1a3a21f0f15ed03e794c1242ea8a242ac158beb56034/numpy-2.0.2-cp39-cp39-win_amd64.whl", hash = "sha256:a3f4ab0caa7f053f6797fcd4e1e25caee367db3112ef2b6ef82d749530768c73", size = 15911314 }, { url = "https://files.pythonhosted.org/packages/8f/3b/df5a870ac6a3be3a86856ce195ef42eec7ae50d2a202be1f5a4b3b340e14/numpy-2.0.2-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:7f0a0c6f12e07fa94133c8a67404322845220c06a9e80e85999afe727f7438b8", size = 21025288 }, { url = "https://files.pythonhosted.org/packages/2c/97/51af92f18d6f6f2d9ad8b482a99fb74e142d71372da5d834b3a2747a446e/numpy-2.0.2-pp39-pypy39_pp73-macosx_14_0_x86_64.whl", hash = "sha256:312950fdd060354350ed123c0e25a71327d3711584beaef30cdaa93320c392d4", size = 6762793 }, { url = "https://files.pythonhosted.org/packages/12/46/de1fbd0c1b5ccaa7f9a005b66761533e2f6a3e560096682683a223631fe9/numpy-2.0.2-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:26df23238872200f63518dd2aa984cfca675d82469535dc7162dc2ee52d9dd5c", size = 19334885 }, { url = "https://files.pythonhosted.org/packages/cc/dc/d330a6faefd92b446ec0f0dfea4c3207bb1fef3c4771d19cf4543efd2c78/numpy-2.0.2-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:a46288ec55ebbd58947d31d72be2c63cbf839f0a63b49cb755022310792a3385", size = 15828784 }, ] [[package]] name = "numpy" version = "2.2.6" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version >= '3.11'", "python_full_version == '3.10.*'", ] sdist = { url = "https://files.pythonhosted.org/packages/76/21/7d2a95e4bba9dc13d043ee156a356c0a8f0c6309dff6b21b4d71a073b8a8/numpy-2.2.6.tar.gz", hash = "sha256:e29554e2bef54a90aa5cc07da6ce955accb83f21ab5de01a62c8478897b264fd", size = 20276440 } wheels = [ { url = "https://files.pythonhosted.org/packages/9a/3e/ed6db5be21ce87955c0cbd3009f2803f59fa08df21b5df06862e2d8e2bdd/numpy-2.2.6-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:b412caa66f72040e6d268491a59f2c43bf03eb6c96dd8f0307829feb7fa2b6fb", size = 21165245 }, { url = "https://files.pythonhosted.org/packages/22/c2/4b9221495b2a132cc9d2eb862e21d42a009f5a60e45fc44b00118c174bff/numpy-2.2.6-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:8e41fd67c52b86603a91c1a505ebaef50b3314de0213461c7a6e99c9a3beff90", size = 14360048 }, { url = "https://files.pythonhosted.org/packages/fd/77/dc2fcfc66943c6410e2bf598062f5959372735ffda175b39906d54f02349/numpy-2.2.6-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:37e990a01ae6ec7fe7fa1c26c55ecb672dd98b19c3d0e1d1f326fa13cb38d163", size = 5340542 }, { url = "https://files.pythonhosted.org/packages/7a/4f/1cb5fdc353a5f5cc7feb692db9b8ec2c3d6405453f982435efc52561df58/numpy-2.2.6-cp310-cp310-macosx_14_0_x86_64.whl", hash = "sha256:5a6429d4be8ca66d889b7cf70f536a397dc45ba6faeb5f8c5427935d9592e9cf", size = 6878301 }, { url = "https://files.pythonhosted.org/packages/eb/17/96a3acd228cec142fcb8723bd3cc39c2a474f7dcf0a5d16731980bcafa95/numpy-2.2.6-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:efd28d4e9cd7d7a8d39074a4d44c63eda73401580c5c76acda2ce969e0a38e83", size = 14297320 }, { url = "https://files.pythonhosted.org/packages/b4/63/3de6a34ad7ad6646ac7d2f55ebc6ad439dbbf9c4370017c50cf403fb19b5/numpy-2.2.6-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fc7b73d02efb0e18c000e9ad8b83480dfcd5dfd11065997ed4c6747470ae8915", size = 16801050 }, { url = "https://files.pythonhosted.org/packages/07/b6/89d837eddef52b3d0cec5c6ba0456c1bf1b9ef6a6672fc2b7873c3ec4e2e/numpy-2.2.6-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:74d4531beb257d2c3f4b261bfb0fc09e0f9ebb8842d82a7b4209415896adc680", size = 15807034 }, { url = "https://files.pythonhosted.org/packages/01/c8/dc6ae86e3c61cfec1f178e5c9f7858584049b6093f843bca541f94120920/numpy-2.2.6-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:8fc377d995680230e83241d8a96def29f204b5782f371c532579b4f20607a289", size = 18614185 }, { url = "https://files.pythonhosted.org/packages/5b/c5/0064b1b7e7c89137b471ccec1fd2282fceaae0ab3a9550f2568782d80357/numpy-2.2.6-cp310-cp310-win32.whl", hash = "sha256:b093dd74e50a8cba3e873868d9e93a85b78e0daf2e98c6797566ad8044e8363d", size = 6527149 }, { url = "https://files.pythonhosted.org/packages/a3/dd/4b822569d6b96c39d1215dbae0582fd99954dcbcf0c1a13c61783feaca3f/numpy-2.2.6-cp310-cp310-win_amd64.whl", hash = "sha256:f0fd6321b839904e15c46e0d257fdd101dd7f530fe03fd6359c1ea63738703f3", size = 12904620 }, { url = "https://files.pythonhosted.org/packages/da/a8/4f83e2aa666a9fbf56d6118faaaf5f1974d456b1823fda0a176eff722839/numpy-2.2.6-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:f9f1adb22318e121c5c69a09142811a201ef17ab257a1e66ca3025065b7f53ae", size = 21176963 }, { url = "https://files.pythonhosted.org/packages/b3/2b/64e1affc7972decb74c9e29e5649fac940514910960ba25cd9af4488b66c/numpy-2.2.6-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:c820a93b0255bc360f53eca31a0e676fd1101f673dda8da93454a12e23fc5f7a", size = 14406743 }, { url = "https://files.pythonhosted.org/packages/4a/9f/0121e375000b5e50ffdd8b25bf78d8e1a5aa4cca3f185d41265198c7b834/numpy-2.2.6-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:3d70692235e759f260c3d837193090014aebdf026dfd167834bcba43e30c2a42", size = 5352616 }, { url = "https://files.pythonhosted.org/packages/31/0d/b48c405c91693635fbe2dcd7bc84a33a602add5f63286e024d3b6741411c/numpy-2.2.6-cp311-cp311-macosx_14_0_x86_64.whl", hash = "sha256:481b49095335f8eed42e39e8041327c05b0f6f4780488f61286ed3c01368d491", size = 6889579 }, { url = "https://files.pythonhosted.org/packages/52/b8/7f0554d49b565d0171eab6e99001846882000883998e7b7d9f0d98b1f934/numpy-2.2.6-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:b64d8d4d17135e00c8e346e0a738deb17e754230d7e0810ac5012750bbd85a5a", size = 14312005 }, { url = "https://files.pythonhosted.org/packages/b3/dd/2238b898e51bd6d389b7389ffb20d7f4c10066d80351187ec8e303a5a475/numpy-2.2.6-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ba10f8411898fc418a521833e014a77d3ca01c15b0c6cdcce6a0d2897e6dbbdf", size = 16821570 }, { url = "https://files.pythonhosted.org/packages/83/6c/44d0325722cf644f191042bf47eedad61c1e6df2432ed65cbe28509d404e/numpy-2.2.6-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:bd48227a919f1bafbdda0583705e547892342c26fb127219d60a5c36882609d1", size = 15818548 }, { url = "https://files.pythonhosted.org/packages/ae/9d/81e8216030ce66be25279098789b665d49ff19eef08bfa8cb96d4957f422/numpy-2.2.6-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:9551a499bf125c1d4f9e250377c1ee2eddd02e01eac6644c080162c0c51778ab", size = 18620521 }, { url = "https://files.pythonhosted.org/packages/6a/fd/e19617b9530b031db51b0926eed5345ce8ddc669bb3bc0044b23e275ebe8/numpy-2.2.6-cp311-cp311-win32.whl", hash = "sha256:0678000bb9ac1475cd454c6b8c799206af8107e310843532b04d49649c717a47", size = 6525866 }, { url = "https://files.pythonhosted.org/packages/31/0a/f354fb7176b81747d870f7991dc763e157a934c717b67b58456bc63da3df/numpy-2.2.6-cp311-cp311-win_amd64.whl", hash = "sha256:e8213002e427c69c45a52bbd94163084025f533a55a59d6f9c5b820774ef3303", size = 12907455 }, { url = "https://files.pythonhosted.org/packages/82/5d/c00588b6cf18e1da539b45d3598d3557084990dcc4331960c15ee776ee41/numpy-2.2.6-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:41c5a21f4a04fa86436124d388f6ed60a9343a6f767fced1a8a71c3fbca038ff", size = 20875348 }, { url = "https://files.pythonhosted.org/packages/66/ee/560deadcdde6c2f90200450d5938f63a34b37e27ebff162810f716f6a230/numpy-2.2.6-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:de749064336d37e340f640b05f24e9e3dd678c57318c7289d222a8a2f543e90c", size = 14119362 }, { url = "https://files.pythonhosted.org/packages/3c/65/4baa99f1c53b30adf0acd9a5519078871ddde8d2339dc5a7fde80d9d87da/numpy-2.2.6-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:894b3a42502226a1cac872f840030665f33326fc3dac8e57c607905773cdcde3", size = 5084103 }, { url = "https://files.pythonhosted.org/packages/cc/89/e5a34c071a0570cc40c9a54eb472d113eea6d002e9ae12bb3a8407fb912e/numpy-2.2.6-cp312-cp312-macosx_14_0_x86_64.whl", hash = "sha256:71594f7c51a18e728451bb50cc60a3ce4e6538822731b2933209a1f3614e9282", size = 6625382 }, { url = "https://files.pythonhosted.org/packages/f8/35/8c80729f1ff76b3921d5c9487c7ac3de9b2a103b1cd05e905b3090513510/numpy-2.2.6-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:f2618db89be1b4e05f7a1a847a9c1c0abd63e63a1607d892dd54668dd92faf87", size = 14018462 }, { url = "https://files.pythonhosted.org/packages/8c/3d/1e1db36cfd41f895d266b103df00ca5b3cbe965184df824dec5c08c6b803/numpy-2.2.6-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fd83c01228a688733f1ded5201c678f0c53ecc1006ffbc404db9f7a899ac6249", size = 16527618 }, { url = "https://files.pythonhosted.org/packages/61/c6/03ed30992602c85aa3cd95b9070a514f8b3c33e31124694438d88809ae36/numpy-2.2.6-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:37c0ca431f82cd5fa716eca9506aefcabc247fb27ba69c5062a6d3ade8cf8f49", size = 15505511 }, { url = "https://files.pythonhosted.org/packages/b7/25/5761d832a81df431e260719ec45de696414266613c9ee268394dd5ad8236/numpy-2.2.6-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:fe27749d33bb772c80dcd84ae7e8df2adc920ae8297400dabec45f0dedb3f6de", size = 18313783 }, { url = "https://files.pythonhosted.org/packages/57/0a/72d5a3527c5ebffcd47bde9162c39fae1f90138c961e5296491ce778e682/numpy-2.2.6-cp312-cp312-win32.whl", hash = "sha256:4eeaae00d789f66c7a25ac5f34b71a7035bb474e679f410e5e1a94deb24cf2d4", size = 6246506 }, { url = "https://files.pythonhosted.org/packages/36/fa/8c9210162ca1b88529ab76b41ba02d433fd54fecaf6feb70ef9f124683f1/numpy-2.2.6-cp312-cp312-win_amd64.whl", hash = "sha256:c1f9540be57940698ed329904db803cf7a402f3fc200bfe599334c9bd84a40b2", size = 12614190 }, { url = "https://files.pythonhosted.org/packages/f9/5c/6657823f4f594f72b5471f1db1ab12e26e890bb2e41897522d134d2a3e81/numpy-2.2.6-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:0811bb762109d9708cca4d0b13c4f67146e3c3b7cf8d34018c722adb2d957c84", size = 20867828 }, { url = "https://files.pythonhosted.org/packages/dc/9e/14520dc3dadf3c803473bd07e9b2bd1b69bc583cb2497b47000fed2fa92f/numpy-2.2.6-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:287cc3162b6f01463ccd86be154f284d0893d2b3ed7292439ea97eafa8170e0b", size = 14143006 }, { url = "https://files.pythonhosted.org/packages/4f/06/7e96c57d90bebdce9918412087fc22ca9851cceaf5567a45c1f404480e9e/numpy-2.2.6-cp313-cp313-macosx_14_0_arm64.whl", hash = "sha256:f1372f041402e37e5e633e586f62aa53de2eac8d98cbfb822806ce4bbefcb74d", size = 5076765 }, { url = "https://files.pythonhosted.org/packages/73/ed/63d920c23b4289fdac96ddbdd6132e9427790977d5457cd132f18e76eae0/numpy-2.2.6-cp313-cp313-macosx_14_0_x86_64.whl", hash = "sha256:55a4d33fa519660d69614a9fad433be87e5252f4b03850642f88993f7b2ca566", size = 6617736 }, { url = "https://files.pythonhosted.org/packages/85/c5/e19c8f99d83fd377ec8c7e0cf627a8049746da54afc24ef0a0cb73d5dfb5/numpy-2.2.6-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:f92729c95468a2f4f15e9bb94c432a9229d0d50de67304399627a943201baa2f", size = 14010719 }, { url = "https://files.pythonhosted.org/packages/19/49/4df9123aafa7b539317bf6d342cb6d227e49f7a35b99c287a6109b13dd93/numpy-2.2.6-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1bc23a79bfabc5d056d106f9befb8d50c31ced2fbc70eedb8155aec74a45798f", size = 16526072 }, { url = "https://files.pythonhosted.org/packages/b2/6c/04b5f47f4f32f7c2b0e7260442a8cbcf8168b0e1a41ff1495da42f42a14f/numpy-2.2.6-cp313-cp313-musllinux_1_2_aarch64.whl", hash = "sha256:e3143e4451880bed956e706a3220b4e5cf6172ef05fcc397f6f36a550b1dd868", size = 15503213 }, { url = "https://files.pythonhosted.org/packages/17/0a/5cd92e352c1307640d5b6fec1b2ffb06cd0dabe7d7b8227f97933d378422/numpy-2.2.6-cp313-cp313-musllinux_1_2_x86_64.whl", hash = "sha256:b4f13750ce79751586ae2eb824ba7e1e8dba64784086c98cdbbcc6a42112ce0d", size = 18316632 }, { url = "https://files.pythonhosted.org/packages/f0/3b/5cba2b1d88760ef86596ad0f3d484b1cbff7c115ae2429678465057c5155/numpy-2.2.6-cp313-cp313-win32.whl", hash = "sha256:5beb72339d9d4fa36522fc63802f469b13cdbe4fdab4a288f0c441b74272ebfd", size = 6244532 }, { url = "https://files.pythonhosted.org/packages/cb/3b/d58c12eafcb298d4e6d0d40216866ab15f59e55d148a5658bb3132311fcf/numpy-2.2.6-cp313-cp313-win_amd64.whl", hash = "sha256:b0544343a702fa80c95ad5d3d608ea3599dd54d4632df855e4c8d24eb6ecfa1c", size = 12610885 }, { url = "https://files.pythonhosted.org/packages/6b/9e/4bf918b818e516322db999ac25d00c75788ddfd2d2ade4fa66f1f38097e1/numpy-2.2.6-cp313-cp313t-macosx_10_13_x86_64.whl", hash = "sha256:0bca768cd85ae743b2affdc762d617eddf3bcf8724435498a1e80132d04879e6", size = 20963467 }, { url = "https://files.pythonhosted.org/packages/61/66/d2de6b291507517ff2e438e13ff7b1e2cdbdb7cb40b3ed475377aece69f9/numpy-2.2.6-cp313-cp313t-macosx_11_0_arm64.whl", hash = "sha256:fc0c5673685c508a142ca65209b4e79ed6740a4ed6b2267dbba90f34b0b3cfda", size = 14225144 }, { url = "https://files.pythonhosted.org/packages/e4/25/480387655407ead912e28ba3a820bc69af9adf13bcbe40b299d454ec011f/numpy-2.2.6-cp313-cp313t-macosx_14_0_arm64.whl", hash = "sha256:5bd4fc3ac8926b3819797a7c0e2631eb889b4118a9898c84f585a54d475b7e40", size = 5200217 }, { url = "https://files.pythonhosted.org/packages/aa/4a/6e313b5108f53dcbf3aca0c0f3e9c92f4c10ce57a0a721851f9785872895/numpy-2.2.6-cp313-cp313t-macosx_14_0_x86_64.whl", hash = "sha256:fee4236c876c4e8369388054d02d0e9bb84821feb1a64dd59e137e6511a551f8", size = 6712014 }, { url = "https://files.pythonhosted.org/packages/b7/30/172c2d5c4be71fdf476e9de553443cf8e25feddbe185e0bd88b096915bcc/numpy-2.2.6-cp313-cp313t-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e1dda9c7e08dc141e0247a5b8f49cf05984955246a327d4c48bda16821947b2f", size = 14077935 }, { url = "https://files.pythonhosted.org/packages/12/fb/9e743f8d4e4d3c710902cf87af3512082ae3d43b945d5d16563f26ec251d/numpy-2.2.6-cp313-cp313t-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f447e6acb680fd307f40d3da4852208af94afdfab89cf850986c3ca00562f4fa", size = 16600122 }, { url = "https://files.pythonhosted.org/packages/12/75/ee20da0e58d3a66f204f38916757e01e33a9737d0b22373b3eb5a27358f9/numpy-2.2.6-cp313-cp313t-musllinux_1_2_aarch64.whl", hash = "sha256:389d771b1623ec92636b0786bc4ae56abafad4a4c513d36a55dce14bd9ce8571", size = 15586143 }, { url = "https://files.pythonhosted.org/packages/76/95/bef5b37f29fc5e739947e9ce5179ad402875633308504a52d188302319c8/numpy-2.2.6-cp313-cp313t-musllinux_1_2_x86_64.whl", hash = "sha256:8e9ace4a37db23421249ed236fdcdd457d671e25146786dfc96835cd951aa7c1", size = 18385260 }, { url = "https://files.pythonhosted.org/packages/09/04/f2f83279d287407cf36a7a8053a5abe7be3622a4363337338f2585e4afda/numpy-2.2.6-cp313-cp313t-win32.whl", hash = "sha256:038613e9fb8c72b0a41f025a7e4c3f0b7a1b5d768ece4796b674c8f3fe13efff", size = 6377225 }, { url = "https://files.pythonhosted.org/packages/67/0e/35082d13c09c02c011cf21570543d202ad929d961c02a147493cb0c2bdf5/numpy-2.2.6-cp313-cp313t-win_amd64.whl", hash = "sha256:6031dd6dfecc0cf9f668681a37648373bddd6421fff6c66ec1624eed0180ee06", size = 12771374 }, { url = "https://files.pythonhosted.org/packages/9e/3b/d94a75f4dbf1ef5d321523ecac21ef23a3cd2ac8b78ae2aac40873590229/numpy-2.2.6-pp310-pypy310_pp73-macosx_10_15_x86_64.whl", hash = "sha256:0b605b275d7bd0c640cad4e5d30fa701a8d59302e127e5f79138ad62762c3e3d", size = 21040391 }, { url = "https://files.pythonhosted.org/packages/17/f4/09b2fa1b58f0fb4f7c7963a1649c64c4d315752240377ed74d9cd878f7b5/numpy-2.2.6-pp310-pypy310_pp73-macosx_14_0_x86_64.whl", hash = "sha256:7befc596a7dc9da8a337f79802ee8adb30a552a94f792b9c9d18c840055907db", size = 6786754 }, { url = "https://files.pythonhosted.org/packages/af/30/feba75f143bdc868a1cc3f44ccfa6c4b9ec522b36458e738cd00f67b573f/numpy-2.2.6-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ce47521a4754c8f4593837384bd3424880629f718d87c5d44f8ed763edd63543", size = 16643476 }, { url = "https://files.pythonhosted.org/packages/37/48/ac2a9584402fb6c0cd5b5d1a91dcf176b15760130dd386bbafdbfe3640bf/numpy-2.2.6-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:d042d24c90c41b54fd506da306759e06e568864df8ec17ccc17e9e884634fd00", size = 12812666 }, ] [[package]] name = "opencv-python" version = "4.12.0.88" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "numpy", version = "1.24.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "numpy", version = "2.0.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version == '3.9.*'" }, { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.10'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ac/71/25c98e634b6bdeca4727c7f6d6927b056080668c5008ad3c8fc9e7f8f6ec/opencv-python-4.12.0.88.tar.gz", hash = "sha256:8b738389cede219405f6f3880b851efa3415ccd674752219377353f017d2994d", size = 95373294 } wheels = [ { url = "https://files.pythonhosted.org/packages/85/68/3da40142e7c21e9b1d4e7ddd6c58738feb013203e6e4b803d62cdd9eb96b/opencv_python-4.12.0.88-cp37-abi3-macosx_13_0_arm64.whl", hash = "sha256:f9a1f08883257b95a5764bf517a32d75aec325319c8ed0f89739a57fae9e92a5", size = 37877727 }, { url = "https://files.pythonhosted.org/packages/33/7c/042abe49f58d6ee7e1028eefc3334d98ca69b030e3b567fe245a2b28ea6f/opencv_python-4.12.0.88-cp37-abi3-macosx_13_0_x86_64.whl", hash = "sha256:812eb116ad2b4de43ee116fcd8991c3a687f099ada0b04e68f64899c09448e81", size = 57326471 }, { url = "https://files.pythonhosted.org/packages/62/3a/440bd64736cf8116f01f3b7f9f2e111afb2e02beb2ccc08a6458114a6b5d/opencv_python-4.12.0.88-cp37-abi3-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:51fd981c7df6af3e8f70b1556696b05224c4e6b6777bdd2a46b3d4fb09de1a92", size = 45887139 }, { url = "https://files.pythonhosted.org/packages/68/1f/795e7f4aa2eacc59afa4fb61a2e35e510d06414dd5a802b51a012d691b37/opencv_python-4.12.0.88-cp37-abi3-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:092c16da4c5a163a818f120c22c5e4a2f96e0db4f24e659c701f1fe629a690f9", size = 67041680 }, { url = "https://files.pythonhosted.org/packages/02/96/213fea371d3cb2f1d537612a105792aa0a6659fb2665b22cad709a75bd94/opencv_python-4.12.0.88-cp37-abi3-win32.whl", hash = "sha256:ff554d3f725b39878ac6a2e1fa232ec509c36130927afc18a1719ebf4fbf4357", size = 30284131 }, { url = "https://files.pythonhosted.org/packages/fa/80/eb88edc2e2b11cd2dd2e56f1c80b5784d11d6e6b7f04a1145df64df40065/opencv_python-4.12.0.88-cp37-abi3-win_amd64.whl", hash = "sha256:d98edb20aa932fd8ebd276a72627dad9dc097695b3d435a4257557bbb49a79d2", size = 39000307 }, ] [[package]] name = "packaging" version = "25.0" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/a1/d4/1fc4078c65507b51b96ca8f8c3ba19e6a61c8253c72794544580a7b6c24d/packaging-25.0.tar.gz", hash = "sha256:d443872c98d677bf60f6a1f2f8c1cb748e8fe762d2bf9d3148b5599295b0fc4f", size = 165727 } wheels = [ { url = "https://files.pythonhosted.org/packages/20/12/38679034af332785aac8774540895e234f4d07f7545804097de4b666afd8/packaging-25.0-py3-none-any.whl", hash = "sha256:29572ef2b1f17581046b3a2227d5c611fb25ec70ca1ba8554b24b0e69331a484", size = 66469 }, ] [[package]] name = "pluggy" version = "1.5.0" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version < '3.9'", ] sdist = { url = "https://files.pythonhosted.org/packages/96/2d/02d4312c973c6050a18b314a5ad0b3210edb65a906f868e31c111dede4a6/pluggy-1.5.0.tar.gz", hash = "sha256:2cffa88e94fdc978c4c574f15f9e59b7f4201d439195c3715ca9e2486f1d0cf1", size = 67955 } wheels = [ { url = "https://files.pythonhosted.org/packages/88/5f/e351af9a41f866ac3f1fac4ca0613908d9a41741cfcf2228f4ad853b697d/pluggy-1.5.0-py3-none-any.whl", hash = "sha256:44e1ad92c8ca002de6377e165f3e0f1be63266ab4d554740532335b9d75ea669", size = 20556 }, ] [[package]] name = "pluggy" version = "1.6.0" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version >= '3.11'", "python_full_version == '3.10.*'", "python_full_version == '3.9.*'", ] sdist = { url = "https://files.pythonhosted.org/packages/f9/e2/3e91f31a7d2b083fe6ef3fa267035b518369d9511ffab804f839851d2779/pluggy-1.6.0.tar.gz", hash = "sha256:7dcc130b76258d33b90f61b658791dede3486c3e6bfb003ee5c9bfb396dd22f3", size = 69412 } wheels = [ { url = "https://files.pythonhosted.org/packages/54/20/4d324d65cc6d9205fabedc306948156824eb9f0ee1633355a8f7ec5c66bf/pluggy-1.6.0-py3-none-any.whl", hash = "sha256:e920276dd6813095e9377c0bc5566d94c932c33b27a3e3945d8389c374dd4746", size = 20538 }, ] [[package]] name = "posebench" version = "0.0.1" source = { git = "https://github.com/Parskatt/posebench.git#dd71433241f29a11946655ae8c032067a2a604a9" } dependencies = [ { name = "h5py", version = "3.11.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "h5py", version = "3.14.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.9'" }, { name = "opencv-python" }, { name = "poselib" }, { name = "pycolmap" }, { name = "requests" }, { name = "tqdm" }, ] [[package]] name = "poselib" source = { editable = "." } dependencies = [ { name = "numpy", version = "1.24.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "numpy", version = "2.0.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version == '3.9.*'" }, { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.10'" }, ] [package.optional-dependencies] test = [ { name = "posebench", marker = "python_full_version < '3.14'" }, { name = "pytest", version = "8.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "pytest", version = "8.4.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.9'" }, ] [package.metadata] requires-dist = [ { name = "numpy" }, { name = "posebench", marker = "python_full_version < '3.14' and extra == 'test'", git = "https://github.com/Parskatt/posebench.git" }, { name = "pytest", marker = "extra == 'test'", specifier = ">=8.3.5" }, ] provides-extras = ["test"] [[package]] name = "pycolmap" version = "3.12.3" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "numpy", version = "1.24.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "numpy", version = "2.0.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version == '3.9.*'" }, { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.10'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/2d/48/d5ea36a6b0fa1e813129f1f9fd01212b812b8e9956d845a475a4abe577c3/pycolmap-3.12.3-cp310-cp310-macosx_13_0_x86_64.whl", hash = "sha256:0f938ff2fb07bbb1ba67102f66f3cd0cc97a0187293869803e8e2be1da28ab4f", size = 16081736 }, { url = "https://files.pythonhosted.org/packages/e0/52/7600a849fb6b79fc0a453fff2b66fe8de867cbde4e659d8c738a4223d8dc/pycolmap-3.12.3-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:d1a2c3b8c47d8704e6f7e78457c7617357770c966964d98c3e10e0d063a6f396", size = 14145822 }, { url = "https://files.pythonhosted.org/packages/99/bd/cb69fda0a657ccfafdf774ca007bba365cc49630e6873cc1592afb4275e2/pycolmap-3.12.3-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:74ba833452e12fe52821dac55b73b7cfc4d181c507b792f378df0121a612ad2b", size = 19932441 }, { url = "https://files.pythonhosted.org/packages/86/66/7b74dc60bb91a73bb7f8e030347289c8c2e5c6a0c5f4605fc4e68f41d62c/pycolmap-3.12.3-cp310-cp310-win_amd64.whl", hash = "sha256:2a5250e906d985641b60a4ecefc580306d92a74ee422468b9285ac9a4e680510", size = 18340729 }, { url = "https://files.pythonhosted.org/packages/ce/e7/78e3b5198cdcb8265779caaffcdc386b5b2f58567b3738435335e5fd1d99/pycolmap-3.12.3-cp311-cp311-macosx_13_0_x86_64.whl", hash = "sha256:81323a811529e31a4510ba04fb071006a9b8eb93fca9d15b946aedaf4870aa0d", size = 16082647 }, { url = "https://files.pythonhosted.org/packages/20/a2/5938a97d24cbecbc1d26c037d459791e6fdaaceb1f1c5ab5e57fd0954938/pycolmap-3.12.3-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:665b3690b7b162d0e443bcdb4588a3ee99f359176f589082f38fd02331f7eb15", size = 14147027 }, { url = "https://files.pythonhosted.org/packages/6d/7a/869bdaf6faf245863b607af5057467818c9087db585929a13e1509a34e12/pycolmap-3.12.3-cp311-cp311-manylinux_2_28_x86_64.whl", hash = "sha256:0a65802d1af25b6757421dbc4ed9a4798713d23fec9f11be6c1a49a92c88c5e8", size = 19935345 }, { url = "https://files.pythonhosted.org/packages/b5/3f/844174eb4322d2c033f62baa6ca6742a32b5c528ac7e6d5aa1005917e72f/pycolmap-3.12.3-cp311-cp311-win_amd64.whl", hash = "sha256:6eafa8ba5536a04c7f98aeb53530e21213e9ff6571f952bbb32f1b4cf66f4576", size = 18341234 }, { url = "https://files.pythonhosted.org/packages/2a/ea/bd6c3662d8d7bd1353b6703525f13d19aa93f512cc28bbec411b2c9b3dd4/pycolmap-3.12.3-cp312-cp312-macosx_13_0_x86_64.whl", hash = "sha256:2ea9b04dae0937f84db84c8d0d413e16e4452190fbca1f501a3af7e83539a859", size = 16155465 }, { url = "https://files.pythonhosted.org/packages/f6/8f/0aaac971c4e00ff5a72c141b0d365cbc5a6d1c82de37f24fe390549fe5f1/pycolmap-3.12.3-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:171fb6777e2edaaff91ae31cd6cb4bf06f5cb4616c50fb0d79aecb31ac002a37", size = 14176578 }, { url = "https://files.pythonhosted.org/packages/9e/61/ecf7b772789a794cc3e866ab5f8eb863f1c4f3699cd6ff0a000f1389aad0/pycolmap-3.12.3-cp312-cp312-manylinux_2_28_x86_64.whl", hash = "sha256:cf673f938395cbbdd1cf8ba7b3666021bd4f5fd75d9eca03c24fe7020639d35b", size = 19948979 }, { url = "https://files.pythonhosted.org/packages/a9/e2/5bee6e3a7b636078178e1f2aa9fb9cb86a59f58833b98c9d00fdca8a3043/pycolmap-3.12.3-cp312-cp312-win_amd64.whl", hash = "sha256:eabca558097d87dc4d0aaa76185efde818ed1bbaaede99e90f09191104b5867f", size = 18356521 }, { url = "https://files.pythonhosted.org/packages/47/35/85cb9fc80da27928e933384bd59d0bb53ebe1228eb61ccb3e8840fad5efa/pycolmap-3.12.3-cp313-cp313-macosx_13_0_x86_64.whl", hash = "sha256:da9fccfc961216ea986175fa43e13093f67549a1a7dcea5190750d849837c945", size = 16155180 }, { url = "https://files.pythonhosted.org/packages/3c/f9/7a749c625c174cdfa730a9c5eafea94fcd66ddcd85bee6ec2611d0bb70a2/pycolmap-3.12.3-cp313-cp313-macosx_14_0_arm64.whl", hash = "sha256:76018b2ce7e4195ed6e48301024c8c83ffb0686c2b068f5aea66c3a7c396bef6", size = 14176720 }, { url = "https://files.pythonhosted.org/packages/43/34/6edc9ed5214bbc90d0d3426a4e38427e8034d762a1c14113dfc5d26a2ce4/pycolmap-3.12.3-cp313-cp313-manylinux_2_28_x86_64.whl", hash = "sha256:e86a74d111e7dfd7bde22266d7f14cdd33ad6e444bc58f1066bf066e093eaa4b", size = 19948787 }, { url = "https://files.pythonhosted.org/packages/5b/a8/5649ec5f3f8d9bae681bdaf9aba3a8d908df9700a018a21d04fc66f1b266/pycolmap-3.12.3-cp313-cp313-win_amd64.whl", hash = "sha256:4a08853e93184fc9860bdc59a3af82ac001306963fa02b59d63c0c25a1bb62f7", size = 18355916 }, { url = "https://files.pythonhosted.org/packages/0b/c1/66a9a6fd6329c9472a60150b25638a98325730cf98f31982d8014603fad8/pycolmap-3.12.3-cp38-cp38-macosx_13_0_x86_64.whl", hash = "sha256:0697fae14bb9df26aa10c0488d776b501663e88997ba17ebd8b7ccff17b16fc0", size = 16081102 }, { url = "https://files.pythonhosted.org/packages/1a/89/e45ee516ea3c98977fd3bfbf5ef6a73ca130812a302d3d913eef11381c6d/pycolmap-3.12.3-cp38-cp38-macosx_14_0_arm64.whl", hash = "sha256:0f97c0799f0f64fbc7f2d38c42e98113b1625eebb2896b93e32fba8afdabd6db", size = 14145224 }, { url = "https://files.pythonhosted.org/packages/e3/0a/714ec2163fb0ad0651fefdae0d8fbf0008977c9483b7c81976305bc4e324/pycolmap-3.12.3-cp38-cp38-manylinux_2_28_x86_64.whl", hash = "sha256:060854961e800b3da977fa4b90185f41a86ba3488c4b0ecffa47ccd844dc3220", size = 19931154 }, { url = "https://files.pythonhosted.org/packages/5f/88/89470219e1f080b4f75d6a371d236559df2192a77236fa45db49720617eb/pycolmap-3.12.3-cp38-cp38-win_amd64.whl", hash = "sha256:361a8379886333e9415554fc6945f777f6b40285587c811b24d821b164e2b5fc", size = 18352009 }, { url = "https://files.pythonhosted.org/packages/aa/6d/b47da8ef4616a9e52ca04fc809223d23aeaefd4efb1e0646a9716d6ae09c/pycolmap-3.12.3-cp39-cp39-macosx_13_0_x86_64.whl", hash = "sha256:6a9fa8ccae93e4c68ad6fe4b72a391f0111625868728d9afd7868f4d047ae7c3", size = 16081497 }, { url = "https://files.pythonhosted.org/packages/83/62/45cc6f907ad52d91d46dd37f83a8737c95261b1facacee15a2f21058b363/pycolmap-3.12.3-cp39-cp39-macosx_14_0_arm64.whl", hash = "sha256:943d0814919724b857925159f7090aad7ad321bff3ebfaf8d173fcc8a2971d16", size = 14145914 }, { url = "https://files.pythonhosted.org/packages/2a/8d/af335c6b940b4c85a84032de8fc3d9c7c287df48858b37bf9832e352c154/pycolmap-3.12.3-cp39-cp39-manylinux_2_28_x86_64.whl", hash = "sha256:d4e7f024327720f79fbf8539daf692b791e2e2b7b9a23f863e0b08c846e8359b", size = 19934625 }, { url = "https://files.pythonhosted.org/packages/56/78/e2dc552971f639cb1ba4ba363927f66af638393f68cff15f55030fb652f3/pycolmap-3.12.3-cp39-cp39-win_amd64.whl", hash = "sha256:1510dbd206b8fe536fb53978385bf55b8ab10c8dcaad4f467f81274acb165313", size = 18526418 }, ] [[package]] name = "pygments" version = "2.19.2" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/b0/77/a5b8c569bf593b0140bde72ea885a803b82086995367bf2037de0159d924/pygments-2.19.2.tar.gz", hash = "sha256:636cb2477cec7f8952536970bc533bc43743542f70392ae026374600add5b887", size = 4968631 } wheels = [ { url = "https://files.pythonhosted.org/packages/c7/21/705964c7812476f378728bdf590ca4b771ec72385c533964653c68e86bdc/pygments-2.19.2-py3-none-any.whl", hash = "sha256:86540386c03d588bb81d44bc3928634ff26449851e99741617ecb9037ee5ec0b", size = 1225217 }, ] [[package]] name = "pytest" version = "8.3.5" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version < '3.9'", ] dependencies = [ { name = "colorama", marker = "python_full_version < '3.9' and sys_platform == 'win32'" }, { name = "exceptiongroup", marker = "python_full_version < '3.9'" }, { name = "iniconfig", marker = "python_full_version < '3.9'" }, { name = "packaging", marker = "python_full_version < '3.9'" }, { name = "pluggy", version = "1.5.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "tomli", marker = "python_full_version < '3.9'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ae/3c/c9d525a414d506893f0cd8a8d0de7706446213181570cdbd766691164e40/pytest-8.3.5.tar.gz", hash = "sha256:f4efe70cc14e511565ac476b57c279e12a855b11f48f212af1080ef2263d3845", size = 1450891 } wheels = [ { url = "https://files.pythonhosted.org/packages/30/3d/64ad57c803f1fa1e963a7946b6e0fea4a70df53c1a7fed304586539c2bac/pytest-8.3.5-py3-none-any.whl", hash = "sha256:c69214aa47deac29fad6c2a4f590b9c4a9fdb16a403176fe154b79c0b4d4d820", size = 343634 }, ] [[package]] name = "pytest" version = "8.4.1" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version >= '3.11'", "python_full_version == '3.10.*'", "python_full_version == '3.9.*'", ] dependencies = [ { name = "colorama", marker = "python_full_version >= '3.9' and sys_platform == 'win32'" }, { name = "exceptiongroup", marker = "python_full_version >= '3.9' and python_full_version < '3.11'" }, { name = "iniconfig", marker = "python_full_version >= '3.9'" }, { name = "packaging", marker = "python_full_version >= '3.9'" }, { name = "pluggy", version = "1.6.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.9'" }, { name = "pygments", marker = "python_full_version >= '3.9'" }, { name = "tomli", marker = "python_full_version >= '3.9' and python_full_version < '3.11'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/08/ba/45911d754e8eba3d5a841a5ce61a65a685ff1798421ac054f85aa8747dfb/pytest-8.4.1.tar.gz", hash = "sha256:7c67fd69174877359ed9371ec3af8a3d2b04741818c51e5e99cc1742251fa93c", size = 1517714 } wheels = [ { url = "https://files.pythonhosted.org/packages/29/16/c8a903f4c4dffe7a12843191437d7cd8e32751d5de349d45d3fe69544e87/pytest-8.4.1-py3-none-any.whl", hash = "sha256:539c70ba6fcead8e78eebbf1115e8b589e7565830d7d006a8723f19ac8a0afb7", size = 365474 }, ] [[package]] name = "requests" version = "2.32.4" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "certifi" }, { name = "charset-normalizer" }, { name = "idna" }, { name = "urllib3", version = "2.2.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.9'" }, { name = "urllib3", version = "2.5.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.9'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/e1/0a/929373653770d8a0d7ea76c37de6e41f11eb07559b103b1c02cafb3f7cf8/requests-2.32.4.tar.gz", hash = "sha256:27d0316682c8a29834d3264820024b62a36942083d52caf2f14c0591336d3422", size = 135258 } wheels = [ { url = "https://files.pythonhosted.org/packages/7c/e4/56027c4a6b4ae70ca9de302488c5ca95ad4a39e190093d6c1a8ace08341b/requests-2.32.4-py3-none-any.whl", hash = "sha256:27babd3cda2a6d50b30443204ee89830707d396671944c998b5975b031ac2b2c", size = 64847 }, ] [[package]] name = "tomli" version = "2.2.1" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/18/87/302344fed471e44a87289cf4967697d07e532f2421fdaf868a303cbae4ff/tomli-2.2.1.tar.gz", hash = "sha256:cd45e1dc79c835ce60f7404ec8119f2eb06d38b1deba146f07ced3bbc44505ff", size = 17175 } wheels = [ { url = "https://files.pythonhosted.org/packages/43/ca/75707e6efa2b37c77dadb324ae7d9571cb424e61ea73fad7c56c2d14527f/tomli-2.2.1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:678e4fa69e4575eb77d103de3df8a895e1591b48e740211bd1067378c69e8249", size = 131077 }, { url = "https://files.pythonhosted.org/packages/c7/16/51ae563a8615d472fdbffc43a3f3d46588c264ac4f024f63f01283becfbb/tomli-2.2.1-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:023aa114dd824ade0100497eb2318602af309e5a55595f76b626d6d9f3b7b0a6", size = 123429 }, { url = "https://files.pythonhosted.org/packages/f1/dd/4f6cd1e7b160041db83c694abc78e100473c15d54620083dbd5aae7b990e/tomli-2.2.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ece47d672db52ac607a3d9599a9d48dcb2f2f735c6c2d1f34130085bb12b112a", size = 226067 }, { url = "https://files.pythonhosted.org/packages/a9/6b/c54ede5dc70d648cc6361eaf429304b02f2871a345bbdd51e993d6cdf550/tomli-2.2.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6972ca9c9cc9f0acaa56a8ca1ff51e7af152a9f87fb64623e31d5c83700080ee", size = 236030 }, { url = "https://files.pythonhosted.org/packages/1f/47/999514fa49cfaf7a92c805a86c3c43f4215621855d151b61c602abb38091/tomli-2.2.1-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:c954d2250168d28797dd4e3ac5cf812a406cd5a92674ee4c8f123c889786aa8e", size = 240898 }, { url = "https://files.pythonhosted.org/packages/73/41/0a01279a7ae09ee1573b423318e7934674ce06eb33f50936655071d81a24/tomli-2.2.1-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:8dd28b3e155b80f4d54beb40a441d366adcfe740969820caf156c019fb5c7ec4", size = 229894 }, { url = "https://files.pythonhosted.org/packages/55/18/5d8bc5b0a0362311ce4d18830a5d28943667599a60d20118074ea1b01bb7/tomli-2.2.1-cp311-cp311-musllinux_1_2_i686.whl", hash = "sha256:e59e304978767a54663af13c07b3d1af22ddee3bb2fb0618ca1593e4f593a106", size = 245319 }, { url = "https://files.pythonhosted.org/packages/92/a3/7ade0576d17f3cdf5ff44d61390d4b3febb8a9fc2b480c75c47ea048c646/tomli-2.2.1-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:33580bccab0338d00994d7f16f4c4ec25b776af3ffaac1ed74e0b3fc95e885a8", size = 238273 }, { url = "https://files.pythonhosted.org/packages/72/6f/fa64ef058ac1446a1e51110c375339b3ec6be245af9d14c87c4a6412dd32/tomli-2.2.1-cp311-cp311-win32.whl", hash = "sha256:465af0e0875402f1d226519c9904f37254b3045fc5084697cefb9bdde1ff99ff", size = 98310 }, { url = "https://files.pythonhosted.org/packages/6a/1c/4a2dcde4a51b81be3530565e92eda625d94dafb46dbeb15069df4caffc34/tomli-2.2.1-cp311-cp311-win_amd64.whl", hash = "sha256:2d0f2fdd22b02c6d81637a3c95f8cd77f995846af7414c5c4b8d0545afa1bc4b", size = 108309 }, { url = "https://files.pythonhosted.org/packages/52/e1/f8af4c2fcde17500422858155aeb0d7e93477a0d59a98e56cbfe75070fd0/tomli-2.2.1-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:4a8f6e44de52d5e6c657c9fe83b562f5f4256d8ebbfe4ff922c495620a7f6cea", size = 132762 }, { url = "https://files.pythonhosted.org/packages/03/b8/152c68bb84fc00396b83e7bbddd5ec0bd3dd409db4195e2a9b3e398ad2e3/tomli-2.2.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:8d57ca8095a641b8237d5b079147646153d22552f1c637fd3ba7f4b0b29167a8", size = 123453 }, { url = "https://files.pythonhosted.org/packages/c8/d6/fc9267af9166f79ac528ff7e8c55c8181ded34eb4b0e93daa767b8841573/tomli-2.2.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:4e340144ad7ae1533cb897d406382b4b6fede8890a03738ff1683af800d54192", size = 233486 }, { url = "https://files.pythonhosted.org/packages/5c/51/51c3f2884d7bab89af25f678447ea7d297b53b5a3b5730a7cb2ef6069f07/tomli-2.2.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:db2b95f9de79181805df90bedc5a5ab4c165e6ec3fe99f970d0e302f384ad222", size = 242349 }, { url = "https://files.pythonhosted.org/packages/ab/df/bfa89627d13a5cc22402e441e8a931ef2108403db390ff3345c05253935e/tomli-2.2.1-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:40741994320b232529c802f8bc86da4e1aa9f413db394617b9a256ae0f9a7f77", size = 252159 }, { url = "https://files.pythonhosted.org/packages/9e/6e/fa2b916dced65763a5168c6ccb91066f7639bdc88b48adda990db10c8c0b/tomli-2.2.1-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:400e720fe168c0f8521520190686ef8ef033fb19fc493da09779e592861b78c6", size = 237243 }, { url = "https://files.pythonhosted.org/packages/b4/04/885d3b1f650e1153cbb93a6a9782c58a972b94ea4483ae4ac5cedd5e4a09/tomli-2.2.1-cp312-cp312-musllinux_1_2_i686.whl", hash = "sha256:02abe224de6ae62c19f090f68da4e27b10af2b93213d36cf44e6e1c5abd19fdd", size = 259645 }, { url = "https://files.pythonhosted.org/packages/9c/de/6b432d66e986e501586da298e28ebeefd3edc2c780f3ad73d22566034239/tomli-2.2.1-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:b82ebccc8c8a36f2094e969560a1b836758481f3dc360ce9a3277c65f374285e", size = 244584 }, { url = "https://files.pythonhosted.org/packages/1c/9a/47c0449b98e6e7d1be6cbac02f93dd79003234ddc4aaab6ba07a9a7482e2/tomli-2.2.1-cp312-cp312-win32.whl", hash = "sha256:889f80ef92701b9dbb224e49ec87c645ce5df3fa2cc548664eb8a25e03127a98", size = 98875 }, { url = "https://files.pythonhosted.org/packages/ef/60/9b9638f081c6f1261e2688bd487625cd1e660d0a85bd469e91d8db969734/tomli-2.2.1-cp312-cp312-win_amd64.whl", hash = "sha256:7fc04e92e1d624a4a63c76474610238576942d6b8950a2d7f908a340494e67e4", size = 109418 }, { url = "https://files.pythonhosted.org/packages/04/90/2ee5f2e0362cb8a0b6499dc44f4d7d48f8fff06d28ba46e6f1eaa61a1388/tomli-2.2.1-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:f4039b9cbc3048b2416cc57ab3bda989a6fcf9b36cf8937f01a6e731b64f80d7", size = 132708 }, { url = "https://files.pythonhosted.org/packages/c0/ec/46b4108816de6b385141f082ba99e315501ccd0a2ea23db4a100dd3990ea/tomli-2.2.1-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:286f0ca2ffeeb5b9bd4fcc8d6c330534323ec51b2f52da063b11c502da16f30c", size = 123582 }, { url = "https://files.pythonhosted.org/packages/a0/bd/b470466d0137b37b68d24556c38a0cc819e8febe392d5b199dcd7f578365/tomli-2.2.1-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a92ef1a44547e894e2a17d24e7557a5e85a9e1d0048b0b5e7541f76c5032cb13", size = 232543 }, { url = "https://files.pythonhosted.org/packages/d9/e5/82e80ff3b751373f7cead2815bcbe2d51c895b3c990686741a8e56ec42ab/tomli-2.2.1-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:9316dc65bed1684c9a98ee68759ceaed29d229e985297003e494aa825ebb0281", size = 241691 }, { url = "https://files.pythonhosted.org/packages/05/7e/2a110bc2713557d6a1bfb06af23dd01e7dde52b6ee7dadc589868f9abfac/tomli-2.2.1-cp313-cp313-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:e85e99945e688e32d5a35c1ff38ed0b3f41f43fad8df0bdf79f72b2ba7bc5272", size = 251170 }, { url = "https://files.pythonhosted.org/packages/64/7b/22d713946efe00e0adbcdfd6d1aa119ae03fd0b60ebed51ebb3fa9f5a2e5/tomli-2.2.1-cp313-cp313-musllinux_1_2_aarch64.whl", hash = "sha256:ac065718db92ca818f8d6141b5f66369833d4a80a9d74435a268c52bdfa73140", size = 236530 }, { url = "https://files.pythonhosted.org/packages/38/31/3a76f67da4b0cf37b742ca76beaf819dca0ebef26d78fc794a576e08accf/tomli-2.2.1-cp313-cp313-musllinux_1_2_i686.whl", hash = "sha256:d920f33822747519673ee656a4b6ac33e382eca9d331c87770faa3eef562aeb2", size = 258666 }, { url = "https://files.pythonhosted.org/packages/07/10/5af1293da642aded87e8a988753945d0cf7e00a9452d3911dd3bb354c9e2/tomli-2.2.1-cp313-cp313-musllinux_1_2_x86_64.whl", hash = "sha256:a198f10c4d1b1375d7687bc25294306e551bf1abfa4eace6650070a5c1ae2744", size = 243954 }, { url = "https://files.pythonhosted.org/packages/5b/b9/1ed31d167be802da0fc95020d04cd27b7d7065cc6fbefdd2f9186f60d7bd/tomli-2.2.1-cp313-cp313-win32.whl", hash = "sha256:d3f5614314d758649ab2ab3a62d4f2004c825922f9e370b29416484086b264ec", size = 98724 }, { url = "https://files.pythonhosted.org/packages/c7/32/b0963458706accd9afcfeb867c0f9175a741bf7b19cd424230714d722198/tomli-2.2.1-cp313-cp313-win_amd64.whl", hash = "sha256:a38aa0308e754b0e3c67e344754dff64999ff9b513e691d0e786265c93583c69", size = 109383 }, { url = "https://files.pythonhosted.org/packages/6e/c2/61d3e0f47e2b74ef40a68b9e6ad5984f6241a942f7cd3bbfbdbd03861ea9/tomli-2.2.1-py3-none-any.whl", hash = "sha256:cb55c73c5f4408779d0cf3eef9f762b9c9f147a77de7b258bef0a5628adc85cc", size = 14257 }, ] [[package]] name = "tqdm" version = "4.67.1" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "colorama", marker = "sys_platform == 'win32'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/a8/4b/29b4ef32e036bb34e4ab51796dd745cdba7ed47ad142a9f4a1eb8e0c744d/tqdm-4.67.1.tar.gz", hash = "sha256:f8aef9c52c08c13a65f30ea34f4e5aac3fd1a34959879d7e59e63027286627f2", size = 169737 } wheels = [ { url = "https://files.pythonhosted.org/packages/d0/30/dc54f88dd4a2b5dc8a0279bdd7270e735851848b762aeb1c1184ed1f6b14/tqdm-4.67.1-py3-none-any.whl", hash = "sha256:26445eca388f82e72884e0d580d5464cd801a3ea01e63e5601bdff9ba6a48de2", size = 78540 }, ] [[package]] name = "typing-extensions" version = "4.13.2" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version < '3.9'", ] sdist = { url = "https://files.pythonhosted.org/packages/f6/37/23083fcd6e35492953e8d2aaaa68b860eb422b34627b13f2ce3eb6106061/typing_extensions-4.13.2.tar.gz", hash = "sha256:e6c81219bd689f51865d9e372991c540bda33a0379d5573cddb9a3a23f7caaef", size = 106967 } wheels = [ { url = "https://files.pythonhosted.org/packages/8b/54/b1ae86c0973cc6f0210b53d508ca3641fb6d0c56823f288d108bc7ab3cc8/typing_extensions-4.13.2-py3-none-any.whl", hash = "sha256:a439e7c04b49fec3e5d3e2beaa21755cadbbdc391694e28ccdd36ca4a1408f8c", size = 45806 }, ] [[package]] name = "typing-extensions" version = "4.14.1" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version == '3.10.*'", "python_full_version == '3.9.*'", ] sdist = { url = "https://files.pythonhosted.org/packages/98/5a/da40306b885cc8c09109dc2e1abd358d5684b1425678151cdaed4731c822/typing_extensions-4.14.1.tar.gz", hash = "sha256:38b39f4aeeab64884ce9f74c94263ef78f3c22467c8724005483154c26648d36", size = 107673 } wheels = [ { url = "https://files.pythonhosted.org/packages/b5/00/d631e67a838026495268c2f6884f3711a15a9a2a96cd244fdaea53b823fb/typing_extensions-4.14.1-py3-none-any.whl", hash = "sha256:d1e1e3b58374dc93031d6eda2420a48ea44a36c2b4766a4fdeb3710755731d76", size = 43906 }, ] [[package]] name = "urllib3" version = "2.2.3" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version < '3.9'", ] sdist = { url = "https://files.pythonhosted.org/packages/ed/63/22ba4ebfe7430b76388e7cd448d5478814d3032121827c12a2cc287e2260/urllib3-2.2.3.tar.gz", hash = "sha256:e7d814a81dad81e6caf2ec9fdedb284ecc9c73076b62654547cc64ccdcae26e9", size = 300677 } wheels = [ { url = "https://files.pythonhosted.org/packages/ce/d9/5f4c13cecde62396b0d3fe530a50ccea91e7dfc1ccf0e09c228841bb5ba8/urllib3-2.2.3-py3-none-any.whl", hash = "sha256:ca899ca043dcb1bafa3e262d73aa25c465bfb49e0bd9dd5d59f1d0acba2f8fac", size = 126338 }, ] [[package]] name = "urllib3" version = "2.5.0" source = { registry = "https://pypi.org/simple" } resolution-markers = [ "python_full_version >= '3.11'", "python_full_version == '3.10.*'", "python_full_version == '3.9.*'", ] sdist = { url = "https://files.pythonhosted.org/packages/15/22/9ee70a2574a4f4599c47dd506532914ce044817c7752a79b6a51286319bc/urllib3-2.5.0.tar.gz", hash = "sha256:3fc47733c7e419d4bc3f6b3dc2b4f890bb743906a30d56ba4a5bfa4bbff92760", size = 393185 } wheels = [ { url = "https://files.pythonhosted.org/packages/a7/c2/fe1e52489ae3122415c51f387e221dd0773709bad6c6cdaa599e8a2c5185/urllib3-2.5.0-py3-none-any.whl", hash = "sha256:e6b01673c0fa6a13e374b50871808eb3bf7046c4b125b216f6bf1cc604cff0dc", size = 129795 }, ]