├── .gitignore ├── cmake └── PnpSolverConfig.cmake.in ├── test ├── CMakeLists.txt └── test_pnp.cc ├── src ├── util │ ├── math.cc │ ├── random.cc │ ├── misc.cc │ ├── timer.h │ ├── timer.cc │ ├── string.h │ ├── matrix.h │ ├── string.cc │ ├── types.h │ ├── random.h │ ├── alignment.h │ ├── misc.h │ └── threading.cc ├── interface │ ├── pnp_solver.h │ └── pnp_solver.cc ├── optim │ ├── weighted_sampler.cc │ ├── random_sampler.h │ ├── weighted_sampler.h │ ├── random_sampler.cc │ ├── progressive_sampler.h │ ├── sprt.h │ ├── support_measurement.cc │ ├── support_measurement.h │ ├── sprt.cc │ ├── sampler.h │ ├── progressive_sampler.cc │ └── bundle_adjustment.h ├── CMakeLists.txt ├── estimators │ ├── utils.h │ ├── pose.h │ ├── utils.cc │ └── absolute_pose.h └── base │ ├── polynomial.h │ ├── projection.h │ ├── projection.cc │ ├── camera.h │ ├── camera_models.cc │ ├── pose.h │ └── polynomial.cc ├── README.md ├── LICENSE ├── CMakeLists.txt └── .clang-format /.gitignore: -------------------------------------------------------------------------------- 1 | /bin/ 2 | /build/ 3 | .idea/ -------------------------------------------------------------------------------- /cmake/PnpSolverConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | IF(NOT TARGET PnpSolver::pnpsolver) 4 | INCLUDE("${CMAKE_CURRENT_LIST_DIR}/pnpsolver-targets.cmake") 5 | ENDIF() -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, ZJU and SenseTime 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright notice, 8 | # this list of conditions and the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 14 | # * Neither the name of ZJU and SenseTime nor the names of its contributors 15 | # may be used to endorse or promote products derived from this software 16 | # without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | # 30 | # Author: Hailin Yu 31 | 32 | include_directories(${PROJECT_SOURCE_DIR}/src) 33 | 34 | add_executable(test_pnp test_pnp.cc) 35 | target_link_libraries(test_pnp pnpsolver) -------------------------------------------------------------------------------- /src/util/math.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "util/math.h" 33 | 34 | namespace colmap { 35 | 36 | size_t NChooseK(const size_t n, const size_t k) { 37 | if (k == 0) { 38 | return 1; 39 | } 40 | 41 | return (n * NChooseK(n - 1, k - 1)) / k; 42 | } 43 | 44 | } // namespace colmap 45 | -------------------------------------------------------------------------------- /src/interface/pnp_solver.h: -------------------------------------------------------------------------------- 1 | #ifndef PNPSOLVER_PNP_SOLVER_H 2 | #define PNPSOLVER_PNP_SOLVER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace colpnp { 8 | 9 | // Sampling method 10 | enum Sampler { 11 | RANDOM_SAMPLE = 1, 12 | WEIGHT_SAMPLE = 2, 13 | }; 14 | 15 | // RANSAC method 16 | enum Robustor { RANSAC = 1, LORANSAC = 2 }; 17 | 18 | // Estimate the camera pose from 2D-3D correspondences and refine pose with all 19 | // inliers 20 | // 21 | // @param points2D 2D image points 22 | // @param points3D 3D world points 23 | // @param camera_model camera model name, for example: SIMPLE_PINHOLE, 24 | // SIMPLE_RADIAL, etc. 25 | // @param params The focal length, principal point, and extra 26 | // parameters. 27 | // @param qvec Quaternion (qw, qx, qy, qz) from world to camera 28 | // @param tvec Translation (x, y, z) from world to camera 29 | // @param error_thres RANSAC max error thres 30 | // @param inlier_ratio RANSAC min inlier ratio 31 | // @param confidence RANSAC confidence 32 | // @param max_iter RANSAC max iteration 33 | // @param mask Inlier mask 34 | // @param robustor RANSAC(with p3p) or LORANSAC(p3p and epnp) 35 | // @param sampler RANSAC sampling method: RANDOM, WEIGHT 36 | // 37 | // @param priors When using weighted sampler 38 | // 39 | // @return Whether the solution is usable. 40 | bool sovle_pnp_ransac(const std::vector &points2D, 41 | const std::vector &points3D, 42 | const std::string &camera_model, 43 | const std::vector ¶ms, Eigen::Vector4d &qvec, 44 | Eigen::Vector3d &tvec, size_t &num_inlier, 45 | double error_thres = 8.0, double inlier_ratio = 0.1, 46 | double confidence = 0.99, size_t max_iter = 10000, 47 | std::vector *mask = nullptr, 48 | Robustor robustor = RANSAC, 49 | Sampler sampler = RANDOM_SAMPLE, 50 | std::vector *priors = nullptr); 51 | 52 | } // namespace colpnp 53 | 54 | #endif // PNPSOLVER_PNP_SOLVER_H 55 | -------------------------------------------------------------------------------- /src/util/random.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "util/random.h" 33 | 34 | namespace colmap { 35 | 36 | thread_local std::mt19937* PRNG = nullptr; 37 | 38 | void SetPRNGSeed(unsigned seed) { 39 | // Avoid race conditions, especially for srand(). 40 | static std::mutex mutex; 41 | std::unique_lock lock(mutex); 42 | 43 | // Overwrite existing PRNG 44 | if (PRNG != nullptr) { 45 | delete PRNG; 46 | } 47 | 48 | seed = 99995939; 49 | 50 | PRNG = new std::mt19937(seed); 51 | srand(seed); 52 | } 53 | 54 | } // namespace colmap 55 | -------------------------------------------------------------------------------- /src/util/misc.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "util/misc.h" 33 | 34 | #include 35 | 36 | namespace colmap { 37 | 38 | void PrintHeading1(const std::string& heading) { 39 | std::cout << std::endl << std::string(78, '=') << std::endl; 40 | std::cout << heading << std::endl; 41 | std::cout << std::string(78, '=') << std::endl << std::endl; 42 | } 43 | 44 | void PrintHeading2(const std::string& heading) { 45 | std::cout << std::endl << heading << std::endl; 46 | std::cout << std::string(std::min(heading.size(), 78), '-') 47 | << std::endl; 48 | } 49 | 50 | } // namespace colmap 51 | -------------------------------------------------------------------------------- /src/optim/weighted_sampler.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021, ZJU and SenseTime 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ZJU and SenseTime nor the names of its contributors 15 | // may be used to endorse or promote products derived from this software 16 | // without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Hailin Yu 31 | 32 | #include "weighted_sampler.h" 33 | 34 | #include 35 | 36 | #include "util/random.h" 37 | #include "util/misc.h" 38 | 39 | namespace colmap { 40 | 41 | WeigthedRandomSampler::WeigthedRandomSampler(const size_t num_samples) 42 | : num_samples_(num_samples) {} 43 | 44 | void WeigthedRandomSampler::Initialize(const size_t total_num_samples) { 45 | // pass 46 | } 47 | 48 | void WeigthedRandomSampler::SetPriors(const std::vector &prob) { 49 | priors_.assign(prob.begin(), prob.end()); 50 | } 51 | 52 | size_t WeigthedRandomSampler::MaxNumSamples() { 53 | return std::numeric_limits::max(); 54 | } 55 | 56 | std::vector WeigthedRandomSampler::Sample() { 57 | return WeightedRandomSample(priors_, num_samples_); 58 | } 59 | 60 | } // namespace colmap -------------------------------------------------------------------------------- /src/util/timer.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_TIMER_H_ 33 | #define COLMAP_SRC_UTIL_TIMER_H_ 34 | 35 | #include 36 | 37 | namespace colmap { 38 | 39 | class Timer { 40 | public: 41 | Timer(); 42 | 43 | void Start(); 44 | void Restart(); 45 | void Pause(); 46 | void Resume(); 47 | void Reset(); 48 | 49 | double ElapsedMicroSeconds() const; 50 | double ElapsedSeconds() const; 51 | double ElapsedMinutes() const; 52 | double ElapsedHours() const; 53 | 54 | private: 55 | bool started_; 56 | bool paused_; 57 | std::chrono::high_resolution_clock::time_point start_time_; 58 | std::chrono::high_resolution_clock::time_point pause_time_; 59 | }; 60 | 61 | } // namespace colmap 62 | 63 | #endif // COLMAP_SRC_UTIL_TIMER_H_ 64 | -------------------------------------------------------------------------------- /src/optim/random_sampler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_OPTIM_RANDOM_SAMPLER_H_ 33 | #define COLMAP_SRC_OPTIM_RANDOM_SAMPLER_H_ 34 | 35 | #include "optim/sampler.h" 36 | 37 | namespace colmap { 38 | 39 | // Random sampler for RANSAC-based methods. 40 | // 41 | // Note that a separate sampler should be instantiated per thread. 42 | class RandomSampler : public Sampler { 43 | public: 44 | explicit RandomSampler(const size_t num_samples); 45 | 46 | void Initialize(const size_t total_num_samples) override; 47 | 48 | size_t MaxNumSamples() override; 49 | 50 | std::vector Sample() override; 51 | 52 | private: 53 | const size_t num_samples_; 54 | std::vector sample_idxs_; 55 | }; 56 | 57 | } // namespace colmap 58 | 59 | #endif // COLMAP_SRC_OPTIM_RANDOM_SAMPLER_H_ 60 | -------------------------------------------------------------------------------- /src/optim/weighted_sampler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021, ZJU and SenseTime 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ZJU and SenseTime nor the names of its contributors 15 | // may be used to endorse or promote products derived from this software 16 | // without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Hailin Yu 31 | 32 | #ifndef PNPSOLVER_WEIGHTED_SAMPLER_H 33 | #define PNPSOLVER_WEIGHTED_SAMPLER_H 34 | 35 | #include 36 | 37 | #include "optim/sampler.h" 38 | 39 | namespace colmap { 40 | // Weighted random sampler for Weighted RANSAC-based methods 41 | // 42 | // Note that a discrete probability distribution should be given 43 | class WeigthedRandomSampler : public Sampler { 44 | public: 45 | explicit WeigthedRandomSampler(const size_t num_sample); 46 | 47 | void Initialize(const size_t total_num_samples) override; 48 | 49 | void SetPriors(const std::vector &prob) override; 50 | 51 | size_t MaxNumSamples() override; 52 | 53 | std::vector Sample() override; 54 | 55 | private: 56 | std::vector priors_; 57 | const size_t num_samples_; 58 | }; 59 | 60 | } // namespace colmap 61 | 62 | #endif // PNPSOLVER_WEIGHTED_SAMPLER_H 63 | -------------------------------------------------------------------------------- /src/interface/pnp_solver.cc: -------------------------------------------------------------------------------- 1 | #include "pnp_solver.h" 2 | 3 | #include "base/camera.h" 4 | #include "estimators/pose.h" 5 | 6 | namespace colpnp { 7 | 8 | bool sovle_pnp_ransac(const std::vector &points2D, 9 | const std::vector &points3D, 10 | const std::string &camera_model, 11 | const std::vector ¶ms, Eigen::Vector4d &qvec, 12 | Eigen::Vector3d &tvec, size_t &num_inlier, 13 | double error_thres, double inlier_ratio, 14 | double confidence, size_t max_iter, 15 | std::vector *mask, Robustor robustor, 16 | Sampler sampler, std::vector *priors) { 17 | // CHECK(points2D.size() == points3D.size()); 18 | if (points2D.size() < 4) { 19 | return false; 20 | } 21 | 22 | colmap::AbsolutePoseEstimationOptions options; 23 | options.estimate_focal_length = false; 24 | options.ransac_options.max_error = error_thres; 25 | options.ransac_options.min_inlier_ratio = inlier_ratio; 26 | options.ransac_options.confidence = confidence; 27 | options.ransac_options.max_num_trials = max_iter; 28 | 29 | colmap::Camera camera; 30 | camera.SetModelIdFromName(camera_model); 31 | camera.SetParams(params); 32 | 33 | num_inlier = 0; 34 | 35 | colmap::RansacSampler abs_pose_sampler; 36 | if (sampler == RANDOM_SAMPLE) { 37 | abs_pose_sampler = colmap::RANDOM_SAMPLE; 38 | // CHECK(priors == nullptr); 39 | } else if (sampler == WEIGHT_SAMPLE) { 40 | abs_pose_sampler = colmap::WEIGHT_SAMPLE; 41 | // CHECK(priors->size() == points3D.size()); 42 | } 43 | 44 | colmap::RansacRobustor abs_pose_robustor; 45 | if (robustor == RANSAC) { 46 | abs_pose_robustor = colmap::ROBUSTRER_RANSAC; 47 | } else if (robustor == LORANSAC) { 48 | abs_pose_robustor = colmap::ROBUSTER_LORANSAC; 49 | } 50 | 51 | auto success = colmap::EstimateAbsolutePose( 52 | options, points2D, points3D, &qvec, &tvec, &camera, &num_inlier, mask, 53 | abs_pose_robustor, abs_pose_sampler, *priors); 54 | 55 | if (!success) { 56 | return false; 57 | } 58 | 59 | colmap::AbsolutePoseRefinementOptions refine_options; 60 | refine_options.refine_focal_length = false; 61 | refine_options.refine_extra_params = false; 62 | refine_options.max_num_iterations = 200; 63 | refine_options.print_summary = false; 64 | 65 | return colmap::RefineAbsolutePose(refine_options, *mask, points2D, points3D, 66 | &qvec, &tvec, &camera); 67 | } 68 | 69 | } // namespace colpnp -------------------------------------------------------------------------------- /src/optim/random_sampler.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "optim/random_sampler.h" 33 | 34 | #include 35 | 36 | #include "util/random.h" 37 | 38 | namespace colmap { 39 | 40 | RandomSampler::RandomSampler(const size_t num_samples) 41 | : num_samples_(num_samples) {} 42 | 43 | void RandomSampler::Initialize(const size_t total_num_samples) { 44 | // CHECK_LE(num_samples_, total_num_samples); 45 | sample_idxs_.resize(total_num_samples); 46 | std::iota(sample_idxs_.begin(), sample_idxs_.end(), 0); 47 | } 48 | 49 | size_t RandomSampler::MaxNumSamples() { 50 | return std::numeric_limits::max(); 51 | } 52 | 53 | std::vector RandomSampler::Sample() { 54 | Shuffle(static_cast(num_samples_), &sample_idxs_); 55 | 56 | std::vector sampled_idxs(num_samples_); 57 | for (size_t i = 0; i < num_samples_; ++i) { 58 | sampled_idxs[i] = sample_idxs_[i]; 59 | } 60 | 61 | return sampled_idxs; 62 | } 63 | 64 | } // namespace colmap 65 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PnP Solver 2 | 3 | Simplified [COLMAP](https://github.com/colmap/colmap) PnP Solver with weight PnP Solver Support. 4 | 5 | ## About 6 | ____ 7 | We have simplified COLMAP project and keeping only the PnP Solver parts. Then enhence the project with weighted PnP Solver. 8 | 9 | ## Dependencies 10 | ____ 11 | - C++11 12 | - [CMake](https://cmake.org/) is a cross platform build system. 13 | - [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) is used extensively for doing nearly all the matrix and linear algebra operations. 14 | - [Ceres Solver](http://ceres-solver.org/) is a library for solving non-linear least squares problems. 15 | - [Glog](https://code.google.com/archive/p/google-glog/) is used for error checking and logging. 16 | - [Boost](https://www.boost.org/) is used for filesystem operation and unit testing. 17 | 18 | ## Install 19 | ____ 20 | ``` 21 | mkdir build && cd build 22 | cmake .. && make -j4 23 | sudo make install 24 | ``` 25 | 26 | ## Acknowledgements 27 | ____ 28 | The Backbone of PnP Solver is based on [COLMAP](https://github.com/colmap/colmap) 29 | 30 | @inproceedings{schoenberger2016sfm, 31 | author={Sch\"{o}nberger, Johannes Lutz and Frahm, Jan-Michael}, 32 | title={Structure-from-Motion Revisited}, 33 | booktitle={Conference on Computer Vision and Pattern Recognition (CVPR)}, 34 | year={2016}, 35 | } 36 | ## Licence 37 | ____ 38 | Copyright (c) 2021, ZJU and SenseTime 39 | All rights reserved. 40 | 41 | Redistribution and use in source and binary forms, with or without 42 | modification, are permitted provided that the following conditions are met: 43 | 44 | * Redistributions of source code must retain the above copyright notice, 45 | this list of conditions and the following disclaimer. 46 | 47 | * Redistributions in binary form must reproduce the above copyright 48 | notice, this list of conditions and the following disclaimer in the 49 | documentation and/or other materials provided with the distribution. 50 | 51 | * Neither the name of ZJU and SenseTime nor the names of its contributors 52 | may be used to endorse or promote products derived from this software 53 | without specific prior written permission. 54 | 55 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 56 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 57 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 58 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 59 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 60 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 61 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 62 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 63 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 64 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 65 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /src/optim/progressive_sampler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_OPTIM_PROGRESSIVE_SAMPLER_H_ 33 | #define COLMAP_SRC_OPTIM_PROGRESSIVE_SAMPLER_H_ 34 | 35 | #include "optim/sampler.h" 36 | 37 | namespace colmap { 38 | 39 | // Random sampler for PROSAC (Progressive Sample Consensus), as described in: 40 | // 41 | // "Matching with PROSAC - Progressive Sample Consensus". 42 | // Ondrej Chum and Matas, CVPR 2005. 43 | // 44 | // Note that a separate sampler should be instantiated per thread and that the 45 | // data to be sampled from is assumed to be sorted according to the quality 46 | // function in descending order, i.e., higher quality data is closer to the 47 | // front of the list. 48 | class ProgressiveSampler : public Sampler { 49 | public: 50 | explicit ProgressiveSampler(const size_t num_samples); 51 | 52 | void Initialize(const size_t total_num_samples) override; 53 | 54 | size_t MaxNumSamples() override; 55 | 56 | std::vector Sample() override; 57 | 58 | private: 59 | const size_t num_samples_; 60 | size_t total_num_samples_; 61 | 62 | // The number of generated samples, i.e. the number of calls to `Sample`. 63 | size_t t_; 64 | size_t n_; 65 | 66 | // Variables defined in equation 3. 67 | double T_n_; 68 | double T_n_p_; 69 | }; 70 | 71 | } // namespace colmap 72 | 73 | #endif // COLMAP_SRC_OPTIM_PROGRESSIVE_SAMPLER_H_ 74 | -------------------------------------------------------------------------------- /src/util/timer.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "util/timer.h" 33 | 34 | using namespace std::chrono; 35 | 36 | namespace colmap { 37 | 38 | Timer::Timer() : started_(false), paused_(false) {} 39 | 40 | void Timer::Start() { 41 | started_ = true; 42 | paused_ = false; 43 | start_time_ = high_resolution_clock::now(); 44 | } 45 | 46 | void Timer::Restart() { 47 | started_ = false; 48 | Start(); 49 | } 50 | 51 | void Timer::Pause() { 52 | paused_ = true; 53 | pause_time_ = high_resolution_clock::now(); 54 | } 55 | 56 | void Timer::Resume() { 57 | paused_ = false; 58 | start_time_ += high_resolution_clock::now() - pause_time_; 59 | } 60 | 61 | void Timer::Reset() { 62 | started_ = false; 63 | paused_ = false; 64 | } 65 | 66 | double Timer::ElapsedMicroSeconds() const { 67 | if (!started_) { 68 | return 0.0; 69 | } 70 | if (paused_) { 71 | return duration_cast(pause_time_ - start_time_).count(); 72 | } else { 73 | return duration_cast(high_resolution_clock::now() - 74 | start_time_) 75 | .count(); 76 | } 77 | } 78 | 79 | double Timer::ElapsedSeconds() const { return ElapsedMicroSeconds() / 1e6; } 80 | 81 | double Timer::ElapsedMinutes() const { return ElapsedSeconds() / 60; } 82 | 83 | double Timer::ElapsedHours() const { return ElapsedMinutes() / 60; } 84 | 85 | } // namespace colmap 86 | -------------------------------------------------------------------------------- /src/util/string.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_STRING_H_ 33 | #define COLMAP_SRC_UTIL_STRING_H_ 34 | 35 | #include 36 | #include 37 | 38 | namespace colmap { 39 | 40 | // Replace all occurrences of `old_str` with `new_str` in the given string. 41 | std::string StringReplace(const std::string& str, const std::string& old_str, 42 | const std::string& new_str); 43 | 44 | // Get substring of string after search key 45 | std::string StringGetAfter(const std::string& str, const std::string& key); 46 | 47 | // Split string into list of words using the given delimiters. 48 | std::vector StringSplit(const std::string& str, 49 | const std::string& delim); 50 | 51 | // Check whether a string starts with a certain prefix. 52 | bool StringStartsWith(const std::string& str, const std::string& prefix); 53 | 54 | // Remove whitespace from string on both, left, or right sides. 55 | void StringTrim(std::string* str); 56 | void StringLeftTrim(std::string* str); 57 | void StringRightTrim(std::string* str); 58 | 59 | // Convert string to lower/upper case. 60 | void StringToLower(std::string* str); 61 | void StringToUpper(std::string* str); 62 | 63 | // Check whether the sub-string is contained in the given string. 64 | bool StringContains(const std::string& str, const std::string& sub_str); 65 | 66 | } // namespace colmap 67 | 68 | #endif // COLMAP_SRC_UTIL_STRING_H_ 69 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2021, ZJU and SenseTime 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 14 | * Neither the name of ZJU and SenseTime nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 31 | All rights reserved. 32 | 33 | Redistribution and use in source and binary forms, with or without 34 | modification, are permitted provided that the following conditions are met: 35 | 36 | * Redistributions of source code must retain the above copyright 37 | notice, this list of conditions and the following disclaimer. 38 | 39 | * Redistributions in binary form must reproduce the above copyright 40 | notice, this list of conditions and the following disclaimer in the 41 | documentation and/or other materials provided with the distribution. 42 | 43 | * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 44 | its contributors may be used to endorse or promote products derived 45 | from this software without specific prior written permission. 46 | 47 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 48 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 49 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 50 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 51 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 52 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 53 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 54 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 55 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 56 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 57 | POSSIBILITY OF SUCH DAMAGE. 58 | 59 | Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) -------------------------------------------------------------------------------- /test/test_pnp.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021, ZJU and SenseTime 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ZJU and SenseTime nor the names of its contributors 15 | // may be used to endorse or promote products derived from this software 16 | // without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Hailin Yu 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include "util/random.h" 39 | 40 | #include "interface/pnp_solver.h" 41 | 42 | int main(int argc, char** argv) { 43 | int num_test = 1000; 44 | 45 | for (int i = 0; i != num_test; ++i) { 46 | int num_correspondences = colmap::RandomInteger(0, 1000); 47 | std::vector points2D(num_correspondences); 48 | std::vector points3D(num_correspondences); 49 | std::vector priors(num_correspondences); 50 | for (int j = 0; j != num_correspondences; ++j) { 51 | points2D[j][0] = colmap::RandomReal(0, 600); 52 | points2D[j][1] = colmap::RandomReal(0, 600); 53 | points3D[j][0] = colmap::RandomReal(0, 1); 54 | points3D[j][1] = colmap::RandomReal(0, 1); 55 | points3D[j][2] = colmap::RandomReal(0, 1); 56 | priors[j] = colmap::RandomReal(0, 1); 57 | } 58 | std::string model_name = "SIMPLE_RADIAL"; 59 | std::vector params = {800.0, 300.0, 300.0, 0.001}; 60 | 61 | Eigen::Vector4d qvec; 62 | Eigen::Vector3d tvec; 63 | std::vector mask; 64 | 65 | size_t num_inliers = 0; 66 | if (sovle_pnp_ransac(points2D, points3D, model_name, params, qvec, tvec, 67 | num_inliers, 8.0, 0.1, 0.9999, 1000, &mask, 68 | colpnp::LORANSAC, colpnp::WEIGHT_SAMPLE, 69 | &priors)) { 70 | std::cout << "Inlier: " << num_inliers << std::endl; 71 | } else { 72 | std::cout << "Failed" << std::endl; 73 | } 74 | } 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /src/optim/sprt.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_OPTIM_SPRT_H_ 33 | #define COLMAP_SRC_OPTIM_SPRT_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace colmap { 40 | 41 | // Sequential Probability Ratio Test as proposed in 42 | // 43 | // "Randomized RANSAC with Sequential Probability Ratio Test", 44 | // Matas et al., 2005 45 | class SPRT { 46 | public: 47 | struct Options { 48 | // Probability of rejecting a good model. 49 | double delta = 0.01; 50 | 51 | // A priori assumed minimum inlier ratio 52 | double epsilon = 0.1; 53 | 54 | // The ratio of the time it takes to estimate a model from a random 55 | // sample over the time it takes to decide whether one data sample is an 56 | // inlier or not. Matas et al. propose 200 for the 7-point algorithm. 57 | double eval_time_ratio = 200; 58 | 59 | // Number of models per random sample, that have to be verified. E.g. 60 | // 1-3 for the 7-point fundamental matrix algorithm, or 1-10 for the 61 | // 5-point essential matrix algorithm. 62 | int num_models_per_sample = 1; 63 | }; 64 | 65 | explicit SPRT(const Options& options); 66 | 67 | void Update(const Options& options); 68 | 69 | bool Evaluate(const std::vector& residuals, 70 | const double max_residual, size_t* num_inliers, 71 | size_t* num_eval_samples); 72 | 73 | private: 74 | void UpdateDecisionThreshold(); 75 | 76 | Options options_; 77 | double delta_epsilon_; 78 | double delta_1_epsilon_1_; 79 | double decision_threshold_; 80 | }; 81 | 82 | } // namespace colmap 83 | 84 | #endif // COLMAP_SRC_OPTIM_SPRT_H_ 85 | -------------------------------------------------------------------------------- /src/optim/support_measurement.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "optim/support_measurement.h" 33 | 34 | namespace colmap { 35 | 36 | InlierSupportMeasurer::Support InlierSupportMeasurer::Evaluate( 37 | const std::vector& residuals, const double max_residual) { 38 | Support support; 39 | support.num_inliers = 0; 40 | support.residual_sum = 0; 41 | 42 | for (const auto residual : residuals) { 43 | if (residual <= max_residual) { 44 | support.num_inliers += 1; 45 | support.residual_sum += residual; 46 | } 47 | } 48 | 49 | return support; 50 | } 51 | 52 | bool InlierSupportMeasurer::Compare(const Support& support1, 53 | const Support& support2) { 54 | if (support1.num_inliers > support2.num_inliers) { 55 | return true; 56 | } else { 57 | return support1.num_inliers == support2.num_inliers && 58 | support1.residual_sum < support2.residual_sum; 59 | } 60 | } 61 | 62 | MEstimatorSupportMeasurer::Support MEstimatorSupportMeasurer::Evaluate( 63 | const std::vector& residuals, const double max_residual) { 64 | Support support; 65 | support.num_inliers = 0; 66 | support.score = 0; 67 | 68 | for (const auto residual : residuals) { 69 | if (residual <= max_residual) { 70 | support.num_inliers += 1; 71 | support.score += residual; 72 | } else { 73 | support.score += max_residual; 74 | } 75 | } 76 | 77 | return support; 78 | } 79 | 80 | bool MEstimatorSupportMeasurer::Compare(const Support& support1, 81 | const Support& support2) { 82 | return support1.score < support2.score; 83 | } 84 | } // namespace colmap 85 | -------------------------------------------------------------------------------- /src/optim/support_measurement.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_OPTIM_SUPPORT_MEASUREMENT_H_ 33 | #define COLMAP_SRC_OPTIM_SUPPORT_MEASUREMENT_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace colmap { 40 | 41 | // Measure the support of a model by counting the number of inliers and 42 | // summing all inlier residuals. The support is better if it has more inliers 43 | // and a smaller residual sum. 44 | struct InlierSupportMeasurer { 45 | struct Support { 46 | // The number of inliers. 47 | size_t num_inliers = 0; 48 | 49 | // The sum of all inlier residuals. 50 | double residual_sum = std::numeric_limits::max(); 51 | }; 52 | 53 | // Compute the support of the residuals. 54 | Support Evaluate(const std::vector& residuals, 55 | const double max_residual); 56 | 57 | // Compare the two supports and return the better support. 58 | bool Compare(const Support& support1, const Support& support2); 59 | }; 60 | 61 | // Measure the support of a model by its fitness to the data as used in MSAC. 62 | // A support is better if it has a smaller MSAC score. 63 | struct MEstimatorSupportMeasurer { 64 | struct Support { 65 | // The number of inliers. 66 | size_t num_inliers = 0; 67 | 68 | // The MSAC score, defined as the truncated sum of residuals. 69 | double score = std::numeric_limits::max(); 70 | }; 71 | 72 | // Compute the support of the residuals. 73 | Support Evaluate(const std::vector& residuals, 74 | const double max_residual); 75 | 76 | // Compare the two supports and return the better support. 77 | bool Compare(const Support& support1, const Support& support2); 78 | }; 79 | 80 | } // namespace colmap 81 | 82 | #endif // COLMAP_SRC_OPTIM_SUPPORT_MEASUREMENT_H_ 83 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 14 | # * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | # its contributors may be used to endorse or promote products derived 16 | # from this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | # 30 | # Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | if(IS_MSVC) 33 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /W3") 34 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W3") 35 | elseif(IS_GNU OR IS_CLANG) 36 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") 37 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall") 38 | endif() 39 | 40 | option(BUILD_SHARED_LIBS "Build shared libraries" ON) # default use dynamic 41 | 42 | set(LIBRARY_NAME pnpsolver) 43 | 44 | include_directories(${PROJECT_SOURCE_DIR}/src) 45 | 46 | aux_source_directory(base base_src) 47 | 48 | aux_source_directory(interface interface_src) 49 | 50 | aux_source_directory(estimators estimators_src) 51 | 52 | aux_source_directory(optim optim_src) 53 | 54 | aux_source_directory(util util_src) 55 | 56 | add_library(${LIBRARY_NAME} 57 | ${base_src} 58 | ${interface_src} 59 | ${estimators_src} 60 | ${optim_src} 61 | ${util_src}) 62 | set_target_properties(${LIBRARY_NAME} PROPERTIES POSITION_INDEPENDENT_CODE ON) 63 | add_library(PnpSolver::pnpsolver ALIAS ${LIBRARY_NAME}) 64 | 65 | if(TARGET ceres) 66 | target_link_libraries(${LIBRARY_NAME} PUBLIC ceres) 67 | else() 68 | target_link_libraries(${LIBRARY_NAME} PUBLIC Ceres::ceres) 69 | endif() 70 | 71 | if(TARGET Eigen3::Eigen) 72 | target_link_libraries(${LIBRARY_NAME} PUBLIC Eigen3::Eigen) 73 | else() 74 | target_include_directories(${LIBRARY_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS}) 75 | endif() 76 | 77 | target_include_directories(${LIBRARY_NAME} PUBLIC 78 | $ 79 | $ 80 | ) 81 | 82 | install(TARGETS ${LIBRARY_NAME} DESTINATION lib EXPORT ${LIBRARY_NAME}-targets) 83 | install(FILES ${PROJECT_SOURCE_DIR}/src/interface/pnp_solver.h DESTINATION include) 84 | 85 | install(EXPORT ${LIBRARY_NAME}-targets DESTINATION lib/cmake NAMESPACE PnpSolver::) 86 | 87 | include(CMakePackageConfigHelpers) 88 | configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/../cmake/PnpSolverConfig.cmake.in 89 | "${CMAKE_CURRENT_BINARY_DIR}/PnpSolverConfig.cmake" 90 | INSTALL_DESTINATION lib/cmake/ 91 | NO_CHECK_REQUIRED_COMPONENTS_MACRO 92 | ) 93 | 94 | install(FILES 95 | "${CMAKE_CURRENT_BINARY_DIR}/PnpSolverConfig.cmake" 96 | DESTINATION lib/cmake 97 | ) 98 | -------------------------------------------------------------------------------- /src/optim/sprt.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "optim/sprt.h" 33 | 34 | namespace colmap { 35 | 36 | SPRT::SPRT(const Options& options) { Update(options); } 37 | 38 | void SPRT::Update(const Options& options) { 39 | options_ = options; 40 | delta_epsilon_ = options.delta / options.epsilon; 41 | delta_1_epsilon_1_ = (1 - options.delta) / (1 - options.epsilon); 42 | UpdateDecisionThreshold(); 43 | } 44 | 45 | bool SPRT::Evaluate(const std::vector& residuals, 46 | const double max_residual, size_t* num_inliers, 47 | size_t* num_eval_samples) { 48 | num_inliers = 0; 49 | 50 | double likelihood_ratio = 1; 51 | 52 | for (size_t i = 0; i < residuals.size(); ++i) { 53 | if (std::abs(residuals[i]) <= max_residual) { 54 | num_inliers += 1; 55 | likelihood_ratio *= delta_epsilon_; 56 | } else { 57 | likelihood_ratio *= delta_1_epsilon_1_; 58 | } 59 | 60 | if (likelihood_ratio > decision_threshold_) { 61 | *num_eval_samples = i + 1; 62 | return false; 63 | } 64 | } 65 | 66 | *num_eval_samples = residuals.size(); 67 | 68 | return true; 69 | } 70 | 71 | void SPRT::UpdateDecisionThreshold() { 72 | // Equation 2 73 | const double C = 74 | (1 - options_.delta) * 75 | std::log((1 - options_.delta) / (1 - options_.epsilon)) + 76 | options_.delta * std::log(options_.delta / options_.epsilon); 77 | 78 | // Equation 6 79 | const double A0 = 80 | options_.eval_time_ratio * C / options_.num_models_per_sample + 1; 81 | 82 | double A = A0; 83 | 84 | const double kEps = 1.5e-8; 85 | 86 | // Compute A using the recursive relation 87 | // A* = lim(n->inf) A 88 | // The series typically converges within 4 iterations 89 | 90 | for (size_t i = 0; i < 100; ++i) { 91 | const double A1 = A0 + std::log(A); 92 | 93 | if (std::abs(A1 - A) < kEps) { 94 | break; 95 | } 96 | 97 | A = A1; 98 | } 99 | 100 | decision_threshold_ = A; 101 | } 102 | 103 | } // namespace colmap 104 | -------------------------------------------------------------------------------- /src/optim/sampler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_OPTIM_SAMPLER_H_ 33 | #define COLMAP_SRC_OPTIM_SAMPLER_H_ 34 | 35 | #include 36 | #include 37 | 38 | namespace colmap { 39 | 40 | // Abstract base class for sampling methods. 41 | class Sampler { 42 | public: 43 | Sampler(){}; 44 | explicit Sampler(const size_t num_samples); 45 | 46 | // Initialize the sampler, before calling the `Sample` method. 47 | virtual void Initialize(const size_t total_num_samples) = 0; 48 | 49 | virtual void SetPriors(const std::vector &priors) {} 50 | 51 | // Maximum number of unique samples that can be generated. 52 | virtual size_t MaxNumSamples() = 0; 53 | 54 | // Sample `num_samples` elements from all samples. 55 | virtual std::vector Sample() = 0; 56 | 57 | // Sample elements from `X` into `X_rand`. 58 | // 59 | // Note that `X.size()` should equal `num_total_samples` and `X_rand.size()` 60 | // should equal `num_samples`. 61 | template 62 | void SampleX(const X_t &X, X_t *X_rand); 63 | 64 | // Sample elements from `X` and `Y` into `X_rand` and `Y_rand`. 65 | // 66 | // Note that `X.size()` should equal `num_total_samples` and `X_rand.size()` 67 | // should equal `num_samples`. The same applies for `Y` and `Y_rand`. 68 | template 69 | void SampleXY(const X_t &X, const Y_t &Y, X_t *X_rand, Y_t *Y_rand); 70 | }; 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | // Implementation 74 | //////////////////////////////////////////////////////////////////////////////// 75 | 76 | template 77 | void Sampler::SampleX(const X_t &X, X_t *X_rand) { 78 | const auto sample_idxs = Sample(); 79 | for (size_t i = 0; i < X_rand->size(); ++i) { 80 | (*X_rand)[i] = X[sample_idxs[i]]; 81 | } 82 | } 83 | 84 | template 85 | void Sampler::SampleXY(const X_t &X, const Y_t &Y, X_t *X_rand, Y_t *Y_rand) { 86 | // CHECK_EQ(X.size(), Y.size()); 87 | // CHECK_EQ(X_rand->size(), Y_rand->size()); 88 | const auto sample_idxs = Sample(); 89 | for (size_t i = 0; i < X_rand->size(); ++i) { 90 | (*X_rand)[i] = X[sample_idxs[i]]; 91 | (*Y_rand)[i] = Y[sample_idxs[i]]; 92 | } 93 | } 94 | 95 | } // namespace colmap 96 | 97 | #endif // COLMAP_SRC_OPTIM_SAMPLER_H_ 98 | -------------------------------------------------------------------------------- /src/util/matrix.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_MATRIX_H_ 33 | #define COLMAP_SRC_UTIL_MATRIX_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace colmap { 40 | 41 | // Check if the given floating point array contains a NaN value. 42 | template 43 | inline bool IsNaN(const Eigen::MatrixBase& x); 44 | 45 | // Check if the given floating point array contains infinity. 46 | template 47 | inline bool IsInf(const Eigen::MatrixBase& x); 48 | 49 | // Perform RQ decomposition on matrix. The RQ decomposition transforms a matrix 50 | // A into the product of an upper triangular matrix R (also known as 51 | // right-triangular) and an orthogonal matrix Q. 52 | template 53 | void DecomposeMatrixRQ(const MatrixType& A, MatrixType* R, MatrixType* Q); 54 | 55 | //////////////////////////////////////////////////////////////////////////////// 56 | // Implementation 57 | //////////////////////////////////////////////////////////////////////////////// 58 | 59 | template 60 | bool IsNaN(const Eigen::MatrixBase& x) { 61 | return !(x.array() == x.array()).all(); 62 | } 63 | 64 | template 65 | bool IsInf(const Eigen::MatrixBase& x) { 66 | return !((x - x).array() == (x - x).array()).all(); 67 | } 68 | 69 | template 70 | void DecomposeMatrixRQ(const MatrixType& A, MatrixType* R, MatrixType* Q) { 71 | const MatrixType A_flipud_transpose = 72 | A.transpose().rowwise().reverse().eval(); 73 | 74 | const Eigen::HouseholderQR QR(A_flipud_transpose); 75 | const MatrixType& Q0 = QR.householderQ(); 76 | const MatrixType& R0 = QR.matrixQR(); 77 | 78 | *R = R0.transpose().colwise().reverse().eval(); 79 | *R = R->rowwise().reverse().eval(); 80 | for (int i = 0; i < R->rows(); ++i) { 81 | for (int j = 0; j < R->cols() && (R->cols() - j) > (R->rows() - i); 82 | ++j) { 83 | (*R)(i, j) = 0; 84 | } 85 | } 86 | 87 | *Q = Q0.transpose().colwise().reverse().eval(); 88 | 89 | // Make the decomposition unique by requiring that det(Q) > 0. 90 | if (Q->determinant() < 0) { 91 | Q->row(1) *= -1.0; 92 | R->col(1) *= -1.0; 93 | } 94 | } 95 | 96 | } // namespace colmap 97 | 98 | #endif // COLMAP_SRC_UTIL_MATRIX_H_ 99 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 14 | # * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | # its contributors may be used to endorse or promote products derived 16 | # from this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | # 30 | # Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | cmake_minimum_required(VERSION 3.16) 33 | project(pnpsolver VERSION 1.0.0) 34 | 35 | add_compile_options(-std=c++14) 36 | add_compile_options(-fPIC) 37 | add_compile_options(-O3) 38 | 39 | # ############################################################################### 40 | # Options 41 | # ############################################################################### 42 | option(SIMD_ENABLED "Whether to enable SIMD optimizations" ON) 43 | option(OPENMP_ENABLED "Whether to enable OpenMP parallelization" ON) 44 | option(BUILD_TEST "Whether to enable test" OFF) 45 | 46 | # ############################################################################### 47 | # Find packages 48 | # ############################################################################### 49 | # when use for add_subdirectory(), need external install Ceres and Eigen3 50 | if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_CURRENT_SOURCE_DIR}) 51 | find_package(Ceres REQUIRED) 52 | find_package(Eigen3 REQUIRED) 53 | endif() 54 | 55 | 56 | # ############################################################################### 57 | # Compiler specific configuration 58 | # ############################################################################### 59 | if(CMAKE_BUILD_TYPE) 60 | message(STATUS "Build type specified as ${CMAKE_BUILD_TYPE}") 61 | else() 62 | message(STATUS "Build type not specified, using Release") 63 | set(CMAKE_BUILD_TYPE Release) 64 | set(IS_DEBUG OFF) 65 | endif() 66 | 67 | if(IS_MSVC) 68 | # Some fixes for the Glog library. 69 | add_definitions("-DGLOG_NO_ABBREVIATED_SEVERITIES") 70 | add_definitions("-DGL_GLEXT_PROTOTYPES") 71 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc") 72 | 73 | # Enable object level parallel builds in Visual Studio. 74 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP") 75 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") 76 | endif() 77 | 78 | if(IS_GNU) 79 | # Hide incorrect warnings for uninitialized Eigen variables under GCC. 80 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-maybe-uninitialized") 81 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-maybe-uninitialized") 82 | endif() 83 | 84 | if(IS_DEBUG) 85 | add_definitions("-DEIGEN_INITIALIZE_MATRICES_BY_NAN") 86 | endif() 87 | 88 | # ############################################################################### 89 | # Add sources 90 | # ############################################################################### 91 | add_subdirectory(src) 92 | 93 | if(BUILD_TEST) 94 | add_subdirectory(test) 95 | endif() -------------------------------------------------------------------------------- /src/util/string.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "util/string.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace colmap { 40 | namespace { 41 | 42 | bool IsNotWhiteSpace(const int character) { 43 | return character != ' ' && character != '\n' && character != '\r' && 44 | character != '\t'; 45 | } 46 | 47 | } // namespace 48 | 49 | std::string StringReplace(const std::string& str, const std::string& old_str, 50 | const std::string& new_str) { 51 | if (old_str.empty()) { 52 | return str; 53 | } 54 | size_t position = 0; 55 | std::string mod_str = str; 56 | while ((position = mod_str.find(old_str, position)) != std::string::npos) { 57 | mod_str.replace(position, old_str.size(), new_str); 58 | position += new_str.size(); 59 | } 60 | return mod_str; 61 | } 62 | 63 | std::string StringGetAfter(const std::string& str, const std::string& key) { 64 | if (key.empty()) { 65 | return str; 66 | } 67 | std::size_t found = str.rfind(key); 68 | if (found != std::string::npos) { 69 | return str.substr(found + key.length(), 70 | str.length() - (found + key.length())); 71 | } 72 | return ""; 73 | } 74 | 75 | bool StringStartsWith(const std::string& str, const std::string& prefix) { 76 | return !prefix.empty() && prefix.size() <= str.size() && 77 | str.substr(0, prefix.size()) == prefix; 78 | } 79 | 80 | void StringLeftTrim(std::string* str) { 81 | str->erase(str->begin(), 82 | std::find_if(str->begin(), str->end(), IsNotWhiteSpace)); 83 | } 84 | 85 | void StringRightTrim(std::string* str) { 86 | str->erase(std::find_if(str->rbegin(), str->rend(), IsNotWhiteSpace).base(), 87 | str->end()); 88 | } 89 | 90 | void StringTrim(std::string* str) { 91 | StringLeftTrim(str); 92 | StringRightTrim(str); 93 | } 94 | 95 | void StringToLower(std::string* str) { 96 | std::transform(str->begin(), str->end(), str->begin(), ::tolower); 97 | } 98 | 99 | void StringToUpper(std::string* str) { 100 | std::transform(str->begin(), str->end(), str->begin(), ::toupper); 101 | } 102 | 103 | bool StringContains(const std::string& str, const std::string& sub_str) { 104 | return str.find(sub_str) != std::string::npos; 105 | } 106 | 107 | } // namespace colmap 108 | -------------------------------------------------------------------------------- /src/estimators/utils.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_ESTIMATORS_UTILS_H_ 33 | #define COLMAP_SRC_ESTIMATORS_UTILS_H_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "util/alignment.h" 40 | #include "util/types.h" 41 | 42 | namespace colmap { 43 | 44 | // Center and normalize image points. 45 | // 46 | // The points are transformed in a two-step procedure that is expressed 47 | // as a transformation matrix. The matrix of the resulting points is usually 48 | // better conditioned than the matrix of the original points. 49 | // 50 | // Center the image points, such that the new coordinate system has its 51 | // origin at the centroid of the image points. 52 | // 53 | // Normalize the image points, such that the mean distance from the points 54 | // to the coordinate system is sqrt(2). 55 | // 56 | // @param points Image coordinates. 57 | // @param normed_points Transformed image coordinates. 58 | // @param matrix 3x3 transformation matrix. 59 | void CenterAndNormalizeImagePoints(const std::vector& points, 60 | std::vector* normed_points, 61 | Eigen::Matrix3d* matrix); 62 | 63 | // Calculate the residuals of a set of corresponding points and a given 64 | // fundamental or essential matrix. 65 | // 66 | // Residuals are defined as the squared Sampson error. 67 | // 68 | // @param points1 First set of corresponding points as Nx2 matrix. 69 | // @param points2 Second set of corresponding points as Nx2 matrix. 70 | // @param E 3x3 fundamental or essential matrix. 71 | // @param residuals Output vector of residuals. 72 | void ComputeSquaredSampsonError(const std::vector& points1, 73 | const std::vector& points2, 74 | const Eigen::Matrix3d& E, 75 | std::vector* residuals); 76 | 77 | // Calculate the squared reprojection error given a set of 2D-3D point 78 | // correspondences and a projection matrix. Returns DBL_MAX if a 3D point is 79 | // behind the given camera. 80 | // 81 | // @param points2D Normalized 2D image points. 82 | // @param points3D 3D world points. 83 | // @param proj_matrix 3x4 projection matrix. 84 | // @param residuals Output vector of residuals. 85 | void ComputeSquaredReprojectionError( 86 | const std::vector& points2D, 87 | const std::vector& points3D, 88 | const Eigen::Matrix3x4d& proj_matrix, std::vector* residuals); 89 | 90 | } // namespace colmap 91 | 92 | #endif // COLMAP_SRC_ESTIMATORS_UTILS_H_ 93 | -------------------------------------------------------------------------------- /src/optim/progressive_sampler.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "optim/progressive_sampler.h" 33 | 34 | #include 35 | 36 | #include "util/misc.h" 37 | #include "util/random.h" 38 | 39 | namespace colmap { 40 | 41 | ProgressiveSampler::ProgressiveSampler(const size_t num_samples) 42 | : num_samples_(num_samples), 43 | total_num_samples_(0), 44 | t_(0), 45 | n_(0), 46 | T_n_(0), 47 | T_n_p_(0) {} 48 | 49 | void ProgressiveSampler::Initialize(const size_t total_num_samples) { 50 | // CHECK_LE(num_samples_, total_num_samples); 51 | total_num_samples_ = total_num_samples; 52 | 53 | t_ = 0; 54 | n_ = num_samples_; 55 | 56 | // Number of iterations before PROSAC behaves like RANSAC. Default value 57 | // is chosen according to the recommended value in the paper. 58 | const size_t kNumProgressiveIterations = 200000; 59 | 60 | // Compute T_n using recurrent relation in equation 3 (first part). 61 | T_n_ = kNumProgressiveIterations; 62 | T_n_p_ = 1.0; 63 | for (size_t i = 0; i < num_samples_; ++i) { 64 | T_n_ *= 65 | static_cast(num_samples_ - i) / (total_num_samples_ - i); 66 | } 67 | } 68 | 69 | size_t ProgressiveSampler::MaxNumSamples() { 70 | return std::numeric_limits::max(); 71 | } 72 | 73 | std::vector ProgressiveSampler::Sample() { 74 | t_ += 1; 75 | 76 | // Compute T_n_p_ using recurrent relation in equation 3 (second part). 77 | if (t_ == T_n_p_ && n_ < total_num_samples_) { 78 | const double T_n_plus_1 = T_n_ * (n_ + 1.0) / (n_ + 1.0 - num_samples_); 79 | T_n_p_ += std::ceil(T_n_plus_1 - T_n_); 80 | T_n_ = T_n_plus_1; 81 | n_ += 1; 82 | } 83 | 84 | // Decide how many samples to draw from which part of the data as 85 | // specified in equation 5. 86 | size_t num_random_samples = num_samples_; 87 | size_t max_random_sample_idx = n_ - 1; 88 | if (T_n_p_ >= t_) { 89 | num_random_samples -= 1; 90 | max_random_sample_idx -= 1; 91 | } 92 | 93 | // Draw semi-random samples as described in algorithm 1. 94 | std::vector sampled_idxs; 95 | sampled_idxs.reserve(num_samples_); 96 | for (size_t i = 0; i < num_random_samples; ++i) { 97 | while (true) { 98 | const size_t random_idx = 99 | RandomInteger(0, max_random_sample_idx); 100 | if (!VectorContainsValue(sampled_idxs, random_idx)) { 101 | sampled_idxs.push_back(random_idx); 102 | break; 103 | } 104 | } 105 | } 106 | 107 | // In progressive sampling mode, the last element is mandatory. 108 | if (T_n_p_ >= t_) { 109 | sampled_idxs.push_back(n_); 110 | } 111 | 112 | return sampled_idxs; 113 | } 114 | 115 | } // namespace colmap 116 | -------------------------------------------------------------------------------- /src/base/polynomial.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_BASE_POLYNOMIAL_H_ 33 | #define COLMAP_SRC_BASE_POLYNOMIAL_H_ 34 | 35 | #include 36 | 37 | namespace colmap { 38 | 39 | // All polynomials are assumed to be the form: 40 | // 41 | // sum_{i=0}^N polynomial(i) x^{N-i}. 42 | // 43 | // and are given by a vector of coefficients of size N + 1. 44 | // 45 | // The implementation is based on COLMAP's old polynomial functionality and is 46 | // inspired by Ceres-Solver's/Theia's implementation to support complex 47 | // polynomials. The companion matrix implementation is based on NumPy. 48 | 49 | // Evaluate the polynomial for the given coefficients at x using the Horner 50 | // scheme. This function is templated such that the polynomial may be evaluated 51 | // at real and/or imaginary points. 52 | template 53 | T EvaluatePolynomial(const Eigen::VectorXd& coeffs, const T& x); 54 | 55 | // Find the root of polynomials of the form: a * x + b = 0. 56 | // The real and/or imaginary variable may be NULL if the output is not needed. 57 | bool FindLinearPolynomialRoots(const Eigen::VectorXd& coeffs, 58 | Eigen::VectorXd* real, Eigen::VectorXd* imag); 59 | 60 | // Find the roots of polynomials of the form: a * x^2 + b * x + c = 0. 61 | // The real and/or imaginary variable may be NULL if the output is not needed. 62 | bool FindQuadraticPolynomialRoots(const Eigen::VectorXd& coeffs, 63 | Eigen::VectorXd* real, Eigen::VectorXd* imag); 64 | 65 | // Find the roots of a polynomial using the Durand-Kerner method, based on: 66 | // 67 | // https://en.wikipedia.org/wiki/Durand%E2%80%93Kerner_method 68 | // 69 | // The Durand-Kerner is comparatively fast but often unstable/inaccurate. 70 | // The real and/or imaginary variable may be NULL if the output is not needed. 71 | bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd& coeffs, 72 | Eigen::VectorXd* real, 73 | Eigen::VectorXd* imag); 74 | 75 | // Find the roots of a polynomial using the companion matrix method, based on: 76 | // 77 | // R. A. Horn & C. R. Johnson, Matrix Analysis. Cambridge, 78 | // UK: Cambridge University Press, 1999, pp. 146-7. 79 | // 80 | // Compared to Durand-Kerner, this method is slower but more stable/accurate. 81 | // The real and/or imaginary variable may be NULL if the output is not needed. 82 | bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd& coeffs, 83 | Eigen::VectorXd* real, 84 | Eigen::VectorXd* imag); 85 | 86 | //////////////////////////////////////////////////////////////////////////////// 87 | // Implementation 88 | //////////////////////////////////////////////////////////////////////////////// 89 | 90 | template 91 | T EvaluatePolynomial(const Eigen::VectorXd& coeffs, const T& x) { 92 | T value = 0.0; 93 | for (Eigen::VectorXd::Index i = 0; i < coeffs.size(); ++i) { 94 | value = value * x + coeffs(i); 95 | } 96 | return value; 97 | } 98 | 99 | } // namespace colmap 100 | 101 | #endif // COLMAP_SRC_BASE_POLYNOMIAL_H_ 102 | -------------------------------------------------------------------------------- /src/util/types.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_TYPES_H_ 33 | #define COLMAP_SRC_UTIL_TYPES_H_ 34 | 35 | #include "util/alignment.h" 36 | 37 | #ifdef _MSC_VER 38 | #if _MSC_VER >= 1600 39 | #include 40 | #else 41 | typedef __int8 int8_t; 42 | typedef __int16 int16_t; 43 | typedef __int32 int32_t; 44 | typedef __int64 int64_t; 45 | typedef unsigned __int8 uint8_t; 46 | typedef unsigned __int16 uint16_t; 47 | typedef unsigned __int32 uint32_t; 48 | typedef unsigned __int64 uint64_t; 49 | #endif 50 | #elif __GNUC__ >= 3 51 | #include 52 | #endif 53 | 54 | // Define non-copyable or non-movable classes. 55 | #define NON_COPYABLE(class_name) \ 56 | class_name(class_name const&) = delete; \ 57 | void operator=(class_name const& obj) = delete; 58 | #define NON_MOVABLE(class_name) class_name(class_name&&) = delete; 59 | 60 | #include 61 | 62 | namespace Eigen { 63 | 64 | typedef Eigen::Matrix Matrix3x4f; 65 | typedef Eigen::Matrix Matrix3x4d; 66 | typedef Eigen::Matrix Vector3ub; 67 | typedef Eigen::Matrix Vector4ub; 68 | typedef Eigen::Matrix Vector6d; 69 | 70 | } // namespace Eigen 71 | 72 | namespace colmap { 73 | 74 | //////////////////////////////////////////////////////////////////////////////// 75 | // Index types, determines the maximum number of objects. 76 | //////////////////////////////////////////////////////////////////////////////// 77 | 78 | // Unique identifier for cameras. 79 | typedef uint32_t camera_t; 80 | 81 | // Unique identifier for images. 82 | typedef uint32_t image_t; 83 | 84 | // Each image pair gets a unique ID, see `Database::ImagePairToPairId`. 85 | typedef uint64_t image_pair_t; 86 | 87 | // Index per image, i.e. determines maximum number of 2D points per image. 88 | typedef uint32_t point2D_t; 89 | 90 | // Unique identifier per added 3D point. Since we add many 3D points, 91 | // delete them, and possibly re-add them again, the maximum number of allowed 92 | // unique indices should be large. 93 | typedef uint64_t point3D_t; 94 | 95 | // Values for invalid identifiers or indices. 96 | const camera_t kInvalidCameraId = std::numeric_limits::max(); 97 | const image_t kInvalidImageId = std::numeric_limits::max(); 98 | const image_pair_t kInvalidImagePairId = 99 | std::numeric_limits::max(); 100 | const point2D_t kInvalidPoint2DIdx = std::numeric_limits::max(); 101 | const point3D_t kInvalidPoint3DId = std::numeric_limits::max(); 102 | 103 | } // namespace colmap 104 | 105 | // This file provides specializations of the templated hash function for 106 | // custom types. These are used for comparison in unordered sets/maps. 107 | namespace std { 108 | 109 | // Hash function specialization for uint32_t pairs, e.g., image_t or camera_t. 110 | template <> 111 | struct hash> { 112 | std::size_t operator()(const std::pair& p) const { 113 | const uint64_t s = (static_cast(p.first) << 32) + 114 | static_cast(p.second); 115 | return std::hash()(s); 116 | } 117 | }; 118 | 119 | } // namespace std 120 | 121 | #endif // COLMAP_SRC_UTIL_TYPES_H_ 122 | -------------------------------------------------------------------------------- /src/util/random.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_RANDOM_H_ 33 | #define COLMAP_SRC_UTIL_RANDOM_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "util/threading.h" 41 | 42 | namespace colmap { 43 | 44 | extern thread_local std::mt19937 *PRNG; 45 | 46 | static int kDefaultPRNGSeed = 0; 47 | 48 | // Initialize the PRNG with the given seed. 49 | // 50 | // @param seed The seed for the PRNG. If the seed is -1, the current time 51 | // is used as the seed. 52 | void SetPRNGSeed(unsigned seed = kDefaultPRNGSeed); 53 | 54 | // Generate uniformly distributed random integer number. 55 | // 56 | // This implementation is unbiased and thread-safe in contrast to `rand()`. 57 | template 58 | T RandomInteger(const T min, const T max); 59 | 60 | // Generate uniformly distributed random real number. 61 | // 62 | // This implementation is unbiased and thread-safe in contrast to `rand()`. 63 | template 64 | T RandomReal(const T min, const T max); 65 | 66 | // Generate Gaussian distributed random real number. 67 | // 68 | // This implementation is unbiased and thread-safe in contrast to `rand()`. 69 | template 70 | T RandomGaussian(const T mean, const T stddev); 71 | 72 | // Fisher-Yates shuffling. 73 | // 74 | // Note that the vector may not contain more values than UINT32_MAX. This 75 | // restriction comes from the fact that the 32-bit version of the 76 | // Mersenne Twister PRNG is significantly faster. 77 | // 78 | // @param elems Vector of elements to shuffle. 79 | // @param num_to_shuffle Optional parameter, specifying the number of first 80 | // N elements in the vector to shuffle. 81 | template 82 | void Shuffle(const uint32_t num_to_shuffle, std::vector *elems); 83 | 84 | // Weighted Sampling 85 | template 86 | std::vector WeightedRandomSample(const std::vector &prob, 87 | size_t size); 88 | 89 | //////////////////////////////////////////////////////////////////////////////// 90 | // Implementation 91 | //////////////////////////////////////////////////////////////////////////////// 92 | 93 | template 94 | T RandomInteger(const T min, const T max) { 95 | if (PRNG == nullptr) { 96 | SetPRNGSeed(); 97 | } 98 | 99 | std::uniform_int_distribution distribution(min, max); 100 | 101 | return distribution(*PRNG); 102 | } 103 | 104 | template 105 | T RandomReal(const T min, const T max) { 106 | if (PRNG == nullptr) { 107 | SetPRNGSeed(); 108 | } 109 | 110 | std::uniform_real_distribution distribution(min, max); 111 | 112 | return distribution(*PRNG); 113 | } 114 | 115 | template 116 | T RandomGaussian(const T mean, const T stddev) { 117 | if (PRNG == nullptr) { 118 | SetPRNGSeed(); 119 | } 120 | 121 | std::normal_distribution distribution(mean, stddev); 122 | return distribution(*PRNG); 123 | } 124 | 125 | template 126 | void Shuffle(const uint32_t num_to_shuffle, std::vector *elems) { 127 | // CHECK_LE(num_to_shuffle, elems->size()); 128 | const uint32_t last_idx = static_cast(elems->size() - 1); 129 | for (uint32_t i = 0; i < num_to_shuffle; ++i) { 130 | const auto j = RandomInteger(i, last_idx); 131 | std::swap((*elems)[i], (*elems)[j]); 132 | } 133 | } 134 | 135 | template 136 | std::vector WeightedRandomSample(const std::vector &prob, 137 | size_t size) { 138 | // CHECK_LE(size, prob.size()); 139 | std::discrete_distribution distribution(prob.begin(), prob.end()); 140 | 141 | if (PRNG == nullptr) { 142 | SetPRNGSeed(); 143 | } 144 | 145 | std::unordered_set udset; 146 | while (udset.size() < size) { 147 | size_t number = static_cast(distribution(*PRNG)); 148 | udset.insert(number); 149 | } 150 | 151 | return std::vector(udset.begin(), udset.end()); 152 | } 153 | 154 | } // namespace colmap 155 | 156 | #endif // COLMAP_SRC_UTIL_RANDOM_H_ 157 | -------------------------------------------------------------------------------- /src/util/alignment.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_ALIGNMENT_H_ 33 | #define COLMAP_SRC_UTIL_ALIGNMENT_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | #ifndef EIGEN_ALIGNED_ALLOCATOR 44 | #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator 45 | #endif 46 | 47 | // Equivalent to EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION but with support for 48 | // initializer lists, which is a C++11 feature and not supported by the Eigen. 49 | // The initializer list extension is inspired by Theia and StackOverflow code. 50 | #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(...) \ 51 | namespace std { \ 52 | template <> \ 53 | class vector<__VA_ARGS__, std::allocator<__VA_ARGS__>> \ 54 | : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> { \ 55 | typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> \ 56 | vector_base; \ 57 | \ 58 | public: \ 59 | typedef __VA_ARGS__ value_type; \ 60 | typedef vector_base::allocator_type allocator_type; \ 61 | typedef vector_base::size_type size_type; \ 62 | typedef vector_base::iterator iterator; \ 63 | explicit vector(const allocator_type& a = allocator_type()) \ 64 | : vector_base(a) {} \ 65 | template \ 66 | vector(InputIterator first, InputIterator last, \ 67 | const allocator_type& a = allocator_type()) \ 68 | : vector_base(first, last, a) {} \ 69 | vector(const vector& c) : vector_base(c) {} \ 70 | explicit vector(size_type num, const value_type& val = value_type()) \ 71 | : vector_base(num, val) {} \ 72 | vector(iterator start, iterator end) : vector_base(start, end) {} \ 73 | vector& operator=(const vector& x) { \ 74 | vector_base::operator=(x); \ 75 | return *this; \ 76 | } \ 77 | vector(initializer_list<__VA_ARGS__> list) \ 78 | : vector_base(list.begin(), list.end()) {} \ 79 | }; \ 80 | } // namespace std 81 | 82 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector2d) 83 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4d) 84 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4f) 85 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2d) 86 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2f) 87 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4d) 88 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4f) 89 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3d) 90 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3f) 91 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaterniond) 92 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaternionf) 93 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) 94 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) 95 | 96 | #define EIGEN_STL_UMAP(KEY, VALUE) \ 97 | std::unordered_map, std::equal_to, \ 98 | Eigen::aligned_allocator>> 99 | 100 | #endif // COLMAP_SRC_UTIL_ALIGNMENT_H_ 101 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: None 7 | AlignConsecutiveAssignments: None 8 | AlignConsecutiveBitFields: None 9 | AlignConsecutiveDeclarations: None 10 | AlignEscapedNewlines: Left 11 | AlignOperands: Align 12 | AlignTrailingComments: true 13 | AllowAllArgumentsOnNextLine: true 14 | AllowAllConstructorInitializersOnNextLine: true 15 | AllowAllParametersOfDeclarationOnNextLine: true 16 | AllowShortEnumsOnASingleLine: true 17 | AllowShortBlocksOnASingleLine: Never 18 | AllowShortCaseLabelsOnASingleLine: false 19 | AllowShortFunctionsOnASingleLine: All 20 | AllowShortLambdasOnASingleLine: All 21 | AllowShortIfStatementsOnASingleLine: WithoutElse 22 | AllowShortLoopsOnASingleLine: true 23 | AlwaysBreakAfterDefinitionReturnType: None 24 | AlwaysBreakAfterReturnType: None 25 | AlwaysBreakBeforeMultilineStrings: true 26 | AlwaysBreakTemplateDeclarations: Yes 27 | AttributeMacros: 28 | - __capability 29 | BinPackArguments: true 30 | BinPackParameters: true 31 | BraceWrapping: 32 | AfterCaseLabel: false 33 | AfterClass: false 34 | AfterControlStatement: Never 35 | AfterEnum: false 36 | AfterFunction: false 37 | AfterNamespace: false 38 | AfterObjCDeclaration: false 39 | AfterStruct: false 40 | AfterUnion: false 41 | AfterExternBlock: false 42 | BeforeCatch: false 43 | BeforeElse: false 44 | BeforeLambdaBody: false 45 | BeforeWhile: false 46 | IndentBraces: false 47 | SplitEmptyFunction: true 48 | SplitEmptyRecord: true 49 | SplitEmptyNamespace: true 50 | BreakBeforeBinaryOperators: None 51 | BreakBeforeConceptDeclarations: true 52 | BreakBeforeBraces: Attach 53 | BreakBeforeInheritanceComma: false 54 | BreakInheritanceList: BeforeColon 55 | BreakBeforeTernaryOperators: true 56 | BreakConstructorInitializersBeforeComma: false 57 | BreakConstructorInitializers: BeforeColon 58 | BreakAfterJavaFieldAnnotations: false 59 | BreakStringLiterals: true 60 | ColumnLimit: 80 61 | CommentPragmas: "^ IWYU pragma:" 62 | CompactNamespaces: false 63 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 64 | ConstructorInitializerIndentWidth: 4 65 | ContinuationIndentWidth: 4 66 | Cpp11BracedListStyle: true 67 | DeriveLineEnding: true 68 | DerivePointerAlignment: true 69 | DisableFormat: false 70 | EmptyLineBeforeAccessModifier: LogicalBlock 71 | ExperimentalAutoDetectBinPacking: false 72 | FixNamespaceComments: true 73 | ForEachMacros: 74 | - foreach 75 | - Q_FOREACH 76 | - BOOST_FOREACH 77 | StatementAttributeLikeMacros: 78 | - Q_EMIT 79 | IncludeBlocks: Regroup 80 | IncludeCategories: 81 | - Regex: '^' 82 | Priority: 2 83 | SortPriority: 0 84 | CaseSensitive: false 85 | - Regex: '^<.*\.h>' 86 | Priority: 1 87 | SortPriority: 0 88 | CaseSensitive: false 89 | - Regex: "^<.*" 90 | Priority: 2 91 | SortPriority: 0 92 | CaseSensitive: false 93 | - Regex: ".*" 94 | Priority: 3 95 | SortPriority: 0 96 | CaseSensitive: false 97 | IncludeIsMainRegex: "([-_](test|unittest))?$" 98 | IncludeIsMainSourceRegex: "" 99 | IndentCaseLabels: true 100 | IndentCaseBlocks: false 101 | IndentGotoLabels: true 102 | IndentPPDirectives: None 103 | IndentExternBlock: AfterExternBlock 104 | IndentRequires: false 105 | IndentWidth: 4 106 | IndentWrappedFunctionNames: false 107 | InsertTrailingCommas: None 108 | JavaScriptQuotes: Leave 109 | JavaScriptWrapImports: true 110 | KeepEmptyLinesAtTheStartOfBlocks: false 111 | MacroBlockBegin: "" 112 | MacroBlockEnd: "" 113 | MaxEmptyLinesToKeep: 1 114 | NamespaceIndentation: None 115 | ObjCBinPackProtocolList: Never 116 | ObjCBlockIndentWidth: 2 117 | ObjCBreakBeforeNestedBlockParam: true 118 | ObjCSpaceAfterProperty: false 119 | ObjCSpaceBeforeProtocolList: true 120 | PenaltyBreakAssignment: 2 121 | PenaltyBreakBeforeFirstCallParameter: 1 122 | PenaltyBreakComment: 300 123 | PenaltyBreakFirstLessLess: 120 124 | PenaltyBreakString: 1000 125 | PenaltyBreakTemplateDeclaration: 10 126 | PenaltyExcessCharacter: 1000000 127 | PenaltyReturnTypeOnItsOwnLine: 200 128 | PenaltyIndentedWhitespace: 0 129 | PointerAlignment: Left 130 | RawStringFormats: 131 | - Language: Cpp 132 | Delimiters: 133 | - cc 134 | - CC 135 | - cpp 136 | - Cpp 137 | - CPP 138 | - "c++" 139 | - "C++" 140 | CanonicalDelimiter: "" 141 | BasedOnStyle: google 142 | - Language: TextProto 143 | Delimiters: 144 | - pb 145 | - PB 146 | - proto 147 | - PROTO 148 | EnclosingFunctions: 149 | - EqualsProto 150 | - EquivToProto 151 | - PARSE_PARTIAL_TEXT_PROTO 152 | - PARSE_TEST_PROTO 153 | - PARSE_TEXT_PROTO 154 | - ParseTextOrDie 155 | - ParseTextProtoOrDie 156 | - ParseTestProto 157 | - ParsePartialTestProto 158 | CanonicalDelimiter: "" 159 | BasedOnStyle: google 160 | ReflowComments: true 161 | SortIncludes: false 162 | SortJavaStaticImport: Before 163 | SortUsingDeclarations: true 164 | SpaceAfterCStyleCast: false 165 | SpaceAfterLogicalNot: false 166 | SpaceAfterTemplateKeyword: true 167 | SpaceBeforeAssignmentOperators: true 168 | SpaceBeforeCaseColon: false 169 | SpaceBeforeCpp11BracedList: false 170 | SpaceBeforeCtorInitializerColon: true 171 | SpaceBeforeInheritanceColon: true 172 | SpaceBeforeParens: ControlStatements 173 | SpaceAroundPointerQualifiers: Default 174 | SpaceBeforeRangeBasedForLoopColon: true 175 | SpaceInEmptyBlock: false 176 | SpaceInEmptyParentheses: false 177 | SpacesBeforeTrailingComments: 2 178 | SpacesInAngles: false 179 | SpacesInConditionalStatement: false 180 | SpacesInContainerLiterals: true 181 | SpacesInCStyleCastParentheses: false 182 | SpacesInParentheses: false 183 | SpacesInSquareBrackets: false 184 | SpaceBeforeSquareBrackets: false 185 | BitFieldColonSpacing: Both 186 | Standard: Auto 187 | StatementMacros: 188 | - Q_UNUSED 189 | - QT_REQUIRE_VERSION 190 | TabWidth: 8 191 | UseCRLF: false 192 | UseTab: Never 193 | WhitespaceSensitiveMacros: 194 | - STRINGIZE 195 | - PP_STRINGIZE 196 | - BOOST_PP_STRINGIZE 197 | - NS_SWIFT_NAME 198 | - CF_SWIFT_NAME 199 | --- 200 | 201 | -------------------------------------------------------------------------------- /src/util/misc.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_UTIL_MISC_H_ 33 | #define COLMAP_SRC_UTIL_MISC_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "util/string.h" 42 | 43 | namespace colmap { 44 | 45 | #ifndef STRINGIFY 46 | #define STRINGIFY(s) STRINGIFY_(s) 47 | #define STRINGIFY_(s) #s 48 | #endif // STRINGIFY 49 | 50 | // Append trailing slash to string if it does not yet end with a slash. 51 | std::string EnsureTrailingSlash(const std::string &str); 52 | 53 | // Check whether file name has the file extension (case insensitive). 54 | bool HasFileExtension(const std::string &file_name, const std::string &ext); 55 | 56 | // Split the path into its root and extension, for example, 57 | // "dir/file.jpg" into "dir/file" and ".jpg". 58 | void SplitFileExtension(const std::string &path, std::string *root, 59 | std::string *ext); 60 | 61 | // Check if the path points to an existing directory. 62 | bool ExistsFile(const std::string &path); 63 | 64 | // Check if the path points to an existing directory. 65 | bool ExistsDir(const std::string &path); 66 | 67 | // Check if the path points to an existing file or directory. 68 | bool ExistsPath(const std::string &path); 69 | 70 | // Create the directory if it does not exist. 71 | void CreateDirIfNotExists(const std::string &path); 72 | 73 | // Extract the base name of a path, e.g., "image.jpg" for "/dir/image.jpg". 74 | std::string GetPathBaseName(const std::string &path); 75 | 76 | // Get the path of the parent directory for the given path. 77 | std::string GetParentDir(const std::string &path); 78 | 79 | // Get the relative path between from and to. Both the from and to paths must 80 | // exist. 81 | std::string GetRelativePath(const std::string &from, const std::string &to); 82 | 83 | // Join multiple paths into one path. 84 | template 85 | std::string JoinPaths(T const &...paths); 86 | 87 | // Return list of files in directory. 88 | std::vector GetFileList(const std::string &path); 89 | 90 | // Return list of files, recursively in all sub-directories. 91 | std::vector GetRecursiveFileList(const std::string &path); 92 | 93 | // Return list of directories, recursively in all sub-directories. 94 | std::vector GetDirList(const std::string &path); 95 | 96 | // Return list of directories, recursively in all sub-directories. 97 | std::vector GetRecursiveDirList(const std::string &path); 98 | 99 | // Get the size in bytes of a file. 100 | size_t GetFileSize(const std::string &path); 101 | 102 | // Print first-order heading with over- and underscores to `std::cout`. 103 | void PrintHeading1(const std::string &heading); 104 | 105 | // Print second-order heading with underscores to `std::cout`. 106 | void PrintHeading2(const std::string &heading); 107 | 108 | // Check if vector contains elements. 109 | template 110 | bool VectorContainsValue(const std::vector &vector, const T value); 111 | 112 | template 113 | bool VectorContainsDuplicateValues(const std::vector &vector); 114 | 115 | // Concatenate values in list to comma-separated list. 116 | template 117 | std::string VectorToCSV(const std::vector &values); 118 | 119 | // Read contiguous binary blob from file. 120 | template 121 | void ReadBinaryBlob(const std::string &path, std::vector *data); 122 | 123 | // Write contiguous binary blob to file. 124 | template 125 | void WriteBinaryBlob(const std::string &path, const std::vector &data); 126 | 127 | // Read each line of a text file into a separate element. Empty lines are 128 | // ignored and leading/trailing whitespace is removed. 129 | std::vector ReadTextFileLines(const std::string &path); 130 | 131 | // Remove an argument from the list of command-line arguments. 132 | void RemoveCommandLineArgument(const std::string &arg, int *argc, char **argv); 133 | 134 | //////////////////////////////////////////////////////////////////////////////// 135 | // Implementation 136 | //////////////////////////////////////////////////////////////////////////////// 137 | 138 | template 139 | bool VectorContainsValue(const std::vector &vector, const T value) { 140 | return std::find_if(vector.begin(), vector.end(), [value](const T element) { 141 | return element == value; 142 | }) != vector.end(); 143 | } 144 | 145 | template 146 | bool VectorContainsDuplicateValues(const std::vector &vector) { 147 | std::vector unique_vector = vector; 148 | return std::unique(unique_vector.begin(), unique_vector.end()) != 149 | unique_vector.end(); 150 | } 151 | 152 | template 153 | std::string VectorToCSV(const std::vector &values) { 154 | std::string string; 155 | for (const T value : values) { 156 | string += std::to_string(value) + ", "; 157 | } 158 | return string.substr(0, string.length() - 2); 159 | } 160 | 161 | } // namespace colmap 162 | 163 | #endif // COLMAP_SRC_UTIL_MISC_H_ 164 | -------------------------------------------------------------------------------- /src/estimators/pose.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_ESTIMATORS_POSE_H_ 33 | #define COLMAP_SRC_ESTIMATORS_POSE_H_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include "base/camera.h" 42 | #include "base/camera_models.h" 43 | #include "optim/loransac.h" 44 | #include "util/alignment.h" 45 | 46 | #include "util/threading.h" 47 | #include "util/types.h" 48 | 49 | namespace colmap { 50 | 51 | enum RansacSampler { 52 | RANDOM_SAMPLE = 1, 53 | WEIGHT_SAMPLE = 2, 54 | }; 55 | 56 | enum RansacRobustor { ROBUSTRER_RANSAC = 1, ROBUSTER_LORANSAC = 2 }; 57 | 58 | struct AbsolutePoseEstimationOptions { 59 | // Whether to estimate the focal length. 60 | bool estimate_focal_length = false; 61 | 62 | // Number of discrete samples for focal length estimation. 63 | size_t num_focal_length_samples = 30; 64 | 65 | // Minimum focal length ratio for discrete focal length sampling 66 | // around focal length of given camera. 67 | double min_focal_length_ratio = 0.2; 68 | 69 | // Maximum focal length ratio for discrete focal length sampling 70 | // around focal length of given camera. 71 | double max_focal_length_ratio = 5; 72 | 73 | // Number of threads for parallel estimation of focal length. 74 | int num_threads = ThreadPool::kMaxNumThreads; 75 | 76 | // Options used for P3P RANSAC. 77 | RANSACOptions ransac_options; 78 | 79 | void Check() const { 80 | // CHECK_GT(num_focal_length_samples, 0); 81 | // CHECK_GT(min_focal_length_ratio, 0); 82 | // CHECK_GT(max_focal_length_ratio, 0); 83 | // CHECK_LT(min_focal_length_ratio, max_focal_length_ratio); 84 | ransac_options.Check(); 85 | } 86 | }; 87 | 88 | struct AbsolutePoseRefinementOptions { 89 | // Convergence criterion. 90 | double gradient_tolerance = 1.0; 91 | 92 | // Maximum number of solver iterations. 93 | int max_num_iterations = 100; 94 | 95 | // Scaling factor determines at which residual robustification takes place. 96 | double loss_function_scale = 1.0; 97 | 98 | // Whether to refine the focal length parameter group. 99 | bool refine_focal_length = true; 100 | 101 | // Whether to refine the extra parameter group. 102 | bool refine_extra_params = true; 103 | 104 | // Whether to print final summary. 105 | bool print_summary = true; 106 | 107 | void Check() const { 108 | // CHECK_GE(gradient_tolerance, 0.0); 109 | // CHECK_GE(max_num_iterations, 0); 110 | // CHECK_GE(loss_function_scale, 0.0); 111 | } 112 | }; 113 | 114 | // Estimate absolute pose (optionally focal length) from 2D-3D correspondences. 115 | // 116 | // Focal length estimation is performed using discrete sampling around the 117 | // focal length of the given camera. The focal length that results in the 118 | // maximal number of inliers is assigned to the given camera. 119 | // 120 | // @param options Absolute pose estimation options. 121 | // @param points2D Corresponding 2D points. 122 | // @param points3D Corresponding 3D points. 123 | // @param qvec Estimated rotation component as 124 | // unit Quaternion coefficients (w, x, y, z). 125 | // @param tvec Estimated translation component. 126 | // @param camera Camera for which to estimate pose. Modified 127 | // in-place to store the estimated focal length. 128 | // @param num_inliers Number of inliers in RANSAC. 129 | // @param inlier_mask Inlier mask for 2D-3D correspondences. 130 | // 131 | // @param robustor ransac or lo-ransac. 132 | // @param sampler random, weighted, progressive sampling. 133 | // @param priors if weighted or progressive sampling, the priors 134 | // should be provided 135 | // 136 | // @return Whether pose is estimated successfully. 137 | bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions& options, 138 | const std::vector& points2D, 139 | const std::vector& points3D, 140 | Eigen::Vector4d* qvec, Eigen::Vector3d* tvec, 141 | Camera* camera, size_t* num_inliers, 142 | std::vector* inlier_mask, 143 | RansacRobustor robustor, RansacSampler sampler, 144 | const std::vector& priors); 145 | 146 | // Refine absolute pose (optionally focal length) from 2D-3D correspondences. 147 | // 148 | // @param options Refinement options. 149 | // @param inlier_mask Inlier mask for 2D-3D correspondences. 150 | // @param points2D Corresponding 2D points. 151 | // @param points3D Corresponding 3D points. 152 | // @param qvec Estimated rotation component as 153 | // unit Quaternion coefficients (w, x, y, z). 154 | // @param tvec Estimated translation component. 155 | // @param camera Camera for which to estimate pose. Modified 156 | // in-place to store the estimated focal length. 157 | // 158 | // @return Whether the solution is usable. 159 | bool RefineAbsolutePose(const AbsolutePoseRefinementOptions& options, 160 | const std::vector& inlier_mask, 161 | const std::vector& points2D, 162 | const std::vector& points3D, 163 | Eigen::Vector4d* qvec, Eigen::Vector3d* tvec, 164 | Camera* camera); 165 | 166 | } // namespace colmap 167 | 168 | #endif // COLMAP_SRC_ESTIMATORS_POSE_H_ 169 | -------------------------------------------------------------------------------- /src/optim/bundle_adjustment.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ 33 | #define COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include "util/alignment.h" 44 | #include "util/types.h" 45 | 46 | namespace colmap { 47 | 48 | struct BundleAdjustmentOptions { 49 | // Loss function types: Trivial (non-robust) and Cauchy (robust) loss. 50 | enum class LossFunctionType { TRIVIAL, SOFT_L1, CAUCHY }; 51 | LossFunctionType loss_function_type = LossFunctionType::TRIVIAL; 52 | 53 | // Scaling factor determines residual at which robustification takes place. 54 | double loss_function_scale = 1.0; 55 | 56 | // Whether to refine the focal length parameter group. 57 | bool refine_focal_length = true; 58 | 59 | // Whether to refine the principal point parameter group. 60 | bool refine_principal_point = false; 61 | 62 | // Whether to refine the extra parameter group. 63 | bool refine_extra_params = true; 64 | 65 | // Whether to refine the extrinsic parameter group. 66 | bool refine_extrinsics = true; 67 | 68 | // Whether to print a final summary. 69 | bool print_summary = true; 70 | 71 | // Minimum number of residuals to enable multi-threading. Note that 72 | // single-threaded is typically better for small bundle adjustment problems 73 | // due to the overhead of threading. 74 | int min_num_residuals_for_multi_threading = 50000; 75 | 76 | // Ceres-Solver options. 77 | ceres::Solver::Options solver_options; 78 | 79 | BundleAdjustmentOptions() { 80 | solver_options.function_tolerance = 0.0; 81 | solver_options.gradient_tolerance = 0.0; 82 | solver_options.parameter_tolerance = 0.0; 83 | solver_options.minimizer_progress_to_stdout = false; 84 | solver_options.max_num_iterations = 100; 85 | solver_options.max_linear_solver_iterations = 200; 86 | solver_options.max_num_consecutive_invalid_steps = 10; 87 | solver_options.max_consecutive_nonmonotonic_steps = 10; 88 | solver_options.num_threads = -1; 89 | #if CERES_VERSION_MAJOR < 2 90 | solver_options.num_linear_solver_threads = -1; 91 | #endif // CERES_VERSION_MAJOR 92 | } 93 | 94 | // Create a new loss function based on the specified options. The caller 95 | // takes ownership of the loss function. 96 | ceres::LossFunction* CreateLossFunction() const; 97 | 98 | // bool Check() const; 99 | }; 100 | 101 | // Configuration container to setup bundle adjustment problems. 102 | class BundleAdjustmentConfig { 103 | public: 104 | BundleAdjustmentConfig(); 105 | 106 | size_t NumImages() const; 107 | size_t NumPoints() const; 108 | size_t NumConstantCameras() const; 109 | size_t NumConstantPoses() const; 110 | size_t NumConstantTvecs() const; 111 | size_t NumVariablePoints() const; 112 | size_t NumConstantPoints() const; 113 | 114 | // Determine the number of residuals for the given reconstruction. The 115 | // number of residuals equals the number of observations times two. size_t 116 | // NumResiduals(const Reconstruction& reconstruction) const; 117 | 118 | // Add / remove images from the configuration. 119 | void AddImage(const image_t image_id); 120 | bool HasImage(const image_t image_id) const; 121 | void RemoveImage(const image_t image_id); 122 | 123 | // Set cameras of added images as constant or variable. By default all 124 | // cameras of added images are variable. Note that the corresponding images 125 | // have to be added prior to calling these methods. 126 | void SetConstantCamera(const camera_t camera_id); 127 | void SetVariableCamera(const camera_t camera_id); 128 | bool IsConstantCamera(const camera_t camera_id) const; 129 | 130 | // Set the pose of added images as constant. The pose is defined as the 131 | // rotational and translational part of the projection matrix. 132 | void SetConstantPose(const image_t image_id); 133 | void SetVariablePose(const image_t image_id); 134 | bool HasConstantPose(const image_t image_id) const; 135 | 136 | // Set the translational part of the pose, hence the constant pose 137 | // indices may be in [0, 1, 2] and must be unique. Note that the 138 | // corresponding images have to be added prior to calling these methods. 139 | void SetConstantTvec(const image_t image_id, const std::vector& idxs); 140 | void RemoveConstantTvec(const image_t image_id); 141 | bool HasConstantTvec(const image_t image_id) const; 142 | 143 | // Add / remove points from the configuration. Note that points can either 144 | // be variable or constant but not both at the same time. 145 | void AddVariablePoint(const point3D_t point3D_id); 146 | void AddConstantPoint(const point3D_t point3D_id); 147 | bool HasPoint(const point3D_t point3D_id) const; 148 | bool HasVariablePoint(const point3D_t point3D_id) const; 149 | bool HasConstantPoint(const point3D_t point3D_id) const; 150 | void RemoveVariablePoint(const point3D_t point3D_id); 151 | void RemoveConstantPoint(const point3D_t point3D_id); 152 | 153 | // Access configuration data. 154 | const std::unordered_set& Images() const; 155 | const std::unordered_set& VariablePoints() const; 156 | const std::unordered_set& ConstantPoints() const; 157 | const std::vector& ConstantTvec(const image_t image_id) const; 158 | 159 | private: 160 | std::unordered_set constant_camera_ids_; 161 | std::unordered_set image_ids_; 162 | std::unordered_set variable_point3D_ids_; 163 | std::unordered_set constant_point3D_ids_; 164 | std::unordered_set constant_poses_; 165 | std::unordered_map> constant_tvecs_; 166 | }; 167 | 168 | void PrintSolverSummary(const ceres::Solver::Summary& summary); 169 | 170 | } // namespace colmap 171 | 172 | #endif // COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ 173 | -------------------------------------------------------------------------------- /src/estimators/utils.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "estimators/utils.h" 33 | 34 | namespace colmap { 35 | 36 | void CenterAndNormalizeImagePoints(const std::vector &points, 37 | std::vector *normed_points, 38 | Eigen::Matrix3d *matrix) { 39 | // Calculate centroid 40 | Eigen::Vector2d centroid(0, 0); 41 | for (const auto point : points) { 42 | centroid += point; 43 | } 44 | centroid /= points.size(); 45 | 46 | // Root mean square error to centroid of all points 47 | double rms_mean_dist = 0; 48 | for (const auto point : points) { 49 | rms_mean_dist += (point - centroid).squaredNorm(); 50 | } 51 | rms_mean_dist = std::sqrt(rms_mean_dist / points.size()); 52 | 53 | // Compose normalization matrix 54 | const double norm_factor = std::sqrt(2.0) / rms_mean_dist; 55 | *matrix << norm_factor, 0, -norm_factor * centroid(0), 0, norm_factor, 56 | -norm_factor * centroid(1), 0, 0, 1; 57 | 58 | // Apply normalization matrix 59 | normed_points->resize(points.size()); 60 | 61 | const double M_00 = (*matrix)(0, 0); 62 | const double M_01 = (*matrix)(0, 1); 63 | const double M_02 = (*matrix)(0, 2); 64 | const double M_10 = (*matrix)(1, 0); 65 | const double M_11 = (*matrix)(1, 1); 66 | const double M_12 = (*matrix)(1, 2); 67 | const double M_20 = (*matrix)(2, 0); 68 | const double M_21 = (*matrix)(2, 1); 69 | const double M_22 = (*matrix)(2, 2); 70 | 71 | for (size_t i = 0; i < points.size(); ++i) { 72 | const double p_0 = points[i](0); 73 | const double p_1 = points[i](1); 74 | 75 | const double np_0 = M_00 * p_0 + M_01 * p_1 + M_02; 76 | const double np_1 = M_10 * p_0 + M_11 * p_1 + M_12; 77 | const double np_2 = M_20 * p_0 + M_21 * p_1 + M_22; 78 | 79 | const double inv_np_2 = 1.0 / np_2; 80 | (*normed_points)[i](0) = np_0 * inv_np_2; 81 | (*normed_points)[i](1) = np_1 * inv_np_2; 82 | } 83 | } 84 | 85 | void ComputeSquaredSampsonError(const std::vector &points1, 86 | const std::vector &points2, 87 | const Eigen::Matrix3d &E, 88 | std::vector *residuals) { 89 | // CHECK_EQ(points1.size(), points2.size()); 90 | 91 | residuals->resize(points1.size()); 92 | 93 | // Note that this code might not be as nice as Eigen expressions, 94 | // but it is significantly faster in various tests 95 | 96 | const double E_00 = E(0, 0); 97 | const double E_01 = E(0, 1); 98 | const double E_02 = E(0, 2); 99 | const double E_10 = E(1, 0); 100 | const double E_11 = E(1, 1); 101 | const double E_12 = E(1, 2); 102 | const double E_20 = E(2, 0); 103 | const double E_21 = E(2, 1); 104 | const double E_22 = E(2, 2); 105 | 106 | for (size_t i = 0; i < points1.size(); ++i) { 107 | const double x1_0 = points1[i](0); 108 | const double x1_1 = points1[i](1); 109 | const double x2_0 = points2[i](0); 110 | const double x2_1 = points2[i](1); 111 | 112 | // Ex1 = E * points1[i].homogeneous(); 113 | const double Ex1_0 = E_00 * x1_0 + E_01 * x1_1 + E_02; 114 | const double Ex1_1 = E_10 * x1_0 + E_11 * x1_1 + E_12; 115 | const double Ex1_2 = E_20 * x1_0 + E_21 * x1_1 + E_22; 116 | 117 | // Etx2 = E.transpose() * points2[i].homogeneous(); 118 | const double Etx2_0 = E_00 * x2_0 + E_10 * x2_1 + E_20; 119 | const double Etx2_1 = E_01 * x2_0 + E_11 * x2_1 + E_21; 120 | 121 | // x2tEx1 = points2[i].homogeneous().transpose() * Ex1; 122 | const double x2tEx1 = x2_0 * Ex1_0 + x2_1 * Ex1_1 + Ex1_2; 123 | 124 | // Sampson distance 125 | (*residuals)[i] = 126 | x2tEx1 * x2tEx1 / 127 | (Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1 + Etx2_0 * Etx2_0 + Etx2_1 * Etx2_1); 128 | } 129 | } 130 | 131 | void ComputeSquaredReprojectionError( 132 | const std::vector &points2D, 133 | const std::vector &points3D, 134 | const Eigen::Matrix3x4d &proj_matrix, std::vector *residuals) { 135 | // CHECK_EQ(points2D.size(), points3D.size()); 136 | 137 | residuals->resize(points2D.size()); 138 | 139 | // Note that this code might not be as nice as Eigen expressions, 140 | // but it is significantly faster in various tests. 141 | 142 | const double P_00 = proj_matrix(0, 0); 143 | const double P_01 = proj_matrix(0, 1); 144 | const double P_02 = proj_matrix(0, 2); 145 | const double P_03 = proj_matrix(0, 3); 146 | const double P_10 = proj_matrix(1, 0); 147 | const double P_11 = proj_matrix(1, 1); 148 | const double P_12 = proj_matrix(1, 2); 149 | const double P_13 = proj_matrix(1, 3); 150 | const double P_20 = proj_matrix(2, 0); 151 | const double P_21 = proj_matrix(2, 1); 152 | const double P_22 = proj_matrix(2, 2); 153 | const double P_23 = proj_matrix(2, 3); 154 | 155 | for (size_t i = 0; i < points2D.size(); ++i) { 156 | const double X_0 = points3D[i](0); 157 | const double X_1 = points3D[i](1); 158 | const double X_2 = points3D[i](2); 159 | 160 | // Project 3D point from world to camera. 161 | const double px_2 = P_20 * X_0 + P_21 * X_1 + P_22 * X_2 + P_23; 162 | 163 | // Check if 3D point is in front of camera. 164 | if (px_2 > std::numeric_limits::epsilon()) { 165 | const double px_0 = P_00 * X_0 + P_01 * X_1 + P_02 * X_2 + P_03; 166 | const double px_1 = P_10 * X_0 + P_11 * X_1 + P_12 * X_2 + P_13; 167 | 168 | const double x_0 = points2D[i](0); 169 | const double x_1 = points2D[i](1); 170 | 171 | const double inv_px_2 = 1.0 / px_2; 172 | const double dx_0 = x_0 - px_0 * inv_px_2; 173 | const double dx_1 = x_1 - px_1 * inv_px_2; 174 | 175 | (*residuals)[i] = dx_0 * dx_0 + dx_1 * dx_1; 176 | } else { 177 | (*residuals)[i] = std::numeric_limits::max(); 178 | } 179 | } 180 | } 181 | 182 | } // namespace colmap 183 | -------------------------------------------------------------------------------- /src/base/projection.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_BASE_PROJECTION_H_ 33 | #define COLMAP_SRC_BASE_PROJECTION_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include "base/camera.h" 42 | 43 | namespace colmap { 44 | 45 | // Compose projection matrix from rotation and translation components. 46 | // 47 | // The projection matrix transforms 3D world to image points. 48 | // 49 | // @param qvec Unit Quaternion rotation coefficients (w, x, y, z). 50 | // @param tvec 3x1 translation vector. 51 | // 52 | // @return 3x4 projection matrix. 53 | Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Vector4d& qvec, 54 | const Eigen::Vector3d& tvec); 55 | 56 | // Compose projection matrix from rotation matrix and translation components). 57 | // 58 | // The projection matrix transforms 3D world to image points. 59 | // 60 | // @param R 3x3 rotation matrix. 61 | // @param t 3x1 translation vector. 62 | // 63 | // @return 3x4 projection matrix. 64 | Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Matrix3d& R, 65 | const Eigen::Vector3d& T); 66 | 67 | // Invert projection matrix, defined as: 68 | // 69 | // P = [R | t] with R \in SO(3) and t \in R^3 70 | // 71 | // and the inverse projection matrix as: 72 | // 73 | // P' = [R^T | -R^T t] 74 | // 75 | // @param proj_matrix 3x4 projection matrix. 76 | // 77 | // @return 3x4 inverse projection matrix. 78 | Eigen::Matrix3x4d InvertProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix); 79 | 80 | // Compute the closes rotation matrix with the closest Frobenius norm by setting 81 | // the singular values of the given matrix to 1. 82 | Eigen::Matrix3d ComputeClosestRotationMatrix(const Eigen::Matrix3d& matrix); 83 | 84 | // Decompose projection matrix into intrinsic camera matrix, rotation matrix and 85 | // translation vector. Returns false if decomposition fails. This implementation 86 | // is inspired by the OpenCV implementation with some additional checks. 87 | bool DecomposeProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix, 88 | Eigen::Matrix3d* K, Eigen::Matrix3d* R, 89 | Eigen::Vector3d* T); 90 | 91 | // Project 3D point to image. 92 | // 93 | // @param points3D 3D world point as 3x1 vector. 94 | // @param proj_matrix 3x4 projection matrix. 95 | // @param camera Camera used to project to image plane. 96 | // 97 | // @return Projected image point. 98 | Eigen::Vector2d ProjectPointToImage(const Eigen::Vector3d& point3D, 99 | const Eigen::Matrix3x4d& proj_matrix, 100 | const Camera& camera); 101 | 102 | // Calculate the reprojection error. 103 | // 104 | // The reprojection error is the Euclidean distance between the observation 105 | // in the image and the projection of the 3D point into the image. If the 106 | // 3D point is behind the camera, then this function returns DBL_MAX. 107 | double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, 108 | const Eigen::Vector3d& point3D, 109 | const Eigen::Vector4d& qvec, 110 | const Eigen::Vector3d& tvec, 111 | const Camera& camera); 112 | double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, 113 | const Eigen::Vector3d& point3D, 114 | const Eigen::Matrix3x4d& proj_matrix, 115 | const Camera& camera); 116 | 117 | // Calculate the angular error. 118 | // 119 | // The angular error is the angle between the observed viewing ray and the 120 | // actual viewing ray from the camera center to the 3D point. 121 | double CalculateAngularError(const Eigen::Vector2d& point2D, 122 | const Eigen::Vector3d& point3D, 123 | const Eigen::Vector4d& qvec, 124 | const Eigen::Vector3d& tvec, const Camera& camera); 125 | double CalculateAngularError(const Eigen::Vector2d& point2D, 126 | const Eigen::Vector3d& point3D, 127 | const Eigen::Matrix3x4d& proj_matrix, 128 | const Camera& camera); 129 | 130 | // Calculate angulate error using normalized image points. 131 | // 132 | // The angular error is the angle between the observed viewing ray and the 133 | // actual viewing ray from the camera center to the 3D point. 134 | double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, 135 | const Eigen::Vector3d& point3D, 136 | const Eigen::Vector4d& qvec, 137 | const Eigen::Vector3d& tvec); 138 | double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, 139 | const Eigen::Vector3d& point3D, 140 | const Eigen::Matrix3x4d& proj_matrix); 141 | 142 | // Calculate depth of 3D point with respect to camera. 143 | // 144 | // The depth is defined as the Euclidean distance of a 3D point from the 145 | // camera and is positive if the 3D point is in front and negative if 146 | // behind of the camera. 147 | // 148 | // @param proj_matrix 3x4 projection matrix. 149 | // @param point3D 3D point as 3x1 vector. 150 | // 151 | // @return Depth of 3D point. 152 | double CalculateDepth(const Eigen::Matrix3x4d& proj_matrix, 153 | const Eigen::Vector3d& point3D); 154 | 155 | // Check if 3D point passes cheirality constraint, 156 | // i.e. it lies in front of the camera and not in the image plane. 157 | // 158 | // @param proj_matrix 3x4 projection matrix. 159 | // @param point3D 3D point as 3x1 vector. 160 | // 161 | // @return True if point lies in front of camera. 162 | bool HasPointPositiveDepth(const Eigen::Matrix3x4d& proj_matrix, 163 | const Eigen::Vector3d& point3D); 164 | 165 | } // namespace colmap 166 | 167 | #endif // COLMAP_SRC_BASE_PROJECTION_H_ 168 | -------------------------------------------------------------------------------- /src/estimators/absolute_pose.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ 33 | #define COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include "util/alignment.h" 41 | #include "util/types.h" 42 | 43 | namespace colmap { 44 | 45 | // Analytic solver for the P3P (Perspective-Three-Point) problem. 46 | // 47 | // The algorithm is based on the following paper: 48 | // 49 | // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang. Complete Solution 50 | // Classification for the Perspective-Three-Point Problem. 51 | // http://www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf 52 | class P3PEstimator { 53 | public: 54 | // The 2D image feature observations. 55 | typedef Eigen::Vector2d X_t; 56 | // The observed 3D features in the world frame. 57 | typedef Eigen::Vector3d Y_t; 58 | // The transformation from the world to the camera frame. 59 | typedef Eigen::Matrix3x4d M_t; 60 | 61 | // The minimum number of samples needed to estimate a model. 62 | static const int kMinNumSamples = 3; 63 | 64 | // Estimate the most probable solution of the P3P problem from a set of 65 | // three 2D-3D point correspondences. 66 | // 67 | // @param points2D Normalized 2D image points as 3x2 matrix. 68 | // @param points3D 3D world points as 3x3 matrix. 69 | // 70 | // @return Most probable pose as length-1 vector of a 3x4 matrix. 71 | static std::vector Estimate(const std::vector &points2D, 72 | const std::vector &points3D); 73 | 74 | // Calculate the squared reprojection error given a set of 2D-3D point 75 | // correspondences and a projection matrix. 76 | // 77 | // @param points2D Normalized 2D image points as Nx2 matrix. 78 | // @param points3D 3D world points as Nx3 matrix. 79 | // @param proj_matrix 3x4 projection matrix. 80 | // @param residuals Output vector of residuals. 81 | static void Residuals(const std::vector &points2D, 82 | const std::vector &points3D, 83 | const M_t &proj_matrix, 84 | std::vector *residuals); 85 | }; 86 | 87 | // EPNP solver for the PNP (Perspective-N-Point) problem. The solver needs a 88 | // minimum of 4 2D-3D correspondences. 89 | // 90 | // The algorithm is based on the following paper: 91 | // 92 | // Lepetit, Vincent, Francesc Moreno-Noguer, and Pascal Fua. 93 | // "Epnp: An accurate o (n) solution to the pnp problem." 94 | // International journal of computer vision 81.2 (2009): 155-166. 95 | // 96 | // The implementation is based on their original open-source release, but is 97 | // ported to Eigen and contains several improvements over the original code. 98 | class EPNPEstimator { 99 | public: 100 | // The 2D image feature observations. 101 | typedef Eigen::Vector2d X_t; 102 | // The observed 3D features in the world frame. 103 | typedef Eigen::Vector3d Y_t; 104 | // The transformation from the world to the camera frame. 105 | typedef Eigen::Matrix3x4d M_t; 106 | 107 | // The minimum number of samples needed to estimate a model. 108 | static const int kMinNumSamples = 4; 109 | 110 | // Estimate the most probable solution of the P3P problem from a set of 111 | // three 2D-3D point correspondences. 112 | // 113 | // @param points2D Normalized 2D image points as 3x2 matrix. 114 | // @param points3D 3D world points as 3x3 matrix. 115 | // 116 | // @return Most probable pose as length-1 vector of a 3x4 matrix. 117 | static std::vector Estimate(const std::vector &points2D, 118 | const std::vector &points3D); 119 | 120 | // Calculate the squared reprojection error given a set of 2D-3D point 121 | // correspondences and a projection matrix. 122 | // 123 | // @param points2D Normalized 2D image points as Nx2 matrix. 124 | // @param points3D 3D world points as Nx3 matrix. 125 | // @param proj_matrix 3x4 projection matrix. 126 | // @param residuals Output vector of residuals. 127 | static void Residuals(const std::vector &points2D, 128 | const std::vector &points3D, 129 | const M_t &proj_matrix, 130 | std::vector *residuals); 131 | 132 | private: 133 | bool ComputePose(const std::vector &points2D, 134 | const std::vector &points3D, 135 | Eigen::Matrix3x4d *proj_matrix); 136 | 137 | void ChooseControlPoints(); 138 | bool ComputeBarycentricCoordinates(); 139 | 140 | Eigen::Matrix ComputeM(); 141 | Eigen::Matrix ComputeL6x10( 142 | const Eigen::Matrix &Ut); 143 | Eigen::Matrix ComputeRho(); 144 | 145 | void FindBetasApprox1(const Eigen::Matrix &L_6x10, 146 | const Eigen::Matrix &rho, 147 | Eigen::Vector4d *betas); 148 | void FindBetasApprox2(const Eigen::Matrix &L_6x10, 149 | const Eigen::Matrix &rho, 150 | Eigen::Vector4d *betas); 151 | void FindBetasApprox3(const Eigen::Matrix &L_6x10, 152 | const Eigen::Matrix &rho, 153 | Eigen::Vector4d *betas); 154 | 155 | void RunGaussNewton(const Eigen::Matrix &L_6x10, 156 | const Eigen::Matrix &rho, 157 | Eigen::Vector4d *betas); 158 | 159 | double ComputeRT(const Eigen::Matrix &Ut, 160 | const Eigen::Vector4d &betas, Eigen::Matrix3d *R, 161 | Eigen::Vector3d *t); 162 | 163 | void ComputeCcs(const Eigen::Vector4d &betas, 164 | const Eigen::Matrix &Ut); 165 | void ComputePcs(); 166 | 167 | void SolveForSign(); 168 | 169 | void EstimateRT(Eigen::Matrix3d *R, Eigen::Vector3d *t); 170 | 171 | double ComputeTotalReprojectionError(const Eigen::Matrix3d &R, 172 | const Eigen::Vector3d &t); 173 | 174 | const std::vector *points2D_ = nullptr; 175 | const std::vector *points3D_ = nullptr; 176 | std::vector pcs_; 177 | std::vector alphas_; 178 | std::array cws_; 179 | std::array ccs_; 180 | }; 181 | 182 | } // namespace colmap 183 | 184 | #endif // COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ 185 | -------------------------------------------------------------------------------- /src/util/threading.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "util/threading.h" 33 | 34 | namespace colmap { 35 | 36 | Thread::Thread() 37 | : started_(false), 38 | stopped_(false), 39 | paused_(false), 40 | pausing_(false), 41 | finished_(false), 42 | setup_(false), 43 | setup_valid_(false) { 44 | RegisterCallback(STARTED_CALLBACK); 45 | RegisterCallback(FINISHED_CALLBACK); 46 | } 47 | 48 | void Thread::Start() { 49 | std::unique_lock lock(mutex_); 50 | // CHECK(!started_ || finished_); 51 | Wait(); 52 | timer_.Restart(); 53 | thread_ = std::thread(&Thread::RunFunc, this); 54 | started_ = true; 55 | stopped_ = false; 56 | paused_ = false; 57 | pausing_ = false; 58 | finished_ = false; 59 | setup_ = false; 60 | setup_valid_ = false; 61 | } 62 | 63 | void Thread::Stop() { 64 | { 65 | std::unique_lock lock(mutex_); 66 | stopped_ = true; 67 | } 68 | Resume(); 69 | } 70 | 71 | void Thread::Pause() { 72 | std::unique_lock lock(mutex_); 73 | paused_ = true; 74 | } 75 | 76 | void Thread::Resume() { 77 | std::unique_lock lock(mutex_); 78 | if (paused_) { 79 | paused_ = false; 80 | pause_condition_.notify_all(); 81 | } 82 | } 83 | 84 | void Thread::Wait() { 85 | if (thread_.joinable()) { 86 | thread_.join(); 87 | } 88 | } 89 | 90 | bool Thread::IsStarted() { 91 | std::unique_lock lock(mutex_); 92 | return started_; 93 | } 94 | 95 | bool Thread::IsStopped() { 96 | std::unique_lock lock(mutex_); 97 | return stopped_; 98 | } 99 | 100 | bool Thread::IsPaused() { 101 | std::unique_lock lock(mutex_); 102 | return paused_; 103 | } 104 | 105 | bool Thread::IsRunning() { 106 | std::unique_lock lock(mutex_); 107 | return started_ && !pausing_ && !finished_; 108 | } 109 | 110 | bool Thread::IsFinished() { 111 | std::unique_lock lock(mutex_); 112 | return finished_; 113 | } 114 | 115 | void Thread::AddCallback(const int id, const std::function &func) { 116 | // CHECK(func); 117 | // CHECK_GT(callbacks_.count(id), 0) << "Callback not registered"; 118 | callbacks_.at(id).push_back(func); 119 | } 120 | 121 | void Thread::RegisterCallback(const int id) { 122 | callbacks_.emplace(id, std::list>()); 123 | } 124 | 125 | void Thread::Callback(const int id) const { 126 | // CHECK_GT(callbacks_.count(id), 0) << "Callback not registered"; 127 | for (const auto &callback : callbacks_.at(id)) { 128 | callback(); 129 | } 130 | } 131 | 132 | std::thread::id Thread::GetThreadId() const { 133 | return std::this_thread::get_id(); 134 | } 135 | 136 | void Thread::SignalValidSetup() { 137 | std::unique_lock lock(mutex_); 138 | // CHECK(!setup_); 139 | setup_ = true; 140 | setup_valid_ = true; 141 | setup_condition_.notify_all(); 142 | } 143 | 144 | void Thread::SignalInvalidSetup() { 145 | std::unique_lock lock(mutex_); 146 | // CHECK(!setup_); 147 | setup_ = true; 148 | setup_valid_ = false; 149 | setup_condition_.notify_all(); 150 | } 151 | 152 | const class Timer &Thread::GetTimer() const { return timer_; } 153 | 154 | void Thread::BlockIfPaused() { 155 | std::unique_lock lock(mutex_); 156 | if (paused_) { 157 | pausing_ = true; 158 | timer_.Pause(); 159 | pause_condition_.wait(lock); 160 | pausing_ = false; 161 | timer_.Resume(); 162 | } 163 | } 164 | 165 | bool Thread::CheckValidSetup() { 166 | std::unique_lock lock(mutex_); 167 | if (!setup_) { 168 | setup_condition_.wait(lock); 169 | } 170 | return setup_valid_; 171 | } 172 | 173 | void Thread::RunFunc() { 174 | Callback(STARTED_CALLBACK); 175 | Run(); 176 | { 177 | std::unique_lock lock(mutex_); 178 | finished_ = true; 179 | timer_.Pause(); 180 | } 181 | Callback(FINISHED_CALLBACK); 182 | } 183 | 184 | ThreadPool::ThreadPool(const int num_threads) 185 | : stopped_(false), num_active_workers_(0) { 186 | const int num_effective_threads = GetEffectiveNumThreads(num_threads); 187 | for (int index = 0; index < num_effective_threads; ++index) { 188 | std::function worker = 189 | std::bind(&ThreadPool::WorkerFunc, this, index); 190 | workers_.emplace_back(worker); 191 | } 192 | } 193 | 194 | ThreadPool::~ThreadPool() { Stop(); } 195 | 196 | void ThreadPool::Stop() { 197 | { 198 | std::unique_lock lock(mutex_); 199 | 200 | if (stopped_) { 201 | return; 202 | } 203 | 204 | stopped_ = true; 205 | 206 | std::queue> empty_tasks; 207 | std::swap(tasks_, empty_tasks); 208 | } 209 | 210 | task_condition_.notify_all(); 211 | 212 | for (auto &worker : workers_) { 213 | worker.join(); 214 | } 215 | 216 | finished_condition_.notify_all(); 217 | } 218 | 219 | void ThreadPool::Wait() { 220 | std::unique_lock lock(mutex_); 221 | if (!tasks_.empty() || num_active_workers_ > 0) { 222 | finished_condition_.wait(lock, [this]() { 223 | return tasks_.empty() && num_active_workers_ == 0; 224 | }); 225 | } 226 | } 227 | 228 | void ThreadPool::WorkerFunc(const int index) { 229 | { 230 | std::lock_guard lock(mutex_); 231 | thread_id_to_index_.emplace(GetThreadId(), index); 232 | } 233 | 234 | while (true) { 235 | std::function task; 236 | { 237 | std::unique_lock lock(mutex_); 238 | task_condition_.wait( 239 | lock, [this] { return stopped_ || !tasks_.empty(); }); 240 | if (stopped_ && tasks_.empty()) { 241 | return; 242 | } 243 | task = std::move(tasks_.front()); 244 | tasks_.pop(); 245 | num_active_workers_ += 1; 246 | } 247 | 248 | task(); 249 | 250 | { 251 | std::unique_lock lock(mutex_); 252 | num_active_workers_ -= 1; 253 | } 254 | 255 | finished_condition_.notify_all(); 256 | } 257 | } 258 | 259 | std::thread::id ThreadPool::GetThreadId() const { 260 | return std::this_thread::get_id(); 261 | } 262 | 263 | int ThreadPool::GetThreadIndex() { 264 | std::unique_lock lock(mutex_); 265 | return thread_id_to_index_.at(GetThreadId()); 266 | } 267 | 268 | int GetEffectiveNumThreads(const int num_threads) { 269 | int num_effective_threads = num_threads; 270 | if (num_threads <= 0) { 271 | num_effective_threads = std::thread::hardware_concurrency(); 272 | } 273 | 274 | if (num_effective_threads <= 0) { 275 | num_effective_threads = 1; 276 | } 277 | 278 | return num_effective_threads; 279 | } 280 | 281 | } // namespace colmap 282 | -------------------------------------------------------------------------------- /src/base/projection.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "base/projection.h" 33 | 34 | #include "base/pose.h" 35 | #include "util/matrix.h" 36 | 37 | namespace colmap { 38 | 39 | Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Vector4d& qvec, 40 | const Eigen::Vector3d& tvec) { 41 | Eigen::Matrix3x4d proj_matrix; 42 | proj_matrix.leftCols<3>() = QuaternionToRotationMatrix(qvec); 43 | proj_matrix.rightCols<1>() = tvec; 44 | return proj_matrix; 45 | } 46 | 47 | Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Matrix3d& R, 48 | const Eigen::Vector3d& T) { 49 | Eigen::Matrix3x4d proj_matrix; 50 | proj_matrix.leftCols<3>() = R; 51 | proj_matrix.rightCols<1>() = T; 52 | return proj_matrix; 53 | } 54 | 55 | Eigen::Matrix3x4d InvertProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix) { 56 | Eigen::Matrix3x4d inv_proj_matrix; 57 | inv_proj_matrix.leftCols<3>() = proj_matrix.leftCols<3>().transpose(); 58 | inv_proj_matrix.rightCols<1>() = ProjectionCenterFromMatrix(proj_matrix); 59 | return inv_proj_matrix; 60 | } 61 | 62 | Eigen::Matrix3d ComputeClosestRotationMatrix(const Eigen::Matrix3d& matrix) { 63 | const Eigen::JacobiSVD svd( 64 | matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); 65 | Eigen::Matrix3d R = svd.matrixU() * (svd.matrixV().transpose()); 66 | if (R.determinant() < 0.0) { 67 | R *= -1.0; 68 | } 69 | return R; 70 | } 71 | 72 | bool DecomposeProjectionMatrix(const Eigen::Matrix3x4d& P, Eigen::Matrix3d* K, 73 | Eigen::Matrix3d* R, Eigen::Vector3d* T) { 74 | Eigen::Matrix3d RR; 75 | Eigen::Matrix3d QQ; 76 | DecomposeMatrixRQ(P.leftCols<3>().eval(), &RR, &QQ); 77 | 78 | *R = ComputeClosestRotationMatrix(QQ); 79 | 80 | const double det_K = RR.determinant(); 81 | if (det_K == 0) { 82 | return false; 83 | } else if (det_K > 0) { 84 | *K = RR; 85 | } else { 86 | *K = -RR; 87 | } 88 | 89 | for (int i = 0; i < 3; ++i) { 90 | if ((*K)(i, i) < 0.0) { 91 | K->col(i) = -K->col(i); 92 | R->row(i) = -R->row(i); 93 | } 94 | } 95 | 96 | *T = K->triangularView().solve(P.col(3)); 97 | if (det_K < 0) { 98 | *T = -(*T); 99 | } 100 | 101 | return true; 102 | } 103 | 104 | Eigen::Vector2d ProjectPointToImage(const Eigen::Vector3d& point3D, 105 | const Eigen::Matrix3x4d& proj_matrix, 106 | const Camera& camera) { 107 | const Eigen::Vector3d world_point = proj_matrix * point3D.homogeneous(); 108 | return camera.WorldToImage(world_point.hnormalized()); 109 | } 110 | 111 | double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, 112 | const Eigen::Vector3d& point3D, 113 | const Eigen::Vector4d& qvec, 114 | const Eigen::Vector3d& tvec, 115 | const Camera& camera) { 116 | const Eigen::Vector3d proj_point3D = 117 | QuaternionRotatePoint(qvec, point3D) + tvec; 118 | 119 | // Check that point is infront of camera. 120 | if (proj_point3D.z() < std::numeric_limits::epsilon()) { 121 | return std::numeric_limits::max(); 122 | } 123 | 124 | const Eigen::Vector2d proj_point2D = 125 | camera.WorldToImage(proj_point3D.hnormalized()); 126 | 127 | return (proj_point2D - point2D).squaredNorm(); 128 | } 129 | 130 | double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, 131 | const Eigen::Vector3d& point3D, 132 | const Eigen::Matrix3x4d& proj_matrix, 133 | const Camera& camera) { 134 | const double proj_z = proj_matrix.row(2).dot(point3D.homogeneous()); 135 | 136 | // Check that point is infront of camera. 137 | if (proj_z < std::numeric_limits::epsilon()) { 138 | return std::numeric_limits::max(); 139 | } 140 | 141 | const double proj_x = proj_matrix.row(0).dot(point3D.homogeneous()); 142 | const double proj_y = proj_matrix.row(1).dot(point3D.homogeneous()); 143 | const double inv_proj_z = 1.0 / proj_z; 144 | 145 | const Eigen::Vector2d proj_point2D = camera.WorldToImage( 146 | Eigen::Vector2d(inv_proj_z * proj_x, inv_proj_z * proj_y)); 147 | 148 | return (proj_point2D - point2D).squaredNorm(); 149 | } 150 | 151 | double CalculateAngularError(const Eigen::Vector2d& point2D, 152 | const Eigen::Vector3d& point3D, 153 | const Eigen::Vector4d& qvec, 154 | const Eigen::Vector3d& tvec, 155 | const Camera& camera) { 156 | return CalculateNormalizedAngularError(camera.ImageToWorld(point2D), 157 | point3D, qvec, tvec); 158 | } 159 | 160 | double CalculateAngularError(const Eigen::Vector2d& point2D, 161 | const Eigen::Vector3d& point3D, 162 | const Eigen::Matrix3x4d& proj_matrix, 163 | const Camera& camera) { 164 | return CalculateNormalizedAngularError(camera.ImageToWorld(point2D), 165 | point3D, proj_matrix); 166 | } 167 | 168 | double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, 169 | const Eigen::Vector3d& point3D, 170 | const Eigen::Vector4d& qvec, 171 | const Eigen::Vector3d& tvec) { 172 | const Eigen::Vector3d ray1 = point2D.homogeneous(); 173 | const Eigen::Vector3d ray2 = QuaternionRotatePoint(qvec, point3D) + tvec; 174 | return std::acos(ray1.normalized().transpose() * ray2.normalized()); 175 | } 176 | 177 | double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, 178 | const Eigen::Vector3d& point3D, 179 | const Eigen::Matrix3x4d& proj_matrix) { 180 | const Eigen::Vector3d ray1 = point2D.homogeneous(); 181 | const Eigen::Vector3d ray2 = proj_matrix * point3D.homogeneous(); 182 | return std::acos(ray1.normalized().transpose() * ray2.normalized()); 183 | } 184 | 185 | double CalculateDepth(const Eigen::Matrix3x4d& proj_matrix, 186 | const Eigen::Vector3d& point3D) { 187 | const double proj_z = proj_matrix.row(2).dot(point3D.homogeneous()); 188 | return proj_z * proj_matrix.col(2).norm(); 189 | } 190 | 191 | bool HasPointPositiveDepth(const Eigen::Matrix3x4d& proj_matrix, 192 | const Eigen::Vector3d& point3D) { 193 | return proj_matrix.row(2).dot(point3D.homogeneous()) >= 194 | std::numeric_limits::epsilon(); 195 | } 196 | 197 | } // namespace colmap 198 | -------------------------------------------------------------------------------- /src/base/camera.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_BASE_CAMERA_H_ 33 | #define COLMAP_SRC_BASE_CAMERA_H_ 34 | 35 | #include 36 | 37 | #include "util/types.h" 38 | 39 | namespace colmap { 40 | 41 | // Camera class that holds the intrinsic parameters. Cameras may be shared 42 | // between multiple images, e.g., if the same "physical" camera took multiple 43 | // pictures with the exact same lens and intrinsics (focal length, etc.). 44 | // This class has a specific distortion model defined by a camera model class. 45 | class Camera { 46 | public: 47 | Camera(); 48 | 49 | // Access the unique identifier of the camera. 50 | inline camera_t CameraId() const; 51 | inline void SetCameraId(const camera_t camera_id); 52 | 53 | // Access the camera model. 54 | inline int ModelId() const; 55 | std::string ModelName() const; 56 | void SetModelId(const int model_id); 57 | void SetModelIdFromName(const std::string &model_name); 58 | 59 | // Access dimensions of the camera sensor. 60 | inline size_t Width() const; 61 | inline size_t Height() const; 62 | inline void SetWidth(const size_t width); 63 | inline void SetHeight(const size_t height); 64 | 65 | // Access focal length parameters. 66 | double MeanFocalLength() const; 67 | double FocalLength() const; 68 | double FocalLengthX() const; 69 | double FocalLengthY() const; 70 | void SetFocalLength(const double focal_length); 71 | void SetFocalLengthX(const double focal_length_x); 72 | void SetFocalLengthY(const double focal_length_y); 73 | 74 | // Check if camera has prior focal length. 75 | inline bool HasPriorFocalLength() const; 76 | inline void SetPriorFocalLength(const bool prior); 77 | 78 | // Access principal point parameters. Only works if there are two 79 | // principal point parameters. 80 | double PrincipalPointX() const; 81 | double PrincipalPointY() const; 82 | void SetPrincipalPointX(const double ppx); 83 | void SetPrincipalPointY(const double ppy); 84 | 85 | // Get the indices of the parameter groups in the parameter vector. 86 | const std::vector &FocalLengthIdxs() const; 87 | const std::vector &PrincipalPointIdxs() const; 88 | const std::vector &ExtraParamsIdxs() const; 89 | 90 | // Get intrinsic calibration matrix composed from focal length and principal 91 | // point parameters, excluding distortion parameters. 92 | Eigen::Matrix3d CalibrationMatrix() const; 93 | 94 | // Get human-readable information about the parameter vector ordering. 95 | std::string ParamsInfo() const; 96 | 97 | // Access the raw parameter vector. 98 | inline size_t NumParams() const; 99 | inline const std::vector &Params() const; 100 | inline std::vector &Params(); 101 | inline double Params(const size_t idx) const; 102 | inline double &Params(const size_t idx); 103 | inline const double *ParamsData() const; 104 | inline double *ParamsData(); 105 | inline void SetParams(const std::vector ¶ms); 106 | 107 | // Set camera parameters from comma-separated list. 108 | bool SetParamsFromString(const std::string &string); 109 | 110 | // Check whether parameters are valid, i.e. the parameter vector has 111 | // the correct dimensions that match the specified camera model. 112 | bool VerifyParams() const; 113 | 114 | // Check whether camera has bogus parameters. 115 | bool HasBogusParams(const double min_focal_length_ratio, 116 | const double max_focal_length_ratio, 117 | const double max_extra_param) const; 118 | 119 | // Initialize parameters for given camera model and focal length, and set 120 | // the principal point to be the image center. 121 | void InitializeWithId(const int model_id, const double focal_length, 122 | const size_t width, const size_t height); 123 | void InitializeWithName(const std::string &model_name, 124 | const double focal_length, const size_t width, 125 | const size_t height); 126 | 127 | // Project point in image plane to world / infinity. 128 | Eigen::Vector2d ImageToWorld(const Eigen::Vector2d &image_point) const; 129 | 130 | // Convert pixel threshold in image plane to world space. 131 | double ImageToWorldThreshold(const double threshold) const; 132 | 133 | // Project point from world / infinity to image plane. 134 | Eigen::Vector2d WorldToImage(const Eigen::Vector2d &world_point) const; 135 | 136 | // Rescale camera dimensions and accordingly the focal length and 137 | // and the principal point. 138 | void Rescale(const double scale); 139 | void Rescale(const size_t width, const size_t height); 140 | 141 | private: 142 | // The unique identifier of the camera. If the identifier is not specified 143 | // it is set to `kInvalidCameraId`. 144 | camera_t camera_id_; 145 | 146 | // The identifier of the camera model. If the camera model is not specified 147 | // the identifier is `kInvalidCameraModelId`. 148 | int model_id_; 149 | 150 | // The dimensions of the image, 0 if not initialized. 151 | size_t width_; 152 | size_t height_; 153 | 154 | // The focal length, principal point, and extra parameters. If the camera 155 | // model is not specified, this vector is empty. 156 | std::vector params_; 157 | 158 | // Whether there is a safe prior for the focal length, 159 | // e.g. manually provided or extracted from EXIF 160 | bool prior_focal_length_; 161 | }; 162 | 163 | //////////////////////////////////////////////////////////////////////////////// 164 | // Implementation 165 | //////////////////////////////////////////////////////////////////////////////// 166 | 167 | camera_t Camera::CameraId() const { return camera_id_; } 168 | 169 | void Camera::SetCameraId(const camera_t camera_id) { camera_id_ = camera_id; } 170 | 171 | int Camera::ModelId() const { return model_id_; } 172 | 173 | size_t Camera::Width() const { return width_; } 174 | 175 | size_t Camera::Height() const { return height_; } 176 | 177 | void Camera::SetWidth(const size_t width) { width_ = width; } 178 | 179 | void Camera::SetHeight(const size_t height) { height_ = height; } 180 | 181 | bool Camera::HasPriorFocalLength() const { return prior_focal_length_; } 182 | 183 | void Camera::SetPriorFocalLength(const bool prior) { 184 | prior_focal_length_ = prior; 185 | } 186 | 187 | size_t Camera::NumParams() const { return params_.size(); } 188 | 189 | const std::vector &Camera::Params() const { return params_; } 190 | 191 | std::vector &Camera::Params() { return params_; } 192 | 193 | double Camera::Params(const size_t idx) const { return params_[idx]; } 194 | 195 | double &Camera::Params(const size_t idx) { return params_[idx]; } 196 | 197 | const double *Camera::ParamsData() const { return params_.data(); } 198 | 199 | double *Camera::ParamsData() { return params_.data(); } 200 | 201 | void Camera::SetParams(const std::vector ¶ms) { params_ = params; } 202 | 203 | } // namespace colmap 204 | 205 | #endif // COLMAP_SRC_BASE_CAMERA_H_ 206 | -------------------------------------------------------------------------------- /src/base/camera_models.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "base/camera_models.h" 33 | 34 | #include 35 | 36 | namespace colmap { 37 | 38 | // Initialize params_info, focal_length_idxs, principal_point_idxs, 39 | // extra_params_idxs 40 | #define CAMERA_MODEL_CASE(CameraModel) \ 41 | const int CameraModel::model_id = InitializeModelId(); \ 42 | const std::string CameraModel::model_name = \ 43 | CameraModel::InitializeModelName(); \ 44 | const size_t CameraModel::num_params = InitializeNumParams(); \ 45 | const std::string CameraModel::params_info = \ 46 | CameraModel::InitializeParamsInfo(); \ 47 | const std::vector CameraModel::focal_length_idxs = \ 48 | CameraModel::InitializeFocalLengthIdxs(); \ 49 | const std::vector CameraModel::principal_point_idxs = \ 50 | CameraModel::InitializePrincipalPointIdxs(); \ 51 | const std::vector CameraModel::extra_params_idxs = \ 52 | CameraModel::InitializeExtraParamsIdxs(); 53 | 54 | CAMERA_MODEL_CASES 55 | 56 | #undef CAMERA_MODEL_CASE 57 | 58 | std::unordered_map InitialzeCameraModelNameToId() { 59 | std::unordered_map camera_model_name_to_id; 60 | 61 | #define CAMERA_MODEL_CASE(CameraModel) \ 62 | camera_model_name_to_id.emplace(CameraModel::model_name, \ 63 | CameraModel::model_id); 64 | 65 | CAMERA_MODEL_CASES 66 | 67 | #undef CAMERA_MODEL_CASE 68 | 69 | return camera_model_name_to_id; 70 | } 71 | 72 | std::unordered_map InitialzeCameraModelIdToName() { 73 | std::unordered_map camera_model_id_to_name; 74 | 75 | #define CAMERA_MODEL_CASE(CameraModel) \ 76 | camera_model_id_to_name.emplace(CameraModel::model_id, \ 77 | CameraModel::model_name); 78 | 79 | CAMERA_MODEL_CASES 80 | 81 | #undef CAMERA_MODEL_CASE 82 | 83 | return camera_model_id_to_name; 84 | } 85 | 86 | static const std::unordered_map CAMERA_MODEL_NAME_TO_ID = 87 | InitialzeCameraModelNameToId(); 88 | 89 | static const std::unordered_map CAMERA_MODEL_ID_TO_NAME = 90 | InitialzeCameraModelIdToName(); 91 | 92 | bool ExistsCameraModelWithName(const std::string &model_name) { 93 | return CAMERA_MODEL_NAME_TO_ID.count(model_name) > 0; 94 | } 95 | 96 | bool ExistsCameraModelWithId(const int model_id) { 97 | return CAMERA_MODEL_ID_TO_NAME.count(model_id) > 0; 98 | } 99 | 100 | int CameraModelNameToId(const std::string &model_name) { 101 | const auto it = CAMERA_MODEL_NAME_TO_ID.find(model_name); 102 | if (it == CAMERA_MODEL_NAME_TO_ID.end()) { 103 | return kInvalidCameraModelId; 104 | } else { 105 | return it->second; 106 | } 107 | } 108 | 109 | std::string CameraModelIdToName(const int model_id) { 110 | const auto it = CAMERA_MODEL_ID_TO_NAME.find(model_id); 111 | if (it == CAMERA_MODEL_ID_TO_NAME.end()) { 112 | return ""; 113 | } else { 114 | return it->second; 115 | } 116 | } 117 | 118 | std::vector CameraModelInitializeParams(const int model_id, 119 | const double focal_length, 120 | const size_t width, 121 | const size_t height) { 122 | // Assuming that image measurements are within [0, dim], i.e. that the 123 | // upper left corner is the (0, 0) coordinate (rather than the center of 124 | // the upper left pixel). This complies with the default SiftGPU convention. 125 | switch (model_id) { 126 | #define CAMERA_MODEL_CASE(CameraModel) \ 127 | case CameraModel::kModelId: \ 128 | return CameraModel::InitializeParams(focal_length, width, height); \ 129 | break; 130 | 131 | CAMERA_MODEL_SWITCH_CASES 132 | 133 | #undef CAMERA_MODEL_CASE 134 | } 135 | } 136 | 137 | std::string CameraModelParamsInfo(const int model_id) { 138 | switch (model_id) { 139 | #define CAMERA_MODEL_CASE(CameraModel) \ 140 | case CameraModel::kModelId: \ 141 | return CameraModel::params_info; \ 142 | break; 143 | 144 | CAMERA_MODEL_SWITCH_CASES 145 | 146 | #undef CAMERA_MODEL_CASE 147 | } 148 | 149 | return "Camera model does not exist"; 150 | } 151 | 152 | static const std::vector EMPTY_IDXS; 153 | 154 | const std::vector &CameraModelFocalLengthIdxs(const int model_id) { 155 | switch (model_id) { 156 | #define CAMERA_MODEL_CASE(CameraModel) \ 157 | case CameraModel::kModelId: \ 158 | return CameraModel::focal_length_idxs; \ 159 | break; 160 | 161 | CAMERA_MODEL_SWITCH_CASES 162 | 163 | #undef CAMERA_MODEL_CASE 164 | } 165 | 166 | return EMPTY_IDXS; 167 | } 168 | 169 | const std::vector &CameraModelPrincipalPointIdxs(const int model_id) { 170 | switch (model_id) { 171 | #define CAMERA_MODEL_CASE(CameraModel) \ 172 | case CameraModel::kModelId: \ 173 | return CameraModel::principal_point_idxs; \ 174 | break; 175 | 176 | CAMERA_MODEL_SWITCH_CASES 177 | 178 | #undef CAMERA_MODEL_CASE 179 | } 180 | 181 | return EMPTY_IDXS; 182 | } 183 | 184 | const std::vector &CameraModelExtraParamsIdxs(const int model_id) { 185 | switch (model_id) { 186 | #define CAMERA_MODEL_CASE(CameraModel) \ 187 | case CameraModel::kModelId: \ 188 | return CameraModel::extra_params_idxs; \ 189 | break; 190 | 191 | CAMERA_MODEL_SWITCH_CASES 192 | 193 | #undef CAMERA_MODEL_CASE 194 | } 195 | 196 | return EMPTY_IDXS; 197 | } 198 | 199 | size_t CameraModelNumParams(const int model_id) { 200 | switch (model_id) { 201 | #define CAMERA_MODEL_CASE(CameraModel) \ 202 | case CameraModel::kModelId: \ 203 | return CameraModel::num_params; 204 | 205 | CAMERA_MODEL_SWITCH_CASES 206 | 207 | #undef CAMERA_MODEL_CASE 208 | } 209 | 210 | return 0; 211 | } 212 | 213 | bool CameraModelVerifyParams(const int model_id, 214 | const std::vector ¶ms) { 215 | switch (model_id) { 216 | #define CAMERA_MODEL_CASE(CameraModel) \ 217 | case CameraModel::kModelId: \ 218 | if (params.size() == CameraModel::num_params) { \ 219 | return true; \ 220 | } \ 221 | break; 222 | 223 | CAMERA_MODEL_SWITCH_CASES 224 | 225 | #undef CAMERA_MODEL_CASE 226 | } 227 | 228 | return false; 229 | } 230 | 231 | bool CameraModelHasBogusParams(const int model_id, 232 | const std::vector ¶ms, 233 | const size_t width, const size_t height, 234 | const double min_focal_length_ratio, 235 | const double max_focal_length_ratio, 236 | const double max_extra_param) { 237 | switch (model_id) { 238 | #define CAMERA_MODEL_CASE(CameraModel) \ 239 | case CameraModel::kModelId: \ 240 | return CameraModel::HasBogusParams( \ 241 | params, width, height, min_focal_length_ratio, \ 242 | max_focal_length_ratio, max_extra_param); \ 243 | break; 244 | 245 | CAMERA_MODEL_SWITCH_CASES 246 | 247 | #undef CAMERA_MODEL_CASE 248 | } 249 | 250 | return false; 251 | } 252 | 253 | } // namespace colmap 254 | -------------------------------------------------------------------------------- /src/base/pose.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #ifndef COLMAP_SRC_BASE_POSE_H_ 33 | #define COLMAP_SRC_BASE_POSE_H_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "util/alignment.h" 40 | #include "util/types.h" 41 | 42 | namespace colmap { 43 | 44 | // Compose the skew symmetric cross product matrix from a vector. 45 | Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d& vector); 46 | 47 | // Convert 3D rotation matrix to Euler angles. 48 | // 49 | // The convention `R = Rx * Ry * Rz` is used, 50 | // using a right-handed coordinate system. 51 | // 52 | // @param R 3x3 rotation matrix. 53 | // @param rx, ry, rz Euler angles in radians. 54 | void RotationMatrixToEulerAngles(const Eigen::Matrix3d& R, double* rx, 55 | double* ry, double* rz); 56 | 57 | // Convert Euler angles to 3D rotation matrix. 58 | // 59 | // The convention `R = Rz * Ry * Rx` is used, 60 | // using a right-handed coordinate system. 61 | // 62 | // @param rx, ry, rz Euler angles in radians. 63 | // 64 | // @return 3x3 rotation matrix. 65 | Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry, 66 | const double rz); 67 | 68 | // Convert 3D rotation matrix to Quaternion representation. 69 | // 70 | // @param rot_mat 3x3 rotation matrix. 71 | // 72 | // @return Unit Quaternion rotation coefficients (w, x, y, z). 73 | Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d& rot_mat); 74 | 75 | // Convert Quaternion representation to 3D rotation matrix. 76 | // 77 | // @param qvec Unit Quaternion rotation coefficients (w, x, y, z). 78 | // 79 | // @return 3x3 rotation matrix. 80 | Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d& qvec); 81 | 82 | // Compose the Quaternion vector corresponding to a identity transformation. 83 | inline Eigen::Vector4d ComposeIdentityQuaternion(); 84 | 85 | // Normalize Quaternion vector. 86 | // 87 | // @param qvec Quaternion rotation coefficients (w, x, y, z). 88 | // 89 | // @return Unit Quaternion rotation coefficients (w, x, y, z). 90 | Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& qvec); 91 | 92 | // Invert Quaternion vector to return Quaternion of inverse rotation. 93 | // 94 | // @param qvec Quaternion rotation coefficients (w, x, y, z). 95 | // 96 | // @return Inverse Quaternion rotation coefficients (w, x, y, z). 97 | Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d& qvec); 98 | 99 | // Concatenate Quaternion rotations such that the rotation of `qvec1` is applied 100 | // before the rotation of `qvec2`. 101 | // 102 | // @param qvec1 Quaternion rotation coefficients (w, x, y, z). 103 | // @param qvec2 Quaternion rotation coefficients (w, x, y, z). 104 | // 105 | // @return Concatenated Quaternion coefficients (w, x, y, z). 106 | Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d& qvec1, 107 | const Eigen::Vector4d& qvec2); 108 | 109 | // Transform point by quaternion rotation. 110 | // 111 | // @param qvec Quaternion rotation coefficients (w, x, y, z). 112 | // @param point Point to rotate. 113 | // 114 | // @return Rotated point. 115 | Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d& qvec, 116 | const Eigen::Vector3d& point); 117 | 118 | // Compute the weighted average of multiple Quaternions according to: 119 | // 120 | // Markley, F. Landis, et al. "Averaging quaternions." 121 | // Journal of Guidance, Control, and Dynamics 30.4 (2007): 1193-1197. 122 | // 123 | // @param qvecs The Quaternions to be averaged. 124 | // @param weights Non-negative weights. 125 | // 126 | // @return The average Quaternion. 127 | Eigen::Vector4d AverageQuaternions(const std::vector& qvecs, 128 | const std::vector& weights); 129 | 130 | // Compose rotation matrix that rotates unit vector 1 to unit vector 2. 131 | // Note that when vector 1 points into the opposite direction of vector 2, 132 | // the function returns an identity rotation. 133 | Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d& vec1, 134 | const Eigen::Vector3d& vec2); 135 | 136 | // Extract camera projection center from projection matrix, i.e. the projection 137 | // center in world coordinates `-R^T t`. 138 | Eigen::Vector3d ProjectionCenterFromMatrix( 139 | const Eigen::Matrix3x4d& proj_matrix); 140 | 141 | // Extract camera projection center from projection parameters. 142 | // 143 | // @param qvec Unit Quaternion rotation coefficients (w, x, y, z). 144 | // @param tvec 3x1 translation vector. 145 | // 146 | // @return 3x1 camera projection center. 147 | Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d& qvec, 148 | const Eigen::Vector3d& tvec); 149 | 150 | // Compute the relative transformation from pose 1 to 2. 151 | // 152 | // @param qvec1, tvec1 First camera pose. 153 | // @param qvec2, tvec2 Second camera pose. 154 | // @param qvec12, tvec12 Relative pose. 155 | void ComputeRelativePose(const Eigen::Vector4d& qvec1, 156 | const Eigen::Vector3d& tvec1, 157 | const Eigen::Vector4d& qvec2, 158 | const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12, 159 | Eigen::Vector3d* tvec12); 160 | 161 | // Concatenate the transformations of the two poses. 162 | // 163 | // @param qvec1, tvec1 First camera pose. 164 | // @param qvec2, tvec2 Second camera pose. 165 | // @param qvec12, tvec12 Concatenated pose. 166 | void ConcatenatePoses(const Eigen::Vector4d& qvec1, 167 | const Eigen::Vector3d& tvec1, 168 | const Eigen::Vector4d& qvec2, 169 | const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12, 170 | Eigen::Vector3d* tvec12); 171 | 172 | // Invert transformation of the pose. 173 | // @param qvec, tvec Input camera pose. 174 | // @param inv_qvec, inv_tvec Inverse camera pose. 175 | void InvertPose(const Eigen::Vector4d& qvec, const Eigen::Vector3d& tvec, 176 | Eigen::Vector4d* inv_qvec, Eigen::Vector3d* inv_tvec); 177 | 178 | // Linearly interpolate camera pose. 179 | // 180 | // @param qvec1, tvec1 Camera pose at t0 = 0. 181 | // @param qvec2, tvec2 Camera pose at t1 = 1. 182 | // @param t Interpolation time. 183 | // @param qveci, tveci Camera pose at time t. 184 | void InterpolatePose(const Eigen::Vector4d& qvec1, const Eigen::Vector3d& tvec1, 185 | const Eigen::Vector4d& qvec2, const Eigen::Vector3d& tvec2, 186 | const double t, Eigen::Vector4d* qveci, 187 | Eigen::Vector3d* tveci); 188 | 189 | // Calculate baseline vector from first to second pose. 190 | // 191 | // The given rotation and orientation is expected as the 192 | // world to camera transformation. 193 | // 194 | // @param qvec1 Unit Quaternion rotation coefficients (w, x, y, z). 195 | // @param tvec1 3x1 translation vector. 196 | // @param qvec2 Unit Quaternion rotation coefficients (w, x, y, z). 197 | // @param tvec2 3x1 translation vector. 198 | // 199 | // @return Baseline vector from 1 to 2. 200 | Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d& qvec1, 201 | const Eigen::Vector3d& tvec1, 202 | const Eigen::Vector4d& qvec2, 203 | const Eigen::Vector3d& tvec2); 204 | 205 | //////////////////////////////////////////////////////////////////////////////// 206 | // Implementation 207 | //////////////////////////////////////////////////////////////////////////////// 208 | 209 | Eigen::Vector4d ComposeIdentityQuaternion() { 210 | return Eigen::Vector4d(1, 0, 0, 0); 211 | } 212 | 213 | } // namespace colmap 214 | 215 | #endif // COLMAP_SRC_BASE_POSE_H_ 216 | -------------------------------------------------------------------------------- /src/base/polynomial.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | // its contributors may be used to endorse or promote products derived 16 | // from this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | #include "base/polynomial.h" 33 | 34 | #include 35 | 36 | namespace colmap { 37 | namespace { 38 | 39 | // Remove leading zero coefficients. 40 | Eigen::VectorXd RemoveLeadingZeros(const Eigen::VectorXd& coeffs) { 41 | Eigen::VectorXd::Index num_zeros = 0; 42 | for (; num_zeros < coeffs.size(); ++num_zeros) { 43 | if (coeffs(num_zeros) != 0) { 44 | break; 45 | } 46 | } 47 | return coeffs.tail(coeffs.size() - num_zeros); 48 | } 49 | 50 | // Remove trailing zero coefficients. 51 | Eigen::VectorXd RemoveTrailingZeros(const Eigen::VectorXd& coeffs) { 52 | Eigen::VectorXd::Index num_zeros = 0; 53 | for (; num_zeros < coeffs.size(); ++num_zeros) { 54 | if (coeffs(coeffs.size() - 1 - num_zeros) != 0) { 55 | break; 56 | } 57 | } 58 | return coeffs.head(coeffs.size() - num_zeros); 59 | } 60 | 61 | } // namespace 62 | 63 | bool FindLinearPolynomialRoots(const Eigen::VectorXd& coeffs, 64 | Eigen::VectorXd* real, Eigen::VectorXd* imag) { 65 | // CHECK_EQ(coeffs.size(), 2); 66 | 67 | if (coeffs(0) == 0) { 68 | return false; 69 | } 70 | 71 | if (real != nullptr) { 72 | real->resize(1); 73 | (*real)(0) = -coeffs(1) / coeffs(0); 74 | } 75 | 76 | if (imag != nullptr) { 77 | imag->resize(1); 78 | (*imag)(0) = 0; 79 | } 80 | 81 | return true; 82 | } 83 | 84 | bool FindQuadraticPolynomialRoots(const Eigen::VectorXd& coeffs, 85 | Eigen::VectorXd* real, 86 | Eigen::VectorXd* imag) { 87 | // CHECK_EQ(coeffs.size(), 3); 88 | 89 | const double a = coeffs(0); 90 | if (a == 0) { 91 | return FindLinearPolynomialRoots(coeffs.tail(2), real, imag); 92 | } 93 | 94 | const double b = coeffs(1); 95 | const double c = coeffs(2); 96 | if (b == 0 && c == 0) { 97 | if (real != nullptr) { 98 | real->resize(1); 99 | (*real)(0) = 0; 100 | } 101 | if (imag != nullptr) { 102 | imag->resize(1); 103 | (*imag)(0) = 0; 104 | } 105 | return true; 106 | } 107 | 108 | const double d = b * b - 4 * a * c; 109 | 110 | if (d >= 0) { 111 | const double sqrt_d = std::sqrt(d); 112 | if (real != nullptr) { 113 | real->resize(2); 114 | if (b >= 0) { 115 | (*real)(0) = (-b - sqrt_d) / (2 * a); 116 | (*real)(1) = (2 * c) / (-b - sqrt_d); 117 | } else { 118 | (*real)(0) = (2 * c) / (-b + sqrt_d); 119 | (*real)(1) = (-b + sqrt_d) / (2 * a); 120 | } 121 | } 122 | if (imag != nullptr) { 123 | imag->resize(2); 124 | imag->setZero(); 125 | } 126 | } else { 127 | if (real != nullptr) { 128 | real->resize(2); 129 | real->setConstant(-b / (2 * a)); 130 | } 131 | if (imag != nullptr) { 132 | imag->resize(2); 133 | (*imag)(0) = std::sqrt(-d) / (2 * a); 134 | (*imag)(1) = -(*imag)(0); 135 | } 136 | } 137 | 138 | return true; 139 | } 140 | 141 | bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd& coeffs_all, 142 | Eigen::VectorXd* real, 143 | Eigen::VectorXd* imag) { 144 | // CHECK_GE(coeffs_all.size(), 2); 145 | 146 | const Eigen::VectorXd coeffs = RemoveLeadingZeros(coeffs_all); 147 | 148 | const int degree = coeffs.size() - 1; 149 | 150 | if (degree <= 0) { 151 | return false; 152 | } else if (degree == 1) { 153 | return FindLinearPolynomialRoots(coeffs, real, imag); 154 | } else if (degree == 2) { 155 | return FindQuadraticPolynomialRoots(coeffs, real, imag); 156 | } 157 | 158 | // Initialize roots. 159 | Eigen::VectorXcd roots(degree); 160 | roots(degree - 1) = std::complex(1, 0); 161 | for (int i = degree - 2; i >= 0; --i) { 162 | roots(i) = roots(i + 1) * std::complex(1, 1); 163 | } 164 | 165 | // Iterative solver. 166 | const int kMaxNumIterations = 100; 167 | const double kMaxRootChange = 1e-10; 168 | for (int iter = 0; iter < kMaxNumIterations; ++iter) { 169 | double max_root_change = 0.0; 170 | for (int i = 0; i < degree; ++i) { 171 | const std::complex root_i = roots(i); 172 | std::complex numerator = coeffs[0]; 173 | std::complex denominator = coeffs[0]; 174 | for (int j = 0; j < degree; ++j) { 175 | numerator = numerator * root_i + coeffs[j + 1]; 176 | if (i != j) { 177 | denominator = denominator * (root_i - roots(j)); 178 | } 179 | } 180 | const std::complex root_i_change = numerator / denominator; 181 | roots(i) = root_i - root_i_change; 182 | max_root_change = 183 | std::max(max_root_change, std::abs(root_i_change.real())); 184 | max_root_change = 185 | std::max(max_root_change, std::abs(root_i_change.imag())); 186 | } 187 | 188 | // Break, if roots do not change anymore. 189 | if (max_root_change < kMaxRootChange) { 190 | break; 191 | } 192 | } 193 | 194 | if (real != nullptr) { 195 | real->resize(degree); 196 | *real = roots.real(); 197 | } 198 | if (imag != nullptr) { 199 | imag->resize(degree); 200 | *imag = roots.imag(); 201 | } 202 | 203 | return true; 204 | } 205 | 206 | bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd& coeffs_all, 207 | Eigen::VectorXd* real, 208 | Eigen::VectorXd* imag) { 209 | // CHECK_GE(coeffs_all.size(), 2); 210 | 211 | Eigen::VectorXd coeffs = RemoveLeadingZeros(coeffs_all); 212 | 213 | const int degree = coeffs.size() - 1; 214 | 215 | if (degree <= 0) { 216 | return false; 217 | } else if (degree == 1) { 218 | return FindLinearPolynomialRoots(coeffs, real, imag); 219 | } else if (degree == 2) { 220 | return FindQuadraticPolynomialRoots(coeffs, real, imag); 221 | } 222 | 223 | // Remove the coefficients where zero is a solution. 224 | coeffs = RemoveTrailingZeros(coeffs); 225 | 226 | // Check if only zero is a solution. 227 | if (coeffs.size() == 1) { 228 | if (real != nullptr) { 229 | real->resize(1); 230 | (*real)(0) = 0; 231 | } 232 | if (imag != nullptr) { 233 | imag->resize(1); 234 | (*imag)(0) = 0; 235 | } 236 | return true; 237 | } 238 | 239 | // Fill the companion matrix. 240 | Eigen::MatrixXd C(coeffs.size() - 1, coeffs.size() - 1); 241 | C.setZero(); 242 | for (Eigen::MatrixXd::Index i = 1; i < C.rows(); ++i) { 243 | C(i, i - 1) = 1; 244 | } 245 | C.row(0) = -coeffs.tail(coeffs.size() - 1) / coeffs(0); 246 | 247 | // Solve for the roots of the polynomial. 248 | Eigen::EigenSolver solver(C, false); 249 | if (solver.info() != Eigen::Success) { 250 | return false; 251 | } 252 | 253 | // If there are trailing zeros, we must add zero as a solution. 254 | const int effective_degree = 255 | coeffs.size() - 1 < degree ? coeffs.size() : coeffs.size() - 1; 256 | 257 | if (real != nullptr) { 258 | real->resize(effective_degree); 259 | real->head(coeffs.size() - 1) = solver.eigenvalues().real(); 260 | if (effective_degree > coeffs.size() - 1) { 261 | (*real)(real->size() - 1) = 0; 262 | } 263 | } 264 | if (imag != nullptr) { 265 | imag->resize(effective_degree); 266 | imag->head(coeffs.size() - 1) = solver.eigenvalues().imag(); 267 | if (effective_degree > coeffs.size() - 1) { 268 | (*imag)(imag->size() - 1) = 0; 269 | } 270 | } 271 | 272 | return true; 273 | } 274 | 275 | } // namespace colmap 276 | --------------------------------------------------------------------------------