├── README.md ├── base ├── CMakeLists.txt ├── camera.cc ├── camera.h ├── camera_database.cc ├── camera_database_test.cc ├── camera_models.cc ├── camera_models.h ├── camera_models_test.cc ├── camera_rig.cc ├── camera_rig.h ├── camera_rig_test.cc ├── camera_test.cc ├── correspondence_graph.cc ├── correspondence_graph.h ├── cost_functions.h ├── cost_functions_test.cc ├── database.cc ├── database.h ├── database_cache.h ├── database_cache_test.cc ├── essential_matrix.cc ├── essential_matrix.h ├── gps.cc ├── gps.h ├── gps_test.cc ├── graph_cut.cc ├── graph_cut_test.cc ├── homography_matrix.cc ├── homography_matrix.h ├── homography_matrix_test.cc ├── image.cc ├── image.h ├── line.cc ├── line.h ├── line_test.cc ├── point2d.h ├── point2d_test.cc ├── point3d.h ├── point3d_test.cc ├── polynomial.h ├── pose.h ├── projection.h ├── projection_test.cc ├── reconstruction.cc ├── reconstruction.h ├── reconstruction_manager.h ├── reconstruction_manager_test.cc ├── reconstruction_test.cc ├── scene_clustering.cc ├── scene_clustering.h ├── scene_clustering_test.cc ├── similarity_transform.h ├── similarity_transform_test.cc ├── track.cc ├── track_test.cc ├── triangulation.cc ├── triangulation.h ├── triangulation_test.cc ├── undistortion.cc ├── undistortion.h ├── undistortion_test.cc ├── visibility_pyramid.cc ├── warp.cc ├── warp.h └── warp_test.cc ├── controllers ├── CMakeLists.txt ├── automatic_reconstruction.cc ├── automatic_reconstruction.h ├── bundle_adjustment.cc ├── bundle_adjustment.h ├── hierarchical_mapper.cc ├── hierarchical_mapper.h ├── incremental_mapper.cc └── incremental_mapper.h ├── example ├── 3D_from_prior_pose.sh ├── model │ ├── 3D.sh │ ├── cameras.txt │ └── images.txt └── trans.py └── optim ├── CMakeLists.txt ├── bundle_adjustment.cc ├── bundle_adjustment.h ├── bundle_adjustment_test.cc ├── combination_sampler.cc ├── combination_sampler.h ├── combination_sampler_test.cc ├── least_absolute_deviations.cc ├── least_absolute_deviations.h ├── least_absolute_deviations_test.cc ├── loransac.h ├── loransac_test.cc ├── progressive_sampler.cc ├── progressive_sampler.h ├── progressive_sampler_test.cc ├── random_sampler.cc ├── random_sampler.h ├── random_sampler_test.cc ├── ransac.h ├── ransac_test.cc ├── sampler.h ├── sprt.cc ├── sprt.h ├── support_measurement.cc ├── support_measurement.h └── support_measurement_test.cc /README.md: -------------------------------------------------------------------------------- 1 | # Mapping-base-lidar-pose-or-vslam-pose 2 | I simply modified the colmap,when it reconstructs from known pose ,only let it optimize rotation ,fixing position! 3 | Why do I do this, because when reconstructing from a known pose, if you use the rtk position, there is no need to optimize the position during BA fine-tuning. You can only fine-tune the rotation(3dof +3Dpoints optimize), but this is a very loose coupling, if you want to use rtk to constrain BA, you can look at openmvg ---"**sfm_data_BA_ceres.cpp**". You can implement it in colmap immediately！！！！！！！ 4 | 5 | Reconstructing from a known pose reference : https://colmap.github.io/faq.html?highlight=known%20pose#reconstruct-sparse-dense-model-from-known-camera-poses 6 | 7 | 8 | ## first step 9 | ### (1) base/cost_functions.h replace your colmap code 10 | ### (2) controllers/bundle_adjustment.cc replace your colmap code 11 | ### (3) optim/bundle_adjustment.cc replace your colmap code 12 | ## second step 13 | ### example folder 14 | 15 | ..........model 16 | 17 | ................images.txt // your piror pose 18 | 19 | ................cameras.txt 20 | 21 | ................points3D.txt //empty file 22 | 23 | ..........trans.py // trans colmap tvec to camera center and trans camera center to colmap tvec(-R.T*tvec=C and tvec=-R*C) 24 | 25 | ..........run 3D_from_prior_pose.sh 26 | 27 | -------------------------------------------------------------------------------- /base/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 | set(FOLDER_NAME "base") 33 | 34 | COLMAP_ADD_SOURCES( 35 | camera.h camera.cc 36 | camera_database.h camera_database.cc 37 | camera_models.h camera_models.cc 38 | camera_rig.h camera_rig.cc 39 | correspondence_graph.h correspondence_graph.cc 40 | database.h database.cc 41 | database_cache.h database_cache.cc 42 | essential_matrix.h essential_matrix.cc 43 | gps.h gps.cc 44 | graph_cut.h graph_cut.cc 45 | homography_matrix.h homography_matrix.cc 46 | image.h image.cc 47 | image_reader.h image_reader.cc 48 | line.h line.cc 49 | point2d.h point2d.cc 50 | point3d.h point3d.cc 51 | polynomial.h polynomial.cc 52 | pose.h pose.cc 53 | projection.h projection.cc 54 | reconstruction.h reconstruction.cc 55 | reconstruction_manager.h reconstruction_manager.cc 56 | scene_clustering.h scene_clustering.cc 57 | similarity_transform.h similarity_transform.cc 58 | track.h track.cc 59 | triangulation.h triangulation.cc 60 | undistortion.h undistortion.cc 61 | visibility_pyramid.h visibility_pyramid.cc 62 | warp.h warp.cc 63 | ) 64 | 65 | COLMAP_ADD_TEST(camera_database_test camera_database_test.cc) 66 | COLMAP_ADD_TEST(camera_models_test camera_models_test.cc) 67 | COLMAP_ADD_TEST(camera_rig_test camera_rig_test.cc) 68 | COLMAP_ADD_TEST(camera_test camera_test.cc) 69 | COLMAP_ADD_TEST(correspondence_graph_test correspondence_graph_test.cc) 70 | COLMAP_ADD_TEST(cost_functions_test cost_functions_test.cc) 71 | COLMAP_ADD_TEST(database_cache_test database_cache_test.cc) 72 | COLMAP_ADD_TEST(database_test database_test.cc) 73 | COLMAP_ADD_TEST(essential_matrix_utils_test essential_matrix_test.cc) 74 | COLMAP_ADD_TEST(gps_test gps_test.cc) 75 | COLMAP_ADD_TEST(graph_cut_test graph_cut_test.cc) 76 | COLMAP_ADD_TEST(homography_matrix_utils_test homography_matrix_test.cc) 77 | COLMAP_ADD_TEST(image_test image_test.cc) 78 | COLMAP_ADD_TEST(line_test line_test.cc) 79 | COLMAP_ADD_TEST(point2d_test point2d_test.cc) 80 | COLMAP_ADD_TEST(point3d_test point3d_test.cc) 81 | COLMAP_ADD_TEST(polynomial_test polynomial_test.cc) 82 | COLMAP_ADD_TEST(pose_test pose_test.cc) 83 | COLMAP_ADD_TEST(projection_test projection_test.cc) 84 | COLMAP_ADD_TEST(reconstruction_test reconstruction_test.cc) 85 | COLMAP_ADD_TEST(reconstruction_manager_test reconstruction_manager_test.cc) 86 | COLMAP_ADD_TEST(scene_clustering_test scene_clustering_test.cc) 87 | COLMAP_ADD_TEST(similarity_transform_test similarity_transform_test.cc) 88 | COLMAP_ADD_TEST(track_test track_test.cc) 89 | COLMAP_ADD_TEST(triangulation_test triangulation_test.cc) 90 | COLMAP_ADD_TEST(undistortion_test undistortion_test.cc) 91 | COLMAP_ADD_TEST(visibility_pyramid_test visibility_pyramid_test.cc) 92 | COLMAP_ADD_TEST(warp_test warp_test.cc) 93 | -------------------------------------------------------------------------------- /base/camera_database.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_database.h" 33 | 34 | #include "util/string.h" 35 | 36 | namespace colmap { 37 | 38 | const camera_specs_t CameraDatabase::specs_ = InitializeCameraSpecs(); 39 | 40 | CameraDatabase::CameraDatabase() {} 41 | 42 | bool CameraDatabase::QuerySensorWidth(const std::string& make, 43 | const std::string& model, 44 | double* sensor_width) { 45 | // Clean the strings from all separators. 46 | std::string cleaned_make = make; 47 | std::string cleaned_model = model; 48 | cleaned_make = StringReplace(cleaned_make, " ", ""); 49 | cleaned_model = StringReplace(cleaned_model, " ", ""); 50 | cleaned_make = StringReplace(cleaned_make, "-", ""); 51 | cleaned_model = StringReplace(cleaned_model, "-", ""); 52 | StringToLower(&cleaned_make); 53 | StringToLower(&cleaned_model); 54 | 55 | // Make sure that make name is not duplicated. 56 | cleaned_model = StringReplace(cleaned_model, cleaned_make, ""); 57 | 58 | // Check if cleaned_make exists in database: Test whether EXIF string is 59 | // substring of database entry and vice versa. 60 | size_t spec_matches = 0; 61 | for (const auto& make_elem : specs_) { 62 | if (StringContains(cleaned_make, make_elem.first) || 63 | StringContains(make_elem.first, cleaned_make)) { 64 | for (const auto& model_elem : make_elem.second) { 65 | if (StringContains(cleaned_model, model_elem.first) || 66 | StringContains(model_elem.first, cleaned_model)) { 67 | *sensor_width = model_elem.second; 68 | if (cleaned_model == model_elem.first) { 69 | // Model exactly matches, return immediately. 70 | return true; 71 | } 72 | spec_matches += 1; 73 | if (spec_matches > 1) { 74 | break; 75 | } 76 | } 77 | } 78 | } 79 | } 80 | 81 | // Only return unique results, if model does not exactly match. 82 | return spec_matches == 1; 83 | } 84 | 85 | } // namespace colmap 86 | -------------------------------------------------------------------------------- /base/camera_database_test.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 | #define TEST_NAME "base/camera_database" 33 | #include "util/testing.h" 34 | 35 | #include "base/camera_database.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestInitialization) { 40 | CameraDatabase database; 41 | camera_specs_t specs = InitializeCameraSpecs(); 42 | BOOST_CHECK_EQUAL(database.NumEntries(), specs.size()); 43 | } 44 | 45 | BOOST_AUTO_TEST_CASE(TestExactMatch) { 46 | CameraDatabase database; 47 | double sensor_width; 48 | BOOST_CHECK( 49 | database.QuerySensorWidth("canon", "digitalixus100is", &sensor_width)); 50 | BOOST_CHECK_EQUAL(sensor_width, 6.1600f); 51 | } 52 | 53 | BOOST_AUTO_TEST_CASE(TestAmbiguousMatch) { 54 | CameraDatabase database; 55 | double sensor_width; 56 | BOOST_CHECK( 57 | !database.QuerySensorWidth("canon", "digitalixus", &sensor_width)); 58 | BOOST_CHECK_EQUAL(sensor_width, 6.1600f); 59 | } 60 | -------------------------------------------------------------------------------- /base/camera_rig.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_RIG_H_ 33 | #define COLMAP_SRC_BASE_CAMERA_RIG_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include "base/camera.h" 39 | #include "base/pose.h" 40 | #include "base/reconstruction.h" 41 | #include "util/alignment.h" 42 | #include "util/types.h" 43 | 44 | namespace colmap { 45 | 46 | // This class holds information about the relative configuration of camera rigs. 47 | // Camera rigs are composed of multiple cameras with a rigid relative extrinsic 48 | // configuration over multiple snapshots. Snapshots are defined as the 49 | // collection of images captured simultaneously by all cameras in the rig. 50 | class CameraRig { 51 | public: 52 | CameraRig(); 53 | 54 | // The number of cameras in the rig. 55 | size_t NumCameras() const; 56 | 57 | // The number of snapshots captured by this rig. 58 | size_t NumSnapshots() const; 59 | 60 | // Check whether the given camera is part of the rig. 61 | bool HasCamera(const camera_t camera_id) const; 62 | 63 | // Access the reference camera. 64 | camera_t RefCameraId() const; 65 | void SetRefCameraId(const camera_t camera_id); 66 | 67 | // Get the identifiers of the cameras in the rig. 68 | std::vector GetCameraIds() const; 69 | 70 | // Get the snapshots of the camera rig. 71 | const std::vector>& Snapshots() const; 72 | 73 | // Add a new camera to the rig. The relative pose may contain dummy values and 74 | // can then be computed automatically from a given reconstruction using the 75 | // method `ComputeRelativePoses`. 76 | void AddCamera(const camera_t camera_id, const Eigen::Vector4d& rel_qvec, 77 | const Eigen::Vector3d& rel_tvec); 78 | 79 | // Add the images of a single snapshot to rig. A snapshot consists of the 80 | // captured images of all cameras of the rig. All images of a snapshot share 81 | // the same global camera rig pose, i.e. all images in the camera rig are 82 | // captured simultaneously. 83 | void AddSnapshot(const std::vector& image_ids); 84 | 85 | // Check whether the camera rig setup is valid. 86 | void Check(const Reconstruction& reconstruction) const; 87 | 88 | // Get the relative poses of the cameras in the rig. 89 | Eigen::Vector4d& RelativeQvec(const camera_t camera_id); 90 | const Eigen::Vector4d& RelativeQvec(const camera_t camera_id) const; 91 | Eigen::Vector3d& RelativeTvec(const camera_t camera_id); 92 | const Eigen::Vector3d& RelativeTvec(const camera_t camera_id) const; 93 | 94 | // Compute the scaling factor from the reconstruction to the camera rig 95 | // dimensions by averaging over the distances between the projection centers. 96 | // Note that this assumes that there is at least one camera pair in the rig 97 | // with non-zero baseline, otherwise the function returns NaN. 98 | double ComputeScale(const Reconstruction& reconstruction) const; 99 | 100 | // Compute the relative poses in the rig from the reconstruction by averaging 101 | // the relative poses over all snapshots. The pose of the reference camera 102 | // will be the identity transformation. This assumes that the camera rig has 103 | // snapshots that are registered in the reconstruction. 104 | void ComputeRelativePoses(const Reconstruction& reconstruction); 105 | 106 | // Compute the absolute camera pose of the rig. The absolute camera pose of 107 | // the rig is computed as the average of all relative camera poses in the rig 108 | // and their corresponding image poses in the reconstruction. 109 | void ComputeAbsolutePose(const size_t snapshot_idx, 110 | const Reconstruction& reconstruction, 111 | Eigen::Vector4d* abs_qvec, 112 | Eigen::Vector3d* abs_tvec) const; 113 | 114 | private: 115 | struct RigCamera { 116 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 117 | Eigen::Vector4d rel_qvec = ComposeIdentityQuaternion(); 118 | Eigen::Vector3d rel_tvec = Eigen::Vector3d(0, 0, 0); 119 | }; 120 | 121 | camera_t ref_camera_id_ = kInvalidCameraId; 122 | EIGEN_STL_UMAP(camera_t, RigCamera) rig_cameras_; 123 | std::vector> snapshots_; 124 | }; 125 | 126 | } // namespace colmap 127 | 128 | #endif // COLMAP_SRC_BASE_CAMERA_RIG_H_ 129 | -------------------------------------------------------------------------------- /base/database_cache_test.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 | #define TEST_NAME "base/database_cache" 33 | #include "util/testing.h" 34 | 35 | #include "base/database_cache.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestEmpty) { 40 | DatabaseCache cache; 41 | BOOST_CHECK_EQUAL(cache.NumCameras(), 0); 42 | BOOST_CHECK_EQUAL(cache.NumImages(), 0); 43 | } 44 | 45 | BOOST_AUTO_TEST_CASE(TestAddCamera) { 46 | DatabaseCache cache; 47 | Camera camera; 48 | camera.SetCameraId(1); 49 | camera.InitializeWithId(SimplePinholeCameraModel::model_id, 1, 1, 1); 50 | cache.AddCamera(camera); 51 | BOOST_CHECK_EQUAL(cache.NumCameras(), 1); 52 | BOOST_CHECK_EQUAL(cache.NumImages(), 0); 53 | BOOST_CHECK(cache.ExistsCamera(camera.CameraId())); 54 | BOOST_CHECK_EQUAL(cache.Camera(camera.CameraId()).ModelId(), 55 | camera.ModelId()); 56 | } 57 | 58 | BOOST_AUTO_TEST_CASE(TestDegenerateCamera) { 59 | DatabaseCache cache; 60 | Camera camera; 61 | camera.InitializeWithId(SimplePinholeCameraModel::model_id, 1, 1, 1); 62 | cache.AddCamera(camera); 63 | BOOST_CHECK_EQUAL(cache.NumCameras(), 1); 64 | BOOST_CHECK_EQUAL(cache.NumImages(), 0); 65 | BOOST_CHECK(cache.ExistsCamera(camera.CameraId())); 66 | BOOST_CHECK_EQUAL(cache.Camera(camera.CameraId()).MeanFocalLength(), 1); 67 | } 68 | 69 | BOOST_AUTO_TEST_CASE(TestAddImage) { 70 | DatabaseCache cache; 71 | Image image; 72 | image.SetImageId(1); 73 | image.SetPoints2D(std::vector(10)); 74 | cache.AddImage(image); 75 | BOOST_CHECK_EQUAL(cache.NumCameras(), 0); 76 | BOOST_CHECK_EQUAL(cache.NumImages(), 1); 77 | BOOST_CHECK(cache.ExistsImage(image.ImageId())); 78 | BOOST_CHECK_EQUAL(cache.Image(image.ImageId()).NumPoints2D(), 79 | image.NumPoints2D()); 80 | BOOST_CHECK(cache.CorrespondenceGraph().ExistsImage(image.ImageId())); 81 | BOOST_CHECK_EQUAL( 82 | cache.CorrespondenceGraph().NumCorrespondencesForImage(image.ImageId()), 83 | 0); 84 | BOOST_CHECK_EQUAL( 85 | cache.CorrespondenceGraph().NumObservationsForImage(image.ImageId()), 0); 86 | } 87 | -------------------------------------------------------------------------------- /base/gps.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/gps.h" 33 | 34 | #include "util/math.h" 35 | 36 | namespace colmap { 37 | 38 | GPSTransform::GPSTransform(const int ellipsoid) { 39 | switch (ellipsoid) { 40 | case GRS80: 41 | a_ = 6378137; 42 | b_ = 6.356752314140356e+06; 43 | f_ = 0.003352810681182; 44 | break; 45 | case WGS84: 46 | a_ = 6378137; 47 | b_ = 6.356752314245179e+06; 48 | f_ = 0.003352810664747; 49 | break; 50 | default: 51 | a_ = std::numeric_limits::quiet_NaN(); 52 | b_ = std::numeric_limits::quiet_NaN(); 53 | f_ = std::numeric_limits::quiet_NaN(); 54 | throw std::invalid_argument("Ellipsoid not defined"); 55 | } 56 | 57 | e2_ = (a_ * a_ - b_ * b_) / (a_ * a_); 58 | } 59 | 60 | std::vector GPSTransform::EllToXYZ( 61 | const std::vector& ell) const { 62 | std::vector xyz(ell.size()); 63 | 64 | for (size_t i = 0; i < ell.size(); ++i) { 65 | const double lat = DegToRad(ell[i](0)); 66 | const double lon = DegToRad(ell[i](1)); 67 | const double alt = ell[i](2); 68 | 69 | const double sin_lat = sin(lat); 70 | const double sin_lon = sin(lon); 71 | const double cos_lat = cos(lat); 72 | const double cos_lon = cos(lon); 73 | 74 | // Normalized radius 75 | const double N = a_ / sqrt(1 - e2_ * sin_lat * sin_lat); 76 | 77 | xyz[i](0) = (N + alt) * cos_lat * cos_lon; 78 | xyz[i](1) = (N + alt) * cos_lat * sin_lon; 79 | xyz[i](2) = (N * (1 - e2_) + alt) * sin_lat; 80 | } 81 | 82 | return xyz; 83 | } 84 | 85 | std::vector GPSTransform::XYZToEll( 86 | const std::vector& xyz) const { 87 | std::vector ell(xyz.size()); 88 | 89 | for (size_t i = 0; i < ell.size(); ++i) { 90 | const double x = xyz[i](0); 91 | const double y = xyz[i](1); 92 | const double z = xyz[i](2); 93 | 94 | const double xx = x * x; 95 | const double yy = y * y; 96 | 97 | const double kEps = 1e-12; 98 | 99 | // Latitude 100 | double lat = atan2(z, sqrt(xx + yy)); 101 | double alt; 102 | 103 | for (size_t j = 0; j < 100; ++j) { 104 | const double sin_lat0 = sin(lat); 105 | const double N = a_ / sqrt(1 - e2_ * sin_lat0 * sin_lat0); 106 | alt = sqrt(xx + yy) / cos(lat) - N; 107 | const double prev_lat = lat; 108 | lat = atan((z / sqrt(xx + yy)) * 1 / (1 - e2_ * N / (N + alt))); 109 | 110 | if (std::abs(prev_lat - lat) < kEps) { 111 | break; 112 | } 113 | } 114 | 115 | ell[i](0) = RadToDeg(lat); 116 | 117 | // Longitude 118 | ell[i](1) = RadToDeg(atan2(y, x)); 119 | // Alt 120 | ell[i](2) = alt; 121 | } 122 | 123 | return ell; 124 | } 125 | 126 | } // namespace colmap 127 | -------------------------------------------------------------------------------- /base/gps.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_GPS_H_ 33 | #define COLMAP_SRC_BASE_GPS_H_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "util/alignment.h" 40 | #include "util/types.h" 41 | 42 | namespace colmap { 43 | 44 | // Transform ellipsoidal GPS coordinates to Cartesian GPS coordinate 45 | // representation and vice versa. 46 | class GPSTransform { 47 | public: 48 | enum ELLIPSOID { GRS80, WGS84 }; 49 | 50 | explicit GPSTransform(const int ellipsoid = GRS80); 51 | 52 | std::vector EllToXYZ( 53 | const std::vector& ell) const; 54 | 55 | std::vector XYZToEll( 56 | const std::vector& xyz) const; 57 | 58 | private: 59 | // Semimajor axis. 60 | double a_; 61 | // Semiminor axis. 62 | double b_; 63 | // Flattening. 64 | double f_; 65 | // Numerical eccentricity. 66 | double e2_; 67 | }; 68 | 69 | } // namespace colmap 70 | 71 | #endif // COLMAP_SRC_BASE_GPS_H_ 72 | -------------------------------------------------------------------------------- /base/homography_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_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ 33 | #define COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_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 | // Decompose an homography matrix into the possible rotations, translations, 45 | // and plane normal vectors, according to: 46 | // 47 | // Malis, Ezio, and Manuel Vargas. "Deeper understanding of the homography 48 | // decomposition for vision-based control." (2007): 90. 49 | // 50 | // The first pose is assumed to be P = [I | 0]. Note that the homography is 51 | // plane-induced if `R.size() == t.size() == n.size() == 4`. If `R.size() == 52 | // t.size() == n.size() == 1` the homography is pure-rotational. 53 | // 54 | // @param H 3x3 homography matrix. 55 | // @param K 3x3 calibration matrix. 56 | // @param R Possible 3x3 rotation matrices. 57 | // @param t Possible translation vectors. 58 | // @param n Possible normal vectors. 59 | void DecomposeHomographyMatrix(const Eigen::Matrix3d& H, 60 | const Eigen::Matrix3d& K1, 61 | const Eigen::Matrix3d& K2, 62 | std::vector* R, 63 | std::vector* t, 64 | std::vector* n); 65 | 66 | // Recover the most probable pose from the given homography matrix. 67 | // 68 | // The pose of the first image is assumed to be P = [I | 0]. 69 | // 70 | // @param H 3x3 homography matrix. 71 | // @param K1 3x3 calibration matrix of first camera. 72 | // @param K2 3x3 calibration matrix of second camera. 73 | // @param points1 First set of corresponding points. 74 | // @param points2 Second set of corresponding points. 75 | // @param inlier_mask Only points with `true` in the inlier mask are 76 | // considered in the cheirality test. Size of the 77 | // inlier mask must match the number of points N. 78 | // @param R Most probable 3x3 rotation matrix. 79 | // @param t Most probable 3x1 translation vector. 80 | // @param n Most probable 3x1 normal vector. 81 | // @param points3D Triangulated 3D points infront of camera 82 | // (only if homography is not pure-rotational). 83 | void PoseFromHomographyMatrix(const Eigen::Matrix3d& H, 84 | const Eigen::Matrix3d& K1, 85 | const Eigen::Matrix3d& K2, 86 | const std::vector& points1, 87 | const std::vector& points2, 88 | Eigen::Matrix3d* R, Eigen::Vector3d* t, 89 | Eigen::Vector3d* n, 90 | std::vector* points3D); 91 | 92 | // Compose homography matrix from relative pose. 93 | // 94 | // @param K1 3x3 calibration matrix of first camera. 95 | // @param K2 3x3 calibration matrix of second camera. 96 | // @param R Most probable 3x3 rotation matrix. 97 | // @param t Most probable 3x1 translation vector. 98 | // @param n Most probable 3x1 normal vector. 99 | // @param d Orthogonal distance from plane. 100 | // 101 | // @return 3x3 homography matrix. 102 | Eigen::Matrix3d HomographyMatrixFromPose(const Eigen::Matrix3d& K1, 103 | const Eigen::Matrix3d& K2, 104 | const Eigen::Matrix3d& R, 105 | const Eigen::Vector3d& t, 106 | const Eigen::Vector3d& n, 107 | const double d); 108 | 109 | } // namespace colmap 110 | 111 | #endif // COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ 112 | -------------------------------------------------------------------------------- /base/homography_matrix_test.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 | #define TEST_NAME "base/homography_matrix" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "base/homography_matrix.h" 38 | 39 | using namespace colmap; 40 | 41 | // Note that the test case values are obtained from OpenCV. 42 | BOOST_AUTO_TEST_CASE(TestDecomposeHomographyMatrix) { 43 | Eigen::Matrix3d H; 44 | H << 2.649157564634028, 4.583875997496426, 70.694447785121326, 45 | -1.072756858861583, 3.533262150437228, 1513.656999614321649, 46 | 0.001303887589576, 0.003042206876298, 1; 47 | H *= 3; 48 | 49 | Eigen::Matrix3d K; 50 | K << 640, 0, 320, 0, 640, 240, 0, 0, 1; 51 | 52 | std::vector R; 53 | std::vector t; 54 | std::vector n; 55 | DecomposeHomographyMatrix(H, K, K, &R, &t, &n); 56 | 57 | BOOST_CHECK_EQUAL(R.size(), 4); 58 | BOOST_CHECK_EQUAL(t.size(), 4); 59 | BOOST_CHECK_EQUAL(n.size(), 4); 60 | 61 | Eigen::Matrix3d R_ref; 62 | R_ref << 0.43307983549125, 0.545749113549648, -0.717356090899523, 63 | -0.85630229674426, 0.497582023798831, -0.138414255706431, 64 | 0.281404038139784, 0.67421809131173, 0.682818960388909; 65 | const Eigen::Vector3d t_ref(1.826751712278038, 1.264718492450820, 66 | 0.195080809998819); 67 | const Eigen::Vector3d n_ref(-0.244875830334816, -0.480857890778889, 68 | -0.841909446789566); 69 | 70 | bool ref_solution_exists = false; 71 | for (size_t i = 0; i < 4; ++i) { 72 | const double kEps = 1e-6; 73 | if ((R[i] - R_ref).norm() < kEps && (t[i] - t_ref).norm() < kEps && 74 | (n[i] - n_ref).norm() < kEps) { 75 | ref_solution_exists = true; 76 | } 77 | } 78 | BOOST_CHECK(ref_solution_exists); 79 | } 80 | 81 | BOOST_AUTO_TEST_CASE(TestPoseFromHomographyMatrix) { 82 | const Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity(); 83 | const Eigen::Matrix3d K2 = Eigen::Matrix3d::Identity(); 84 | const Eigen::Matrix3d R_ref = Eigen::Matrix3d::Identity(); 85 | const Eigen::Vector3d t_ref(1, 0, 0); 86 | const Eigen::Vector3d n_ref(-1, 0, 0); 87 | const double d_ref = 1; 88 | const Eigen::Matrix3d H = 89 | HomographyMatrixFromPose(K1, K2, R_ref, t_ref, n_ref, d_ref); 90 | 91 | std::vector points1; 92 | points1.emplace_back(0.1, 0.4); 93 | points1.emplace_back(0.2, 0.3); 94 | points1.emplace_back(0.3, 0.2); 95 | points1.emplace_back(0.4, 0.1); 96 | 97 | std::vector points2; 98 | for (const auto& point1 : points1) { 99 | const Eigen::Vector3d point2 = H * point1.homogeneous(); 100 | points2.push_back(point2.hnormalized()); 101 | } 102 | 103 | Eigen::Matrix3d R; 104 | Eigen::Vector3d t; 105 | Eigen::Vector3d n; 106 | std::vector points3D; 107 | PoseFromHomographyMatrix(H, K1, K2, points1, points2, &R, &t, &n, &points3D); 108 | 109 | BOOST_CHECK_EQUAL(R, R_ref); 110 | BOOST_CHECK_EQUAL(t, t_ref); 111 | BOOST_CHECK_EQUAL(n, n_ref); 112 | BOOST_CHECK_EQUAL(points3D.size(), points1.size()); 113 | } 114 | 115 | BOOST_AUTO_TEST_CASE(TestHomographyMatrixFromPosePureRotation) { 116 | const Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity(); 117 | const Eigen::Matrix3d K2 = Eigen::Matrix3d::Identity(); 118 | const Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); 119 | const Eigen::Vector3d t(0, 0, 0); 120 | const Eigen::Vector3d n(-1, 0, 0); 121 | const double d = 1; 122 | const Eigen::Matrix3d H = HomographyMatrixFromPose(K1, K2, R, t, n, d); 123 | BOOST_CHECK_EQUAL(H, Eigen::Matrix3d::Identity()); 124 | } 125 | 126 | BOOST_AUTO_TEST_CASE(TestHomographyMatrixFromPosePlanarScene) { 127 | const Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity(); 128 | const Eigen::Matrix3d K2 = Eigen::Matrix3d::Identity(); 129 | const Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); 130 | const Eigen::Vector3d t(1, 0, 0); 131 | const Eigen::Vector3d n(-1, 0, 0); 132 | const double d = 1; 133 | const Eigen::Matrix3d H = HomographyMatrixFromPose(K1, K2, R, t, n, d); 134 | Eigen::Matrix3d H_ref; 135 | H_ref << 2, 0, 0, 0, 1, 0, 0, 0, 1; 136 | BOOST_CHECK_EQUAL(H, H_ref); 137 | } 138 | -------------------------------------------------------------------------------- /base/line.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/line.h" 33 | 34 | #include "util/logging.h" 35 | 36 | extern "C" { 37 | #include "LSD/lsd.h" 38 | } 39 | 40 | namespace colmap { 41 | 42 | std::vector DetectLineSegments(const Bitmap& bitmap, 43 | const double min_length) { 44 | const double min_length_squared = min_length * min_length; 45 | 46 | std::vector bitmap_data; 47 | if (bitmap.IsGrey()) { 48 | bitmap_data = bitmap.ConvertToRowMajorArray(); 49 | } else { 50 | const Bitmap bitmap_gray = bitmap.CloneAsGrey(); 51 | bitmap_data = bitmap_gray.ConvertToRowMajorArray(); 52 | } 53 | 54 | std::vector bitmap_data_double(bitmap_data.begin(), 55 | bitmap_data.end()); 56 | 57 | int num_segments; 58 | std::unique_ptr segments_data(lsd(&num_segments, 59 | bitmap_data_double.data(), 60 | bitmap.Width(), bitmap.Height())); 61 | 62 | std::vector segments; 63 | segments.reserve(num_segments); 64 | for (int i = 0; i < num_segments; ++i) { 65 | const Eigen::Vector2d start(segments_data.get()[i * 7], 66 | segments_data.get()[i * 7 + 1]); 67 | const Eigen::Vector2d end(segments_data.get()[i * 7 + 2], 68 | segments_data.get()[i * 7 + 3]); 69 | if ((start - end).squaredNorm() >= min_length_squared) { 70 | segments.emplace_back(); 71 | segments.back().start = start; 72 | segments.back().end = end; 73 | } 74 | } 75 | 76 | return segments; 77 | } 78 | 79 | std::vector ClassifyLineSegmentOrientations( 80 | const std::vector& segments, const double tolerance) { 81 | CHECK_LE(tolerance, 0.5); 82 | 83 | std::vector orientations; 84 | orientations.reserve(segments.size()); 85 | 86 | for (const auto& segment : segments) { 87 | const Eigen::Vector2d direction = 88 | (segment.end - segment.start).normalized(); 89 | if (std::abs(direction.x()) + tolerance > 1) { 90 | orientations.push_back(LineSegmentOrientation::HORIZONTAL); 91 | } else if (std::abs(direction.y()) + tolerance > 1) { 92 | orientations.push_back(LineSegmentOrientation::VERTICAL); 93 | } else { 94 | orientations.push_back(LineSegmentOrientation::UNDEFINED); 95 | } 96 | } 97 | 98 | return orientations; 99 | } 100 | 101 | } // namespace colmap 102 | -------------------------------------------------------------------------------- /base/line.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_LINE_H_ 33 | #define COLMAP_SRC_BASE_LINE_H_ 34 | 35 | #include 36 | 37 | #include "util/alignment.h" 38 | #include "util/bitmap.h" 39 | 40 | namespace colmap { 41 | 42 | struct LineSegment { 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | Eigen::Vector2d start; 45 | Eigen::Vector2d end; 46 | }; 47 | 48 | enum class LineSegmentOrientation { 49 | HORIZONTAL = 1, 50 | VERTICAL = -1, 51 | UNDEFINED = 0, 52 | }; 53 | 54 | // Detect line segments in the given bitmap image. 55 | std::vector DetectLineSegments(const Bitmap& bitmap, 56 | const double min_length = 3); 57 | 58 | // Classify line segments into horizontal/vertical. 59 | std::vector ClassifyLineSegmentOrientations( 60 | const std::vector& segments, const double tolerance = 0.25); 61 | 62 | } // namespace colmap 63 | 64 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::LineSegment) 65 | 66 | #endif // COLMAP_SRC_BASE_LINE_H_ 67 | -------------------------------------------------------------------------------- /base/line_test.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 | #define TEST_NAME "base/line" 33 | #include "util/testing.h" 34 | 35 | #include "base/line.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestDetectLineSegments) { 40 | Bitmap bitmap; 41 | bitmap.Allocate(100, 100, false); 42 | for (size_t i = 0; i < 100; ++i) { 43 | bitmap.SetPixel(i, i, BitmapColor(255)); 44 | } 45 | 46 | const auto line_segments = DetectLineSegments(bitmap, 0); 47 | 48 | BOOST_CHECK_EQUAL(line_segments.size(), 2); 49 | 50 | const Eigen::Vector2d ref_start(0, 0); 51 | const Eigen::Vector2d ref_end(100, 100); 52 | BOOST_CHECK_LT((line_segments[0].start - ref_start).norm(), 5); 53 | BOOST_CHECK_LT((line_segments[0].end - ref_end).norm(), 5); 54 | BOOST_CHECK_LT((line_segments[1].start - ref_end).norm(), 5); 55 | BOOST_CHECK_LT((line_segments[1].end - ref_start).norm(), 5); 56 | 57 | BOOST_CHECK_EQUAL(DetectLineSegments(bitmap, 150).size(), 0); 58 | } 59 | 60 | BOOST_AUTO_TEST_CASE(TestClassifyLineSegmentOrientations) { 61 | Bitmap bitmap; 62 | bitmap.Allocate(100, 100, false); 63 | for (size_t i = 60; i < 100; ++i) { 64 | bitmap.SetPixel(i, 50, BitmapColor(255)); 65 | bitmap.SetPixel(50, i, BitmapColor(255)); 66 | bitmap.SetPixel(i, i, BitmapColor(255)); 67 | } 68 | 69 | const auto line_segments = DetectLineSegments(bitmap, 0); 70 | BOOST_CHECK_EQUAL(line_segments.size(), 6); 71 | 72 | const auto orientations = ClassifyLineSegmentOrientations(line_segments); 73 | BOOST_CHECK_EQUAL(orientations.size(), 6); 74 | 75 | BOOST_CHECK(orientations[0] == LineSegmentOrientation::VERTICAL); 76 | BOOST_CHECK(orientations[1] == LineSegmentOrientation::VERTICAL); 77 | BOOST_CHECK(orientations[2] == LineSegmentOrientation::HORIZONTAL); 78 | BOOST_CHECK(orientations[3] == LineSegmentOrientation::HORIZONTAL); 79 | BOOST_CHECK(orientations[4] == LineSegmentOrientation::UNDEFINED); 80 | BOOST_CHECK(orientations[5] == LineSegmentOrientation::UNDEFINED); 81 | } 82 | -------------------------------------------------------------------------------- /base/point2d.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_POINT2D_H_ 33 | #define COLMAP_SRC_BASE_POINT2D_H_ 34 | 35 | #include 36 | 37 | #include "util/alignment.h" 38 | #include "util/types.h" 39 | 40 | namespace colmap { 41 | 42 | // 2D point class corresponds to a feature in an image. It may or may not have a 43 | // corresponding 3D point if it is part of a triangulated track. 44 | class Point2D { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | Point2D(); 49 | 50 | // The coordinate in image space in pixels. 51 | inline const Eigen::Vector2d& XY() const; 52 | inline Eigen::Vector2d& XY(); 53 | inline double X() const; 54 | inline double Y() const; 55 | inline void SetXY(const Eigen::Vector2d& xy); 56 | 57 | // The identifier of the observed 3D point. If the image point does not 58 | // observe a 3D point, the identifier is `kInvalidPoint3Did`. 59 | inline point3D_t Point3DId() const; 60 | inline bool HasPoint3D() const; 61 | inline void SetPoint3DId(const point3D_t point3D_id); 62 | 63 | private: 64 | // The image coordinates in pixels, starting at upper left corner with 0. 65 | Eigen::Vector2d xy_; 66 | 67 | // The identifier of the 3D point. If the 2D point is not part of a 3D point 68 | // track the identifier is `kInvalidPoint3DId` and `HasPoint3D() = false`. 69 | point3D_t point3D_id_; 70 | }; 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | // Implementation 74 | //////////////////////////////////////////////////////////////////////////////// 75 | 76 | const Eigen::Vector2d& Point2D::XY() const { return xy_; } 77 | 78 | Eigen::Vector2d& Point2D::XY() { return xy_; } 79 | 80 | double Point2D::X() const { return xy_.x(); } 81 | 82 | double Point2D::Y() const { return xy_.y(); } 83 | 84 | void Point2D::SetXY(const Eigen::Vector2d& xy) { xy_ = xy; } 85 | 86 | point3D_t Point2D::Point3DId() const { return point3D_id_; } 87 | 88 | bool Point2D::HasPoint3D() const { return point3D_id_ != kInvalidPoint3DId; } 89 | 90 | void Point2D::SetPoint3DId(const point3D_t point3D_id) { 91 | point3D_id_ = point3D_id; 92 | } 93 | 94 | } // namespace colmap 95 | 96 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point2D) 97 | 98 | #endif // COLMAP_SRC_BASE_POINT2D_H_ 99 | -------------------------------------------------------------------------------- /base/point2d_test.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 | #define TEST_NAME "base/point2d" 33 | #include "util/testing.h" 34 | 35 | #include "base/point2d.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestDefault) { 40 | Point2D point2D; 41 | BOOST_CHECK_EQUAL(point2D.X(), 0); 42 | BOOST_CHECK_EQUAL(point2D.Y(), 0); 43 | BOOST_CHECK_EQUAL(point2D.XY()[0], point2D.X()); 44 | BOOST_CHECK_EQUAL(point2D.XY()[1], point2D.Y()); 45 | BOOST_CHECK_EQUAL(point2D.Point3DId(), kInvalidPoint3DId); 46 | BOOST_CHECK_EQUAL(point2D.HasPoint3D(), false); 47 | } 48 | 49 | BOOST_AUTO_TEST_CASE(TestXY) { 50 | Point2D point2D; 51 | BOOST_CHECK_EQUAL(point2D.X(), 0); 52 | BOOST_CHECK_EQUAL(point2D.Y(), 0); 53 | BOOST_CHECK_EQUAL(point2D.XY()[0], point2D.X()); 54 | BOOST_CHECK_EQUAL(point2D.XY()[1], point2D.Y()); 55 | point2D.SetXY(Eigen::Vector2d(0.1, 0.2)); 56 | BOOST_CHECK_EQUAL(point2D.X(), 0.1); 57 | BOOST_CHECK_EQUAL(point2D.Y(), 0.2); 58 | BOOST_CHECK_EQUAL(point2D.XY()[0], point2D.X()); 59 | BOOST_CHECK_EQUAL(point2D.XY()[1], point2D.Y()); 60 | } 61 | 62 | BOOST_AUTO_TEST_CASE(TestPoint3DId) { 63 | Point2D point2D; 64 | BOOST_CHECK_EQUAL(point2D.Point3DId(), kInvalidPoint3DId); 65 | BOOST_CHECK_EQUAL(point2D.HasPoint3D(), false); 66 | point2D.SetPoint3DId(1); 67 | BOOST_CHECK_EQUAL(point2D.Point3DId(), 1); 68 | BOOST_CHECK_EQUAL(point2D.HasPoint3D(), true); 69 | point2D.SetPoint3DId(kInvalidPoint3DId); 70 | BOOST_CHECK_EQUAL(point2D.Point3DId(), kInvalidPoint3DId); 71 | BOOST_CHECK_EQUAL(point2D.HasPoint3D(), false); 72 | } 73 | -------------------------------------------------------------------------------- /base/point3d.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_POINT3D_H_ 33 | #define COLMAP_SRC_BASE_POINT3D_H_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "base/track.h" 40 | #include "util/logging.h" 41 | #include "util/types.h" 42 | 43 | namespace colmap { 44 | 45 | // 3D point class that holds information about triangulated 2D points. 46 | class Point3D { 47 | public: 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | 50 | Point3D(); 51 | 52 | // The point coordinate in world space. 53 | inline const Eigen::Vector3d& XYZ() const; 54 | inline Eigen::Vector3d& XYZ(); 55 | inline double XYZ(const size_t idx) const; 56 | inline double& XYZ(const size_t idx); 57 | inline double X() const; 58 | inline double Y() const; 59 | inline double Z() const; 60 | inline void SetXYZ(const Eigen::Vector3d& xyz); 61 | 62 | // The RGB color of the point. 63 | inline const Eigen::Vector3ub& Color() const; 64 | inline Eigen::Vector3ub& Color(); 65 | inline uint8_t Color(const size_t idx) const; 66 | inline uint8_t& Color(const size_t idx); 67 | inline void SetColor(const Eigen::Vector3ub& color); 68 | 69 | // The mean reprojection error in image space. 70 | inline double Error() const; 71 | inline bool HasError() const; 72 | inline void SetError(const double error); 73 | 74 | inline const class Track& Track() const; 75 | inline class Track& Track(); 76 | inline void SetTrack(const class Track& track); 77 | 78 | private: 79 | // The 3D position of the point. 80 | Eigen::Vector3d xyz_; 81 | 82 | // The color of the point in the range [0, 255]. 83 | Eigen::Vector3ub color_; 84 | 85 | // The mean reprojection error in pixels. 86 | double error_; 87 | 88 | // The track of the point as a list of image observations. 89 | class Track track_; 90 | }; 91 | 92 | //////////////////////////////////////////////////////////////////////////////// 93 | // Implementation 94 | //////////////////////////////////////////////////////////////////////////////// 95 | 96 | const Eigen::Vector3d& Point3D::XYZ() const { return xyz_; } 97 | 98 | Eigen::Vector3d& Point3D::XYZ() { return xyz_; } 99 | 100 | double Point3D::XYZ(const size_t idx) const { return xyz_(idx); } 101 | 102 | double& Point3D::XYZ(const size_t idx) { return xyz_(idx); } 103 | 104 | double Point3D::X() const { return xyz_.x(); } 105 | 106 | double Point3D::Y() const { return xyz_.y(); } 107 | 108 | double Point3D::Z() const { return xyz_.z(); } 109 | 110 | void Point3D::SetXYZ(const Eigen::Vector3d& xyz) { xyz_ = xyz; } 111 | 112 | const Eigen::Vector3ub& Point3D::Color() const { return color_; } 113 | 114 | Eigen::Vector3ub& Point3D::Color() { return color_; } 115 | 116 | uint8_t Point3D::Color(const size_t idx) const { return color_(idx); } 117 | 118 | uint8_t& Point3D::Color(const size_t idx) { return color_(idx); } 119 | 120 | void Point3D::SetColor(const Eigen::Vector3ub& color) { color_ = color; } 121 | 122 | double Point3D::Error() const { return error_; } 123 | 124 | bool Point3D::HasError() const { return error_ != -1.0; } 125 | 126 | void Point3D::SetError(const double error) { error_ = error; } 127 | 128 | const class Track& Point3D::Track() const { return track_; } 129 | 130 | class Track& Point3D::Track() { 131 | return track_; 132 | } 133 | 134 | void Point3D::SetTrack(const class Track& track) { track_ = track; } 135 | 136 | } // namespace colmap 137 | 138 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point3D) 139 | 140 | #endif // COLMAP_SRC_BASE_POINT3D_H_ 141 | -------------------------------------------------------------------------------- /base/point3d_test.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 | #define TEST_NAME "base/point3d" 33 | #include "util/testing.h" 34 | 35 | #include "base/point3d.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestDefault) { 40 | Point3D point3D; 41 | BOOST_CHECK_EQUAL(point3D.X(), 0); 42 | BOOST_CHECK_EQUAL(point3D.Y(), 0); 43 | BOOST_CHECK_EQUAL(point3D.Z(), 0); 44 | BOOST_CHECK_EQUAL(point3D.XYZ()[0], point3D.X()); 45 | BOOST_CHECK_EQUAL(point3D.XYZ()[1], point3D.Y()); 46 | BOOST_CHECK_EQUAL(point3D.XYZ()[2], point3D.Z()); 47 | BOOST_CHECK_EQUAL(point3D.Color()[0], 0); 48 | BOOST_CHECK_EQUAL(point3D.Color()[1], 0); 49 | BOOST_CHECK_EQUAL(point3D.Color()[2], 0); 50 | BOOST_CHECK_EQUAL(point3D.Error(), -1.0); 51 | BOOST_CHECK_EQUAL(point3D.HasError(), false); 52 | BOOST_CHECK_EQUAL(point3D.Track().Length(), 0); 53 | } 54 | 55 | BOOST_AUTO_TEST_CASE(TestXYZ) { 56 | Point3D point3D; 57 | BOOST_CHECK_EQUAL(point3D.X(), 0); 58 | BOOST_CHECK_EQUAL(point3D.Y(), 0); 59 | BOOST_CHECK_EQUAL(point3D.Z(), 0); 60 | BOOST_CHECK_EQUAL(point3D.XYZ()[0], point3D.X()); 61 | BOOST_CHECK_EQUAL(point3D.XYZ()[1], point3D.Y()); 62 | BOOST_CHECK_EQUAL(point3D.XYZ()[2], point3D.Z()); 63 | point3D.SetXYZ(Eigen::Vector3d(0.1, 0.2, 0.3)); 64 | BOOST_CHECK_EQUAL(point3D.X(), 0.1); 65 | BOOST_CHECK_EQUAL(point3D.Y(), 0.2); 66 | BOOST_CHECK_EQUAL(point3D.Z(), 0.3); 67 | BOOST_CHECK_EQUAL(point3D.XYZ()[0], point3D.X()); 68 | BOOST_CHECK_EQUAL(point3D.XYZ()[1], point3D.Y()); 69 | BOOST_CHECK_EQUAL(point3D.XYZ()[2], point3D.Z()); 70 | point3D.XYZ() = Eigen::Vector3d(0.2, 0.3, 0.4); 71 | BOOST_CHECK_EQUAL(point3D.X(), 0.2); 72 | BOOST_CHECK_EQUAL(point3D.Y(), 0.3); 73 | BOOST_CHECK_EQUAL(point3D.Z(), 0.4); 74 | BOOST_CHECK_EQUAL(point3D.XYZ()[0], point3D.X()); 75 | BOOST_CHECK_EQUAL(point3D.XYZ()[1], point3D.Y()); 76 | BOOST_CHECK_EQUAL(point3D.XYZ()[2], point3D.Z()); 77 | } 78 | 79 | BOOST_AUTO_TEST_CASE(TestRGB) { 80 | Point3D point3D; 81 | BOOST_CHECK_EQUAL(point3D.Color()[0], 0); 82 | BOOST_CHECK_EQUAL(point3D.Color()[1], 0); 83 | BOOST_CHECK_EQUAL(point3D.Color()[2], 0); 84 | point3D.SetColor(Eigen::Vector3ub(1, 2, 3)); 85 | BOOST_CHECK_EQUAL(point3D.Color()[0], 1); 86 | BOOST_CHECK_EQUAL(point3D.Color()[1], 2); 87 | BOOST_CHECK_EQUAL(point3D.Color()[2], 3); 88 | } 89 | 90 | BOOST_AUTO_TEST_CASE(TestError) { 91 | Point3D point3D; 92 | BOOST_CHECK_EQUAL(point3D.Error(), -1.0); 93 | BOOST_CHECK_EQUAL(point3D.HasError(), false); 94 | point3D.SetError(1.0); 95 | BOOST_CHECK_EQUAL(point3D.Error(), 1.0); 96 | BOOST_CHECK_EQUAL(point3D.HasError(), true); 97 | point3D.SetError(-1.0); 98 | BOOST_CHECK_EQUAL(point3D.Error(), -1.0); 99 | BOOST_CHECK_EQUAL(point3D.HasError(), false); 100 | } 101 | 102 | BOOST_AUTO_TEST_CASE(TestTrack) { 103 | Point3D point3D; 104 | BOOST_CHECK_EQUAL(point3D.Track().Length(), 0); 105 | point3D.SetTrack(Track()); 106 | BOOST_CHECK_EQUAL(point3D.Track().Length(), 0); 107 | Track track; 108 | track.AddElement(0, 1); 109 | track.AddElement(0, 2); 110 | point3D.SetTrack(track); 111 | BOOST_CHECK_EQUAL(point3D.Track().Length(), 2); 112 | track.AddElement(0, 3); 113 | BOOST_CHECK_EQUAL(point3D.Track().Length(), 2); 114 | } 115 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /base/reconstruction_manager.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_RECONSTRUCTION_MANAGER_H_ 33 | #define COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ 34 | 35 | #include "base/reconstruction.h" 36 | 37 | namespace colmap { 38 | 39 | class OptionManager; 40 | 41 | class ReconstructionManager { 42 | public: 43 | ReconstructionManager(); 44 | 45 | // Move constructor and assignment. 46 | ReconstructionManager(ReconstructionManager&& other); 47 | ReconstructionManager& operator=(ReconstructionManager&& other); 48 | 49 | // The number of reconstructions managed. 50 | size_t Size() const; 51 | 52 | // Get a reference to a specific reconstruction. 53 | const Reconstruction& Get(const size_t idx) const; 54 | Reconstruction& Get(const size_t idx); 55 | 56 | // Add a new empty reconstruction and return its index. 57 | size_t Add(); 58 | 59 | // Delete a specific reconstruction. 60 | void Delete(const size_t idx); 61 | 62 | // Delete all reconstructions. 63 | void Clear(); 64 | 65 | // Read and add a new reconstruction and return its index. 66 | size_t Read(const std::string& path); 67 | 68 | // Write all managed reconstructions into sub-folders "0", "1", "2", ... 69 | // If the option manager object is not null, the options are written 70 | // to each respective reconstruction folder as well. 71 | void Write(const std::string& path, const OptionManager* options) const; 72 | 73 | private: 74 | NON_COPYABLE(ReconstructionManager) 75 | 76 | std::vector> reconstructions_; 77 | }; 78 | 79 | } // namespace colmap 80 | 81 | #endif // COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ 82 | -------------------------------------------------------------------------------- /base/reconstruction_manager_test.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 | #define TEST_NAME "base/reconstruction_manager" 33 | #include "util/testing.h" 34 | 35 | #include "base/reconstruction_manager.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestEmpty) { 40 | ReconstructionManager reconstruction_manager; 41 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 0); 42 | } 43 | 44 | BOOST_AUTO_TEST_CASE(TestAddGet) { 45 | ReconstructionManager reconstruction_manager; 46 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 0); 47 | for (size_t i = 0; i < 10; ++i) { 48 | const size_t idx = reconstruction_manager.Add(); 49 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), i + 1); 50 | BOOST_CHECK_EQUAL(idx, i); 51 | BOOST_CHECK_EQUAL(reconstruction_manager.Get(idx).NumCameras(), 0); 52 | BOOST_CHECK_EQUAL(reconstruction_manager.Get(idx).NumImages(), 0); 53 | BOOST_CHECK_EQUAL(reconstruction_manager.Get(idx).NumPoints3D(), 0); 54 | } 55 | } 56 | 57 | BOOST_AUTO_TEST_CASE(TestDelete) { 58 | ReconstructionManager reconstruction_manager; 59 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 0); 60 | for (size_t i = 0; i < 10; ++i) { 61 | reconstruction_manager.Add(); 62 | } 63 | 64 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 10); 65 | for (size_t i = 0; i < 10; ++i) { 66 | reconstruction_manager.Delete(0); 67 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 9 - i); 68 | } 69 | } 70 | 71 | BOOST_AUTO_TEST_CASE(TestClear) { 72 | ReconstructionManager reconstruction_manager; 73 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 0); 74 | for (size_t i = 0; i < 10; ++i) { 75 | reconstruction_manager.Add(); 76 | } 77 | 78 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 10); 79 | reconstruction_manager.Clear(); 80 | BOOST_CHECK_EQUAL(reconstruction_manager.Size(), 0); 81 | } 82 | -------------------------------------------------------------------------------- /base/scene_clustering.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_SCENE_CLUSTERING_H_ 33 | #define COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include "util/types.h" 39 | 40 | namespace colmap { 41 | 42 | // Scene clustering approach using normalized cuts on the scene graph. The scene 43 | // is hierarchically partitioned into overlapping clusters until a maximum 44 | // number of images is in a leaf node. 45 | class SceneClustering { 46 | public: 47 | struct Options { 48 | // The branching factor of the hierarchical clustering. 49 | int branching = 2; 50 | 51 | // The number of overlapping images between child clusters. 52 | int image_overlap = 50; 53 | 54 | // The maximum number of images in a leaf node cluster, otherwise the 55 | // cluster is further partitioned using the given branching factor. Note 56 | // that a cluster leaf node will have at most `leaf_max_num_images + 57 | // overlap` images to satisfy the overlap constraint. 58 | int leaf_max_num_images = 500; 59 | 60 | bool Check() const; 61 | }; 62 | 63 | struct Cluster { 64 | std::vector image_ids; 65 | std::vector child_clusters; 66 | }; 67 | 68 | SceneClustering(const Options& options); 69 | 70 | void Partition(const std::vector>& image_pairs, 71 | const std::vector& num_inliers); 72 | 73 | const Cluster* GetRootCluster() const; 74 | std::vector GetLeafClusters() const; 75 | 76 | private: 77 | void PartitionCluster(const std::vector>& edges, 78 | const std::vector& weights, Cluster* cluster); 79 | 80 | const Options options_; 81 | std::unique_ptr root_cluster_; 82 | }; 83 | 84 | } // namespace colmap 85 | 86 | #endif // COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ 87 | -------------------------------------------------------------------------------- /base/similarity_transform.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_SIMILARITY_TRANSFORM_H_ 33 | #define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "util/alignment.h" 41 | #include "util/types.h" 42 | 43 | namespace colmap { 44 | 45 | struct RANSACOptions; 46 | class Reconstruction; 47 | 48 | // 3D similarity transformation with 7 degrees of freedom. 49 | class SimilarityTransform3 { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 52 | 53 | SimilarityTransform3(); 54 | 55 | explicit SimilarityTransform3(const Eigen::Matrix3x4d& matrix); 56 | 57 | explicit SimilarityTransform3( 58 | const Eigen::Transform& transform); 59 | 60 | SimilarityTransform3(const double scale, const Eigen::Vector4d& qvec, 61 | const Eigen::Vector3d& tvec); 62 | 63 | bool Estimate(const std::vector& src, 64 | const std::vector& dst); 65 | 66 | SimilarityTransform3 Inverse() const; 67 | 68 | void TransformPoint(Eigen::Vector3d* xyz) const; 69 | void TransformPose(Eigen::Vector4d* qvec, Eigen::Vector3d* tvec) const; 70 | 71 | Eigen::Matrix4d Matrix() const; 72 | double Scale() const; 73 | Eigen::Vector4d Rotation() const; 74 | Eigen::Vector3d Translation() const; 75 | 76 | private: 77 | Eigen::Transform transform_; 78 | }; 79 | 80 | // Robustly compute alignment between reconstructions by finding images that 81 | // are registered in both reconstructions. The alignment is then estimated 82 | // robustly inside RANSAC from corresponding projection centers. An alignment 83 | // is verified by reprojecting common 3D point observations. 84 | // The min_inlier_observations threshold determines how many observations 85 | // in a common image must reproject within the given threshold. 86 | bool ComputeAlignmentBetweenReconstructions( 87 | const Reconstruction& src_reconstruction, 88 | const Reconstruction& ref_reconstruction, 89 | const double min_inlier_observations, const double max_reproj_error, 90 | Eigen::Matrix3x4d* alignment); 91 | 92 | } // namespace colmap 93 | 94 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::SimilarityTransform3) 95 | 96 | #endif // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ 97 | -------------------------------------------------------------------------------- /base/similarity_transform_test.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 | #define TEST_NAME "base/similarity_transform" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "base/pose.h" 38 | #include "base/similarity_transform.h" 39 | 40 | using namespace colmap; 41 | 42 | BOOST_AUTO_TEST_CASE(TestInitialization) { 43 | const Eigen::Vector4d qvec = 44 | NormalizeQuaternion(Eigen::Vector4d(0.1, 0.3, 0.2, 0.4)); 45 | 46 | const SimilarityTransform3 tform(2, qvec, Eigen::Vector3d(100, 10, 0.5)); 47 | 48 | BOOST_CHECK_CLOSE(tform.Scale(), 2, 1e-10); 49 | 50 | BOOST_CHECK_CLOSE(tform.Rotation()(0), qvec(0), 1e-10); 51 | BOOST_CHECK_CLOSE(tform.Rotation()(1), qvec(1), 1e-10); 52 | BOOST_CHECK_CLOSE(tform.Rotation()(2), qvec(2), 1e-10); 53 | BOOST_CHECK_CLOSE(tform.Rotation()(3), qvec(3), 1e-10); 54 | 55 | BOOST_CHECK_CLOSE(tform.Translation()[0], 100, 1e-10); 56 | BOOST_CHECK_CLOSE(tform.Translation()[1], 10, 1e-10); 57 | BOOST_CHECK_CLOSE(tform.Translation()[2], 0.5, 1e-10); 58 | } 59 | 60 | void TestEstimationWithNumCoords(const size_t num_coords) { 61 | const SimilarityTransform3 orig_tform(2, Eigen::Vector4d(0.1, 0.3, 0.2, 0.4), 62 | Eigen::Vector3d(100, 10, 0.5)); 63 | 64 | std::vector src; 65 | std::vector dst; 66 | 67 | for (size_t i = 0; i < num_coords; ++i) { 68 | src.emplace_back(i, i + 2, i * i); 69 | dst.push_back(src.back()); 70 | orig_tform.TransformPoint(&dst.back()); 71 | } 72 | 73 | SimilarityTransform3 est_tform; 74 | BOOST_CHECK(est_tform.Estimate(src, dst)); 75 | 76 | BOOST_CHECK((orig_tform.Matrix() - est_tform.Matrix()).norm() < 1e-6); 77 | 78 | std::vector invalid_src_dst(3, Eigen::Vector3d::Zero()); 79 | BOOST_CHECK(!est_tform.Estimate(invalid_src_dst, invalid_src_dst)); 80 | } 81 | 82 | BOOST_AUTO_TEST_CASE(TestEstimation) { 83 | TestEstimationWithNumCoords(3); 84 | TestEstimationWithNumCoords(100); 85 | } 86 | -------------------------------------------------------------------------------- /base/track.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/track.h" 33 | 34 | namespace colmap { 35 | 36 | Track::Track() {} 37 | 38 | TrackElement::TrackElement() 39 | : image_id(kInvalidImageId), point2D_idx(kInvalidPoint2DIdx) {} 40 | 41 | TrackElement::TrackElement(const image_t image_id, const point2D_t point2D_idx) 42 | : image_id(image_id), point2D_idx(point2D_idx) {} 43 | 44 | void Track::DeleteElement(const image_t image_id, const point2D_t point2D_idx) { 45 | elements_.erase( 46 | std::remove_if(elements_.begin(), elements_.end(), 47 | [image_id, point2D_idx](const TrackElement& element) { 48 | return element.image_id == image_id && 49 | element.point2D_idx == point2D_idx; 50 | }), 51 | elements_.end()); 52 | } 53 | 54 | } // namespace colmap 55 | -------------------------------------------------------------------------------- /base/track_test.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 | #define TEST_NAME "base/track" 33 | #include "util/testing.h" 34 | 35 | #include "base/track.h" 36 | 37 | using namespace colmap; 38 | 39 | BOOST_AUTO_TEST_CASE(TestTrackElement) { 40 | TrackElement track_el; 41 | BOOST_CHECK_EQUAL(track_el.image_id, kInvalidImageId); 42 | BOOST_CHECK_EQUAL(track_el.point2D_idx, kInvalidPoint2DIdx); 43 | } 44 | 45 | BOOST_AUTO_TEST_CASE(TestDefault) { 46 | Track track; 47 | BOOST_CHECK_EQUAL(track.Length(), 0); 48 | BOOST_CHECK_EQUAL(track.Elements().size(), track.Length()); 49 | } 50 | 51 | BOOST_AUTO_TEST_CASE(TestSetElements) { 52 | Track track; 53 | std::vector elements; 54 | elements.emplace_back(0, 1); 55 | elements.emplace_back(0, 2); 56 | track.SetElements(elements); 57 | BOOST_CHECK_EQUAL(track.Length(), 2); 58 | BOOST_CHECK_EQUAL(track.Elements().size(), track.Length()); 59 | BOOST_CHECK_EQUAL(track.Element(0).image_id, 0); 60 | BOOST_CHECK_EQUAL(track.Element(0).point2D_idx, 1); 61 | BOOST_CHECK_EQUAL(track.Element(1).image_id, 0); 62 | BOOST_CHECK_EQUAL(track.Element(1).point2D_idx, 2); 63 | for (size_t i = 0; i < track.Length(); ++i) { 64 | BOOST_CHECK_EQUAL(track.Element(i).image_id, track.Elements()[i].image_id); 65 | BOOST_CHECK_EQUAL(track.Element(i).point2D_idx, 66 | track.Elements()[i].point2D_idx); 67 | } 68 | } 69 | 70 | BOOST_AUTO_TEST_CASE(TestAddElement) { 71 | Track track; 72 | track.AddElement(0, 1); 73 | track.AddElement(TrackElement(0, 2)); 74 | std::vector elements; 75 | elements.emplace_back(0, 1); 76 | elements.emplace_back(0, 2); 77 | track.AddElements(elements); 78 | BOOST_CHECK_EQUAL(track.Length(), 4); 79 | BOOST_CHECK_EQUAL(track.Elements().size(), track.Length()); 80 | BOOST_CHECK_EQUAL(track.Element(0).image_id, 0); 81 | BOOST_CHECK_EQUAL(track.Element(0).point2D_idx, 1); 82 | BOOST_CHECK_EQUAL(track.Element(1).image_id, 0); 83 | BOOST_CHECK_EQUAL(track.Element(1).point2D_idx, 2); 84 | BOOST_CHECK_EQUAL(track.Element(2).image_id, 0); 85 | BOOST_CHECK_EQUAL(track.Element(2).point2D_idx, 1); 86 | BOOST_CHECK_EQUAL(track.Element(3).image_id, 0); 87 | BOOST_CHECK_EQUAL(track.Element(3).point2D_idx, 2); 88 | for (size_t i = 0; i < track.Length(); ++i) { 89 | BOOST_CHECK_EQUAL(track.Element(i).image_id, track.Elements()[i].image_id); 90 | BOOST_CHECK_EQUAL(track.Element(i).point2D_idx, 91 | track.Elements()[i].point2D_idx); 92 | } 93 | } 94 | 95 | BOOST_AUTO_TEST_CASE(TestDeleteElement) { 96 | Track track; 97 | track.AddElement(0, 1); 98 | track.AddElement(0, 2); 99 | track.AddElement(0, 3); 100 | track.AddElement(0, 3); 101 | BOOST_CHECK_EQUAL(track.Length(), 4); 102 | BOOST_CHECK_EQUAL(track.Elements().size(), track.Length()); 103 | track.DeleteElement(0); 104 | BOOST_CHECK_EQUAL(track.Length(), 3); 105 | BOOST_CHECK_EQUAL(track.Elements().size(), track.Length()); 106 | BOOST_CHECK_EQUAL(track.Element(0).image_id, 0); 107 | BOOST_CHECK_EQUAL(track.Element(0).point2D_idx, 2); 108 | BOOST_CHECK_EQUAL(track.Element(1).image_id, 0); 109 | BOOST_CHECK_EQUAL(track.Element(1).point2D_idx, 3); 110 | BOOST_CHECK_EQUAL(track.Element(2).image_id, 0); 111 | BOOST_CHECK_EQUAL(track.Element(2).point2D_idx, 3); 112 | track.DeleteElement(0, 3); 113 | BOOST_CHECK_EQUAL(track.Length(), 1); 114 | BOOST_CHECK_EQUAL(track.Elements().size(), track.Length()); 115 | BOOST_CHECK_EQUAL(track.Element(0).image_id, 0); 116 | BOOST_CHECK_EQUAL(track.Element(0).point2D_idx, 2); 117 | } 118 | 119 | BOOST_AUTO_TEST_CASE(TestReserve) { 120 | Track track; 121 | track.Reserve(2); 122 | BOOST_CHECK_EQUAL(track.Elements().capacity(), 2); 123 | } 124 | 125 | BOOST_AUTO_TEST_CASE(TestCompress) { 126 | Track track; 127 | track.AddElement(0, 1); 128 | track.AddElement(0, 2); 129 | track.AddElement(0, 3); 130 | track.AddElement(0, 3); 131 | BOOST_CHECK_EQUAL(track.Elements().capacity(), 4); 132 | track.DeleteElement(0); 133 | track.DeleteElement(0); 134 | BOOST_CHECK_EQUAL(track.Elements().capacity(), 4); 135 | track.Compress(); 136 | BOOST_CHECK_EQUAL(track.Elements().capacity(), 2); 137 | } 138 | -------------------------------------------------------------------------------- /base/triangulation.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_TRIANGULATION_H_ 33 | #define COLMAP_SRC_BASE_TRIANGULATION_H_ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "base/camera.h" 40 | #include "util/alignment.h" 41 | #include "util/math.h" 42 | #include "util/types.h" 43 | 44 | namespace colmap { 45 | 46 | // Triangulate 3D point from corresponding image point observations. 47 | // 48 | // Implementation of the direct linear transform triangulation method in 49 | // R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, 50 | // Cambridge Univ. Press, 2003. 51 | // 52 | // @param proj_matrix1 Projection matrix of the first image as 3x4 matrix. 53 | // @param proj_matrix2 Projection matrix of the second image as 3x4 matrix. 54 | // @param point1 Corresponding 2D point in first image. 55 | // @param point2 Corresponding 2D point in second image. 56 | // 57 | // @return Triangulated 3D point. 58 | Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& proj_matrix1, 59 | const Eigen::Matrix3x4d& proj_matrix2, 60 | const Eigen::Vector2d& point1, 61 | const Eigen::Vector2d& point2); 62 | 63 | // Triangulate multiple 3D points from multiple image correspondences. 64 | std::vector TriangulatePoints( 65 | const Eigen::Matrix3x4d& proj_matrix1, 66 | const Eigen::Matrix3x4d& proj_matrix2, 67 | const std::vector& points1, 68 | const std::vector& points2); 69 | 70 | // Triangulate point from multiple views minimizing the L2 error. 71 | // 72 | // @param proj_matrices Projection matrices of multi-view observations. 73 | // @param points Image observations of multi-view observations. 74 | // 75 | // @return Estimated 3D point. 76 | Eigen::Vector3d TriangulateMultiViewPoint( 77 | const std::vector& proj_matrices, 78 | const std::vector& points); 79 | 80 | // Triangulate optimal 3D point from corresponding image point observations by 81 | // finding the optimal image observations. 82 | // 83 | // Note that camera poses should be very good in order for this method to yield 84 | // good results. Otherwise just use `TriangulatePoint`. 85 | // 86 | // Implementation of the method described in 87 | // P. Lindstrom, "Triangulation Made Easy," IEEE Computer Vision and Pattern 88 | // Recognition 2010, pp. 1554-1561, June 2010. 89 | // 90 | // @param proj_matrix1 Projection matrix of the first image as 3x4 matrix. 91 | // @param proj_matrix2 Projection matrix of the second image as 3x4 matrix. 92 | // @param point1 Corresponding 2D point in first image. 93 | // @param point2 Corresponding 2D point in second image. 94 | // 95 | // @return Triangulated optimal 3D point. 96 | Eigen::Vector3d TriangulateOptimalPoint(const Eigen::Matrix3x4d& proj_matrix1, 97 | const Eigen::Matrix3x4d& proj_matrix2, 98 | const Eigen::Vector2d& point1, 99 | const Eigen::Vector2d& point2); 100 | 101 | // Triangulate multiple optimal 3D points from multiple image correspondences. 102 | std::vector TriangulateOptimalPoints( 103 | const Eigen::Matrix3x4d& proj_matrix1, 104 | const Eigen::Matrix3x4d& proj_matrix2, 105 | const std::vector& points1, 106 | const std::vector& points2); 107 | 108 | // Calculate angle in radians between the two rays of a triangulated point. 109 | double CalculateTriangulationAngle(const Eigen::Vector3d& proj_center1, 110 | const Eigen::Vector3d& proj_center2, 111 | const Eigen::Vector3d& point3D); 112 | std::vector CalculateTriangulationAngles( 113 | const Eigen::Vector3d& proj_center1, const Eigen::Vector3d& proj_center2, 114 | const std::vector& points3D); 115 | 116 | } // namespace colmap 117 | 118 | #endif // COLMAP_SRC_BASE_TRIANGULATION_H_ 119 | -------------------------------------------------------------------------------- /base/triangulation_test.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 | #define TEST_NAME "base/triangulation" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "base/similarity_transform.h" 38 | #include "base/triangulation.h" 39 | 40 | using namespace colmap; 41 | 42 | BOOST_AUTO_TEST_CASE(TestTriangulatePoint) { 43 | std::vector points3D(6); 44 | points3D[0] = Eigen::Vector3d(0, 0.1, 0.1); 45 | points3D[1] = Eigen::Vector3d(0, 1, 3); 46 | points3D[2] = Eigen::Vector3d(0, 1, 2); 47 | points3D[3] = Eigen::Vector3d(0.01, 0.2, 3); 48 | points3D[4] = Eigen::Vector3d(-1, 0.1, 1); 49 | points3D[5] = Eigen::Vector3d(0.1, 0.1, 0.2); 50 | 51 | Eigen::Matrix3x4d proj_matrix1 = Eigen::MatrixXd::Identity(3, 4); 52 | 53 | for (double qz = 0; qz < 1; qz += 0.2) { 54 | for (double tx = 0; tx < 10; tx += 2) { 55 | SimilarityTransform3 tform(1, Eigen::Vector4d(0.2, 0.3, 0.4, qz), 56 | Eigen::Vector3d(tx, 2, 3)); 57 | 58 | Eigen::Matrix3x4d proj_matrix2 = tform.Matrix().topLeftCorner<3, 4>(); 59 | 60 | for (size_t i = 0; i < points3D.size(); ++i) { 61 | const Eigen::Vector3d& point3D = points3D[i]; 62 | const Eigen::Vector4d point3D1(point3D(0), point3D(1), point3D(2), 1); 63 | Eigen::Vector3d point2D1 = proj_matrix1 * point3D1; 64 | Eigen::Vector3d point2D2 = proj_matrix2 * point3D1; 65 | point2D1 /= point2D1(2); 66 | point2D2 /= point2D2(2); 67 | 68 | const Eigen::Vector2d point2D1_N(point2D1(0), point2D1(1)); 69 | const Eigen::Vector2d point2D2_N(point2D2(0), point2D2(1)); 70 | 71 | const Eigen::Vector3d tri_point3D = TriangulatePoint( 72 | proj_matrix1, proj_matrix2, point2D1_N, point2D2_N); 73 | 74 | BOOST_CHECK((point3D - tri_point3D).norm() < 1e-10); 75 | } 76 | } 77 | } 78 | } 79 | 80 | BOOST_AUTO_TEST_CASE(TestCalculateTriangulationAngle) { 81 | const Eigen::Vector3d tvec1(0, 0, 0); 82 | const Eigen::Vector3d tvec2(0, 1, 0); 83 | 84 | BOOST_CHECK_CLOSE( 85 | CalculateTriangulationAngle(tvec1, tvec2, Eigen::Vector3d(0, 0, 100)), 86 | 0.009999666687, 1e-8); 87 | BOOST_CHECK_CLOSE( 88 | CalculateTriangulationAngle(tvec1, tvec2, Eigen::Vector3d(0, 0, 50)), 89 | 0.019997333973, 1e-8); 90 | BOOST_CHECK_CLOSE(CalculateTriangulationAngles( 91 | tvec1, tvec2, {Eigen::Vector3d(0, 0, 50)})[0], 92 | 0.019997333973, 1e-8); 93 | } 94 | -------------------------------------------------------------------------------- /base/visibility_pyramid.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/visibility_pyramid.h" 33 | #include "util/logging.h" 34 | #include "util/math.h" 35 | 36 | namespace colmap { 37 | 38 | VisibilityPyramid::VisibilityPyramid() : VisibilityPyramid(0, 0, 0) {} 39 | 40 | VisibilityPyramid::VisibilityPyramid(const size_t num_levels, 41 | const size_t width, const size_t height) 42 | : width_(width), height_(height), score_(0), max_score_(0) { 43 | pyramid_.resize(num_levels); 44 | for (size_t level = 0; level < num_levels; ++level) { 45 | const size_t level_plus_one = level + 1; 46 | const int dim = 1 << level_plus_one; 47 | pyramid_[level].setZero(dim, dim); 48 | max_score_ += dim * dim * dim * dim; 49 | } 50 | } 51 | 52 | void VisibilityPyramid::SetPoint(const double x, const double y) { 53 | CHECK_GT(pyramid_.size(), 0); 54 | 55 | size_t cx = 0; 56 | size_t cy = 0; 57 | CellForPoint(x, y, &cx, &cy); 58 | 59 | for (int i = static_cast(pyramid_.size() - 1); i >= 0; --i) { 60 | auto& level = pyramid_[i]; 61 | 62 | level(cy, cx) += 1; 63 | if (level(cy, cx) == 1) { 64 | score_ += level.size(); 65 | } 66 | 67 | cx = cx >> 1; 68 | cy = cy >> 1; 69 | } 70 | 71 | CHECK_LE(score_, max_score_); 72 | } 73 | 74 | void VisibilityPyramid::ResetPoint(const double x, const double y) { 75 | CHECK_GT(pyramid_.size(), 0); 76 | 77 | size_t cx = 0; 78 | size_t cy = 0; 79 | CellForPoint(x, y, &cx, &cy); 80 | 81 | for (int i = static_cast(pyramid_.size() - 1); i >= 0; --i) { 82 | auto& level = pyramid_[i]; 83 | 84 | level(cy, cx) -= 1; 85 | if (level(cy, cx) == 0) { 86 | score_ -= level.size(); 87 | } 88 | 89 | cx = cx >> 1; 90 | cy = cy >> 1; 91 | } 92 | 93 | CHECK_LE(score_, max_score_); 94 | } 95 | 96 | void VisibilityPyramid::CellForPoint(const double x, const double y, size_t* cx, 97 | size_t* cy) const { 98 | CHECK_GT(width_, 0); 99 | CHECK_GT(height_, 0); 100 | const int max_dim = 1 << pyramid_.size(); 101 | *cx = Clip(static_cast(max_dim * x / width_), 0, 102 | static_cast(max_dim - 1)); 103 | *cy = Clip(static_cast(max_dim * y / height_), 0, 104 | static_cast(max_dim - 1)); 105 | } 106 | 107 | } // namespace colmap 108 | -------------------------------------------------------------------------------- /base/warp.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_WARP_H_ 33 | #define COLMAP_SRC_BASE_WARP_H_ 34 | 35 | #include "base/camera.h" 36 | #include "util/alignment.h" 37 | #include "util/bitmap.h" 38 | 39 | namespace colmap { 40 | 41 | // Warp source image to target image by projecting the pixels of the target 42 | // image up to infinity and projecting it down into the source image 43 | // (i.e. an inverse mapping). The function allocates the target image. 44 | void WarpImageBetweenCameras(const Camera& source_camera, 45 | const Camera& target_camera, 46 | const Bitmap& source_image, Bitmap* target_image); 47 | 48 | // Warp an image with the given homography, where H defines the pixel mapping 49 | // from the target to source image. Note that the pixel centers are assumed to 50 | // have coordinates (0.5, 0.5). 51 | void WarpImageWithHomography(const Eigen::Matrix3d& H, 52 | const Bitmap& source_image, Bitmap* target_image); 53 | 54 | // First, warp source image to target image by projecting the pixels of the 55 | // target image up to infinity and projecting it down into the source image 56 | // (i.e. an inverse mapping). Second, warp the coordinates from the first 57 | // warping with the given homography. The function allocates the target image. 58 | void WarpImageWithHomographyBetweenCameras(const Eigen::Matrix3d& H, 59 | const Camera& source_camera, 60 | const Camera& target_camera, 61 | const Bitmap& source_image, 62 | Bitmap* target_image); 63 | 64 | // Resample row-major image using bilinear interpolation. 65 | void ResampleImageBilinear(const float* data, const int rows, const int cols, 66 | const int new_rows, const int new_cols, 67 | float* resampled); 68 | 69 | // Smooth row-major image using a Gaussian filter kernel. 70 | void SmoothImage(const float* data, const int rows, const int cols, 71 | const float sigma_r, const float sigma_c, float* smoothed); 72 | 73 | // Downsample row-major image by first smoothing and then resampling. 74 | void DownsampleImage(const float* data, const int rows, const int cols, 75 | const int new_rows, const int new_cols, 76 | float* downsampled); 77 | 78 | } // namespace colmap 79 | 80 | #endif // COLMAP_SRC_BASE_WARP_H_ 81 | -------------------------------------------------------------------------------- /controllers/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 | set(FOLDER_NAME "controllers") 33 | 34 | COLMAP_ADD_SOURCES( 35 | automatic_reconstruction.h automatic_reconstruction.cc 36 | bundle_adjustment.h bundle_adjustment.cc 37 | hierarchical_mapper.h hierarchical_mapper.cc 38 | incremental_mapper.h incremental_mapper.cc 39 | ) 40 | -------------------------------------------------------------------------------- /controllers/automatic_reconstruction.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_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ 33 | #define COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ 34 | 35 | #include 36 | 37 | #include "base/reconstruction_manager.h" 38 | #include "util/option_manager.h" 39 | #include "util/threading.h" 40 | 41 | namespace colmap { 42 | 43 | class AutomaticReconstructionController : public Thread { 44 | public: 45 | enum class DataType { INDIVIDUAL, VIDEO, INTERNET }; 46 | enum class Quality { LOW, MEDIUM, HIGH, EXTREME }; 47 | enum class Mesher { POISSON, DELAUNAY }; 48 | 49 | struct Options { 50 | // The path to the workspace folder in which all results are stored. 51 | std::string workspace_path; 52 | 53 | // The path to the image folder which are used as input. 54 | std::string image_path; 55 | 56 | // The path to the mask folder which are used as input. 57 | std::string mask_path; 58 | 59 | // The path to the vocabulary tree for feature matching. 60 | std::string vocab_tree_path; 61 | 62 | // The type of input data used to choose optimal mapper settings. 63 | DataType data_type = DataType::INDIVIDUAL; 64 | 65 | // Whether to perform low- or high-quality reconstruction. 66 | Quality quality = Quality::HIGH; 67 | 68 | // Whether to use shared intrinsics or not. 69 | bool single_camera = false; 70 | 71 | // Which camera model to use for images. 72 | std::string camera_model = "SIMPLE_RADIAL"; 73 | 74 | // Whether to perform sparse mapping. 75 | bool sparse = true; 76 | 77 | // Whether to perform dense mapping. 78 | #ifdef CUDA_ENABLED 79 | bool dense = true; 80 | #else 81 | bool dense = false; 82 | #endif 83 | 84 | // The meshing algorithm to be used. 85 | Mesher mesher = Mesher::POISSON; 86 | 87 | // The number of threads to use in all stages. 88 | int num_threads = -1; 89 | 90 | // Whether to use the GPU in feature extraction and matching. 91 | bool use_gpu = true; 92 | 93 | // Index of the GPU used for GPU stages. For multi-GPU computation, 94 | // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". 95 | // By default, all GPUs will be used in all stages. 96 | std::string gpu_index = "-1"; 97 | }; 98 | 99 | AutomaticReconstructionController( 100 | const Options& options, ReconstructionManager* reconstruction_manager); 101 | 102 | void Stop() override; 103 | 104 | private: 105 | void Run() override; 106 | void RunFeatureExtraction(); 107 | void RunFeatureMatching(); 108 | void RunSparseMapper(); 109 | void RunDenseMapper(); 110 | 111 | const Options options_; 112 | OptionManager option_manager_; 113 | ReconstructionManager* reconstruction_manager_; 114 | Thread* active_thread_; 115 | std::unique_ptr feature_extractor_; 116 | std::unique_ptr exhaustive_matcher_; 117 | std::unique_ptr sequential_matcher_; 118 | std::unique_ptr vocab_tree_matcher_; 119 | }; 120 | 121 | } // namespace colmap 122 | 123 | #endif // COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ 124 | -------------------------------------------------------------------------------- /controllers/bundle_adjustment.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 "controllers/bundle_adjustment.h" 33 | 34 | #include 35 | 36 | #include "optim/bundle_adjustment.h" 37 | #include "util/misc.h" 38 | 39 | namespace colmap { 40 | namespace { 41 | 42 | // Callback functor called after each bundle adjustment iteration. 43 | class BundleAdjustmentIterationCallback : public ceres::IterationCallback { 44 | public: 45 | explicit BundleAdjustmentIterationCallback(Thread* thread) 46 | : thread_(thread) {} 47 | 48 | virtual ceres::CallbackReturnType operator()( 49 | const ceres::IterationSummary& summary) { 50 | CHECK_NOTNULL(thread_); 51 | thread_->BlockIfPaused(); 52 | if (thread_->IsStopped()) { 53 | return ceres::SOLVER_TERMINATE_SUCCESSFULLY; 54 | } else { 55 | return ceres::SOLVER_CONTINUE; 56 | } 57 | } 58 | 59 | private: 60 | Thread* thread_; 61 | }; 62 | 63 | } // namespace 64 | 65 | BundleAdjustmentController::BundleAdjustmentController( 66 | const OptionManager& options, Reconstruction* reconstruction) 67 | : options_(options), reconstruction_(reconstruction) {} 68 | 69 | void BundleAdjustmentController::Run() { 70 | CHECK_NOTNULL(reconstruction_); 71 | 72 | PrintHeading1("Global bundle adjustment"); 73 | 74 | const std::vector& reg_image_ids = reconstruction_->RegImageIds(); 75 | 76 | if (reg_image_ids.size() < 2) { 77 | std::cout << "ERROR: Need at least two views." << std::endl; 78 | return; 79 | } 80 | 81 | // Avoid degeneracies in bundle adjustment. 82 | reconstruction_->FilterObservationsWithNegativeDepth(); 83 | 84 | BundleAdjustmentOptions ba_options = *options_.bundle_adjustment; 85 | ba_options.solver_options.minimizer_progress_to_stdout = true; 86 | 87 | BundleAdjustmentIterationCallback iteration_callback(this); 88 | ba_options.solver_options.callbacks.push_back(&iteration_callback); 89 | 90 | // Configure bundle adjustment. 91 | BundleAdjustmentConfig ba_config; 92 | for (const image_t image_id : reg_image_ids) { 93 | ba_config.AddImage(image_id); 94 | } 95 | // ba_config.SetConstantPose(100); 96 | //ba_config.SetConstantPose(2); 97 | // ba_config.SetConstantTvec(reg_image_ids[1], {0}); 98 | //std::ifstream unvalid_image_id("/boyuanlu_west/unvalid_image_id.txt"); 99 | //std::vectoru_image_id_all; 100 | //int u_image_id; 101 | //std::string image_name; 102 | //while(unvalid_image_id >> image_name) 103 | //{ 104 | // unvalid_image_id >> u_image_id; 105 | // u_image_id_all.push_back(u_image_id); 106 | //} 107 | //for(int i=0;i= 0.5: 30 | temp = 0.5 31 | elif temp <= -0.5: 32 | temp = -0.5 33 | else: 34 | pass 35 | euler.Pitch = asin(2 * temp) 36 | euler.Yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2)) 37 | if euler.Yaw < 0: 38 | euler.Yaw += 2 * pi 39 | return euler 40 | 41 | def Euler2quatern(euler): 42 | roll = euler.Roll / 2 43 | pitch = euler.Pitch / 2 44 | yaw = euler.Yaw / 2 45 | w = cos(roll) * cos(pitch) * cos(yaw) + \ 46 | sin(roll) * sin(pitch) * sin(yaw) 47 | x = sin(roll) * cos(pitch) * cos(yaw) - \ 48 | cos(roll) * sin(pitch) * sin(yaw) 49 | y = cos(roll) * sin(pitch) * cos(yaw) + \ 50 | sin(roll) * cos(pitch) * sin(yaw) 51 | z = cos(roll) * cos(pitch) * sin(yaw) + \ 52 | sin(roll) * sin(pitch) * cos(yaw) 53 | qua = [w, x, y, z] 54 | return qua 55 | 56 | def Euler2rotMat(euler): 57 | qua = Rot.Euler2quatern(euler) 58 | R = Rot.quatern2rotMat(qua) 59 | return R 60 | 61 | def rotMat2Euler(R): 62 | qua = Rot.rotMat2quatern(R) 63 | euler = Rot.quatern2Euler(qua) 64 | return(euler) 65 | def eulerAnglesToRotationMatrix(theta): 66 | R_x = np.array([[1, 0, 0], 67 | [0, math.cos(theta[0]), -math.sin(theta[0])], 68 | [0, math.sin(theta[0]), math.cos(theta[0])] 69 | ]) 70 | 71 | R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])], 72 | [0, 1, 0], 73 | [-math.sin(theta[1]), 0, math.cos(theta[1])] 74 | ]) 75 | 76 | R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], 77 | [math.sin(theta[2]), math.cos(theta[2]), 0], 78 | [0, 0, 1] 79 | ]) 80 | 81 | R = np.dot(R_z, np.dot(R_y, R_x)) 82 | return R 83 | def change_tvec_to_C(image_txt,output_path): 84 | newFile=open(os.path.join(output_path,"images.txt"),"w+") 85 | i=0 86 | with open(image_txt,"r+") as file: 87 | for line in file.readlines(): 88 | if i<4: 89 | newFile.writelines(line) 90 | i+=1 91 | continue 92 | value=line.strip().split() 93 | #start trans 94 | if len(value)==10: 95 | #eu=Rot.quatern2Euler([float(value[1]),float(value[2]),float(value[3]),float(value[4])]) 96 | r = RR.from_quat([float(value[2]),float(value[3]),float(value[4]),float(value[1])]) 97 | #R=eulerAnglesToRotationMatrix([eu.Roll,eu.Pitch,eu.Yaw]) 98 | R=r.as_matrix() 99 | tvec=np.array([float(value[5]),float(value[6]),float(value[7])]).reshape(3,1) 100 | c=np.dot(R.T,-tvec) 101 | newFile.writelines("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9}".format(value[0],\ 102 | value[1],value[2],value[3],value[4],c[0,0],c[1,0],c[2,0],value[8],value[9])) 103 | newFile.writelines("\n") 104 | else: 105 | newFile.writelines(line) 106 | newFile.close() 107 | def change_C_to_tvec(image_txt,output_path): 108 | # camera center to cv's tvec 109 | newFile=open(os.path.join(output_path,"images.txt"),"w+") 110 | i=0 111 | with open(image_txt,"r+") as file: 112 | for line in file.readlines(): 113 | if i<4: 114 | newFile.writelines(line) 115 | i+=1 116 | continue 117 | value=line.strip().split() 118 | #start trans 119 | if len(value)==10: 120 | #eu=Rot.quatern2Euler([float(value[1]),float(value[2]),float(value[3]),float(value[4])]) 121 | r = RR.from_quat([float(value[2]),float(value[3]),float(value[4]),float(value[1])]) 122 | #R=eulerAnglesToRotationMatrix([eu.Roll,eu.Pitch,eu.Yaw]) 123 | R=r.as_matrix() 124 | C=np.array([float(value[5]),float(value[6]),float(value[7])]).reshape(3,1) 125 | tvec=np.dot(R,-C) 126 | newFile.writelines("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9}".format(value[0],\ 127 | value[1],value[2], value[3],value[4],tvec[0,0],tvec[1,0],tvec[2,0],value[8],value[9])) 128 | newFile.writelines("\n") 129 | else: 130 | newFile.writelines(line) 131 | newFile.close() 132 | if __name__ == "__main__": 133 | parser = argparse.ArgumentParser() 134 | parser.add_argument("--input_path", required=True) 135 | parser.add_argument("--output_path",required=True) 136 | parser.add_argument("--flag",required=True) 137 | args = parser.parse_args() 138 | if args.flag=="1": # t to C 139 | change_tvec_to_C(args.input_path,args.output_path) 140 | else: # C to t 141 | change_C_to_tvec(args.input_path,args.output_path) 142 | -------------------------------------------------------------------------------- /optim/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 | set(FOLDER_NAME "optim") 33 | 34 | COLMAP_ADD_SOURCES( 35 | bundle_adjustment.h bundle_adjustment.cc 36 | combination_sampler.h combination_sampler.cc 37 | least_absolute_deviations.h least_absolute_deviations.cc 38 | progressive_sampler.h progressive_sampler.cc 39 | random_sampler.h random_sampler.cc 40 | sprt.h sprt.cc 41 | support_measurement.h support_measurement.cc 42 | ) 43 | 44 | COLMAP_ADD_TEST(bundle_adjustment_test bundle_adjustment_test.cc) 45 | COLMAP_ADD_TEST(combination_sampler_test combination_sampler_test.cc) 46 | COLMAP_ADD_TEST(least_absolute_deviations_test 47 | least_absolute_deviations_test.cc) 48 | COLMAP_ADD_TEST(loransac_test loransac_test.cc) 49 | COLMAP_ADD_TEST(progressive_sampler_test progressive_sampler_test.cc) 50 | COLMAP_ADD_TEST(random_sampler_test random_sampler_test.cc) 51 | COLMAP_ADD_TEST(ransac_test ransac_test.cc) 52 | COLMAP_ADD_TEST(support_measurement_test support_measurement_test.cc) 53 | -------------------------------------------------------------------------------- /optim/combination_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/combination_sampler.h" 33 | 34 | #include 35 | 36 | #include "util/math.h" 37 | #include "util/random.h" 38 | 39 | namespace colmap { 40 | 41 | CombinationSampler::CombinationSampler(const size_t num_samples) 42 | : num_samples_(num_samples) {} 43 | 44 | void CombinationSampler::Initialize(const size_t total_num_samples) { 45 | CHECK_LE(num_samples_, total_num_samples); 46 | total_sample_idxs_.resize(total_num_samples); 47 | // Note that the samples must be in increasing order for `NextCombination`. 48 | std::iota(total_sample_idxs_.begin(), total_sample_idxs_.end(), 0); 49 | } 50 | 51 | size_t CombinationSampler::MaxNumSamples() { 52 | return NChooseK(total_sample_idxs_.size(), num_samples_); 53 | } 54 | 55 | std::vector CombinationSampler::Sample() { 56 | std::vector sampled_idxs(num_samples_); 57 | for (size_t i = 0; i < num_samples_; ++i) { 58 | sampled_idxs[i] = total_sample_idxs_[i]; 59 | } 60 | 61 | if (!NextCombination(total_sample_idxs_.begin(), 62 | total_sample_idxs_.begin() + num_samples_, 63 | total_sample_idxs_.end())) { 64 | // Reached all possible combinations, so reset to original state. 65 | // Note that the samples must be in increasing order for `NextCombination`. 66 | std::iota(total_sample_idxs_.begin(), total_sample_idxs_.end(), 0); 67 | } 68 | 69 | return sampled_idxs; 70 | } 71 | 72 | } // namespace colmap 73 | -------------------------------------------------------------------------------- /optim/combination_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_COMBINATION_SAMPLER_H_ 33 | #define COLMAP_SRC_OPTIM_COMBINATION_SAMPLER_H_ 34 | 35 | #include "optim/sampler.h" 36 | 37 | namespace colmap { 38 | 39 | // Random sampler for RANSAC-based methods that generates unique samples. 40 | // 41 | // Note that a separate sampler should be instantiated per thread and it assumes 42 | // that the input data is shuffled in advance. 43 | class CombinationSampler : public Sampler { 44 | public: 45 | explicit CombinationSampler(const size_t num_samples); 46 | 47 | void Initialize(const size_t total_num_samples) override; 48 | 49 | size_t MaxNumSamples() override; 50 | 51 | std::vector Sample() override; 52 | 53 | private: 54 | const size_t num_samples_; 55 | std::vector total_sample_idxs_; 56 | }; 57 | 58 | } // namespace colmap 59 | 60 | #endif // COLMAP_SRC_OPTIM_COMBINATION_SAMPLER_H_ 61 | -------------------------------------------------------------------------------- /optim/combination_sampler_test.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 | #define TEST_NAME "optim/combination_sampler" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "optim/combination_sampler.h" 38 | #include "util/math.h" 39 | 40 | using namespace colmap; 41 | 42 | BOOST_AUTO_TEST_CASE(TestLessSamples) { 43 | CombinationSampler sampler(2); 44 | sampler.Initialize(5); 45 | BOOST_CHECK_EQUAL(sampler.MaxNumSamples(), 10); 46 | std::vector> sample_sets; 47 | for (size_t i = 0; i < 10; ++i) { 48 | const auto samples = sampler.Sample(); 49 | BOOST_CHECK_EQUAL(samples.size(), 2); 50 | sample_sets.emplace_back(samples.begin(), samples.end()); 51 | BOOST_CHECK_EQUAL(sample_sets.back().size(), 2); 52 | for (size_t j = 0; j < i; ++j) { 53 | BOOST_CHECK(sample_sets[j].count(samples[0]) == 0 || 54 | sample_sets[j].count(samples[1]) == 0); 55 | } 56 | } 57 | const auto samples = sampler.Sample(); 58 | BOOST_CHECK(sample_sets[0].count(samples[0]) == 1 && 59 | sample_sets[0].count(samples[1]) == 1); 60 | } 61 | 62 | BOOST_AUTO_TEST_CASE(TestEqualSamples) { 63 | CombinationSampler sampler(5); 64 | sampler.Initialize(5); 65 | BOOST_CHECK_EQUAL(sampler.MaxNumSamples(), 1); 66 | for (size_t i = 0; i < 100; ++i) { 67 | const auto samples = sampler.Sample(); 68 | BOOST_CHECK_EQUAL(samples.size(), 5); 69 | BOOST_CHECK_EQUAL( 70 | std::unordered_set(samples.begin(), samples.end()).size(), 5); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /optim/least_absolute_deviations.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/least_absolute_deviations.h" 33 | 34 | #include 35 | 36 | namespace colmap { 37 | namespace { 38 | 39 | Eigen::VectorXd Shrinkage(const Eigen::VectorXd& a, const double kappa) { 40 | const Eigen::VectorXd a_plus_kappa = a.array() + kappa; 41 | const Eigen::VectorXd a_minus_kappa = a.array() - kappa; 42 | return a_plus_kappa.cwiseMin(0) + a_minus_kappa.cwiseMax(0); 43 | } 44 | 45 | } // namespace 46 | 47 | bool SolveLeastAbsoluteDeviations(const LeastAbsoluteDeviationsOptions& options, 48 | const Eigen::SparseMatrix& A, 49 | const Eigen::VectorXd& b, 50 | Eigen::VectorXd* x) { 51 | CHECK_NOTNULL(x); 52 | CHECK_GT(options.rho, 0); 53 | CHECK_GT(options.alpha, 0); 54 | CHECK_GT(options.max_num_iterations, 0); 55 | CHECK_GE(options.absolute_tolerance, 0); 56 | CHECK_GE(options.relative_tolerance, 0); 57 | 58 | Eigen::SimplicialLLT> linear_solver; 59 | linear_solver.compute(A.transpose() * A); 60 | 61 | Eigen::VectorXd z = Eigen::VectorXd::Zero(A.rows()); 62 | Eigen::VectorXd z_old(A.rows()); 63 | Eigen::VectorXd u = Eigen::VectorXd::Zero(A.rows()); 64 | 65 | Eigen::VectorXd Ax(A.rows()); 66 | Eigen::VectorXd Ax_hat(A.rows()); 67 | 68 | const double b_norm = b.norm(); 69 | const double eps_pri_threshold = 70 | std::sqrt(A.rows()) * options.absolute_tolerance; 71 | const double eps_dual_threshold = 72 | std::sqrt(A.cols()) * options.absolute_tolerance; 73 | 74 | for (int i = 0; i < options.max_num_iterations; ++i) { 75 | *x = linear_solver.solve(A.transpose() * (b + z - u)); 76 | if (linear_solver.info() != Eigen::Success) { 77 | return false; 78 | } 79 | 80 | Ax = A * *x; 81 | Ax_hat = options.alpha * Ax + (1 - options.alpha) * (z + b); 82 | 83 | z_old = z; 84 | z = Shrinkage(Ax_hat - b + u, 1 / options.rho); 85 | 86 | u += Ax_hat - z - b; 87 | 88 | const double r_norm = (Ax - z - b).norm(); 89 | const double s_norm = (-options.rho * A.transpose() * (z - z_old)).norm(); 90 | const double eps_pri = 91 | eps_pri_threshold + options.relative_tolerance * 92 | std::max(b_norm, std::max(Ax.norm(), z.norm())); 93 | const double eps_dual = 94 | eps_dual_threshold + 95 | options.relative_tolerance * (options.rho * A.transpose() * u).norm(); 96 | 97 | if (r_norm < eps_pri && s_norm < eps_dual) { 98 | break; 99 | } 100 | } 101 | 102 | return true; 103 | } 104 | 105 | } // namespace colmap 106 | -------------------------------------------------------------------------------- /optim/least_absolute_deviations.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_LEAST_ABSOLUTE_DEVIATIONS_H_ 33 | #define COLMAP_SRC_OPTIM_LEAST_ABSOLUTE_DEVIATIONS_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include "util/logging.h" 39 | 40 | namespace colmap { 41 | 42 | struct LeastAbsoluteDeviationsOptions { 43 | // Augmented Lagrangian parameter. 44 | double rho = 1.0; 45 | 46 | // Over-relaxation parameter, typical values are between 1.0 and 1.8. 47 | double alpha = 1.0; 48 | 49 | // Maximum solver iterations. 50 | int max_num_iterations = 1000; 51 | 52 | // Absolute and relative solution thresholds, as suggested by Boyd et al. 53 | double absolute_tolerance = 1e-4; 54 | double relative_tolerance = 1e-2; 55 | }; 56 | 57 | // Least absolute deviations (LAD) fitting via ADMM by solving the problem: 58 | // 59 | // min || A x - b ||_1 60 | // 61 | // The solution is returned in the vector x and the iterative solver is 62 | // initialized with the given value. This implementation is based on the paper 63 | // "Distributed Optimization and Statistical Learning via the Alternating 64 | // Direction Method of Multipliers" by Boyd et al. and the Matlab implementation 65 | // at https://web.stanford.edu/~boyd/papers/admm/least_abs_deviations/lad.html 66 | bool SolveLeastAbsoluteDeviations(const LeastAbsoluteDeviationsOptions& options, 67 | const Eigen::SparseMatrix& A, 68 | const Eigen::VectorXd& b, Eigen::VectorXd* x); 69 | 70 | } // namespace colmap 71 | 72 | #endif // COLMAP_SRC_OPTIM_LEAST_ABSOLUTE_DEVIATIONS_H_ 73 | -------------------------------------------------------------------------------- /optim/least_absolute_deviations_test.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 | #define TEST_NAME "optim/least_absolute_deviations" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "optim/least_absolute_deviations.h" 38 | #include "util/random.h" 39 | 40 | using namespace colmap; 41 | 42 | BOOST_AUTO_TEST_CASE(TestOverDetermined) { 43 | Eigen::SparseMatrix A(4, 3); 44 | for (int i = 0; i < A.rows(); ++i) { 45 | for (int j = 0; j < A.cols(); ++j) { 46 | A.insert(i, j) = i * A.cols() + j + 1; 47 | } 48 | } 49 | A.coeffRef(0, 0) = 10; 50 | 51 | Eigen::VectorXd b(A.rows()); 52 | for (int i = 0; i < b.size(); ++i) { 53 | b(i) = i + 1; 54 | } 55 | 56 | Eigen::VectorXd x = Eigen::VectorXd::Zero(A.cols()); 57 | 58 | LeastAbsoluteDeviationsOptions options; 59 | BOOST_CHECK(SolveLeastAbsoluteDeviations(options, A, b, &x)); 60 | 61 | // Reference solution obtained with Boyd's Matlab implementation. 62 | const Eigen::Vector3d x_ref(0, 0, 1 / 3.0); 63 | BOOST_CHECK(x.isApprox(x_ref)); 64 | 65 | const Eigen::VectorXd residual = A * x - b; 66 | BOOST_CHECK_LE(residual.norm(), 1e-6); 67 | } 68 | 69 | BOOST_AUTO_TEST_CASE(TestWellDetermined) { 70 | Eigen::SparseMatrix A(3, 3); 71 | for (int i = 0; i < A.rows(); ++i) { 72 | for (int j = 0; j < A.cols(); ++j) { 73 | A.insert(i, j) = i * A.cols() + j + 1; 74 | } 75 | } 76 | A.coeffRef(0, 0) = 10; 77 | 78 | Eigen::VectorXd b(A.rows()); 79 | for (int i = 0; i < b.size(); ++i) { 80 | b(i) = i + 1; 81 | } 82 | 83 | Eigen::VectorXd x = Eigen::VectorXd::Zero(A.cols()); 84 | 85 | LeastAbsoluteDeviationsOptions options; 86 | BOOST_CHECK(SolveLeastAbsoluteDeviations(options, A, b, &x)); 87 | 88 | // Reference solution obtained with Boyd's Matlab implementation. 89 | const Eigen::Vector3d x_ref(0, 0, 1 / 3.0); 90 | BOOST_CHECK(x.isApprox(x_ref)); 91 | 92 | const Eigen::VectorXd residual = A * x - b; 93 | BOOST_CHECK_LE(residual.norm(), 1e-6); 94 | } 95 | 96 | BOOST_AUTO_TEST_CASE(TestUnderDetermined) { 97 | // In this case, the system is rank-deficient and not positive semi-definite. 98 | Eigen::SparseMatrix A(2, 3); 99 | Eigen::VectorXd b(A.rows()); 100 | Eigen::VectorXd x = Eigen::VectorXd::Zero(A.cols()); 101 | LeastAbsoluteDeviationsOptions options; 102 | BOOST_CHECK(!SolveLeastAbsoluteDeviations(options, A, b, &x)); 103 | } 104 | -------------------------------------------------------------------------------- /optim/loransac_test.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 | #define TEST_NAME "optim/ransac" 33 | #include "util/testing.h" 34 | 35 | #include 36 | #include 37 | 38 | #include "base/pose.h" 39 | #include "base/similarity_transform.h" 40 | #include "estimators/similarity_transform.h" 41 | #include "optim/loransac.h" 42 | #include "util/random.h" 43 | 44 | using namespace colmap; 45 | 46 | BOOST_AUTO_TEST_CASE(TestReport) { 47 | LORANSAC, 48 | SimilarityTransformEstimator<3>>::Report report; 49 | BOOST_CHECK_EQUAL(report.success, false); 50 | BOOST_CHECK_EQUAL(report.num_trials, 0); 51 | BOOST_CHECK_EQUAL(report.support.num_inliers, 0); 52 | BOOST_CHECK_EQUAL(report.support.residual_sum, 53 | std::numeric_limits::max()); 54 | BOOST_CHECK_EQUAL(report.inlier_mask.size(), 0); 55 | } 56 | 57 | BOOST_AUTO_TEST_CASE(TestSimilarityTransform) { 58 | SetPRNGSeed(0); 59 | 60 | const size_t num_samples = 1000; 61 | const size_t num_outliers = 400; 62 | 63 | // Create some arbitrary transformation. 64 | const SimilarityTransform3 orig_tform(2, ComposeIdentityQuaternion(), 65 | Eigen::Vector3d(100, 10, 10)); 66 | 67 | // Generate exact data 68 | std::vector src; 69 | std::vector dst; 70 | for (size_t i = 0; i < num_samples; ++i) { 71 | src.emplace_back(i, std::sqrt(i) + 2, std::sqrt(2 * i + 2)); 72 | dst.push_back(src.back()); 73 | orig_tform.TransformPoint(&dst.back()); 74 | } 75 | 76 | // Add some faulty data. 77 | for (size_t i = 0; i < num_outliers; ++i) { 78 | dst[i] = Eigen::Vector3d(RandomReal(-3000.0, -2000.0), 79 | RandomReal(-4000.0, -3000.0), 80 | RandomReal(-5000.0, -4000.0)); 81 | } 82 | 83 | // Robustly estimate transformation using RANSAC. 84 | RANSACOptions options; 85 | options.max_error = 10; 86 | LORANSAC, SimilarityTransformEstimator<3>> 87 | ransac(options); 88 | const auto report = ransac.Estimate(src, dst); 89 | 90 | BOOST_CHECK_EQUAL(report.success, true); 91 | BOOST_CHECK_GT(report.num_trials, 0); 92 | 93 | // Make sure outliers were detected correctly. 94 | BOOST_CHECK_EQUAL(report.support.num_inliers, num_samples - num_outliers); 95 | for (size_t i = 0; i < num_samples; ++i) { 96 | if (i < num_outliers) { 97 | BOOST_CHECK(!report.inlier_mask[i]); 98 | } else { 99 | BOOST_CHECK(report.inlier_mask[i]); 100 | } 101 | } 102 | 103 | // Make sure original transformation is estimated correctly. 104 | const double matrix_diff = 105 | (orig_tform.Matrix().topLeftCorner<3, 4>() - report.model).norm(); 106 | BOOST_CHECK(std::abs(matrix_diff) < 1e-6); 107 | } 108 | -------------------------------------------------------------------------------- /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_ *= static_cast(num_samples_ - i) / (total_num_samples_ - i); 65 | } 66 | } 67 | 68 | size_t ProgressiveSampler::MaxNumSamples() { 69 | return std::numeric_limits::max(); 70 | } 71 | 72 | std::vector ProgressiveSampler::Sample() { 73 | t_ += 1; 74 | 75 | // Compute T_n_p_ using recurrent relation in equation 3 (second part). 76 | if (t_ == T_n_p_ && n_ < total_num_samples_) { 77 | const double T_n_plus_1 = T_n_ * (n_ + 1.0) / (n_ + 1.0 - num_samples_); 78 | T_n_p_ += std::ceil(T_n_plus_1 - T_n_); 79 | T_n_ = T_n_plus_1; 80 | n_ += 1; 81 | } 82 | 83 | // Decide how many samples to draw from which part of the data as 84 | // specified in equation 5. 85 | size_t num_random_samples = num_samples_; 86 | size_t max_random_sample_idx = n_ - 1; 87 | if (T_n_p_ >= t_) { 88 | num_random_samples -= 1; 89 | max_random_sample_idx -= 1; 90 | } 91 | 92 | // Draw semi-random samples as described in algorithm 1. 93 | std::vector sampled_idxs; 94 | sampled_idxs.reserve(num_samples_); 95 | for (size_t i = 0; i < num_random_samples; ++i) { 96 | while (true) { 97 | const size_t random_idx = 98 | RandomInteger(0, max_random_sample_idx); 99 | if (!VectorContainsValue(sampled_idxs, random_idx)) { 100 | sampled_idxs.push_back(random_idx); 101 | break; 102 | } 103 | } 104 | } 105 | 106 | // In progressive sampling mode, the last element is mandatory. 107 | if (T_n_p_ >= t_) { 108 | sampled_idxs.push_back(n_); 109 | } 110 | 111 | return sampled_idxs; 112 | } 113 | 114 | } // namespace colmap 115 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /optim/progressive_sampler_test.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 | #define TEST_NAME "optim/progressive_sampler" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "optim/progressive_sampler.h" 38 | 39 | using namespace colmap; 40 | 41 | BOOST_AUTO_TEST_CASE(TestLessSamples) { 42 | ProgressiveSampler sampler(2); 43 | sampler.Initialize(5); 44 | BOOST_CHECK_EQUAL(sampler.MaxNumSamples(), 45 | std::numeric_limits::max()); 46 | for (size_t i = 0; i < 100; ++i) { 47 | const auto samples = sampler.Sample(); 48 | BOOST_CHECK_EQUAL(samples.size(), 2); 49 | BOOST_CHECK_EQUAL( 50 | std::unordered_set(samples.begin(), samples.end()).size(), 2); 51 | } 52 | } 53 | 54 | BOOST_AUTO_TEST_CASE(TestEqualSamples) { 55 | ProgressiveSampler sampler(5); 56 | sampler.Initialize(5); 57 | BOOST_CHECK_EQUAL(sampler.MaxNumSamples(), 58 | std::numeric_limits::max()); 59 | for (size_t i = 0; i < 100; ++i) { 60 | const auto samples = sampler.Sample(); 61 | BOOST_CHECK_EQUAL(samples.size(), 5); 62 | BOOST_CHECK_EQUAL( 63 | std::unordered_set(samples.begin(), samples.end()).size(), 5); 64 | } 65 | } 66 | 67 | BOOST_AUTO_TEST_CASE(TestProgressive) { 68 | const size_t kNumSamples = 5; 69 | ProgressiveSampler sampler(kNumSamples); 70 | sampler.Initialize(50); 71 | size_t prev_last_sample = 5; 72 | for (size_t i = 0; i < 100; ++i) { 73 | const auto samples = sampler.Sample(); 74 | for (size_t i = 0; i < samples.size() - 1; ++i) { 75 | BOOST_CHECK_LT(samples[i], samples.back()); 76 | BOOST_CHECK_GE(samples.back(), prev_last_sample); 77 | prev_last_sample = samples.back(); 78 | } 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /optim/random_sampler_test.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 | #define TEST_NAME "optim/random_sampler" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "optim/random_sampler.h" 38 | 39 | using namespace colmap; 40 | 41 | BOOST_AUTO_TEST_CASE(TestLessSamples) { 42 | RandomSampler sampler(2); 43 | sampler.Initialize(5); 44 | BOOST_CHECK_EQUAL(sampler.MaxNumSamples(), 45 | std::numeric_limits::max()); 46 | for (size_t i = 0; i < 100; ++i) { 47 | const auto samples = sampler.Sample(); 48 | BOOST_CHECK_EQUAL(samples.size(), 2); 49 | BOOST_CHECK_EQUAL( 50 | std::unordered_set(samples.begin(), samples.end()).size(), 2); 51 | } 52 | } 53 | 54 | BOOST_AUTO_TEST_CASE(TestEqualSamples) { 55 | RandomSampler sampler(5); 56 | sampler.Initialize(5); 57 | BOOST_CHECK_EQUAL(sampler.MaxNumSamples(), 58 | std::numeric_limits::max()); 59 | for (size_t i = 0; i < 100; ++i) { 60 | const auto samples = sampler.Sample(); 61 | BOOST_CHECK_EQUAL(samples.size(), 5); 62 | BOOST_CHECK_EQUAL( 63 | std::unordered_set(samples.begin(), samples.end()).size(), 5); 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /optim/ransac_test.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 | #define TEST_NAME "optim/ransac" 33 | #include "util/testing.h" 34 | 35 | #include 36 | #include 37 | 38 | #include "base/pose.h" 39 | #include "base/similarity_transform.h" 40 | #include "estimators/similarity_transform.h" 41 | #include "optim/ransac.h" 42 | #include "util/random.h" 43 | 44 | using namespace colmap; 45 | 46 | BOOST_AUTO_TEST_CASE(TestOptions) { 47 | RANSACOptions options; 48 | BOOST_CHECK_EQUAL(options.max_error, 0); 49 | BOOST_CHECK_EQUAL(options.min_inlier_ratio, 0.1); 50 | BOOST_CHECK_EQUAL(options.confidence, 0.99); 51 | BOOST_CHECK_EQUAL(options.min_num_trials, 0); 52 | BOOST_CHECK_EQUAL(options.max_num_trials, std::numeric_limits::max()); 53 | } 54 | 55 | BOOST_AUTO_TEST_CASE(TestReport) { 56 | RANSAC>::Report report; 57 | BOOST_CHECK_EQUAL(report.success, false); 58 | BOOST_CHECK_EQUAL(report.num_trials, 0); 59 | BOOST_CHECK_EQUAL(report.support.num_inliers, 0); 60 | BOOST_CHECK_EQUAL(report.support.residual_sum, 61 | std::numeric_limits::max()); 62 | BOOST_CHECK_EQUAL(report.inlier_mask.size(), 0); 63 | } 64 | 65 | BOOST_AUTO_TEST_CASE(TestNumTrials) { 66 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 67 | 1, 100, 0.99, 1.0), 68 | 4605168); 69 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 70 | 10, 100, 0.99, 1.0), 71 | 4603); 72 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 73 | 10, 100, 0.999, 1.0), 74 | 6905); 75 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 76 | 10, 100, 0.999, 2.0), 77 | 13809); 78 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 79 | 100, 100, 0.99, 1.0), 80 | 1); 81 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 82 | 100, 100, 0.999, 1.0), 83 | 1); 84 | BOOST_CHECK_EQUAL(RANSAC>::ComputeNumTrials( 85 | 100, 100, 0, 1.0), 86 | 1); 87 | } 88 | 89 | BOOST_AUTO_TEST_CASE(TestSimilarityTransform) { 90 | SetPRNGSeed(0); 91 | 92 | const size_t num_samples = 1000; 93 | const size_t num_outliers = 400; 94 | 95 | // Create some arbitrary transformation. 96 | const SimilarityTransform3 orig_tform(2, ComposeIdentityQuaternion(), 97 | Eigen::Vector3d(100, 10, 10)); 98 | 99 | // Generate exact data. 100 | std::vector src; 101 | std::vector dst; 102 | for (size_t i = 0; i < num_samples; ++i) { 103 | src.emplace_back(i, std::sqrt(i) + 2, std::sqrt(2 * i + 2)); 104 | dst.push_back(src.back()); 105 | orig_tform.TransformPoint(&dst.back()); 106 | } 107 | 108 | // Add some faulty data. 109 | for (size_t i = 0; i < num_outliers; ++i) { 110 | dst[i] = Eigen::Vector3d(RandomReal(-3000.0, -2000.0), 111 | RandomReal(-4000.0, -3000.0), 112 | RandomReal(-5000.0, -4000.0)); 113 | } 114 | 115 | // Robustly estimate transformation using RANSAC. 116 | RANSACOptions options; 117 | options.max_error = 10; 118 | RANSAC> ransac(options); 119 | const auto report = ransac.Estimate(src, dst); 120 | 121 | BOOST_CHECK_EQUAL(report.success, true); 122 | BOOST_CHECK_GT(report.num_trials, 0); 123 | 124 | // Make sure outliers were detected correctly. 125 | BOOST_CHECK_EQUAL(report.support.num_inliers, num_samples - num_outliers); 126 | for (size_t i = 0; i < num_samples; ++i) { 127 | if (i < num_outliers) { 128 | BOOST_CHECK(!report.inlier_mask[i]); 129 | } else { 130 | BOOST_CHECK(report.inlier_mask[i]); 131 | } 132 | } 133 | 134 | // Make sure original transformation is estimated correctly. 135 | const double matrix_diff = 136 | (orig_tform.Matrix().topLeftCorner<3, 4>() - report.model).norm(); 137 | BOOST_CHECK(std::abs(matrix_diff) < 1e-6); 138 | } 139 | -------------------------------------------------------------------------------- /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 | #include "util/logging.h" 39 | 40 | namespace colmap { 41 | 42 | // Abstract base class for sampling methods. 43 | class Sampler { 44 | public: 45 | Sampler(){}; 46 | explicit Sampler(const size_t num_samples); 47 | 48 | // Initialize the sampler, before calling the `Sample` method. 49 | virtual void Initialize(const size_t total_num_samples) = 0; 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 | -------------------------------------------------------------------------------- /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 = (1 - options_.delta) * 74 | std::log((1 - options_.delta) / (1 - options_.epsilon)) + 75 | options_.delta * std::log(options_.delta / options_.epsilon); 76 | 77 | // Equation 6 78 | const double A0 = 79 | options_.eval_time_ratio * C / options_.num_models_per_sample + 1; 80 | 81 | double A = A0; 82 | 83 | const double kEps = 1.5e-8; 84 | 85 | // Compute A using the recursive relation 86 | // A* = lim(n->inf) A 87 | // The series typically converges within 4 iterations 88 | 89 | for (size_t i = 0; i < 100; ++i) { 90 | const double A1 = A0 + std::log(A); 91 | 92 | if (std::abs(A1 - A) < kEps) { 93 | break; 94 | } 95 | 96 | A = A1; 97 | } 98 | 99 | decision_threshold_ = A; 100 | } 101 | 102 | } // namespace colmap 103 | -------------------------------------------------------------------------------- /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 sample 55 | // 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. 1-3 60 | // for the 7-point fundamental matrix algorithm, or 1-10 for the 5-point 61 | // 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, const double max_residual, 70 | size_t* num_inliers, size_t* num_eval_samples); 71 | 72 | private: 73 | void UpdateDecisionThreshold(); 74 | 75 | Options options_; 76 | double delta_epsilon_; 77 | double delta_1_epsilon_1_; 78 | double decision_threshold_; 79 | }; 80 | 81 | } // namespace colmap 82 | 83 | #endif // COLMAP_SRC_OPTIM_SPRT_H_ 84 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /optim/support_measurement_test.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 | #define TEST_NAME "optim/support_measurement" 33 | #include "util/testing.h" 34 | 35 | #include 36 | 37 | #include "optim/support_measurement.h" 38 | #include "util/math.h" 39 | 40 | using namespace colmap; 41 | 42 | BOOST_AUTO_TEST_CASE(TestInlierSupportMeasuremer) { 43 | InlierSupportMeasurer::Support support1; 44 | BOOST_CHECK_EQUAL(support1.num_inliers, 0); 45 | BOOST_CHECK_EQUAL(support1.residual_sum, std::numeric_limits::max()); 46 | InlierSupportMeasurer measurer; 47 | std::vector residuals = {-1.0, 0.0, 1.0, 2.0}; 48 | support1 = measurer.Evaluate(residuals, 1.0); 49 | BOOST_CHECK_EQUAL(support1.num_inliers, 3); 50 | BOOST_CHECK_EQUAL(support1.residual_sum, 0.0); 51 | InlierSupportMeasurer::Support support2; 52 | support2.num_inliers = 2; 53 | BOOST_CHECK(measurer.Compare(support1, support2)); 54 | BOOST_CHECK(!measurer.Compare(support2, support1)); 55 | support2.residual_sum = support1.residual_sum; 56 | BOOST_CHECK(measurer.Compare(support1, support2)); 57 | BOOST_CHECK(!measurer.Compare(support2, support1)); 58 | support2.num_inliers = support1.num_inliers; 59 | support2.residual_sum += 0.01; 60 | BOOST_CHECK(measurer.Compare(support1, support2)); 61 | BOOST_CHECK(!measurer.Compare(support2, support1)); 62 | support2.residual_sum -= 0.01; 63 | BOOST_CHECK(!measurer.Compare(support1, support2)); 64 | BOOST_CHECK(!measurer.Compare(support2, support1)); 65 | support2.residual_sum -= 0.01; 66 | BOOST_CHECK(!measurer.Compare(support1, support2)); 67 | BOOST_CHECK(measurer.Compare(support2, support1)); 68 | } 69 | 70 | BOOST_AUTO_TEST_CASE(TestMEstimatorSupportMeasurer) { 71 | MEstimatorSupportMeasurer::Support support1; 72 | BOOST_CHECK_EQUAL(support1.num_inliers, 0); 73 | BOOST_CHECK_EQUAL(support1.score, std::numeric_limits::max()); 74 | MEstimatorSupportMeasurer measurer; 75 | std::vector residuals = {-1.0, 0.0, 1.0, 2.0}; 76 | support1 = measurer.Evaluate(residuals, 1.0); 77 | BOOST_CHECK_EQUAL(support1.num_inliers, 3); 78 | BOOST_CHECK_EQUAL(support1.score, 1.0); 79 | MEstimatorSupportMeasurer::Support support2 = support1; 80 | BOOST_CHECK(!measurer.Compare(support1, support2)); 81 | BOOST_CHECK(!measurer.Compare(support2, support1)); 82 | support2.num_inliers -= 1; 83 | support2.score += 0.01; 84 | BOOST_CHECK(measurer.Compare(support1, support2)); 85 | BOOST_CHECK(!measurer.Compare(support2, support1)); 86 | support2.score -= 0.01; 87 | BOOST_CHECK(!measurer.Compare(support1, support2)); 88 | BOOST_CHECK(!measurer.Compare(support2, support1)); 89 | support2.score -= 0.01; 90 | BOOST_CHECK(!measurer.Compare(support1, support2)); 91 | BOOST_CHECK(measurer.Compare(support2, support1)); 92 | } 93 | --------------------------------------------------------------------------------